一种基于无迹卡尔曼滤波的无人机组合导航方法和系统技术方案

本发明专利技术公开了一种基于无迹卡尔曼滤波的无人机组合导航方法和系统，通过组合导航算法，可以实现无人机位置速度姿态估计；本发明专利技术提出的组合导航算法，与同维数的kalman滤波，扩展卡拉曼滤波为基础的组合导航算法相比，具有更高的精度；本发明专利技术提出的组合导航算法，可以作为导航系统中另一备用算法，以此来做冗余设计，增加系统的稳定性；发明专利技术采用的是序贯滤波的方式，在嵌入式系统中可以减少计算量，降低了对配置的要求。低了对配置的要求。低了对配置的要求。

【技术实现步骤摘要】
一种基于无迹卡尔曼滤波的无人机组合导航方法和系统


[0001]本专利技术属于无人机应用
，涉及一种无人机的定位导航方法，具体为一种基于无迹卡尔曼滤波的无人机组合导航方法和系统。

技术介绍

[0002]无人机在民用领域有着很大的发展前景，特别是在航拍，石油管道巡检，电力巡检，救援，侦查等领域发挥着重要作用。在控制无人机进行飞行的过程中，无人机需要实时准确的知道自身的位置，而为了知道自身的位置，无人机上安装有GPS，IMU，超声波，视觉，气压计等传感器，这些传感器都是了实现自身的导航定位。
[0003]在实现无人机自身导航定位中，常用的就是kalman滤波算法，这种滤波算法在处理系统是线性系统的时候表现非常好，对于非线性系统需要对系统进行线性化这样就衍生了扩展kalman滤波(EKF)，线性化过程中需要求取系统的雅克比矩阵这样会损失一部分精度。

技术实现思路

[0004]本专利技术的目的在于克服上述现有技术的缺点，提供一种基于无迹卡尔曼滤波的无人机组合导航方法和系统。而无迹kalman滤波(UKF)无需求取雅克比矩阵，精度更高，对无人机的姿态速度位置估计更加准确。在实际测试中发现，与机载另一套以EKF为基础同维数的组合导航算法相比，UKF组合导航算法在估计无人机的位置速度以及姿态中表现更为优越。
[0005]为达到上述目的，本专利技术采用以下技术方案予以实现：
[0006]一种基于无迹卡尔曼滤波的无人机组合导航方法，包括以下步骤：
[0007]步骤1，将无人机的状态变量生成为2n+1个原始sigma点，计算无人机状态变量的预测值和预测误差的协方差矩阵的加权平均值；
[0008]步骤2，通过无迹卡尔曼滤波器和k+1时刻各传感器的观测值对无人机状态变量的预测值进行估计，获得无人机状态变量的估计值和误差的协方差；
[0009]步骤2.1，通过协方差矩阵的加权平均值获得2n+1个新sigma点；
[0010]步骤2.2，通过测量方程对新sigma点进行传播；
[0011]步骤2.3，测量传播后的新的sigma点，进行加权平均，得到新sigma点的加权平均值；
[0012]步骤2.4，通过新sigma点的加权平均值计算量测的残差协方差矩阵；
[0013]步骤2.5，通过传播后的原始sigma点的加权平均值和新sigma点的加权平均值，计算无人机状态变量和测量变量的交叉协方差矩阵；
[0014]步骤2.6，通过残差协方差矩阵和交叉协方差矩阵计算卡尔曼增益系数；
[0015]步骤2.7，通过卡尔曼增益系数更新步骤1中的状态变量，获得无人机状态变量的估计值和误差的协方差；
[0016]步骤3，无人机运行期间不断重复步骤1和2，持续更新无人机的状态变量，对无人机进行导航。
[0017]本专利技术的进一步改进在于：
[0018]优选的，步骤1中，所述无人机的状态变量为：
[0019][0020]其中：
[0021][q
1 q
2 q
3 q4]为无人机姿态的四元素；[v
x v
y v
z
]为n系下的无人机三轴速度，其中v
x
为北向的速度，v
y
为东向的速度，v
z
为地向的速度；
[0022][p
x p
y p
z
]为n系下的无人机三轴位置，其中，p
x
为在北向的位置，p
y
为在东向的位置，p
z
为在地向的位置；
[0023][gyro_bias
x gyro_bias
y gyro_bias
z
]为陀螺仪在机体系下三轴零偏；
[0024][acc_bias
x acc_bias
y acc_bias
z
]为加速度计在机体系三轴零偏。
[0025]优选的，步骤1的具体过程包括：
[0026]步骤1.1，将无人机的状态变量转变为2n+1个原始sigma点；
[0027]步骤1.2，计算2n+1个原始sigma点的权重；
[0028]步骤1.3，通过非线性状态方程获得传播后的原始sigma点；
[0029]步骤1.4，通过权重对传播后的原始sigma点进行加权平均处理，然后计算原始sigma点的协方差矩阵；
[0030]步骤1.5，在原始sigma点的协方差矩阵中添加过程噪声，然后进行加权平均，得到协方差矩阵的加权平均值。
[0031]优选的，步骤1.2中，原始sigma点的权重计算公式为：
[0032][0033][0034]在实际使用中，取κ＝0，其中W0为初始sigma点的权重值，W
i
为过程sigma点的权重值。
[0035]优选的，步骤1.4中，协方差矩阵为：
[0036][0037]其中，为加权平均值，为传播后的sigma点；
[0038]步骤1.5中，协方差矩阵的加权平均值M
k
为
[0039][0040]Q
k
‑1为对角方阵，阶数为16维。
[0041]优选的，步骤2.1中，新sigma点通过下式计算得到：
[0042][0043][0044]其中，为初始新sigma点，为过程新sigma点；
[0045]x
i
的计算可以通过下式计算得到
[0046][0047][0048]优选的，步骤2.2中，测量方程为：
[0049][0050]步骤2.3中，sigma点的加权平均值的计算公式为：
[0051][0052]优选的，步骤2.4中，所述量测的残差协方差矩阵；
[0053][0054]其中，R
k
为测量变量的噪声矩阵；
[0055]步骤2.5中，交叉协方差矩阵为：
[0056][0057]步骤2.6中，所述卡尔曼增益系数的计算过程为：
[0058][0059]为S
k
的逆矩阵。
[0060]优选的，步骤2.7中，更新状态的协方差矩阵P
k
为：
[0061][0062]为K
k
的转置矩阵；
[0063]步骤2.8中，状态变量更新公式为：
[0064][0065]一种基于无迹卡尔曼滤波的无人机组合导航系统，包括：
[0066]预测模块，用于将无人机的状态变量生成为2n+1个原始sigma点，计算无人机状态变量的预测值和预测误差的协方差矩阵的加权平均值；
[0067]估计模块，用于通过无迹卡尔曼滤波器和k+1时刻各传感器的观测值对无人机状态变量的预测值进行估计，获得无人机状态变量的估计值和误差的协方差；
[0068]迭代模块，无人机运行期间不断重复预测模块和本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于无迹卡尔曼滤波的无人机组合导航方法，其特征在于，包括以下步骤：步骤1，将无人机的状态变量生成为2n+1个原始sigma点，计算无人机状态变量的预测值和预测误差的协方差矩阵的加权平均值；步骤2，通过无迹卡尔曼滤波器和k+1时刻各传感器的观测值对无人机状态变量的预测值进行估计，获得无人机状态变量的估计值和误差的协方差；步骤2.1，通过协方差矩阵的加权平均值获得2n+1个新sigma点；步骤2.2，通过测量方程对新sigma点进行传播；步骤2.3，测量传播后的新的sigma点，进行加权平均，得到新sigma点的加权平均值；步骤2.4，通过新sigma点的加权平均值计算量测的残差协方差矩阵；步骤2.5，通过传播后的原始sigma点的加权平均值和新sigma点的加权平均值，计算无人机状态变量和测量变量的交叉协方差矩阵；步骤2.6，通过残差协方差矩阵和交叉协方差矩阵计算卡尔曼增益系数；步骤2.7，通过卡尔曼增益系数更新步骤1中的状态变量，获得无人机状态变量的估计值和误差的协方差；步骤3，无人机运行期间不断重复步骤1和2，持续更新无人机的状态变量，对无人机进行导航。2.根据权利要求1所述的一种基于无迹卡尔曼滤波的无人机组合导航方法，其特征在于，步骤1中，所述无人机的状态变量为：其中：[q
1 q
2 q
3 q4]为无人机姿态的四元素；[v
x v
y v
z
]为n系下的无人机三轴速度，其中v
x
为北向的速度，v
y
为东向的速度，v
z
为地向的速度；[p
x p
y p
z
]为n系下的无人机三轴位置，其中，p
x
为在北向的位置，p
y
为在东向的位置，p
z
为在地向的位置；[gyro_bias
x gyro_bias
y gyro_bias
z
]为陀螺仪在机体系下三轴零偏；[acc_bias
x acc_bias
y acc_bias
z
]为加速度计在机体系三轴零偏。3.根据权利要求1所述的一种基于无迹卡尔曼滤波的无人机组合导航方法，其特征在于，步骤1的具体过程包括：步骤1.1，将无人机的状态变量转变为2n+1个原始sigma点；步骤1.2，计算2n+1个原始sigma点的权重；步骤1.3，通过非线性状态方程获得传播后的原始sigma点；步骤1.4，通过权重对传播后...

【专利技术属性】
技术研发人员：李定涌，
申请(专利权)人：西安因诺航空科技有限公司，
类型：发明
国别省市：