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

权利要求书

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

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

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

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

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

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

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

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

X·(t)=F(t)X(t)+G(t)W(t)---(1)]]>

式中，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、利用卫星导航定位系统输出的位置和速度与惯性导航系统输出的位置和速度建立系统的量测方程为：

Z1=VEI-VESVNS-VNS(LS-LS)RM(λS-λS)RNcosLI+vEvNvLvλ=H1X+V1---(2)]]>

H1=03×303×103×103×103×101000001RM00000RNcosL---(3)]]>

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

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