CN103697889A - 一种基于多模型分布式滤波的无人机自主导航与定位方法 - Google Patents

Abstract

本发明公开了一种基于多模型分布式滤波的无人机自主导航与定位方法，目的是不仅要降低无人机自主导航与定位方法的计算复杂度，满足嵌入式处理器实时处理的要求，还要保证对无人机进行稳定控制，而且在卫星导航定位系统短期或长期不可用时，也能保证无人机实现自主导航和定位。该方法通过建立不同阶次的多模型分布式滤波状态估计的系统状态方程，利用不同量测系统和传感器获得无人机运动的量测值，采用分布式滤波方法来估计并补偿低成本惯性导航系统的误差，为无人机提供连续、可靠的导航和定位信息。本发明能够长时间连续、稳定地为无人机控制系统提供精确的导航和定位信息。

Description

一种基于多模型分布式滤波的无人机自主导航与定位方法

技术领域

本发明涉及一种无人机自主导航和定位方法，特别是一种基于多模型分布式滤波的无人机自主导航与定位方法，适用于无人飞行器和有人飞行器自主导航和定位系统。

背景技术

与有人机相比，无人机是低风险和低成本，而且在军民用领域中被广泛应用，例如空中监视、地面侦察、灾后重建、遥感探测以及地面目标跟踪等。在实际应用中，无人机对自主导航系统精度和可靠性要求越来越高，要求实时、可靠地提供无人飞行器的位置、速度和姿态信息。由于低成本惯性导航系统误差发散很快，依靠单一的低成本惯性导航系统很难满足要求。将惯性导航系统与卫星定位系统信息进行融合来提供载体的运动信息已被广泛应用。但由于卫星定位信号易受干扰或欺骗，甚至信号转发源被摧毁，不能够长期提供稳定可靠的定位信息，在军民用领域中尚不能完全依赖于卫星定位来辅助惯性导航实现飞行器的精确导航定位。因此，发展一些替代的导航系统用于处理卫星导航系统短期和长期故障时的无人机自主导航和定位问题，已成为军民用领域无人机应用研究的的重点。视觉相机作为无人机配重包中的标准传感器既可用于导航传感器获取无人机的导航和定位信息，又可以用于任务传感器执行特定的任务，例如侦查，运动目标跟踪以及遥感探测，已成为无人机自主导航和定位的有效补充导航源之一。

根据无人机飞行的环境是否已知将基于视觉信息的导航和定位分为两类，一类是飞行环境未知，常用的方法有同时完成定位和生成环境地图的SLAM算法和视觉测距方法，前者在室内无人机和机器人导航定位中已广泛应用，但在室外环境多变的大视场环境无法有效使用；虽然后者可以用于室外多变环境，但需要精确获取无人机的飞行姿态和稳定平台的姿态；另一类是飞行环境已知，常用方法有陆标匹配和图像匹配定位，可以给出无人机精确的导航和位置信息。

本发明为中提到的一种基于多模型分布式滤波的无人机自主导航与定位方法，该方法在东北天地里坐标系下，根据惯性导航系统力学方程编排以及惯性导航系统误差方程建立不同阶次的多模型分布式滤波状态估计的系统状态方程，利用不同量测系统和传感器获得无人机运动的量测值，采用分布式滤波方法来估计并补偿低成本惯性导航系统的误差，为无人机提供连续、可靠的导航和定位信息。

本发明与其它无人机导航和定位方法不同在于：在当前已公开发表的文献中，依靠惯性导航/GPS组合导航或者是依靠视觉辅助惯性导航来提供无人机的导航和定位信息，但这些导航方法在实际应用中依然存在计算复杂度高以及在一些环境中无法使用的局限性，特别是在卫星导航定位系统信号长时间不可用时，导航系统无法提供长时间稳定的姿态信息而导致无人机及其机载设备损毁的风险。

发明内容

本发明要解决的技术问题：为解决在卫星导航定位系统长时间不可用时，导航系统能够长时间提供稳定的姿态和定位信息来控制无人机稳定、可靠地飞行，提出一种基于多模型分布式滤波的无人机自主导航与定位方法。

本发明采用的技术方案为：一种基于多模型分布式滤波的无人机自主导航与定位方法，该方法在东北天地里坐标系下，根据惯性导航系统力学方程编排以及惯性导航系统误差方程建立不同阶次的多模型分布式滤波系统状态方程，利用不同量测系统和传感器获得无人机运动的量测值，采用分布式滤波方法来估计并补偿低成本惯性导航系统的误差，为无人机提供连续、可靠的导航和定位信息。保证在卫星导航定位系统短期或长期不可用时，能够长时间连续、稳定地为无人机提供精确的导航和定位信息。具体实现步骤为：

步骤（1）、当卫星导航系统可用时，采用七状态反馈Kalman滤波处理卫星导航定位系统输出的位置和速度信息来估计惯性导航系统水平通道和姿态误差，使惯性导航系统输出精确的位置、速度和姿态信息，其具体方法为：

a1、根据惯性导航系统力学方程编排，利用加速度计和陀螺仪的输出值实时计算无人机的运动信息，包括位置、速度和姿态；

a2、根据惯性导航系统误差方程建立惯性导航/卫星导航组合导航系统状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(1\right)$

式中，X(t)^T＝[φ_E,φ_N,φ_U,δV_E,δV_N,δL,δλ]为连续时间系统状态；上标T为转置；E、N和U分别为东北天地理坐标系的坐标轴指向；t为时间；φ_E、φ_N和φ_U分别为惯性导航系统在东、北和天向平台误差角；δV_E和δV_N分别为惯性导航系统东向和北向速度误差；δL和δλ分别为惯性导航系统北向和东向位置误差；W(t)为系统噪声；F(t)和G(t)分别为状态转移矩阵和系统噪声矩阵；R_N和R_M分别为卯酉圈和子午面半径；

a3、利用卫星导航定位系统输出的位置和速度与惯性导航系统输出的位置和速度建立系统的量测方程为：

$Z_1=\left[\begin{array}{c}{V}_E^I-V_E^S\\ V_N^S-V_N^S\\ (L_S-L_S)R_M\\ ({\mathrm{\λ}}_S-{\mathrm{\λ}}_S)R_N\cos L_I\end{array}\right]+\left[\begin{array}{c}{v}_E\\ v_N\\ v_L\\ v_{\mathrm{\λ}}\end{array}\right]=H_{1}X+V_1---\left(2\right)$

$H_1=\left[\begin{array}{ccccc}{0}_{3\×3}& 0_{3\×1}& 0_{3\×1}& 0_{3\×1}& 0_{3\×1}\\ 0& 1& 0& 0& 0\\ 0& 0& 1& R_M& 0\\ 0& 0& 0& 0& R_N\cos L\end{array}\right]---\left(3\right)$

式中，Z₁为卫星导航系统与惯性导航系统输出值相减获得的量测值；

和

分别为惯性导航系统输出的东向和北向速度；L_I和λ_I分别指惯性导行系统输出北向位置和东向位置；

和

分别指卫星导航系统输出的东向和北向速度；L_S和λ_S分别指卫星导航定位系统输出的北向和东向位置；上标或下标中的I和S分别为惯性导航系统和卫星导航定位系统的输出值；V₁＝[v_E v_N v_L v_λ]^T为卫星导航系统接收机的噪声；

a4、对式（1）进行离散化后，利用七状态反馈Kalman滤波方法实时估计并补偿惯性导航系统的误差，获得无人机精确的位置、速度和姿态信息。

步骤（2）、采用四状态反馈Kalman滤波处理气压计或卫星导航定位系统输出的高度来估计并补偿惯性导航系统垂直通道的误差，获得惯性导航系统输出精确的高度和垂直通道速度信息，其具体方法为：

b1、根据惯性导航系统力学方程编排，利用加速度计和陀螺仪输出信息计算无人机的高度和垂向速度；

b2、根据惯性导航垂直通道的力学方程编排以及气压计误差模型建立垂直通道误差估计的系统状态方程为：

X_k＝Φ_k|k-1X_k-1+W_k-1  （4）

${\mathrm{\Φ}}_{k|k-1}=\left[\begin{array}{cccc}1& \mathrm{\Δt}& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& \mathrm{\Δt}\\ 0& 0& -{\mathrm{\α}}^2\mathrm{\Δt}& -2\mathrm{\β\Δt}\end{array}\right]---\left(5\right)$

式中， $X_k={\left[\begin{array}{cccc}\mathrm{\δh}& \mathrm{\δ}{V}_U& \mathrm{\δ}{h}_a& \mathrm{\δ}{\tilde{h}}_a\end{array}\right]}^T$ 为系统状态；δh和δV_U分别为惯性导航系统输出的高度和垂向速度；δh_a和分别为气压高计输出高度误差和高度误差的一阶马尔科夫过程；Φ_k|k-1为系统从k-1到k时刻一步状态转移矩阵；W_k-1为系统在k-1时刻的量测噪声；Δt为采样时间；α和β为气压计误差模型中参数；

b3、利用气压计输出的高度与惯性导航系统输出的高度建立系统的量测方程为：

Z₂＝H₂X_k+V₂    （6）

式中，Z₂为气压计与惯性导航系统输出的高度值相减获得的量测值；H₂＝[1 0 -1 0]^T；V₂为气压计的测量噪声；

b4、根据式（4）和式（6），利用四状态反馈Kalman滤波方法实时估计并补偿惯性导航系统垂直通道的误差，获得无人机运动过程中精确的高度和垂向速度信息。

步骤（3）、当卫星导航定位系统不可用时，采用四状态扩展Kalman滤波估计并补偿四元数误差，保证惯性导航系统长时间输出稳定的姿态信息，其具体方法为：

c1、利用加速度计、陀螺仪和磁传感器输出的信息计算无人机的姿态为：

${\mathrm{\γ}}_a={\tan }^{-1}\left(\frac{f_x^b}{(-f_z^b)}\right)---\left(7\right)$

${\mathrm{\θ}}_a={\tan }^{-1}\left(\frac{f_y^b}{\sqrt{f_x^{b^2}+f_z^{b^2}}}\right)---\left(8\right)$

式中，tan^-1(*)为反正切函数；

和

分别为加速度计沿机体坐标系b输出的x、y和z轴向的比力；

和

分别为磁传感器沿机体坐标系b输出的x、y和z轴向的磁场强度；γ_a、θ_a和ψ_m分别为由加速度计和磁传感器信息计算出的无人机滚动角、俯仰角和航向角；

c2、根据惯性导航系统的力学方程编排建立四元数估计的系统状态方程为：

$\stackrel{\·}{X}=\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x^b& -{\mathrm{\ω}}_y^b& -{\mathrm{\ω}}_z^b\\ {\mathrm{\ω}}_x^b& 0& {\mathrm{\ω}}_z^b& {-\mathrm{\ω}}_y^b\\ {\mathrm{\ω}}_y^b& {-\mathrm{\ω}}_z^b& 0& {\mathrm{\ω}}_x^b\\ {\mathrm{\ω}}_z^b& {\mathrm{\ω}}_y^b& {-\mathrm{\ω}}_x^b& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]+\frac{1}{2}\left[\begin{array}{ccc}{q}_1& q_2& q_3\\ -q_0& q_3& -q_2\\ -q_3& -q_0& +q_1\\ q_2& -q_1& -q_0\end{array}\right]\left[\begin{array}{c}{\mathrm{\δ}}_x^b\\ {\mathrm{\δ}}_y^b\\ {\mathrm{\δ}}_z^b\end{array}\right]---\left(10\right)$

式中，

为连续时间系统状态量； ${\widehat{\mathrm{\ω}}}^b={\left[\begin{array}{ccc}{\widehat{\mathrm{\ω}}}_x^b& {\widehat{\mathrm{\ω}}}_y^b& {\widehat{\mathrm{\ω}}}_z^b\end{array}\right]}^T={\left[\begin{array}{ccc}{\mathrm{\ω}}_x^b-{\mathrm{\δ}}_x^b& {\mathrm{\ω}}_y^b-{\mathrm{\δ}}_y^b& {\mathrm{\ω}}_z^b-{\mathrm{\δ}}_z^b\end{array}\right]}^T$ 为陀螺仪在载体坐标系b输出的理想值； ${\mathrm{\ω}}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}_x^b& {\mathrm{\ω}}_y^b& {\mathrm{\ω}}_z^b\end{array}\right]}^T$ 为陀螺仪在载体坐标系b输出的实际值； ${\mathrm{\δ}}^b={\left[\begin{array}{ccc}{\mathrm{\δ}}_x^b& {\mathrm{\δ}}_y^b& {\mathrm{\δ}}_z^b\end{array}\right]}^T$ 为陀螺仪在载体坐标系b输出的漂移值；q₀、q₁、q₂和q₃为四元数的四个分量；

c3、利用加速度计和磁传感器输出值计算出的无人机姿态以及根据惯性导航力学方程编排和陀螺仪输出计算的载体姿态建立系统的量测方程为：

${\mathrm{\ψ}}_q={\tan 1}^{-1}\left(\frac{2(q_1{q}_2-q_0{q}_3)}{q_0^2-q_1^2+q_2^2-q_3^2}\right)---\left(12\right)$

${\mathrm{\γ}}_q={\tan }^{-1}\left(\frac{2(q_1{q}_3-q_0{q}_2)}{q_0^2-q_1^2+q_2^2-q_3^2}\right)---\left(14\right)$

式中，为系统时间t有关的观测值；ψ_q、

和γ_q分别为根据惯性导航系统力学方程和四元数计算出的无人机偏航角、俯仰角和滚动角；H(t,q)为与时间t和四元数q有关的非线性方程；V(t)为系统量测噪声；

c4、对式（10）和式（11）进行线性化后，利用四状态扩展Kalman滤波实时估计四元数，并实时计算无人机的俯仰角、滚动角和航向角。

步骤（4）、当卫星导航定位系统不可用时，采用不等间隔Kalman滤波处理视觉传感器提供的量测信息来估计并补偿惯性导航系统的位置和速度误差；保证惯性导航系统输出稳定的位置和速度信息，其具体方法为：

d1、当无人机飞过区域的地物特征丰富且预装了飞行区域的基准图像，采用基于灰度信息的景像匹配定位方法获取无人机的位置，基于灰度信息的景像匹配方法有粗匹配和精匹配两个过程，粗匹配采用交叉相关算法，精匹配采用最小二乘算法；

d2、当无人机飞过的区域地物特征不丰富或无人机未预装飞行区域的基准图，采用加速鲁棒特征SURF算法提取相邻两帧实时图像的特征点并进行同名特征点匹配，利用随机抽样一致性RANSAC算法剔除误匹配点，并根据相邻两帧的同名特征点计算无人机位置信息为：

式中，为无人机的相对位移；下标i为像平面上的第i个特征点；下标j为图像序列中的第j帧；下标p为像素坐标系；u和v为像素坐标；

为从像素坐标系p到导航坐标系n的转换矩阵；

为无人机的位置信息；

为卫星导航系统或景像匹配系统在不可用前获取的最后时刻无人机精确位置信息，并将该时刻记为t₀；

d3、对步骤d1和d2利用相机获取的无人机位置信息进行可靠性评估，舍弃不可靠位置后获得的可靠定位信息频率低且是非等间隔，根据系统状态方程式（1）和利用视觉传感器获得的视觉定位信息，建立视觉辅助惯性导航系统的量测方程为：

$Z_2=\left[\begin{array}{c}(L_I-L_c)R_M\\ ({\mathrm{\λ}}_I-{\mathrm{\λ}}_c)R_N\cos L_I\end{array}\right]+\left[\begin{array}{c}{v}_N^c\\ v_E^c\end{array}\right]=H_{2}X+V_2---\left(17\right)$

式中，Z₂为视觉导航系统输出的位置与惯性导航系统输出的位置相减获得的量测值；L_c和λ_c分别为视觉导航系统输出的北向和东向位置；H₂＝[0_2×5 diag(R_m R_NcosL_I)]为量测矩阵；

$V_2={\left[\begin{array}{cc}{v}_N^c& v_E^c\end{array}\right]}^T$ 为视觉导航系统的量测噪声；

d4、对式（1）进行离散化后，利用不等间隔Kalman滤波实时估计并补偿惯性导航系统的位置误差，实时获得无人机精确的位置和速度信息。

本发明的优点在于：针对不同的飞行条件，建立不同阶次的惯性导航系统误差估计的状态方程，采用分布式滤波方法估计并补偿惯性导航系统的误差，有效降低了计算复杂度；当卫星导航定位系统不可用时，通过陀螺仪、加速度计、磁传感器和视觉传感器来计算无人机的位置、速度和姿态信息，保证了无人机可以长时间、稳定可靠地飞行和操控。

附图说明

图1为本发明一种基于多模型分布式滤波的无人机自主导航与定位方法的原理图；

图2为本发明一种基于多模型分布式滤波的无人机自主导航与定位方法的Kalman滤波工原理图；

图3为本发明一种基于多模型分布式滤波的无人机自主导航与定位方法的景像匹配流程图；

图4为本发明一种基于多模型分布式滤波的无人机自主导航与定位方法的景像匹配粗匹配示意图；

图5为本发明一种基于多模型分布式滤波的无人机自主导航与定位方法的SURF特征点匹配示意图；

图6为本发明一种基于多模型分布式滤波的无人机自主导航与定位方法的RANSAC算法剔除误匹配点后的示意图；

图7为本发明一种基于多模型分布式滤波的无人机自主导航与定位方法的利用视觉推算位置信息的示意图。

具体实施方式

下面结合附图和具体实施方式对本发明进一步说明，本发明的具体实现步骤如下：

（1）对加速度计、陀螺仪、磁传感器、气压计和相机进行标定和误差补偿；

（2）如图1所示，根据惯性导航系统力学方程编排，利用加速度计和陀螺仪的输出值实时计算无人机的运动信息，包括位置、速度和姿态；

（3）如图1和2所示，当卫星导航系统可用时，采用七状态反馈Kalman滤波处理卫星导航定位系统输出的位置和速度信息来估计惯性导航系统水平通道和姿态误差，使惯性导航系统输出精确的位置、速度和姿态信息，其具体步骤为：

a1、根据惯性导航系统误差方程建立惯性导航/卫星导航组合导航系统状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(1\right)$

式中，X(t)^T＝[φ_E,φ_N,φ_U,δV_E,δV_N,δL,δλ]为连续时间系统状态；上标T为转置；E、N和U分别为东北天地理坐标系的坐标轴指向；t为时间；φ_E、φ_N和φ_U分别为惯性导航系统在东、北和天向平台误差角；δV_E和δV_N分别为惯性导航系统东向和北向速度误差；δL和δλ分别为惯性导航系统北向和东向位置误差；W(t)为系统噪声；F(t)和G(t)分别为状态转移矩阵和系统噪声矩阵；R_N和R_M分别为卯酉圈和子午面半径；

a2、利用卫星导航定位系统输出的位置和速度与惯性导航系统输出的位置和速度建立系统的量测方程为：

$Z_1=\left[\begin{array}{c}{V}_E^I-V_E^S\\ V_N^S-V_N^S\\ (L_S-L_S)R_M\\ ({\mathrm{\λ}}_S-{\mathrm{\λ}}_S)R_N\cos L_I\end{array}\right]+\left[\begin{array}{c}{v}_E\\ v_N\\ v_L\\ v_{\mathrm{\λ}}\end{array}\right]=H_{1}X+V_1---\left(2\right)$

$H_1=\left[\begin{array}{ccccc}{0}_{3\×3}& 0_{3\×1}& 0_{3\×1}& 0_{3\×1}& 0_{3\×1}\\ 0& 1& 0& 0& 0\\ 0& 0& 1& R_M& 0\\ 0& 0& 0& 0& R_N\cos L\end{array}\right]---\left(3\right)$

式中，Z₁为卫星导航系统与惯性导航系统输出值相减获得的量测值；

和

分别为惯性导航系统输出的东向和北向速度；L_I和λ_I分别指惯性导行系统输出北向位置和东向位置；和

分别指卫星导航系统输出的东向和北向速度；L_S和λ_S分别指卫星导航定位系统输出的北向和东向位置；上标或下标中的I和S分别为惯性导航系统和卫星导航定位系统的输出值；V₁＝[v_E v_N v_L v_λ]^T为卫星导航系统接收机的噪声；

a3、七状态反馈Kalman滤波器初始化，包括初始协方差矩阵，测量噪声方差矩阵和系统噪声反差矩阵；

a4、对式（1）进行离散化后，利用七状态反馈Kalman滤波方法实时估计并补偿惯性导航系统的误差，获得无人机精确的位置、速度和姿态信息；

（4）如图1和2所示，采用四状态反馈Kalman滤波处理气压计或卫星导航定位系统输出的高度来估计并补偿惯性导航系统垂直通道的误差，获得惯性导航系统输出精确的高度和垂直通道速度信息，其具体步骤为：

b1、根据惯性导航垂直通道的力学方程编排以及气压计误差模型建立垂直通道误差估计的系统状态方程为：

X_k＝Φ_k|k-1X_k-1+W_k-1  （4）

${\mathrm{\Φ}}_{k|k-1}=\left[\begin{array}{cccc}1& \mathrm{\Δt}& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& \mathrm{\Δt}\\ 0& 0& -{\mathrm{\α}}^2\mathrm{\Δt}& -2\mathrm{\β\Δt}\end{array}\right]---\left(5\right)$

式中， $X_k={\left[\begin{array}{cccc}\mathrm{\δh}& \mathrm{\δ}{V}_U& \mathrm{\δ}{h}_a& \mathrm{\δ}{\tilde{h}}_a\end{array}\right]}^T$ 为系统状态；δh和δV_U分别为惯性导航系统输出的高度和垂向速度；δh_a和

分别为气压高计输出高度误差和高度误差的一阶马尔科夫过程；Φ_k|k-1为系统从k-1到k时刻一步状态转移矩阵；W_k-1为系统在k-1时刻的量测噪声；Δt为采样时间；α和β为气压计误差模型中参数；

b2、利用气压计输出的高度与惯性导航系统输出的高度建立系统的量测方程为：

Z₂＝H₂X_k+V₂     （6）

式中，Z₂为气压计与惯性导航系统输出的高度值相减获得的量测值；H₂＝[1 0 -1 0]^T；V₂为气压计的测量噪声；

b3、四状态反馈Kalman滤波器初始化，包括初始协方差矩阵，测量噪声方差矩阵和系统噪声反差矩阵；

b4、根据式（4）和式（6），利用四状态反馈Kalman滤波方法实时估计并补偿惯性导航系统垂直通道的误差，获得无人机运动过程中精确的高度和垂向速度信息。

（5）当卫星导航定位系统不可用时，采用四状态扩展Kalman滤波估计并补偿四元数误差，保证惯性导航系统长时间输出稳定的姿态信息，其具体步骤为：

c1、利用加速度计、陀螺仪和磁传感器输出的信息计算无人机的姿态为：

${\mathrm{\γ}}_a={\tan }^{-1}\left(\frac{f_x^b}{(-f_z^b)}\right)---\left(7\right)$

${\mathrm{\θ}}_a={\tan }^{-1}\left(\frac{f_y^b}{\sqrt{f_x^{b^2}+f_z^{b^2}}}\right)---\left(8\right)$

式中，tan^-1(*)为反正切函数；

和

分别为加速度计沿机体坐标系b输出的x、y和z轴向的比力；

和

分别为磁传感器沿机体坐标系b输出的x、y和z轴向的磁场强度；γ_a、θ_a和ψ_m分别为由加速度计和磁传感器信息计算出的无人机滚动角、俯仰角和航向角；

c2、根据惯性导航系统的力学方程编排建立四元数估计的系统状态方程为：

$\stackrel{\·}{X}=\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x^b& -{\mathrm{\ω}}_y^b& -{\mathrm{\ω}}_z^b\\ {\mathrm{\ω}}_x^b& 0& {\mathrm{\ω}}_z^b& {-\mathrm{\ω}}_y^b\\ {\mathrm{\ω}}_y^b& {-\mathrm{\ω}}_z^b& 0& {\mathrm{\ω}}_x^b\\ {\mathrm{\ω}}_z^b& {\mathrm{\ω}}_y^b& {-\mathrm{\ω}}_x^b& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]+\frac{1}{2}\left[\begin{array}{ccc}{q}_1& q_2& q_3\\ -q_0& q_3& -q_2\\ -q_3& -q_0& +q_1\\ q_2& -q_1& -q_0\end{array}\right]\left[\begin{array}{c}{\mathrm{\δ}}_x^b\\ {\mathrm{\δ}}_y^b\\ {\mathrm{\δ}}_z^b\end{array}\right]---\left(10\right)$

式中，

为连续时间系统状态量； ${\widehat{\mathrm{\ω}}}^b={\left[\begin{array}{ccc}{\widehat{\mathrm{\ω}}}_x^b& {\widehat{\mathrm{\ω}}}_y^b& {\widehat{\mathrm{\ω}}}_z^b\end{array}\right]}^T={\left[\begin{array}{ccc}{\mathrm{\ω}}_x^b-{\mathrm{\δ}}_x^b& {\mathrm{\ω}}_y^b-{\mathrm{\δ}}_y^b& {\mathrm{\ω}}_z^b-{\mathrm{\δ}}_z^b\end{array}\right]}^T$ 为陀螺仪在载体坐标系b输出的理想值； ${\mathrm{\ω}}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}_x^b& {\mathrm{\ω}}_y^b& {\mathrm{\ω}}_z^b\end{array}\right]}^T$ 为陀螺仪在载体坐标系b输出的实际值； ${\mathrm{\δ}}^b={\left[\begin{array}{ccc}{\mathrm{\δ}}_x^b& {\mathrm{\δ}}_y^b& {\mathrm{\δ}}_z^b\end{array}\right]}^T$ 为陀螺仪在载体坐标系b输出的漂移值；q₀、q₁、q₂和q₃为四元数的四个分量；

c3、如图1和图2所示，利用加速度计和磁传感器输出值计算出的无人机姿态以及根据惯性导航力学方程编排和陀螺仪输出计算的载体姿态建立系统的量测方程为：

${\mathrm{\ψ}}_q={\tan 1}^{-1}\left(\frac{2(q_1{q}_2-q_0{q}_3)}{q_0^2-q_1^2+q_2^2-q_3^2}\right)---\left(12\right)$

${\mathrm{\γ}}_q={\tan }^{-1}\left(\frac{2(q_1{q}_3-q_0{q}_2)}{q_0^2-q_1^2+q_2^2-q_3^2}\right)---\left(14\right)$

式中，

为系统时间t有关的观测值；ψ_q、和γ_q分别为根据惯性导航系统力学方程和四元数计算出的无人机偏航角、俯仰角和滚动角；H(t,q)为与时间t和四元数q有关的非线性方程；V(t)为系统量测噪声；

c4、四状态扩展Kalman滤波器初始化，包括初始协方差矩阵，测量噪声方差矩阵和系统噪声反差矩阵；

c5、对式（10）和式（11）进行线性化后，利用四状态扩展Kalman滤波实时估计四元数，并实时计算无人机的俯仰角、滚动角和航向角；

（6）如图1和图2所示，当卫星导航定位系统不可用时，采用不等间隔Kalman滤波处理视觉传感器提供的量测信息来估计并补偿惯性导航系统的位置和速度误差；保证惯性导航系统输出稳定的位置和速度信息，其具体步骤为：

d1、如图3和图4所示，当无人机飞过区域的地物特征丰富且预装了飞行区域的基准图像，采用基于灰度信息的景像匹配定位方法获取无人机的位置，基于灰度信息的景像匹配方法有粗匹配和精匹配两个过程，粗匹配采用交叉相关算法，精匹配采用最小二乘算法；

d2、如图1、图5、图6和图7所示，当无人机飞过的区域地物特征不丰富或无人机未预装飞行区域的基准图，采用加速鲁棒特征SURF算法提取相邻两帧实时图像的特征点并进行同名特征点匹配，利用随机抽样一致性RANSAC算法剔除误匹配点，并根据相邻两帧的同名特征点计算无人机位置信息为：

式中，为无人机的相对位移；下标i为像平面上的第i个特征点；下标j为图像序列中的第j帧；下标p为像素坐标系；u和v为像素坐标；

为从像素坐标系p到导航坐标系n的转换矩阵；

为无人机的位置信息；

为卫星导航系统或景像匹配系统在不可用前获取的最后时刻无人机精确位置信息，并将该时刻记为t₀；

d3、对步骤d1和d2利用相机获取的无人机位置信息进行可靠性评估，舍弃不可靠位置后获得的可靠定位信息频率低且是非等间隔，根据系统状态方程式（1）和利用视觉传感器获得的视觉定位信息，建立视觉辅助惯性导航系统的量测方程为：

$Z_2=\left[\begin{array}{c}(L_I-L_c)R_M\\ ({\mathrm{\λ}}_I-{\mathrm{\λ}}_c)R_N\cos L_I\end{array}\right]+\left[\begin{array}{c}{v}_N^c\\ v_E^c\end{array}\right]=H_{2}X+V_2---\left(17\right)$

式中，Z₂为视觉导航系统输出的位置与惯性导航系统输出的位置相减获得的量测值；L_c和λ_c分别为视觉导航系统输出的北向和东向位置；H₂＝[0_2×5 diag(R_m R_NcosL_I)]为量测矩阵； $V_2={\left[\begin{array}{cc}{v}_N^c& v_E^c\end{array}\right]}^T$ 为视觉导航系统的量测噪声；

d4、对不等间隔量测值采用内插和外推的方法实现，量测值等间隔输出；

d5、对式（1）进行离散化后，利用不等间隔Kalman滤波实时估计并补偿惯性导航系统的位置误差，实时获得无人机精确的位置和速度信息。

惯性导航系统力学方程编排、惯性导航系统误差方程以及内插和外推算法都是本领域公知和常用的方法，不再赘述。

Claims (5)

1.一种基于多模型分布式滤波的无人机自主导航与定位方法，其特征在于：该方法在东北天地里坐标系下，根据惯性导航系统力学方程编排以及惯性导航系统误差方程建立不同阶次的多模型分布式滤波状态估计的系统状态方程，利用不同量测系统和传感器获得无人机运动的量测值，采用分布式滤波方法来估计并补偿低成本惯性导航系统的误差，其具体实现过程如下：

步骤（1）、当卫星导航系统可用时，采用七状态反馈Kalman滤波处理卫星导航定位系统输出的位置和速度信息来估计惯性导航系统水平通道和姿态误差，使惯性导航系统输出精确的位置、速度和姿态信息；

步骤（2）、采用四状态反馈Kalman滤波处理气压计或卫星导航定位系统输出的高度来估计并补偿惯性导航系统垂直通道的误差，获得惯性导航系统输出精确的高度和垂直通道速度信息；

步骤（3）、当卫星导航定位系统不可用时，采用四状态扩展Kalman滤波估计并补偿四元数误差，保证惯性导航系统长时间输出稳定的姿态信息；

步骤（4）、当卫星导航定位系统不可用时，采用不等间隔Kalman滤波处理视觉传感器提供的量测信息来估计并补偿惯性导航系统的位置和速度误差；保证惯性导航系统输出稳定的位置和速度信息。

2.根据权利要求1所述的一种基于多模型分布式滤波的无人机自主导航与定位方法，其特征在于：所述步骤（1）当卫星导航系统可用时，采用七状态反馈Kalman滤波处理卫星导航定位系统输出的位置和速度信息来估计惯性导航系统水平通道和姿态误差，使惯性导航系统输出精确的位置、速度和姿态信息，其具体方法为：

a1、根据惯性导航系统力学方程编排，利用加速度计和陀螺仪的输出值实时计算无人机的运动信息，包括位置、速度和姿态；

a2、根据惯性导航系统误差方程建立惯性导航/卫星导航组合导航系统状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(1\right)$

式中，X(t)^T＝[φ_E,φ_N,φ_U,δV_E,δV_N,δL,δλ]为连续时间系统状态；上标T为转置；E、N和U分别为东北天地理坐标系的坐标轴指向；t为时间；φ_E、φ_N和φ_U分别为惯性导航系统在东、北和天向平台误差角；δV_E和δV_N分别为惯性导航系统东向和北向速度误差；δL和δλ分别为惯性导航系统北向和东向位置误差；W(t)为系统噪声；F(t)和G(t)分别为状态转移矩阵和系统噪声矩阵；R_N和R_M分别为卯酉圈和子午面半径；

a3、利用卫星导航定位系统输出的位置和速度与惯性导航系统输出的位置和速度建立系统的量测方程为：

$Z_1=\left[\begin{array}{c}{V}_E^I-V_E^S\\ V_N^S-V_N^S\\ (L_S-L_S)R_M\\ ({\mathrm{\λ}}_S-{\mathrm{\λ}}_S)R_N\cos L_I\end{array}\right]+\left[\begin{array}{c}{v}_E\\ v_N\\ v_L\\ v_{\mathrm{\λ}}\end{array}\right]=H_{1}X+V_1---\left(2\right)$

$H_1=\left[\begin{array}{ccccc}{0}_{3\×3}& 0_{3\×1}& 0_{3\×1}& 0_{3\×1}& 0_{3\×1}\\ 0& 1& 0& 0& 0\\ 0& 0& 1& R_M& 0\\ 0& 0& 0& 0& R_N\cos L\end{array}\right]---\left(3\right)$

式中，Z₁为卫星导航系统与惯性导航系统输出值相减获得的量测值；

和

分别为惯性导航系统输出的东向和北向速度；L_I和λ_I分别指惯性导行系统输出北向位置和东向位置；

和分别指卫星导航系统输出的东向和北向速度；L_S和λ_S分别指卫星导航定位系统输出的北向和东向位置；上标或下标中的I和S分别为惯性导航系统和卫星导航定位系统的输出值；V₁＝[v_E v_N v_L v_λ]^T为卫星导航系统接收机的噪声；

a4、对式（1）进行离散化后，利用七状态反馈Kalman滤波方法实时估计并补偿惯性导航系统的误差，获得无人机精确的位置、速度和姿态信息。

3.根据权利要求1所述的一种基于多模型分布式滤波的无人机自主导航与定位方法，其特征在于：所述步骤（2）采用四状态反馈Kalman滤波处理气压计或卫星导航定位系统输出的高度来估计并补偿惯性导航系统垂直通道的误差，获得惯性导航系统输出精确的高度和垂直通道速度信息，其具体方法为：

b1、根据惯性导航系统力学方程编排，利用加速度计和陀螺仪输出信息计算无人机的高度和垂向速度；

b2、根据惯性导航垂直通道的力学方程编排以及气压计误差模型建立垂直通道误差估计的系统状态方程为：

X_k＝Φ_k|k-1X_k-1+W_k-1    （4）

${\mathrm{\Φ}}_{k|k-1}=\left[\begin{array}{cccc}1& \mathrm{\Δt}& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& \mathrm{\Δt}\\ 0& 0& -{\mathrm{\α}}^2\mathrm{\Δt}& -2\mathrm{\β\Δt}\end{array}\right]---\left(5\right)$

式中， $X_k={\left[\begin{array}{cccc}\mathrm{\δh}& \mathrm{\δ}{V}_U& \mathrm{\δ}{h}_a& \mathrm{\δ}{\tilde{h}}_a\end{array}\right]}^T$ 为系统状态；δh和δV_U分别为惯性导航系统输出的高度和垂向速度；δh_a和

分别为气压高计输出高度误差和高度误差的一阶马尔科夫过程；Φ_k|k-1为系统从k-1到k时刻一步状态转移矩阵；W_k-1为系统在k-1时刻的量测噪声；Δt为采样时间；α和β为气压计误差模型中参数；

b3、利用气压计输出的高度与惯性导航系统输出的高度建立系统的量测方程为：

Z₂＝H₂X_k+V₂      （6）

式中，Z₂为气压计与惯性导航系统输出的高度值相减获得的量测值；H₂＝[1 0 -1 0]^T；V₂为气压计的测量噪声；

b4、根据式（4）和式（6），利用四状态反馈Kalman滤波方法实时估计并补偿惯性导航系统垂直通道的误差，获得无人机运动过程中精确的高度和垂向速度信息。

4.根据权利要求1所述的一种基于多模型分布式滤波的无人机自主导航与定位方法，其特征在于：所述步骤（3）当卫星导航定位系统不可用时，采用四状态扩展Kalman滤波估计并补偿四元数误差，保证惯性导航系统长时间输出稳定的姿态信息，其具体方法为：

c1、利用加速度计、陀螺仪和磁传感器输出的信息计算无人机的姿态为：

${\mathrm{\γ}}_a={\tan }^{-1}\left(\frac{f_x^b}{(-f_z^b)}\right)---\left(7\right)$

${\mathrm{\θ}}_a={\tan }^{-1}\left(\frac{f_y^b}{\sqrt{f_x^{b^2}+f_z^{b^2}}}\right)---\left(8\right)$

式中，tan^-1(*)为反正切函数；

和

分别为加速度计沿机体坐标系b输出的x、y和z轴向的比力；和

分别为磁传感器沿机体坐标系b输出的x、y和z轴向的磁场强度；γ_a、θ_a和ψ_m分别为由加速度计和磁传感器信息计算出的无人机滚动角、俯仰角和航向角；

c2、根据惯性导航系统的力学方程编排建立四元数估计的系统状态方程为：

$\stackrel{\·}{X}=\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x^b& -{\mathrm{\ω}}_y^b& -{\mathrm{\ω}}_z^b\\ {\mathrm{\ω}}_x^b& 0& {\mathrm{\ω}}_z^b& {-\mathrm{\ω}}_y^b\\ {\mathrm{\ω}}_y^b& {-\mathrm{\ω}}_z^b& 0& {\mathrm{\ω}}_x^b\\ {\mathrm{\ω}}_z^b& {\mathrm{\ω}}_y^b& {-\mathrm{\ω}}_x^b& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]+\frac{1}{2}\left[\begin{array}{ccc}{q}_1& q_2& q_3\\ -q_0& q_3& -q_2\\ -q_3& -q_0& +q_1\\ q_2& -q_1& -q_0\end{array}\right]\left[\begin{array}{c}{\mathrm{\δ}}_x^b\\ {\mathrm{\δ}}_y^b\\ {\mathrm{\δ}}_z^b\end{array}\right]---\left(10\right)$

式中，

为连续时间系统状态量； ${\widehat{\mathrm{\ω}}}^b={\left[\begin{array}{ccc}{\widehat{\mathrm{\ω}}}_x^b& {\widehat{\mathrm{\ω}}}_y^b& {\widehat{\mathrm{\ω}}}_z^b\end{array}\right]}^T={\left[\begin{array}{ccc}{\mathrm{\ω}}_x^b-{\mathrm{\δ}}_x^b& {\mathrm{\ω}}_y^b-{\mathrm{\δ}}_y^b& {\mathrm{\ω}}_z^b-{\mathrm{\δ}}_z^b\end{array}\right]}^T$ 为陀螺仪在载体坐标系b输出的理想值； ${\mathrm{\ω}}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}_x^b& {\mathrm{\ω}}_y^b& {\mathrm{\ω}}_z^b\end{array}\right]}^T$ 为陀螺仪在载体坐标系b输出的实际值； ${\mathrm{\δ}}^b={\left[\begin{array}{ccc}{\mathrm{\δ}}_x^b& {\mathrm{\δ}}_y^b& {\mathrm{\δ}}_z^b\end{array}\right]}^T$ 为陀螺仪在载体坐标系b输出的漂移值；q₀、q₁、q₂和q₃为四元数的四个分量；

c3、利用加速度计和磁传感器输出值计算出的无人机姿态以及根据惯性导航力学方程编排和陀螺仪输出计算的载体姿态建立系统的量测方程为：

${\mathrm{\ψ}}_q={\tan 1}^{-1}\left(\frac{2(q_1{q}_2-q_0{q}_3)}{q_0^2-q_1^2+q_2^2-q_3^2}\right)---\left(12\right)$

${\mathrm{\γ}}_q={\tan }^{-1}\left(\frac{2(q_1{q}_3-q_0{q}_2)}{q_0^2-q_1^2+q_2^2-q_3^2}\right)---\left(14\right)$

式中，

为系统时间t有关的观测值；ψ_q、

_q和γ_q分别为根据惯性导航系统力学方程和四元数计算出的无人机偏航角、俯仰角和滚动角；H(t,q)为与时间t和四元数q有关的非线性方程；V(t)为系统量测噪声；

c4、对式（10）和式（11）进行线性化后，利用四状态扩展Kalman滤波实时估计四元数，并实时计算无人机的俯仰角、滚动角和航向角。

5.根据权利要求2所述的一种基于多模型分布式滤波的无人机自主导航与定位方法，其特征在于：所述步骤（4）当卫星导航定位系统不可用时，采用不等间隔Kalman滤波处理视觉传感器提供的量测信息来估计并补偿惯性导航系统的位置和速度误差；保证惯性导航系统输出稳定的位置和速度信息，其具体方法为：

d1、当无人机飞过区域的地物特征丰富且预装了飞行区域的基准图像，采用基于灰度信息的景像匹配定位方法获取无人机的位置，基于灰度信息的景像匹配方法有粗匹配和精匹配两个过程，粗匹配采用交叉相关算法，精匹配采用最小二乘算法；

d2、当无人机飞过的区域地物特征不丰富或无人机未预装飞行区域的基准图，采用加速鲁棒特征SURF算法提取相邻两帧实时图像的特征点并进行同名特征点匹配，利用随机抽样一致性RANSAC算法剔除误匹配点，并根据相邻两帧的同名特征点计算无人机位置信息为：

式中，

为无人机的相对位移；下标i为像平面上的第i个特征点；下标j为图像序列中的第j帧；下标p为像素坐标系；u和v为像素坐标；

为从像素坐标系p到导航坐标系n的转换矩阵；

为无人机的位置信息；

为卫星导航系统或景像匹配系统在不可用前获取的最后时刻无人机精确位置信息，并将该时刻记为t₀；

d3、对步骤d1和d2利用相机获取的无人机位置信息进行可靠性评估，舍弃不可靠位置后获得的可靠定位信息频率低且是非等间隔，根据系统状态方程式（1）和利用视觉传感器获得的视觉定位信息，建立视觉辅助惯性导航系统的量测方程为：

$Z_2=\left[\begin{array}{c}(L_I-L_c)R_M\\ ({\mathrm{\λ}}_I-{\mathrm{\λ}}_c)R_N\cos L_I\end{array}\right]+\left[\begin{array}{c}{v}_N^c\\ v_E^c\end{array}\right]=H_{2}X+V_2---\left(17\right)$

式中，Z₂为视觉导航系统输出的位置与惯性导航系统输出的位置相减获得的量测值；L_c和λ_c分别为视觉导航系统输出的北向和东向位置；H₂＝[0_2×5 diag(R_m R_NcosL_I)]为量测矩阵； $V_2={\left[\begin{array}{cc}{v}_N^c& v_E^c\end{array}\right]}^T$ 为视觉导航系统的量测噪声；

d4、对式（1）进行离散化后，利用不等间隔Kalman滤波实时估计并补偿惯性导航系统的位置误差，实时获得无人机精确的位置和速度信息。

Images