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

本发明专利技术公开了一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法。该方法包括：首先，建立传感器的测量模型，推导四元数形式的导航方程，并对该方程进行线性化处理得到误差状态方程；然后，基于GPS、磁力计和加速度计的测量信息，建立用于卡尔曼滤波估计的测量方程；最后，结合惯性‑GPS松组合模式，采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计，并实现对导航状态的校正。本发明专利技术使用四元数时计算量较小，而且能够消除惯性导航系统存在的累计误差，实现高精度导航。

【技术实现步骤摘要】
基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法
本专利技术属于导航
，特别是一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法。
技术介绍
导航系统及其配套子系统是自主无人机研究的重点。自主导航系统是利用各个子系统提供的信息来实现三个基本任务的系统，三个基本任务包括：1)定位：估计无人机的位置；2)障碍检测和躲避：识别周围障碍物并因此采取行动以躲避它们；3)发出控制指令：发送指令以使无人机稳定姿态并遵循导航指令。随着SUAVs(小型无人机)和MAVs(微型无人机)的出现，比如Parrot公司的Ar.DroneParrot、大疆无人机公司的DJIPhantomseries、Walkera公司的Voyager3、TALIH500和3DRobotics公司的3DRSOLO等，无人机的尺寸越来越小、重量越来越小，导致有效载荷变小，需要找到合适的板载传感器来实现相应的导航目的。在大多数用于IMU姿态估计的卡尔曼滤波方法中，首先通过积分来自陀螺仪的角速率传播姿态，以用作输入信号，然后使用来自加速度计和磁力计的信息更新姿态。在静态或本文档来自技高网...

【技术保护点】
1.一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，其特征在于，包括以下步骤：/n步骤1，建立传感器的测量模型，推导四元数形式的导航方程，并对该方程进行线性化处理得到误差状态方程；/n步骤2，基于GPS、磁力计和加速度计的测量信息，建立用于卡尔曼滤波估计的测量方程；/n步骤3，结合惯性-GPS松组合模式，采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计，并实现对导航状态的校正。/n

【技术特征摘要】
1.一种基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，其特征在于，包括以下步骤：
步骤1，建立传感器的测量模型，推导四元数形式的导航方程，并对该方程进行线性化处理得到误差状态方程；
步骤2，基于GPS、磁力计和加速度计的测量信息，建立用于卡尔曼滤波估计的测量方程；
步骤3，结合惯性-GPS松组合模式，采用误差状态卡尔曼滤波器ESKF对误差状态进行最优估计，并实现对导航状态的校正。


2.根据权利要求1所述的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，其特征在于，步骤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磁力计噪声的标准差。


3.根据权利要求1所述的基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法，其特征在于，步骤1所述推导四元数形式的导航方程，并对该方程进行线性化处理得到误差状态方程，具体如下：
基于误差状态卡尔曼滤波器ESKF的工作原理，将真实状态向量x视为标称状态向量xnom和误差状态向量δx的组合，其中误差状态表示标称状态与真实状态之间的差，产生以下关系：



其中，表示除四元数状态之外的所有属于空间状态变量的典型加法运算即标量和矢量的加减运算；
四元数误差状态在机体系中的定义为：



其中q是真实四元数，qnom是标称四元数，δq是四元数偏差，为四元数运算：



所述用于导航信息估计的ESKF状态由包含19个元素的真实状态向量x，包含19个元素的标称状态向量xnom和包含18个元素的误差状态向量δx组成，表示如下：



其中，r＝[xn,yn,zn]Τ是多旋翼在导航系下的位置，v＝[vxn,vyn,vzn]Τ是多旋翼在导航系下的速度；q＝[q0q1q2q3]Τ为四元数，表示多旋翼的姿态；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是加速度偏差噪...

【专利技术属性】
技术研发人员：张永，徐贤鹏，秦伟，刘自力，黄成，马国梁，
申请(专利权)人：南京理工大学，
类型：发明
国别省市：江苏;32