CN111351482A - 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 - Google Patents
基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 Download PDFInfo
- Publication number
- CN111351482A CN111351482A CN202010193890.8A CN202010193890A CN111351482A CN 111351482 A CN111351482 A CN 111351482A CN 202010193890 A CN202010193890 A CN 202010193890A CN 111351482 A CN111351482 A CN 111351482A
- Authority
- CN
- China
- Prior art keywords
- noise
- error state
- measurement
- state
- equation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 29
- 238000000034 method Methods 0.000 title claims abstract description 29
- 238000005259 measurement Methods 0.000 claims abstract description 90
- 238000012545 processing Methods 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 51
- 230000001133 acceleration Effects 0.000 claims description 19
- 239000000203 mixture Substances 0.000 claims description 4
- 238000012937 correction Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 235000008331 Pinus X rigitaeda Nutrition 0.000 claims description 2
- 235000011613 Pinus brutia Nutrition 0.000 claims description 2
- 241000018646 Pinus brutia Species 0.000 claims description 2
- 230000004069 differentiation Effects 0.000 claims description 2
- 238000011156 evaluation Methods 0.000 claims description 2
- 230000005358 geomagnetic field Effects 0.000 claims description 2
- 230000007704 transition Effects 0.000 claims description 2
- 238000004364 calculation method Methods 0.000 abstract description 4
- 230000003068 static effect Effects 0.000 description 15
- 238000004088 simulation Methods 0.000 description 14
- 238000010586 diagram Methods 0.000 description 5
- 230000004927 fusion Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 2
- 229910000831 Steel Inorganic materials 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 230000000644 propagated effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000010959 steel Substances 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法。该方法包括:首先,建立传感器的测量模型,推导四元数形式的导航方程,并对该方程进行线性化处理得到误差状态方程;然后,基于GPS、磁力计和加速度计的测量信息,建立用于卡尔曼滤波估计的测量方程;最后,结合惯性‑GPS松组合模式,采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计,并实现对导航状态的校正。本发明使用四元数时计算量较小,而且能够消除惯性导航***存在的累计误差,实现高精度导航。
Description
技术领域
本发明属于导航技术领域,特别是一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法。
背景技术
导航***及其配套子***是自主无人机研究的重点。自主导航***是利用各个子***提供的信息来实现三个基本任务的***,三个基本任务包括:1)定位:估计无人机的位置;2)障碍检测和躲避:识别周围障碍物并因此采取行动以躲避它们;3)发出控制指令:发送指令以使无人机稳定姿态并遵循导航指令。
随着SUAVs(小型无人机)和MAVs(微型无人机)的出现,比如Parrot公司的Ar.Drone Parrot、大疆无人机公司的DJI Phantom series、Walkera公司的Voyager3、TALIH500和3D Robotics公司的3DR SOLO等,无人机的尺寸越来越小、重量越来越小,导致有效载荷变小,需要找到合适的板载传感器来实现相应的导航目的。
在大多数用于IMU姿态估计的卡尔曼滤波方法中,首先通过积分来自陀螺仪的角速率传播姿态,以用作输入信号,然后使用来自加速度计和磁力计的信息更新姿态。在静态或低动力条件下,加速度计可测量地球的重力并提供横滚和俯仰的信息,而磁力计主要是通过测量地球的磁场来提供偏航信息。由于除了地球赤道以外,磁场的倾斜角都不为零,因此磁力计还提供了一些关于俯仰和横滚的信息。所以,可以将同时使用加速度计和磁力计测量的信息融合过程用作卡尔曼滤波方案中的测量更新过程。然而,由于测量噪声的存在,IMU对位置的估计存在长期不稳定性,借助GPS进行辅助导航是一种较为实际的方案。
GPS/IMU组合导航***是将来自IMU和GPS的信息融合在一起以补偿两个传感器产生的误差,提高定位过程的精度。因为GPS和IMU测量噪声近似为高斯噪声,所以线性卡尔曼滤波器能够将来自多个天线GPS的数据与来自机载IMU的信息融合。为了减小位置误差,文献1(Barczyk M,Lynch AF.Integration of a Triaxial Magnetometer into aHelicopter UAV GPS-Aided INS[J].IEEE Transactions on Aerospace and ElectronicSystems,2012,48(4):2947-2960.)采用扩展卡尔曼滤波器,文献2(Yang Yuhong,Zhou J,Loffeld O.Quaternion-based Kalman filtering on INS/GPS[C]Information Fusion(FUSION),2012 15th International Conference.IEEE,2012:511-518.)分析了使用基于四元数的EKF和UKF算法的紧密耦合的IMU/GPS***的***性能。然而,这些方法都是直接法,即直接以导航参数作为估计对象,由于各子***的误差源和量测误差都是随机的,导致惯性导航***存在大量累计误差,降低了导航精度。
发明内容
本发明的目的在于提供一种精度高、实时性好、计算量小的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法。
实现本发明目的的技术解决方案为:一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法,包括以下步骤:
步骤1,建立传感器的测量模型,推导四元数形式的导航方程,并对该方程进行线性化处理得到误差状态方程;
步骤2,基于GPS、磁力计和加速度计的测量信息,建立用于卡尔曼滤波估计的测量方程;
步骤3,结合惯性-GPS松组合模式,采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计,并实现对导航状态的校正。
本发明与现有技术相比,其显著优点为:(1)采用误差状态卡尔曼滤波方法ESKF,误差状态较小并且位于原点附近,从而避免了可能的奇异性和万向节锁定问题,同时始终确保线性化的有效性;(2)ESKF方法可确保数值稳定性,并允许四元数以小信号表示进行处理,使得雅可比矩阵的计算相对容易,减少了计算量;(3)将ESKF用作基本滤波器,并将GPS的测量信息加入到IMU测量信息中进行信息融合,获得多旋翼状态的优化估计,并提高***的容错性;(4)能够消除惯性导航***存在的累计误差,实现高精度导航,且实时性好。
附图说明
图1为本发明基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法的流程图。
图2为本发明基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法的原理图。
图3为本发明实施例提供的MPU9250模型示意图,其中(a)为加速度和陀螺仪方向和极性示意图,(b)为磁力计方向示意图。
图4为本发明实施例提供的GPS测量状态仿真曲线图,其中(a)为静态GPS位置测量曲线图,(b)为静态GPS速度测量曲线图。
图5为本发明实施例提供的静态组合导航姿态估计仿真曲线图,其中(a)为静态组合导航姿态估计状态曲线图,(b)为静态组合导航姿态误差状态曲线图。
图6为本发明实施例提供的静态组合导航速度估计仿真曲线图,其中(a)为静态组合导航速度估计状态曲线图,(b)为静态组合导航速度误差状态曲线图。
图7为本发明实施例提供的静态组合导航位置估计仿真曲线图,其中(a)为静态组合导航位置估计状态曲线图,(b)为静态组合导航位置误差状态曲线图。
图8为本发明实施例提供的动态组合导航姿态估计仿真曲线图,其中(a)为动态组合导航姿态估计状态曲线图,(b)为动态组合导航姿态误差状态曲线图。
图9为本发明实施例提供的动态组合导航速度估计仿真曲线图,其中(a)为动态组合导航速度估计状态曲线图,(b)为动态组合导航速度误差状态曲线图。
图10为本发明实施例提供的动态组合导航位置估计仿真曲线图,其中(a)为动态组合导航位置估计状态曲线图,(b)为动态组合导航位置误差状态曲线图。
具体实施例
本发明基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法,包括以下步骤:
步骤1,建立传感器的测量模型,推导四元数形式的导航方程,并对该方程进行线性化处理得到误差状态方程;
步骤2,基于GPS、磁力计和加速度计的测量信息,建立用于卡尔曼滤波估计的测量方程;
步骤3,结合惯性-GPS松组合模式,采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计,并实现对导航状态的校正。
进一步地,步骤1所述建立传感器的测量模型,具体如下:
基于惯性+GPS组合导航的信息测量元件包括陀螺仪、加速度计、磁力计和GPS,做出以下4条假设:
(1)陀螺仪和加速度计测量信息的零位偏差为固定偏差;
(2)陀螺仪和加速度计的测量噪声为高斯噪声,满足正态分布;
(3)磁力计不存在零位偏差,测量噪声为高斯噪声,满足正态分布;
(4)GPS不存在零位偏差,测量噪声为高斯噪声,满足正态分布;
基于以上假设,陀螺仪的测量模型为:
加速度计的测量模型为:
磁力计的测量模型为:
其中,σm磁力计噪声的标准差。
进一步地,步骤1所述推导四元数形式的导航方程,并对该方程进行线性化处理得到误差状态方程,具体如下:
基于误差状态卡尔曼滤波器ESKF的工作原理,将真实状态向量x视为标称状态向量xnom和误差状态向量δx的组合,其中误差状态表示标称状态与真实状态之间的差,产生以下关系:
四元数误差状态在机体系中的定义为:
所述用于导航信息估计的ESKF状态由包含19个元素的真实状态向量x,包含19个元素的标称状态向量xnom和包含18个元素的误差状态向量δx组成,表示如下:
其中,r=[xn,yn,zn]Τ是多旋翼在导航系下的位置,v=[vxn,vyn,vzn]Τ是多旋翼在导航系下的速度;q=[q0 q1 q2 q3]Τ为四元数,表示多旋翼的姿态;bg=[bωx,bωy,bωz]Τ是三轴陀螺仪的固定零偏,ba=[bax,bay,baz]Τ是三轴加速度计的固定零偏,g=[0,0,g]Τ是重力加速度在导航系下的表示;
所述标称状态运动学方程为:
其中,k表示tk时刻,k+1表示tk+1时刻,xk=x(tk),tk=kΔt,Δt=tk+1-tk;
所述误差状态运动学方程为:
其中,δg表示重力加速度误差;vi,θi,bai,bgi是速度噪声、角度噪声、加速度偏差噪声和陀螺仪偏差噪声的离散化能量;vi,θi,bai,bgi的均值是0,协方差矩阵通过对na,ng,ba,bg的方差在间隔时间Δt内进行积分获得:
其中,σa,σg根据IMU数据表中的信息确定,σa单位为m/s2,σg单位为rad/s;σwa是加速度偏差噪声的标准差,σwg是陀螺仪偏差噪声的标准差,Vi是速度噪声的协方差矩阵、Θi是角度噪声的协方差矩阵、Ai是加速度偏差噪声的协方差矩阵、Ωi是陀螺仪偏差噪声的协方差矩阵;
所述扰动向量wi=[vi,θi,ai,ωi]Τ,那么误差状态方程为:
δxk+1=Fx,kδxk+Fi,kwi,k (14)
其中,Fx是误差状态转移矩阵,Fi是扰动向量wi的雅可比矩阵,Fx和Fi表达式如下式所示:
误差状态卡尔曼滤波器的误差状态的更新方程为:
误差状态卡尔曼滤波器的协方差Pk的更新方程为:
其中,Qi是***噪声的协方差矩阵,表达式如下式所示:
进一步地,步骤2所述基于GPS、磁力计和加速度计的测量信息,建立用于卡尔曼滤波估计的测量方程,测量方程包括GPS测量方程、加速度计测量方程、磁力计测量方程;
所述GPS的位置测量噪声用υpos表示,位置测量噪声的标准差用σr表示,速度测量噪声用υvel表示,速度测量噪声的标准差用σv表示;
所述导航系下的位置和经纬度关系为:
其中,地球的经度、纬度和高度分别为L,λ,h,参考椭球子午面曲率半径用RM表示,卯酉面曲率半径用RN表示,[δxn,δyn,δzn]Τ是导航系下的位置变化量;
所述多旋翼的经纬高度和导航系下位置的关系为:
其中,[L0,λ0,h0]Τ是起飞点的经纬高度,[Lm,λm,hm]Τ是经纬高度的测量值;υpos是经纬高度的测量噪声,满足正态分布;
所述加速度计的测量方程为:
所述磁力计的测量方程为:
其中,地磁场强度用M表示,磁偏角用D表示,磁倾角用I表示;
设测量状态向量为y,组成为y=[Lm,λm,hm,vxm,vym,vzm,am,Mm]Τ,测量噪声为υ,组成为υ=[υpos,υvel,na,nM,wa]Τ,所述测量方程为:
yk=h(xnom,k)+υk (24)
其中,h()是***状态的非线性函数,υ是方差为V的高斯噪声,满足υ~N{0,V}。
进一步地,步骤3所述结合惯性-GPS松组合模式,采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计,并实现对导航状态的校正,具体如下:
根据tk+1时刻的测量值yk+1,通过滤波器估计误差状态,得到滤波器增益矩阵:
Kk+1=Pk+1/kHk+1 Τ(Hk+1Pk+1/kHk+1 Τ+Vk+1)-1 (25)
其中,Pk+1/k为第k+1时刻误差状态的协方差的预测值,Hk+1是第k+1时刻h()关于误差状态δx的雅可比矩阵,Vk+1是第k+1时刻测量方程噪声的协方差矩阵,Kk+1是第k+1时刻的滤波器增益矩阵;
所述tk+1时刻的误差状态的最优估计为:
协方差矩阵为:
Pk+1/k+1=(I-Kk+1Hk+1)Pk+1/k (27)
所述雅可比矩阵利用链式规则来计算:
其中,Hx是h()的标准雅可比矩阵,取决于所使用的传感器的测量功能;Xδx是真实状态关于误差状态的雅可比行列式,由于仅取决于状态的ESKF组成,因此得到:
化简得Qδθ:
推导出:
相对于标称状态的雅可比Hx满足:
获得ESKF更新方程之后,通过误差状态在tk+1时刻的近似最优估计,对标称状态进行更新,得到:
下面结合附图及具体实施例对本发明做进一步详细说明。
实施例
结合图1~2,本实施例提供一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法,包括:
S1:首先,建立传感器的测量模型,推导四元数形式的导航方程,并对该方程进行线性化处理得到误差状态方程;
S2:然后,基于GPS、磁力计和加速度计的测量信息建立用于卡尔曼滤波估计的测量方程;
S3:最后,结合惯性-GPS松组合思想,采用误差状态卡尔曼滤波器(ESKF)对误差状态进行最优估计,并实现对导航状态的校正。
本实施例所述的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法,首先建立四元数形式的导航方程并对该方程进行线性化,得到误差状态方程;然后,基于GPS、磁力计和加速度计的测量信息建立用于卡尔曼滤波估计的测量方程;最后,结合惯性-GPS松组合思想,采用误差状态卡尔曼滤波器(ESKF)对误差状态进行最优估计,并实现对导航状态的校正。该方法的优势在于,和欧拉角及方向余弦矩阵相比,使用四元数时计算量较小,而且能够消除惯性导航***存在的累计误差,实现高精度导航。
在上述基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法的具体实施方式中,可选地,根据所述传感器测量模型和精度要求,建立传感器模型包括:
采用MPU9250作为惯性测量单元,MPU9250是一种惯性传感器,它集成了三轴加速度、三轴角速度(陀螺仪)和三轴磁力计,MPU9250模型示意图如图3(a)~(b)所示。惯性传感器和GPS的关键参数如表1所示。
表1惯性传感器和GPS关键参数
设置GPS的刷新频率为5Hz,磁力计的刷新频率为50Hz,陀螺仪和加速度计的刷新频率为100Hz。初始经纬高度对应导航坐标系下的坐标为[0,0,0],初始姿态为0。
位置估计误差初值为[-2m,-2m,-4m],速度估计误差初值为[0m/s,0m/s,0m/s],姿态估计误差初值为[3°,-3°,5°];陀螺仪零偏为[0.5o/s,-0.5o/s,1o/s],测量噪声标准差为σg=0.1°/s;加速度计零偏为[0.06m/s2,-0.06m/s2,0.08m/s2],测量噪声标准差为σa=2.94×10-2m/s2;磁力计的测量噪声标准差为σm=2×10-3Gauss;GPS水平位置测量噪声标准差为σxy=5m,高度测量噪声标准差为σz=10m。速度测量噪声标准差为σv=0.1m/s。
根据上述给定条件,得到滤波器的参数如下:
(1)初始协方差矩阵:P0=diag(E(x0)2);
(2)测量噪声协方差矩阵:V=diag([σxy 2Δt2,σz 2Δt2,σv 2Δt2,σa 2Δt2,σm 2Δt2]);
基于上述初始条件,对静态时的IMU+GPS组合导航进行仿真,得到仿真结果如图4(a)~(b)、图5(a)~(b)、图6(a)~(b)、图7(a)~(b)所示。
图4为GPS测量的位置和速度静态信息仿真曲线。图5~图7表明,导航状态经过短暂调整后到达稳定状态。姿态角的估计偏差在±3°以内,北向速度、东向速度和地向速度的估计偏差均在±0.02m/s以内,北向位置、东向位置和地向位置的估计偏差均在±0.5m以内。稳态误差是***噪声和测量噪声共同造成的。图5(b)、图6(b)和图7(b)是误差状态估计曲线,姿态角的误差状态稳定在±1.5°以内,速度的误差状态稳定在±0.02m/s,位置的误差状态稳定在±0.5m以内。仿真结果表明,本章设计的组合导航方案在静态时基本能够消除惯性导航***带来的状态偏差,使得导航精度得到明显的提升。
下面进行动态过程仿真,考虑陀螺仪和加速度的零偏和测量噪声,真实状态为多旋翼跟踪位置指令(0,0,0)→(0,0,5)→(5,0,5)→(5,5,5)→(0,5,5)→(0,0,5)时多旋翼的真实航迹,仿真时间为25s/初始条件保持不变,使用IMU+GPS组合导航算法对多旋翼动态时的状态进行估计,通过仿真得到状态估计曲线如图8(a)~(b)、图9(a)~(b)、图10(a)~(b)所示。
仿真曲线表明,组合导航***估计的状态基本能够跟踪真实状态。状态误差曲线反映出,当真实状态发生变化时,组合导航***能够及时估计***的误差状态,并对原惯性导航***的估计状态进行校正,从而得到精度更高的估计。
Claims (5)
1.一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法,其特征在于,包括以下步骤:
步骤1,建立传感器的测量模型,推导四元数形式的导航方程,并对该方程进行线性化处理得到误差状态方程;
步骤2,基于GPS、磁力计和加速度计的测量信息,建立用于卡尔曼滤波估计的测量方程;
步骤3,结合惯性-GPS松组合模式,采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计,并实现对导航状态的校正。
2.根据权利要求1所述的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法,其特征在于,步骤1所述建立传感器的测量模型,具体如下:
基于惯性+GPS组合导航的信息测量元件包括陀螺仪、加速度计、磁力计和GPS,做出以下4条假设:
(1)陀螺仪和加速度计测量信息的零位偏差为固定偏差;
(2)陀螺仪和加速度计的测量噪声为高斯噪声,满足正态分布;
(3)磁力计不存在零位偏差,测量噪声为高斯噪声,满足正态分布;
(4)GPS不存在零位偏差,测量噪声为高斯噪声,满足正态分布;
基于以上假设,陀螺仪的测量模型为:
加速度计的测量模型为:
磁力计的测量模型为:
其中,σm磁力计噪声的标准差。
3.根据权利要求1所述的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法,其特征在于,步骤1所述推导四元数形式的导航方程,并对该方程进行线性化处理得到误差状态方程,具体如下:
基于误差状态卡尔曼滤波器ESKF的工作原理,将真实状态向量x视为标称状态向量xnom和误差状态向量δx的组合,其中误差状态表示标称状态与真实状态之间的差,产生以下关系:
四元数误差状态在机体系中的定义为:
所述用于导航信息估计的ESKF状态由包含19个元素的真实状态向量x,包含19个元素的标称状态向量xnom和包含18个元素的误差状态向量δx组成,表示如下:
其中,r=[xn,yn,zn]Τ是多旋翼在导航系下的位置,v=[vxn,vyn,vzn]Τ是多旋翼在导航系下的速度;q=[q0 q1 q2 q3]Τ为四元数,表示多旋翼的姿态;bg=[bωx,bωy,bωz]Τ是三轴陀螺仪的固定零偏,ba=[bax,bay,baz]Τ是三轴加速度计的固定零偏,g=[0,0,g]Τ是重力加速度在导航系下的表示;
所述标称状态运动学方程为:
其中,k表示tk时刻,k+1表示tk+1时刻,xk=x(tk),tk=kΔt,Δt=tk+1-tk;
所述误差状态运动学方程为:
其中,δg表示重力加速度误差;vi,θi,bai,bgi是速度噪声、角度噪声、加速度偏差噪声和陀螺仪偏差噪声的离散化能量;vi,θi,bai,bgi的均值是0,协方差矩阵通过对na,ng,ba,bg的方差在间隔时间Δt内进行积分获得:
其中,σa,σg根据IMU数据表中的信息确定,σa单位为m/s2,σg单位为rad/s;σwa是加速度偏差噪声的标准差,σwg是陀螺仪偏差噪声的标准差,Vi是速度噪声的协方差矩阵、Θi是角度噪声的协方差矩阵、Ai是加速度偏差噪声的协方差矩阵、Ωi是陀螺仪偏差噪声的协方差矩阵;
所述扰动向量wi=[vi,θi,ai,ωi]Τ,那么误差状态方程为:
δxk+1=Fx,kδxk+Fi,kwi,k (14)
其中,Fx是误差状态转移矩阵,Fi是扰动向量wi的雅可比矩阵,Fx和Fi表达式如下式所示:
误差状态卡尔曼滤波器的误差状态的更新方程为:
误差状态卡尔曼滤波器的协方差Pk的更新方程为:
其中,Qi是***噪声的协方差矩阵,表达式如下式所示:
4.根据权利要求1所述的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法,其特征在于,步骤2所述基于GPS、磁力计和加速度计的测量信息,建立用于卡尔曼滤波估计的测量方程,测量方程包括GPS测量方程、加速度计测量方程、磁力计测量方程;
所述GPS的位置测量噪声用υpos表示,位置测量噪声的标准差用σr表示,速度测量噪声用υvel表示,速度测量噪声的标准差用σv表示;
所述导航系下的位置和经纬度关系为:
其中,地球的经度、纬度和高度分别为L,λ,h,参考椭球子午面曲率半径用RM表示,卯酉面曲率半径用RN表示,[δxn,δyn,δzn]Τ是导航系下的位置变化量;
所述多旋翼的经纬高度和导航系下位置的关系为:
其中,[L0,λ0,h0]Τ是起飞点的经纬高度,[Lm,λm,hm]Τ是经纬高度的测量值;υpos是经纬高度的测量噪声,满足正态分布;
所述加速度计的测量方程为:
所述磁力计的测量方程为:
其中,地磁场强度用M表示,磁偏角用D表示,磁倾角用I表示;
设测量状态向量为y,组成为y=[Lm,λm,hm,vxm,vym,vzm,am,Mm]Τ,测量噪声为υ,组成为υ=[υpos,υvel,na,nM,wa]Τ,所述测量方程为:
yk=h(xnom,k)+υk (24)
其中,h()是***状态的非线性函数,υ是方差为V的高斯噪声,满足υ~N{0,V}。
5.根据权利要求1所述的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法,其特征在于,步骤3所述结合惯性-GPS松组合模式,采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计,并实现对导航状态的校正,具体如下:
根据tk+1时刻的测量值yk+1,通过滤波器估计误差状态,得到滤波器增益矩阵:
Kk+1=Pk+1/kHk+1 Τ(Hk+1Pk+1/kHk+1 Τ+Vk+1)-1 (25)
其中,Pk+1/k为第k+1时刻误差状态的协方差的预测值,Hk+1是第k+1时刻h()关于误差状态δx的雅可比矩阵,Vk+1是第k+1时刻测量方程噪声的协方差矩阵,Kk+1是第k+1时刻的滤波器增益矩阵;
所述tk+1时刻的误差状态的最优估计为:
协方差矩阵为:
Pk+1/k+1=(I-Kk+1Hk+1)Pk+1/k (27)
所述雅可比矩阵利用链式规则来计算:
其中,Hx是h()的标准雅可比矩阵,取决于所使用的传感器的测量功能;Xδx是真实状态关于误差状态的雅可比行列式,由于仅取决于状态的ESKF组成,因此得到:
化简得Qδθ:
推导出:
相对于标称状态的雅可比Hx满足:
获得ESKF更新方程之后,通过误差状态在tk+1时刻的近似最优估计,对标称状态进行更新,得到:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010193890.8A CN111351482B (zh) | 2020-03-19 | 2020-03-19 | 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010193890.8A CN111351482B (zh) | 2020-03-19 | 2020-03-19 | 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111351482A true CN111351482A (zh) | 2020-06-30 |
CN111351482B CN111351482B (zh) | 2023-08-18 |
Family
ID=71192897
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010193890.8A Active CN111351482B (zh) | 2020-03-19 | 2020-03-19 | 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111351482B (zh) |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111798491A (zh) * | 2020-07-13 | 2020-10-20 | 哈尔滨工业大学 | 一种基于Elman神经网络的机动目标跟踪方法 |
CN111929699A (zh) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及*** |
CN111982126A (zh) * | 2020-08-31 | 2020-11-24 | 郑州轻工业大学 | 一种全源BeiDou/SINS弹性状态观测器模型设计方法 |
CN112285738A (zh) * | 2020-10-23 | 2021-01-29 | 中车株洲电力机车研究所有限公司 | 一种轨道交通车辆的定位方法及其装置 |
CN112539777A (zh) * | 2020-11-30 | 2021-03-23 | 武汉大学 | 一种九轴传感器的误差参数标定方法 |
CN112650281A (zh) * | 2020-12-14 | 2021-04-13 | 一飞(海南)科技有限公司 | 多传感器三余度***、控制方法、无人机、介质及终端 |
CN113639744A (zh) * | 2021-07-07 | 2021-11-12 | 武汉工程大学 | 一种用于仿生机器鱼的导航定位方法和*** |
CN113984049A (zh) * | 2021-11-30 | 2022-01-28 | 北京信息科技大学 | 飞行器的飞行轨迹的估计方法、装置及*** |
CN114526756A (zh) * | 2022-01-04 | 2022-05-24 | 华南理工大学 | 一种无人机机载多传感器校正方法、装置及存储介质 |
CN114659488A (zh) * | 2022-02-08 | 2022-06-24 | 东风悦享科技有限公司 | 一种基于误差卡尔曼滤波的高动态车辆姿态估计方法 |
WO2022135070A1 (zh) * | 2020-12-24 | 2022-06-30 | 北京紫光展锐通信技术有限公司 | 惯性导航方法及设备 |
CN115685278A (zh) * | 2022-10-28 | 2023-02-03 | 南京航空航天大学 | 基于kf的低空无人机航迹定位修正方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105973238A (zh) * | 2016-05-09 | 2016-09-28 | 郑州轻工业学院 | 一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法 |
CN106767797A (zh) * | 2017-03-23 | 2017-05-31 | 南京航空航天大学 | 一种基于对偶四元数的惯性/gps组合导航方法 |
CN107036598A (zh) * | 2017-03-30 | 2017-08-11 | 南京航空航天大学 | 基于陀螺误差修正的对偶四元数惯性/天文组合导航方法 |
-
2020
- 2020-03-19 CN CN202010193890.8A patent/CN111351482B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105973238A (zh) * | 2016-05-09 | 2016-09-28 | 郑州轻工业学院 | 一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法 |
CN106767797A (zh) * | 2017-03-23 | 2017-05-31 | 南京航空航天大学 | 一种基于对偶四元数的惯性/gps组合导航方法 |
CN107036598A (zh) * | 2017-03-30 | 2017-08-11 | 南京航空航天大学 | 基于陀螺误差修正的对偶四元数惯性/天文组合导航方法 |
Cited By (19)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111798491A (zh) * | 2020-07-13 | 2020-10-20 | 哈尔滨工业大学 | 一种基于Elman神经网络的机动目标跟踪方法 |
CN111929699A (zh) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及*** |
CN111982126A (zh) * | 2020-08-31 | 2020-11-24 | 郑州轻工业大学 | 一种全源BeiDou/SINS弹性状态观测器模型设计方法 |
CN112285738A (zh) * | 2020-10-23 | 2021-01-29 | 中车株洲电力机车研究所有限公司 | 一种轨道交通车辆的定位方法及其装置 |
CN112539777A (zh) * | 2020-11-30 | 2021-03-23 | 武汉大学 | 一种九轴传感器的误差参数标定方法 |
CN112539777B (zh) * | 2020-11-30 | 2021-09-14 | 武汉大学 | 一种九轴传感器的误差参数标定方法 |
CN112650281A (zh) * | 2020-12-14 | 2021-04-13 | 一飞(海南)科技有限公司 | 多传感器三余度***、控制方法、无人机、介质及终端 |
CN112650281B (zh) * | 2020-12-14 | 2023-08-22 | 一飞(海南)科技有限公司 | 多传感器三余度***、控制方法、无人机、介质及终端 |
WO2022135070A1 (zh) * | 2020-12-24 | 2022-06-30 | 北京紫光展锐通信技术有限公司 | 惯性导航方法及设备 |
CN113639744A (zh) * | 2021-07-07 | 2021-11-12 | 武汉工程大学 | 一种用于仿生机器鱼的导航定位方法和*** |
CN113639744B (zh) * | 2021-07-07 | 2023-10-20 | 武汉工程大学 | 一种用于仿生机器鱼的导航定位方法和*** |
CN113984049A (zh) * | 2021-11-30 | 2022-01-28 | 北京信息科技大学 | 飞行器的飞行轨迹的估计方法、装置及*** |
CN113984049B (zh) * | 2021-11-30 | 2024-01-26 | 北京信息科技大学 | 飞行器的飞行轨迹的估计方法、装置及*** |
CN114526756B (zh) * | 2022-01-04 | 2023-06-16 | 华南理工大学 | 一种无人机机载多传感器校正方法、装置及存储介质 |
CN114526756A (zh) * | 2022-01-04 | 2022-05-24 | 华南理工大学 | 一种无人机机载多传感器校正方法、装置及存储介质 |
CN114659488B (zh) * | 2022-02-08 | 2023-06-23 | 东风悦享科技有限公司 | 一种基于误差卡尔曼滤波的高动态车辆姿态估计方法 |
CN114659488A (zh) * | 2022-02-08 | 2022-06-24 | 东风悦享科技有限公司 | 一种基于误差卡尔曼滤波的高动态车辆姿态估计方法 |
CN115685278A (zh) * | 2022-10-28 | 2023-02-03 | 南京航空航天大学 | 基于kf的低空无人机航迹定位修正方法 |
CN115685278B (zh) * | 2022-10-28 | 2023-11-24 | 南京航空航天大学 | 基于kf的低空无人机航迹定位修正方法 |
Also Published As
Publication number | Publication date |
---|---|
CN111351482B (zh) | 2023-08-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111351482B (zh) | 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 | |
Leishman et al. | Quadrotors and accelerometers: State estimation with an improved dynamic model | |
CN109813311B (zh) | 一种无人机编队协同导航方法 | |
Jun et al. | State estimation of an autonomous helicopter using Kalman filtering | |
CN102809377B (zh) | 飞行器惯性/气动模型组合导航方法 | |
CN102692225B (zh) | 一种用于低成本小型无人机的姿态航向参考*** | |
Kelly et al. | Combined visual and inertial navigation for an unmanned aerial vehicle | |
Saripalli et al. | A tale of two helicopters | |
Cheviron et al. | Robust nonlinear fusion of inertial and visual data for position, velocity and attitude estimation of UAV | |
CN103837151B (zh) | 一种四旋翼飞行器的气动模型辅助导航方法 | |
CN110954102B (zh) | 用于机器人定位的磁力计辅助惯性导航***及方法 | |
CN105929836B (zh) | 用于四旋翼飞行器的控制方法 | |
CN111964688B (zh) | 结合无人机动力学模型和mems传感器的姿态估计方法 | |
Weiss et al. | 4dof drift free navigation using inertial cues and optical flow | |
CN111207745A (zh) | 一种适用于大机动无人机垂直陀螺仪的惯性测量方法 | |
CN111189442A (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
Svacha et al. | Inertial velocity and attitude estimation for quadrotors | |
CN108871319B (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
Bisgaard et al. | Full state estimation for helicopter slung load system | |
CN107063248A (zh) | 基于旋翼转速的动力学模型辅助惯导的导航方法 | |
Nistler et al. | Gravity compensation in accelerometer measurements for robot navigation on inclined surfaces | |
Hua et al. | Velocity-aided attitude estimation for accelerated rigid bodies | |
CN116429101A (zh) | 基于惯性导航的轨迹跟踪控制***及方法 | |
Chen et al. | An integrated GNSS/INS/DR positioning strategy considering nonholonomic constraints for intelligent vehicle | |
Jun et al. | State estimation via sensor modeling for helicopter control using an indirect kalman filter |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |