CN111351482B - 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 - Google Patents

基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 Download PDF

Info

Publication number
CN111351482B
CN111351482B CN202010193890.8A CN202010193890A CN111351482B CN 111351482 B CN111351482 B CN 111351482B CN 202010193890 A CN202010193890 A CN 202010193890A CN 111351482 B CN111351482 B CN 111351482B
Authority
CN
China
Prior art keywords
noise
measurement
state
error 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.)
Active
Application number
CN202010193890.8A
Other languages
English (en)
Other versions
CN111351482A (zh
Inventor
张永
徐贤鹏
秦伟
刘自力
黄成�
马国梁
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Science and Technology
Original Assignee
Nanjing University of Science and Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Nanjing University of Science and Technology filed Critical Nanjing University of Science and Technology
Priority to CN202010193890.8A priority Critical patent/CN111351482B/zh
Publication of CN111351482A publication Critical patent/CN111351482A/zh
Application granted granted Critical
Publication of CN111351482B publication Critical patent/CN111351482B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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角为速度测量值，为机体系相对于惯性系的角速度在机体系下的坐标表示，bg为陀螺仪的零偏；ng为高斯噪声，各个方向不相关，满足正态分布：
其中，E(ng)＝0为高斯噪声的均值，为高斯噪声的协方差矩阵；σg是高斯噪声的标准差，I是3阶单位阵；
加速度计的测量模型为：
其中，am是加速度的测量值，ai是惯性加速度，gi是重力加速度在惯性系下的坐标表示，是惯性系到机体系的旋转矩阵，ba为加速度计的零偏；na表示高斯白噪声，各个方向不相关，满足正态分布：
其中，E(na)＝0为高斯白噪声的均值，为高斯白噪声的协方差矩阵；σa为角速度计噪声的标准差；
磁力计的测量模型为：
其中，Mm是磁场强度的测量值，是导航系到机体系的旋转矩阵，Mn是磁场强度在导航系下的坐标表示；nM是磁力计的测量噪声，满足正态分布：
其中，σm磁力计噪声的标准差。
进一步地，步骤1所述推导四元数形式的导航方程，并对该方程进行线性化处理得到误差状态方程，具体如下：
基于误差状态卡尔曼滤波器ESKF的工作原理，将真实状态向量x视为标称状态向量xnom和误差状态向量δx的组合，其中误差状态表示标称状态与真实状态之间的差，产生以下关系：
其中，表示除四元数状态之外的所有属于/>空间状态变量的典型加法运算即标量和矢量的加减运算；
四元数误差状态在机体系中的定义为：
其中q是真实四元数，qnom是标称四元数，δq是四元数偏差，为四元数运算：
所述用于导航信息估计的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)
根据所述要求定义关于误差状态δx的雅可比矩阵H，并以最佳真实状态估计值进行估计；由于此阶段的误差状态平均值为零，假设/>使用标称状态xnom作为评估点，从而得出雅可比矩阵：
所述雅可比矩阵利用链式规则来计算：
其中，Hx是h()的标准雅可比矩阵，取决于所使用的传感器的测量功能；Xδx是真实状态关于误差状态的雅可比行列式，由于仅取决于状态的ESKF组成，因此得到：
其中，为a对b求偏导数的形式；
除了q其余状态的微分结果都是3阶单位阵I，设得到：
化简得Qδθ：
推导出：
相对于标称状态的雅可比Hx满足：
获得ESKF更新方程之后，通过误差状态在tk+1时刻的近似最优估计，对标称状态进行更新，得到：
其中，即tk+1时刻的真实值的最优估计，/>是第k+1时刻真实状态的估计值，/>是第k+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])；
(3)***噪声协方差矩阵：
基于上述初始条件，对静态时的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 (3)

1.一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，其特征在于，包括以下步骤：
步骤1，建立传感器的测量模型，推导四元数形式的导航方程，并对该方程进行线性化处理得到误差状态方程；
步骤2，基于GPS、磁力计和加速度计的测量信息，建立用于卡尔曼滤波估计的测量方程；
步骤3，结合惯性-GPS松组合模式，采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计，并实现对导航状态的校正；
步骤1所述建立传感器的测量模型，具体如下：
基于惯性+GPS组合导航的信息测量元件包括陀螺仪、加速度计、磁力计和GPS，做出以下4条假设：
(1)陀螺仪和加速度计测量信息的零位偏差为固定偏差；
(2)陀螺仪和加速度计的测量噪声为高斯噪声，满足正态分布；
(3)磁力计不存在零位偏差，测量噪声为高斯噪声，满足正态分布；
(4)GPS不存在零位偏差，测量噪声为高斯噪声，满足正态分布；
基于以上假设，陀螺仪的测量模型为：
其中，ωm角为速度测量值，为机体系相对于惯性系的角速度在机体系下的坐标表示，bg为陀螺仪的零偏；ng为高斯噪声，各个方向不相关，满足正态分布：
其中，E(ng)＝0为高斯噪声的均值，为高斯噪声的协方差矩阵；σg是高斯噪声的标准差，I是3阶单位阵；
加速度计的测量模型为：
其中，am是加速度的测量值，ai是惯性加速度，gi是重力加速度在惯性系下的坐标表示，是惯性系到机体系的旋转矩阵，ba为加速度计的零偏；na表示高斯白噪声，各个方向不相关，满足正态分布：
其中，E(na)＝0为高斯白噪声的均值，为高斯白噪声的协方差矩阵；σa为角速度计噪声的标准差；
磁力计的测量模型为：
其中，Mm是磁场强度的测量值，是导航系到机体系的旋转矩阵，Mn是磁场强度在导航系下的坐标表示；nM是磁力计的测量噪声，满足正态分布：
其中，σm磁力计噪声的标准差；
步骤1所述推导四元数形式的导航方程，并对该方程进行线性化处理得到误差状态方程，具体如下：
基于误差状态卡尔曼滤波器ESKF的工作原理，将真实状态向量x视为标称状态向量xnom和误差状态向量δx的组合，其中误差状态表示标称状态与真实状态之间的差，产生以下关系：
其中，⊕表示除四元数状态之外的所有属于空间状态变量的典型加法运算即标量和矢量的加减运算；
四元数误差状态在机体系中的定义为：
其中q是真实四元数，qnom是标称四元数，δq是四元数偏差，为四元数运算：
所述用于导航信息估计的ESKF状态由包含19个元素的真实状态向量x，包含19个元素的标称状态向量xnom和包含18个元素的误差状态向量δx组成，表示如下：
其中，r＝[xn,yn,zn]T是多旋翼在导航系下的位置，v＝[vxn,vyn,vzn]T是多旋翼在导航系下的速度；q＝[q0 q1 q2 q3]T为四元数，表示多旋翼的姿态；bg＝[bωx,bωy,bωz]T是三轴陀螺仪的固定零偏，ba＝[bax,bay,baz]T是三轴加速度计的固定零偏，g＝[0,0,g]T是重力加速度在导航系下的表示；
角速度ω和旋转偏差矢量δθ是在机体坐标系下的表示，是机体系到导航系的旋转矩阵；
所述标称状态运动学方程为：
其中，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]T，那么误差状态方程为：
其中，Fx是误差状态转移矩阵，Fi是扰动向量wi的雅可比矩阵，Fx和Fi表达式如下式所示：
误差状态卡尔曼滤波器的误差状态的更新方程为：
误差状态卡尔曼滤波器的协方差Pk的更新方程为：
其中，Qi是***噪声的协方差矩阵，表达式如下式所示：
2.根据权利要求1所述的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，其特征在于，步骤2所述基于GPS、磁力计和加速度计的测量信息，建立用于卡尔曼滤波估计的测量方程，测量方程包括GPS测量方程、加速度计测量方程、磁力计测量方程；
所述GPS的位置测量噪声用υpos表示，位置测量噪声的标准差用σr表示，速度测量噪声用υvel表示，速度测量噪声的标准差用σv表示；
所述导航系下的位置和经纬度关系为：
其中，地球的经度、纬度和高度分别为L,λ,h，参考椭球子午面曲率半径用RM表示，卯酉面曲率半径用RN表示，[δxn,δyn,δzn]T是导航系下的位置变化量；
所述多旋翼的经纬高度和导航系下位置的关系为：
其中，[L0,λ0,h0]T是起飞点的经纬高度，[Lm,λm,hm]T是经纬高度的测量值；υpos是经纬高度的测量噪声，满足正态分布；
所述加速度计的测量方程为：
所述磁力计的测量方程为：
其中，地磁场强度用M表示，磁偏角用D表示，磁倾角用I表示；
设测量状态向量为y，组成为y＝[Lm,λm,hm,vxm,vym,vzm,am,Mm]T，测量噪声为υ，组成为υ＝[υpos,υvel,na,nM,wa]T，所述测量方程为：
yk＝h(xnom,k)+υk (24)
其中，h()是***状态的非线性函数，υ是方差为V的高斯噪声，满足υ～N{0,V}。
3.根据权利要求1所述的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，其特征在于，步骤3所述结合惯性-GPS松组合模式，采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计，并实现对导航状态的校正，具体如下：
根据tk+1时刻的测量值yk+1，通过滤波器估计误差状态，得到滤波器增益矩阵：
Kk+1＝Pk+1/kHk+1 T(Hk+1Pk+1/kHk+1 T+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)
根据所述要求定义关于误差状态δx的雅可比矩阵H，并以最佳真实状态估计值进行估计；由于此阶段的误差状态平均值为零，假设/>使用标称状态xnom作为评估点，从而得出雅可比矩阵：
所述雅可比矩阵利用链式规则来计算：
其中，Hx是h()的标准雅可比矩阵，取决于所使用的传感器的测量功能；Xδx是真实状态关于误差状态的雅可比行列式，由于仅取决于状态的ESKF组成，因此得到：
其中，为a对b求偏导数的形式；
除了q其余状态的微分结果都是3阶单位阵I，设得到：
化简得Qδθ：
推导出：
相对于标称状态的雅可比Hx满足：
获得ESKF更新方程之后，通过误差状态在tk+1时刻的近似最优估计，对标称状态进行更新，得到：
其中，即tk+1时刻的真实值的最优估计，/>是第k+1时刻真实状态的估计值，是第k+1时刻误差状态的最优估计。
CN202010193890.8A 2020-03-19 2020-03-19 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 Active CN111351482B (zh)

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 CN111351482A (zh) 2020-06-30
CN111351482B true 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)

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111798491B (zh) * 2020-07-13 2022-09-06 哈尔滨工业大学 一种基于Elman神经网络的机动目标跟踪方法
CN111929699B (zh) * 2020-07-21 2023-05-09 北京建筑大学 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及***
CN111982126B (zh) * 2020-08-31 2023-03-17 郑州轻工业大学 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN112285738B (zh) * 2020-10-23 2023-01-31 中车株洲电力机车研究所有限公司 一种轨道交通车辆的定位方法及其装置
CN112539777B (zh) * 2020-11-30 2021-09-14 武汉大学 一种九轴传感器的误差参数标定方法
CN112650281B (zh) * 2020-12-14 2023-08-22 一飞(海南)科技有限公司 多传感器三余度***、控制方法、无人机、介质及终端
CN112525190A (zh) * 2020-12-24 2021-03-19 北京紫光展锐通信技术有限公司 惯性导航方法及设备
CN113639744B (zh) * 2021-07-07 2023-10-20 武汉工程大学 一种用于仿生机器鱼的导航定位方法和***
CN113984049B (zh) * 2021-11-30 2024-01-26 北京信息科技大学 飞行器的飞行轨迹的估计方法、装置及***
CN114526756B (zh) * 2022-01-04 2023-06-16 华南理工大学 一种无人机机载多传感器校正方法、装置及存储介质
CN114659488B (zh) * 2022-02-08 2023-06-23 东风悦享科技有限公司 一种基于误差卡尔曼滤波的高动态车辆姿态估计方法
CN115685278B (zh) * 2022-10-28 2023-11-24 南京航空航天大学 基于kf的低空无人机航迹定位修正方法

Citations (3)

* Cited by examiner, † Cited by third party
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 南京航空航天大学 基于陀螺误差修正的对偶四元数惯性/天文组合导航方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
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 南京航空航天大学 基于陀螺误差修正的对偶四元数惯性/天文组合导航方法

Also Published As

Publication number Publication date
CN111351482A (zh) 2020-06-30

Similar Documents

Publication Publication Date Title
CN111351482B (zh) 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法
Leishman et al. Quadrotors and accelerometers: State estimation with an improved dynamic model
CN106643737B (zh) 风力干扰环境下四旋翼飞行器姿态解算方法
CN109813311B (zh) 一种无人机编队协同导航方法
Kingston et al. Real-time attitude and position estimation for small UAVs using low-cost sensors
Jun et al. State estimation of an autonomous helicopter using Kalman filtering
Redding et al. Vision-based target localization from a fixed-wing miniature air vehicle
JP4782111B2 (ja) 輸送手段の位置、姿勢、および／または飛行方向を推定するシステムおよび方法
CN102809377B (zh) 飞行器惯性/气动模型组合导航方法
Cheviron et al. Robust nonlinear fusion of inertial and visual data for position, velocity and attitude estimation of UAV
CN112629538A (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN111207745B (zh) 一种适用于大机动无人机垂直陀螺仪的惯性测量方法
CN111189442B (zh) 基于cepf的无人机多源导航信息状态预测方法
CN109683628B (zh) 一种基于有限时间分布式速度观测器的航天器相对位置控制方法
Weiss et al. 4dof drift free navigation using inertial cues and optical flow
CN106403952A (zh) 一种动中通低成本组合姿态测量方法
CN111238469B (zh) 一种基于惯性/数据链的无人机编队相对导航方法
CN110793515A (zh) 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法
CN106352897B (zh) 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法
CN111964688B (zh) 结合无人机动力学模型和mems传感器的姿态估计方法
Svacha et al. Inertial velocity and attitude estimation for quadrotors
Ko et al. Three-dimensional dynamic-model-aided navigation of multirotor unmanned aerial vehicles
CN107063248A (zh) 基于旋翼转速的动力学模型辅助惯导的导航方法
Ali et al. Gyroscopic drift compensation by using low cost sensors for improved attitude determination
Emran et al. A cascaded approach for quadrotor's attitude estimation

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