[发明专利]一种能够灵敏检测故障的IMU/GPS组合导航方法

权利要求书

1.一种能够灵敏检测故障的IMU/GPS组合导航方法，其特征在于：所述方法包括步骤如下：

S1：采用UTM投影算法将WGS84坐标系下的GPS测量信息投影到UTM平面；

S2：引入卡方检验算法，建立基于预积分理论的GPS故障检测模型，将基于IMU的预积分值将作为基于GPS的预积分值的期望值；将基于IMU的预积分值与基于GPS的预积分值的偏差转化成卡方值用于反映GPS测量的异常程度；

S3：当系统通过卡方检验算法检测出最新一次GPS测量异常时，首先将该GPS测量点剔除，并使用IMU测量传播作为该时刻的系统状态估计；

S4：为了进一步提高IMU/GPS融合系统的鲁棒性，采用M-estimator函数减轻GPS异常测量对于系统输出结果的影响；

S5：采用因子图优化构建联合优化框架，对IMU和GPS的测量信息进行融合，实现IMU/GPS组合导航方法；

步骤S2，具体步骤如下：

S201：定义IMU的角速度和加速度的测量模型；

S202：使用IMU的测量模型来推断系统在t+Δt时刻世界坐标系下的速度、位移和旋转：

S203：根据式(4)，基于IMU测量计算系统在时刻i到时刻j间的预积分值的测量值；

S204：根据式(4)，基于GPS测量计算得到系统在时刻i到时刻j间的预积分值的估计值；

S205：基于残差卡方检验，残差值r被定义为基于IMU的和基于GPS的之间的差值；r反映了GPS测量和IMU测量的一致性程度，如：

如果GPS测量是正常的，r服从零高斯分布，否则r含有偏置；决策变量s被定义为由相关协方差矩阵A加权的残差平方和：

决策变量s服从3自由度的χ²分布；

S206：基于卡方检验，GPS异常点由如下准则进行判断：

步骤S201，所述的测量模型表达式如下：

其中，和是t时刻在基体坐标系下的原始IMU测量；ω_t和a_t是角速度和加速度的真实值；b_t表示偏置；n_t表示高斯白噪声；是t时刻世界坐标系W到基体坐标系B下的旋转；g是世界坐标系下的重力向量；

步骤S202，速度v_t+Δt、位移p_t+Δt和旋转R_t+Δt的表达式如下：

式中，

2.根据权利要求1所述的能够灵敏检测故障的IMU/GPS组合导航方法，其特征在于：步骤S1，具体地，定义一个在WGS84坐标系下的点的坐标为(L,B)，其中，L为经度，B为纬度；将该点投影到UTM平面上的(x,y)位置；给定地球的赤道半径为a，第一离心率为e，第二离心率为e′；WGS84坐标系的原点设为(L₀,0)；基于UTM投影算法，(L,B)转化为对应的(x,y)，如下：

其中，T,C,A,M,N均为中间变量。

3.根据权利要求2所述的能够灵敏检测故障的IMU/GPS组合导航方法，其特征在于：使用线性插值将不同时间的GPS测量与IMU测量对齐；设第k+1个IMU测量对应的时间戳为t_I，第k+1个IMU测量的前一个GPS测量数据G₁对应的时间戳为第k+1个IMU测量的后一个GPS测量数据G₂对应的时间戳为所述的线性插值的过程可表达为：

其中，G_k+1表示第k+1个GPS测量数据；

至此，GPS测量与IMU测量的时间戳对齐。

4.根据权利要求1所述的能够灵敏检测故障的IMU/GPS组合导航方法，其特征在于：步骤S203，基于IMU测量计算系统在时刻i到时刻j间的预积分值的测量值的表达式如下：

式中，表示IMU测量数据中的线性加速度向量；表示时刻k时加速度计的静态偏置、表示时刻k时加速度计的高斯白噪。