CN112683274A - 一种基于无迹卡尔曼滤波的无人机组合导航方法和系统 - Google Patents

Abstract

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

Description

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

【技术领域】

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

【背景技术】

无人机在民用领域有着很大的发展前景，特别是在航拍，石油管道巡检，电力巡检，救援，侦查等领域发挥着重要作用。在控制无人机进行飞行的过程中，无人机需要实时准确的知道自身的位置，而为了知道自身的位置，无人机上安装有GPS，IMU，超声波，视觉，气压计等传感器，这些传感器都是了实现自身的导航定位。

在实现无人机自身导航定位中，常用的就是kalman滤波算法，这种滤波算法在处理系统是线性系统的时候表现非常好，对于非线性系统需要对系统进行线性化这样就衍生了扩展kalman滤波(EKF)，线性化过程中需要求取系统的雅克比矩阵这样会损失一部分精度。

【发明内容】

本发明的目的在于克服上述现有技术的缺点，提供一种基于无迹卡尔曼滤波的无人机组合导航方法和系统。而无迹kalman滤波(UKF)无需求取雅克比矩阵，精度更高，对无人机的姿态速度位置估计更加准确。在实际测试中发现，与机载另一套以EKF为基础同维数的组合导航算法相比，UKF组合导航算法在估计无人机的位置速度以及姿态中表现更为优越。

为达到上述目的，本发明采用以下技术方案予以实现：

一种基于无迹卡尔曼滤波的无人机组合导航方法，包括以下步骤：

步骤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，持续更新无人机的状态变量，对无人机进行导航。

本发明的进一步改进在于：

优选的，步骤1中，所述无人机的状态变量为：

其中：

[q₁ q₂ q₃ q₄]为无人机姿态的四元素；[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]为加速度计在机体系三轴零偏。

优选的，步骤1的具体过程包括：

步骤1.1，将无人机的状态变量转变为2n+1个原始sigma点；

步骤1.2，计算2n+1个原始sigma点的权重；

步骤1.3，通过非线性状态方程获得传播后的原始sigma点；

步骤1.4，通过权重对传播后的原始sigma点进行加权平均处理，然后计算原始sigma点的协方差矩阵；

步骤1.5，在原始sigma点的协方差矩阵中添加过程噪声，然后进行加权平均，得到协方差矩阵的加权平均值。

优选的，步骤1.2中，原始sigma点的权重计算公式为：

在实际使用中，取κ＝0，其中W⁰为初始sigma点的权重值，Wⁱ为过程sigma点的权重值。

优选的，步骤1.4中，协方差矩阵

为：

其中，

为加权平均值，

为传播后的sigma点；

步骤1.5中，协方差矩阵的加权平均值M_k为

Q_k-1为对角方阵，阶数为16维。

优选的，步骤2.1中，新sigma点通过下式计算得到：

其中，

为初始新sigma点，

为过程新sigma点；

xⁱ的计算可以通过下式计算得到

优选的，步骤2.2中，测量方程为：

步骤2.3中，sigma点的加权平均值

的计算公式为：

优选的，步骤2.4中，所述量测的残差协方差矩阵；

其中，R_k为测量变量的噪声矩阵；

步骤2.5中，交叉协方差矩阵

为：

步骤2.6中，所述卡尔曼增益系数的计算过程为：

为S_k的逆矩阵。

优选的，步骤2.7中，更新状态的协方差矩阵P_k为：

为K_k的转置矩阵；

步骤2.8中，状态变量更新公式为：

一种基于无迹卡尔曼滤波的无人机组合导航系统，包括：

预测模块，用于将无人机的状态变量生成为2n+1个原始sigma点，计算无人机状态变量的预测值和预测误差的协方差矩阵的加权平均值；

估计模块，用于通过无迹卡尔曼滤波器和k+1时刻各传感器的观测值对无人机状态变量的预测值进行估计，获得无人机状态变量的估计值和误差的协方差；

迭代模块，无人机运行期间不断重复预测模块和估计模块，持续更新无人机的状态变量，对无人机进行导航与现有技术相比，本发明具有以下有益效果：

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

本发明还公开了一种基于无迹卡尔曼滤波的无人机组合导航系统，该系统包括三个模块，通过三个模块实现了组合导航算法，进而实现无人机位置速度姿态估计。

【附图说明】

图1为实施例中无人机姿态滚转角的模拟图；

图2为实施例中无人机姿态俯仰角的模拟图；

图3为实施例中无人机姿态航向角的模拟图；

图4为实施例中无人机东向速度的模拟图；

图5为实施例中无人机北向速度的模拟图；

图6为实施例中无人机天向速度的模拟图；

图7为实施例中无人机东向位置的模拟图；

图8为实施例中无人机北向位置的模拟图；

图9为实施例中无人机天向位置的模拟图；

图10为实施例步骤逻辑关系图。

【具体实施方式】

下面结合附图对本发明做进一步详细描述：

在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制；术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性；此外，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

本发明公开了一种基于无迹卡尔曼滤波的无人机组合导航算法，整体包括以下步骤：

(1)坐标系定义、状态变量定义以及系统建模；

(2)无迹变换(unscented transformation，简称UT变换)以及序贯unscentedkalman滤波(简称序贯UKF滤波)计算步骤；

(3)序贯Unscented Kalman filter组合导航算法。

具体步骤为：

步骤1，坐标系定义、状态变量定义以及系统建模

(1)坐标系定义

无人机为低速运动载体，选取当地局部地理坐标系为导航坐标系(固定参考坐标系，无人机初始起飞点为原点)，即为n系；无人机重心为原点的前右下坐标系为机体坐标系(动坐标系)，即为b系，该坐标系中，X轴指向飞行器机头前进方向，Y轴由原点指向飞行器右侧，Z轴方向根据X、Y轴由右手法则确定。；

(2)状态变量

选取的n系下的无人机的三轴姿态，三轴速度，三轴位置，三轴加速度计零偏，三轴陀螺仪零偏作为系统的状态变量，共16维状态方程，即：

其中：

[q₁ q₂ q₃ q₄]为无人机姿态的四元素表示方法；本发明用到为四元素定义如下：

Q＝q₁+q_v＝q₁+q₂i+q₂j+q₃k

q₁为四元素的实部，q_v＝q₂i+q₃j+q₄k称为四元素的虚部。

[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)系统建模

根据状态变量以及分析可以得到系统状态方程，状态方程包含姿态四元素的更新，速度更新，位置更新，以及陀螺仪和加速度计的零偏的更新，下面分别列出上述5个状态方程：

姿态更新

其中

表示t_m时刻的姿态变换四元素，相对应的，

表示t_m-1时刻的姿态变换四元素，

是从t_m-1时刻到t_m时刻姿态四元素变化，Δθ是陀螺在时间段[t_m-1,t_m]内输出的角增量并且Δθ＝|Δθ|，Δθ为向量Δθ的模值，MEMS低精度陀螺仪和加速度计来说，一般采用角增量输出采样的方式，只需要简单地将其乘以采样间隔时间T_s即可变换为角增量。

速度更新

对于低中速行驶的运载体，例如地速v＜100m/s，可以忽略地球自转以及地球曲率的影响，速度更新简化方程为：

其中

为t_m时刻n系下无人机速度，

为与四元素

对应的姿态阵，二者可以进行相互转换，ΔV_m是加速度计在时间段[t_m-1,t_m]时间内输出的比力增量，实际中可以采用比力乘以采样间隔进行近似，式中的gⁿ重力加速度矢量。

T_s为时间段[t_m-1,t_m]间隔，即

T_s＝t_m-t_m-1 (6)

位置更新

为t_m时刻n系下无人机位置，

为t_m-1n系下无人机位置，

为t_m时刻n系下无人机速度，

为t_m-1时刻n系下无人机速度，T_s为时间段[t_m-1,t_m]间隔，即

T_s＝t_m-t_m-1 (8)

陀螺bias更新

向量

为t_m时刻陀螺bias更新值，

为t_m-1时刻的陀螺bias值。

加速度计bias更新

向量

为t_m时刻陀螺加速度计bias更新值，

为t_m-1时刻的陀螺加速度计bias值。

根据上述五个状态更新方程，即可得到状态转移矩阵记为F，由状态方程可知，该系统为非线性系统，使用UKF滤波可以不必进行线性化，并且使用序贯UKF滤波不仅可以不用进行线性化操作，还可以减少计算量，在嵌入式软件中，减少计算量显得尤为重要。

步骤2，UT变换以及序贯UKF滤波

为了说明本方法的实际使用过程，由于UKF滤波用到了一个UT变换的数学概念，所以先对UT变换进行说明，然后再详细说明序贯UKF滤波，最后再结合实际提出基于序贯UKF滤波的无人机组合导航算法，如有需要请查阅相关资料。

UT变换

UT变换，指在确保采样均值

和协方差P_xx的前提下，选择一组点集(称之为sigma点集)，将非线性变换应用于每一个sigma点集，得到非线性转换后的

和P_yy就是变换后sigma点集的统计量。

本算法应用对称策略，基于该策略的UT变换算法框架如下：

假设对基变量x经过非线性变化y＝f(x)，其中x的均值和斜方差分别分为

和P_xx，系统的状态变量的维数为n，则y的统计特性可以通过如下步骤得到：

①求取sigma点集

根据均值

以及协方差P_xx，利用对称策略可以得到2n+1个sigma点，以及相对应的权重w_i(i＝0,1,…,2n)。Sigma点记为χ_i，其中i＝0,1,…,2n。

其中

其中λ为调整参数，

为cholesky分解的第i列。

②所采样的输入变量sigma点集{χ_i,i＝0,1,…,2n}中的每个sigma点进行f(·)非线性变换，得到变换后的sigma点集{y_i,i＝0,1,…,2n}。

y_i＝f(χ_i),i＝0,1,…,2n (17)

③对变换后的sigma点集{y_i,i＝0,1,…,2n}进行加权处理，从而得到输出变量y的统计量

和P_yy。

序贯UKF滤波

有了UT变换的基础，就可以使用序贯滤波方法了，序贯UKF滤波的基本思想：已知k时刻状态x_k，基于全局的估计值

以及相应的估计误差的协方差P_k|k，在k+1时刻，利用Unscented kalman滤波器和k+1时刻各传感器的观测值依次对状态x_k+1进行估计，最后得到基于全局的信息的估计值

和相应误差的协方差P_k+1|k+1，具体步骤为：

(1)用

和估计误差的协方差的平方根S_k|k产生2n+1个sigma点，计算状态的一步预测值

和预测误差协方差平方根S_k+1|k；

(2)用

对

进行更新，得到k+1时刻基于测量z^k及观测值

的状态估计

以及相应的估计误差的平方根

(3)用

对

进行更新，得到k+1时刻基于测量z^k及观测值

的状态估计

以及相应的估计误差的平方根

(4)用

对

进行更新，得到k+1时刻基于测量z^k及观测值

的状态估计

以及相应的估计误差的平方根

(5)最终得到系统在k+1时刻基于量测信息z^k+1的状态估计和相应的估计误差为：

步骤3，序贯UKF滤波组合导航算法

参见图10，本发明详细的序贯UKF滤波组合导航算法详细如下：

(1)初始状态变量x₀(如前面建立状态方程所述)和协方差矩阵P₀，计算2n+1个原始sigma点

每一个原始sigma点的计算过程为：

其中的初始状态变量为

协方差矩阵的初始值

[q_o q₁ q₂ q₃]对应姿态协方差初始值，可以使用3deg的不确定性进行设置；

[v_x v_y v_z p_x p_y p_z]对应的位置和速度协方差初始值，可以使用GNSS的静态噪声进行设置；

对应陀螺和加计的零偏噪声可以使根据实际使用的式(21)中的

进行IMU设置。

对其中i＝1,2,…,2n，n为系统的状态阶数，n＝16，这里

为状态变量x的上一时刻的估计值，xⁱ的计算为：

其中P_k-1为上一时刻的协方差矩阵，并且有

κ为调整参数，

为第i列Cholesky分解。

(2)计算原始sigma点权重

对于i＝1,2,…,2n，那么权重有第零个原始sigma点的权重W⁰以及后续权重Wⁱ

在实际使用中，取κ＝0。

(3)原始sigma点传播

在一个运行周期[t_k-1,t_k]，通过非线性状态方程进行传播得到传播后的原始sigma点

上式中的f对应状态方程见前面状态方程的建立。

(4)对原始sigma点进行加权平均得到

对传播后的原始sigma点进行加权平均

(5)通过式(27)计算出的加权平均值，计算协方差矩阵

通过传播后的sigma点

以及加权平均后的值

计算协方差矩阵。

(6)添加过程噪声以及对协方差矩阵进行加权平均得到协方差矩阵的加权平均值M_k

Q_k-1为对角方阵，阶数为16维。

(7)计算新sigma点

新sigma点通过以下计算得到

xⁱ的计算可以通过步骤(6)的M_k计算得到

(8)通过新sigma点和测量方程(非线性或者是线性)传播测量sigma点

(9)测量传播后的新sigma点加权平均得到

(10)通过新sigma点的加权平均值计算量测的残差协方差矩阵；

R_k为测量变量的噪声矩阵。

(11)计算状态变量和测量变量交叉协方差矩阵

(12)计算kalman增益系数K_k

为S_k逆矩阵。

(13)更新状态协方差矩阵P_k

为K_k的转置矩阵。

(14)状态更新

(15)跳转到第一步，并且初始值变为

协方差矩阵变为P_k，进行下一步迭代，直至无人机停止运动。

实施例

本发明无需对状态方程进行线性化，并且计算量相较于同状态变量的EKF组合导航算法少。下面结合前面所述步骤，说一个典型实施例。

无人机的初始姿态(滚转角roll和俯仰角pitch)可以通过静态加计数值计算得到：

无人机的初始航向角(heading)可以通过磁力计计算得到：

那么通过初始姿态就可以确定无人机姿态四元素

[q₀ q₁ q₂ q₃]＝[1 0 0 0]

无人机的初始速度可以通过GNSS得到：

[v_x v_y v_z]＝[0 0 0]

无人机的初始位置可以通过GNSS得到：

[p_x p_y p_z]＝[0 0 0]

无人机陀螺和加速度计的零偏可以设置为零：

因此我们可以得到组合导航系统的初始状态x₀。

无人机的初始协方差矩阵可以设置如下：

姿态初始协方差P_quat，速度初始协方差P_vel，位置初始协方差P_pos，陀螺零偏初始协方差P_{gyro_bias}，加速度计零偏初始协方差P_{accel_bias}，GNSS和气压高的测量信息由仿真器给出。

通过步骤3结合系统的状态方程就可以得到组合导航系统状态的估计：

图1～图9为状态估计结果，结果显示数据平滑无跳变，能很好的跟踪测量信息。实际使用，需要根据测量信号的噪声进行调整量测噪声的值。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

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₁ q₂ q₃ q₄]为无人机姿态的四元素；[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，通过权重对传播后的原始sigma点进行加权平均处理，然后计算原始sigma点的协方差矩阵；

步骤1.5，在原始sigma点的协方差矩阵中添加过程噪声，然后进行加权平均，得到协方差矩阵的加权平均值。

4.根据权利要求3所述的一种基于无迹卡尔曼滤波的无人机组合导航方法，其特征在于，步骤1.2中，原始sigma点的权重计算公式为：

在实际使用中，取κ＝0，其中W⁰为初始sigma点的权重值，Wⁱ为过程sigma点的权重值。

5.根据权利要求3所述的一种基于无迹卡尔曼滤波的无人机组合导航方法，其特征在于，步骤1.4中，协方差矩阵

为：

其中，

为加权平均值，

为传播后的sigma点；

步骤1.5中，协方差矩阵的加权平均值M_k为

Q_k-1为对角方阵，阶数为16维。

6.根据权利要求1所述的一种基于无迹卡尔曼滤波的无人机组合导航方法，其特征在于，步骤2.1中，新sigma点通过下式计算得到：

其中，

为初始新sigma点，

为过程新sigma点；

xⁱ的计算可以通过下式计算得到

7.根据权利要求1所述的一种基于无迹卡尔曼滤波的无人机组合导航方法，其特征在于，步骤2.2中，测量方程为：

步骤2.3中，sigma点的加权平均值

的计算公式为：

8.根据权利要求1所述的一种基于无迹卡尔曼滤波的无人机组合导航方法，其特征在于，步骤2.4中，所述量测的残差协方差矩阵；

其中，R_k为测量变量的噪声矩阵；

步骤2.5中，交叉协方差矩阵P_xyk为：

步骤2.6中，所述卡尔曼增益系数的计算过程为：

为S_k的逆矩阵。

9.根据权利要求1所述的一种基于无迹卡尔曼滤波的无人机组合导航方法，其特征在于，

步骤2.7中，更新状态的协方差矩阵P_k为：

为K_k的转置矩阵；

步骤2.8中，状态变量更新公式为：

10.一种基于无迹卡尔曼滤波的无人机组合导航系统，其特征在于，包括：

预测模块，用于将无人机的状态变量生成为2n+1个原始sigma点，计算无人机状态变量的预测值和预测误差的协方差矩阵的加权平均值；

估计模块，用于通过无迹卡尔曼滤波器和k+1时刻各传感器的观测值对无人机状态变量的预测值进行估计，获得无人机状态变量的估计值和误差的协方差；

迭代模块，无人机运行期间不断重复预测模块和估计模块，持续更新无人机的状态变量，对无人机进行导航。

Images