本发明公开了一种能够灵敏检测故障的IMU/GPS组合导航方法，包括步骤如下：采用UTM投影算法将WGS84坐标系下的GPS测量信息投影到UTM平面；引入卡方检验算法，建立基于预积分理论的GPS故障检测模型，基于IMU的预积分值将作为基于GPS的预积分值的期望值；将基于IMU的预积分值与基于GPS的预积分值的偏差转化成卡方值用于反映GPS测量的异常程度；当系统通过卡方检验算法检测出最新一次GPS测量异常时，首先将该GPS测量点剔除，并使用IMU测量传播作为该时刻的系统状态估计；采用M‑estimator函数减轻GPS异常测量对于系统输出结果的影响；采用因子图优化构建联合优化框架，对IMU和GPS的测量信息进行融合，实现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组合导航方法；
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加权的残差平方和：2
S206：基于卡方检验，GPS异常点由如下准则进行判断：
其中， 和 是t时刻在基体坐标系下的原始IMU测量；ωt和at是角速度和加速度的真实值；bt表示偏置；nt表示高斯白噪声； 是t时刻世界坐标系W到基体坐标系B下的旋转；g是世界坐标系下的重力向量；
步骤S202，速度vt+Δt、位移pt+Δt和旋转Rt+Δt的表达式如下：式中，
2.根据权利要求1所述的能够灵敏检测故障的IMU/GPS组合导航方法，其特征在于：步骤S1，具体地，定义一个在WGS84坐标系下的点的坐标为(L,B)，其中，L为经度，B为纬度；将该点投影到UTM平面上的(x,y)位置；给定地球的赤道半径为a，第一离心率为e，第二离心率为e′；WGS84坐标系的原点设为(L0,0)；基于UTM投影算法，(L,B)转化为对应的(x,y)，如下：其中，T,C,A,M,N均为中间变量。
3.根据权利要求2所述的能够灵敏检测故障的IMU/GPS组合导航方法，其特征在于：使用线性插值将不同时间的GPS测量与IMU测量对齐；设第k+1个IMU测量对应的时间戳为tI，第k+1个IMU测量的前一个GPS测量数据G1对应的时间戳为 第k+1个IMU测量的后一个GPS测量数据G2对应的时间戳为 所述的线性插值的过程可表达为：其中，Gk+1表示第k+1个GPS测量数据；
4.根据权利要求1所述的能够灵敏检测故障的IMU/GPS组合导航方法，其特征在于：步骤S203，基于IMU测量计算系统在时刻i到时刻j间的预积分值的测量值的表达式如下：式中， 表示IMU测量数据中的线性加速度向量； 表示时刻k时加速度计的静态偏置、表示时刻k时加速度计的高斯白噪。
5.根据权利要求1所述的能够灵敏检测故障的IMU/GPS组合导航方法，其特征在于：步骤S204，基于GPS测量计算得到系统在时刻i到时刻j间的预积分值的估计值的表达式如下：
6.根据权利要求5所述的能够灵敏检测故障的IMU/GPS组合导航方法，其特征在于：具体来说，当GPS测量作为约束项参与位姿优化时，采用Huber损失函数ρ(·)替代目标优化对象的马氏距离函数；目标优化函数抽象表达如下：
7.根据权利要求6所述的能够灵敏检测故障的IMU/GPS组合导航方法，其特征在于：为了实现式(10)的联合优化，从而使得系统融合IMU和GPS的测量信息，采用因子图优化构建联合优化框架；在联合优化框架下，每次GPS测量信息返回时，系统将生成一个新的关键帧；
该关键帧存储当前时间戳t下系统的位姿(x,y,z)和姿态(roll,pitch,yaw)的信息，并且关键帧将被转化为节点加入到因子图中作为优化对象；
同时，GPS测量数据将转化为一元因子加入因子图中，而相邻两次GPS测量间的IMU数据将转化为IMU预积分因子加入到因子图中；
构建完上述因子图优化框架后，使用GTSAM因子图优化库对所构建的因子图进行非线性最优化，求解得到最新关键帧的最优估计，实现IMU/GPS组合导航方法。
一种能够灵敏检测故障的IMU/GPS组合导航方法
技术领域
[0001] 本发明涉及定位导航技术领域，更具体的，涉及一种能够灵敏检测故障的IMU/GPS组合导航方法。
背景技术
[0002] 目前，IMU/GPS组合导航方案比较单一，主要采用基于扩展卡尔曼滤波算法，或者非线性优化算法对IMU和GPS测量进行融合，具体方案如下：
[0003] 如中国专利公开号：CN108709552A，公开日：2018‑10‑26，提供了一种基于MEMS的IMU和GPS紧组合方法，该方法采用最小二乘法对GPS原始数据进行估计，输出GPS导航估计值；将位置、速度的导航估计值提供给IMU作为初始位置和速度信息；IMU采用卡尔曼滤波方法进行初始对准，并对IMU测量数据进行解算，得到IMU导航信息；利用IMU导航信息计算多普勒频率提供给GPS进行捕获跟踪辅助，提高其性能；建立动态误差模型，采用扩展卡尔曼滤波器将GPS和IMU的导航信息进行数据融合，反馈误差对同时刻的IMU的导航信息校正，输出融合并校正的最佳位置、速度、姿态信息。该方案将IMU测量传播方程作为系统预测步骤，基于欧拉积分或中值积分对系统状态进行传播。然后GPS测量作为外部观测量，UTM投影算法作为观测方程。基于卡尔曼滤波器算法，GPS观测量将用于校正系统预测的状态，从而实现IMU和GPS的融合定位。
[0004] 如中国专利公开号：CN111121767A，公开日：2020‑05‑08，公开了融合GPS的机器人视觉惯导组合定位方法，包括提取并匹配双目相机左右图像与前后图像特征点，计算特征点三维坐标与图像帧相对位姿；选取图像流中的关键帧，创建滑动窗口，并将关键帧加入滑动窗口；计算视觉重投影误差、IMU预积分残差与零偏残差并组合为联合位姿估计残差；使用L‑M法对联合位姿估计残差进行非线性优化，得到优化后的视觉惯导(VIO)机器人位姿；若当前时刻有GPS数据，则对GPS位置数据与VIO位姿估计数据作自适应抗差卡尔曼滤波，得到最终的机器人位姿；若无GPS数据，则以VIO位姿数据代替最终位姿数据。该方案：在非线性优化算法中，系统的状态量作为待优化变量，IMU测量和GPS测量作为优化约束项。通过测量方程，IMU测量和GPS测量被分别转化为代价函数的参数，而测量值与系统的状态量的马氏距离作为残差。最终通过非线性优化理论计算使得残差最小化的系统的状态量。该最优的系统的状态量即为IMU和GPS的融合定位结果。
[0005] 以上所列的IMU/GPS组合导航的融合方法缺少一种可靠的传感器异常检测方法。GPS系统可以提供厘米级定位服务，但是在GPS失效场景(例如隧道、建筑密集区域、峡谷和其他遮挡GPS信号的环境)下，GPS接收器难以捕获有效的GPS信号导致GPS测量存在异常点。
GPS异常测量点往往远离实际位置，一旦它被引入到融合系统将会使得定位系统输出严重失真。因此GPS异常值检测是非常重要的步骤，但是目前的IMU/GPS组合导航系统普遍依赖GPS接收器搜索到的卫星数目来估计当前测量状态。当搜索到的卫星数目小于一个阈值后，则认为GPS发生异常。但是由于多径效应(常见于隧道出入口或建筑密集区域)，往往发生所搜索到的卫星数目明显大于阈值，但是此时GPS测量已经发生大尺度的异常。
发明内容
[0006] 本发明为了解决以上现有技术存在的不足，提供了一种能够灵敏检测故障的IMU/GPS组合导航方法。
[0007] 为实现上述本发明目的，采用的技术方案如下：
[0008] 一种能够灵敏检测故障的IMU/GPS组合导航方法，所述方法包括步骤如下：
[0009] S1：采用UTM投影算法将WGS84坐标系下的GPS测量信息投影到UTM平面；
[0010] S2：引入卡方检验算法，建立基于预积分理论的GPS故障检测模型，基于IMU的预积分值将作为基于GPS的预积分值的期望值；将基于IMU的预积分值与基于GPS的预积分值的偏差转化成卡方值用于反映GPS测量的异常程度；
[0011] S3：当系统通过卡方检验算法检测出最新一次GPS测量异常时，首先将该GPS测量点剔除，并使用IMU测量传播作为该时刻的系统状态估计；
[0012] S4：为了进一步提高IMU/GPS融合系统的鲁棒性，采用M‑estimator函数减轻GPS异常测量对于系统输出结果的影响；
[0013] S5：采用因子图优化构建联合优化框架，对IMU和GPS的测量信息进行融合。
[0014] 优选地，步骤S1，具体地，定义一个在WGS84坐标系下的点的坐标为(L,B)，其中，L为经度，B为纬度；将该点投影到UTM平面上的(x,y)位置；给定地球的赤道半径为a，第一离心率为e，第二离心率为e′；WGS84坐标系的原点设为(L0,0)；基于UTM投影算法，(L,B)转化为对应的(x,y)，如下：
[0015]
[0016] 其中，T,C,A,M,N均为中间变量。
[0017] 进一步地，使用线性插值将不同时间的GPS测量与IMU测量对齐；设第k+1个IMU测量对应的时间戳为tI，第k+1个IMU测量的前一个GPS测量数据G1对应的时间戳为 第k+1个IMU测量的后一个GPS测量数据G2对应的时间戳为 所述的线性插值的过程可表达为：
[0018]
[0019] 其中，Gk+1表示第k+1个GPS测量数据；
[0020] 至此，GPS测量与IMU测量的时间戳对齐。
[0021] 再进一步地，步骤S2，具体步骤如下：
[0022] S201：定义IMU的角速度和加速度的测量模型；
[0023] S202：使用IMU的测量模型来推断系统在t+Δt时刻世界坐标系下的速度、位移和旋转：
[0024] S203：根据式(4)，基于IMU测量计算系统在时刻i到时刻j间的预积分值的测量值；
[0025] S204：根据式(4)，基于GPS测量计算得到系统在时刻i到时刻j间的预积分值的估计值；
[0026] S205：基于残差卡方检验，残差值r被定义为基于IMU的 和基于GPS的之间的差值；r反映了GPS测量和IMU测量的一致性程度，如：
[0027]
[0028] 如果GPS测量是正常的，r服从零高斯分布，否则r含有偏置；决策变量s被定义为由相关协方差矩阵A加权的残差平方和：
[0029]
[0030] 决策变量s服从3自由度的χ2分布；
[0031] S206：基于卡方检验，GPS异常点由如下准则进行判断：
[0032]
[0033] 再进一步地，步骤S201，所述的测量模型表达式如下：
[0034]
[0035] 其中， 和 是t时刻在基体坐标系下的原始IMU测量；ωt和at是角速度和加速度的真实值；bt表示偏置；nt表示高斯白噪声； 是t时刻世界坐标系W到基体坐标系B下的旋转；g是世界坐标系下的重力向量。
[0036] 再进一步地，步骤S202，速度、位移和旋转的表达式如下：
[0037]
[0038] 式中，
[0039] 再进一步地，步骤S203，基于IMU测量计算系统在时刻i到时刻j间的预积分值的测量值的表达式如下：
[0040]
[0041] 式中， 表示IMU测量数据中的线性加速度向量； 表示时刻k时加速度计的静态偏置、 表示时刻k时加速度计的高斯白噪；
[0042] 再进一步地，步骤S204，基于GPS测量计算得到系统在时刻i到时刻j间的预积分值的估计值的表达式如下：
[0043]
[0044] 再进一步地，具体来说，当GPS测量作为约束项参与位姿优化时，采用Huber损失函数ρ(·)替代目标优化对象的马氏距离函数；目标优化函数抽象表达如下：
[0045]
[0046] 为了实现式(10)的联合优化，从而使得系统融合IMU和GPS的测量信息，采用因子图优化构建联合优化框架；在联合优化框架下，每次GPS测量信息返回时，系统将生成一个新的关键帧；该关键帧存储当前时间戳t下系统的位姿(x,y,z)和姿态(roll,pitch,yaw)的信息，并且关键帧将被转化为节点加入到因子图中作为优化对象；
[0047] 同时，GPS测量数据将转化为一元因子加入因子图中，而相邻两次GPS测量间的IMU数据将转化为IMU预积分因子加入到因子图中；
[0048] 构建完上述因子图优化框架后，使用GTSAM因子图优化库对所构建的因子图进行非线性最优化，求解得到最新关键帧的最优估计，至此，实现IMU/GPS组合导航方法。
[0049] 本发明的有益效果如下：
[0050] 本发明在于利用IMU固有的稳定性、可靠性以及隐蔽性，让IMU测量值去衡量并判断GPS测量的异常情况。更具体地，本发明创新性地利用IMU预积分理论以及卡方检验理论，构建出能够描述IMU测量与GPS测量关系的理论公式，有效地避免系统融合GPS异常测量。同时引入M‑estimator改进因子图优化框架下的非线性优化函数，进一步降低GPS异常测量对系统的影响。除此之外，由于系统建模之初即认为系统是使用低成本IMU，因此本发明非常适用于低成本的IMU/GPS组合导航系统，系统可以在不更改硬件的情况下，仅通过融合算法的改进就可以实现比传统的基于卡尔曼滤波器的IMU/GPS组合导航更可靠的定位性能。
附图说明
[0051] 图1是实施例1所述的IMU/GPS组合导航方法的步骤流程图。
[0052] 图2是实施例1所述的因子图优化框架示意图。
具体实施方式
[0053] 下面结合附图和具体实施方式对本发明做详细描述。
[0054] 实施例1
[0055] 如图1所示，一种能够灵敏检测故障的IMU/GPS组合导航方法，所述方法包括步骤如下：
[0056] S1：由于GPS获取的定位信息为WGS84地理坐标系下的经纬度数据，而IMU获取到的惯性测量数据的单位为米和弧度。两者获取的数据的量纲不同，难以进行数据融合。因此，采用UTM投影算法将WGS84坐标系下的GPS测量信息投影到UTM平面；
[0057] 在一个具体的实施例中，定义一个在WGS84坐标系下的点的坐标为(L,B)，其中，L为经度，B为纬度；将该点投影到UTM平面上的(x,y)位置；给定地球的赤道半径为a，第一离心率为e，第二离心率为e′；WGS84坐标系的原点设为(L0,0)；基于UTM投影算法，(L,B)转化为对应的(x,y)，如下：
[0058]
[0059] 其中，T,C,A,M,N均为中间变量。
[0060] 通过式(1)，GPS的测量数据被投影到UTM平面，其数据的量纲与IMU一致。由于IMU和GPS的测量是异步的，测量时间误差会影响信息融合系统的精度。例如在第3s时GPS_1数据返回，第4s时IMU_1数据返回，第5秒时GPS_2数据返回，为了让两种数据的时间戳统一，所以对GPS_1和GPS_2进行线性插值，估计出第4秒时的GPS数据应该是多少。
[0061] 具体地，采用线性插值将不同时间的GPS测量与IMU测量对齐；设第k+1个IMU测量对应的时间戳为tI，第k+1个IMU测量的前一个GPS测量数据G1对应的时间戳为 第k+1个IMU测量的后一个GPS测量数据G2对应的时间戳为 所述的线性插值的过程可表达为：
[0062]
[0063] 其中，Gk+1表示第k+1个GPS测量数据；
[0064] 至此，GPS测量与IMU测量的时间戳对齐。
[0065] S2：在室内或者建筑物密集区域，GPS信号往往被遮挡。如果GPS异常测量(GPS异常点)被引入到融合系统，系统输出结果往往会发生严重失真甚至系统发生崩溃，为了快速且有效地检测出GPS异常点，IMU测量将作为GPS离群程度的参考对象。尽管IMU积分的累积误差是无界的，但是由于其固有的稳定性、隐蔽性，在本系统中它被认为不会发生异常测量。为了避免IMU长时间积分产生大尺度的误差，采用IMU预积分理论评价GPS测量的可靠程度。
[0066] 引入卡方检验算法，建立基于预积分理论的GPS故障检测模型，基于IMU的预积分值将作为基于GPS的预积分值的期望值；将基于IMU的预积分值与基于GPS的预积分值的偏差转化成卡方值用于反映GPS测量的异常程度；
[0067] 步骤S2，具体步骤如下：
[0068] S201：定义IMU的角速度和加速度的测量模型，表达式如下：
[0069]
[0070] 其中， 和 是t时刻在基体坐标系下的原始IMU测量；ωt和at是角速度和加速度的真实值；bt表示偏置；nt表示高斯白噪声； 是t时刻世界坐标系W到基体坐标系B下的旋转；g是世界坐标系下的重力向量。
[0071] S202：使用IMU的测量模型来推断系统在t+Δt时刻世界坐标系下的速度、位移和旋转，表达式如下：
[0072]
[0073] 式中，
[0074] S203：根据式(4)，基于IMU测量计算系统在时刻i到时刻j间的预积分值的测量值，其表达式如下：
[0075]
[0076] 式中， 表示IMU测量数据中的线性加速度向量； 表示时刻k时加速度计的静态偏置、 表示时刻k时加速度计的高斯白噪。
[0077] S204：根据式(4)，基于GPS测量计算得到系统在时刻i到时刻j间的预积分值的估计值，表达式如下：
[0078]
[0079] S205：需要说明的是，Δvij和Δpij都不是对应实际的速度和位移变化量，式(5)和式(6)最重要的作用是使得IMU测量与系统状态量分离。通过式(5)和式(6)，可以分别通过IMU测量或GPS测量计算预积分值，从而使得IMU测量和GPS测量具有可比性。除此之外，预积分值仅与时刻i到时刻j间的IMU测量有关，这避免了每次都要应用(4)式对所有IMU测量重新积分，很大程度上降低系统计算量。
[0080] 通过式(6)，GPS测量可以估计时刻i到时刻j间的预积分值。基于残差卡方检验，残差值r被定义为基于IMU的 和基于GPS的 之间的差值；r反映了GPS测量和IMU测量的一致性程度，如：
[0081]
[0082] 如果GPS测量是正常的，r服从零高斯分布，否则r含有偏置；决策变量s被定义为由相关协方差矩阵A加权的残差平方和：
[0083]
[0084] 决策变量s服从3自由度的χ2分布；
[0085] S206：基于卡方检验，GPS异常点由如下准则进行判断：
[0086]
[0087] S3：当系统通过卡方检验算法检测出最新一次GPS测量异常时，首先将该GPS测量点剔除，并使用IMU测量传播作为该时刻的系统状态估计；
[0088] S4：在实际应用中难以保证式(9)能够检测出所有GPS异常点。为了进一步提高IMU/GPS融合系统的鲁棒性，采用M‑estimator函数减轻GPS异常测量对于系统输出结果的影响；具体来说，当GPS测量作为约束项参与位姿优化时，采用Huber损失函数ρ(·)替代目标优化对象的马氏距离函数。目标优化函数抽象表达如下：
[0089]
[0090] S5：采用因子图优化构建联合优化框架，对IMU和GPS的测量信息进行融合。为了实现式(10)的联合优化，从而使得系统融合IMU和GPS的测量信息，采用因子图优化构建联合优化框架；在联合优化框架下，每次GPS测量信息返回时，系统将生成一个新的关键帧；该关键帧存储当前时间戳t下系统的位姿(x,y,z)和姿态(roll,pitch,yaw)的信息，并且关键帧将被转化为节点加入到因子图中作为优化对象；
[0091] 同时，GPS测量数据将转化为一元因子加入因子图中，而相邻两次GPS测量间的IMU数据将转化为IMU预积分因子加入到因子图中；
[0092] 构建完上述因子图优化框架后，使用GTSAM因子图优化库对所构建的因子图进行非线性最优化，求解得到最新关键帧的最优估计，至此，实现IMU/GPS组合导航方法。
[0093] 本实施例首先是通过UTM投影算法将WGS84坐标系下的GPS测量信息投影到UTM平面，使得GPS测量(经纬度)和IMU测量信息(米)的量纲一致。引入卡方检验的思想，建立了基于预积分理论的GPS故障检测模型。IMU测量和GPS测量都能独立计算出预积分值，基于IMU的预积分值将作为基于GPS的预积分值的期望值，它们之间的偏差将被转化成卡方值用于反映GPS测量的异常程度。为了进一步提高IMU/GPS融合系统的鲁棒性，采用M‑estimator算法减轻GPS异常测量对于系统输出结果的影响。最后，因子图优化被应用融合IMU和GPS的测量信息，实现高频率、高精度、高鲁棒性的定位系统。
[0094] 在本实施例中，系统可以在不更改硬件的情况下，仅通过融合算法的改进就可以实现比传统的基于卡尔曼滤波器的IMU/GPS组合导航更可靠的定位性能。在一个较低成本的情况下，大大提高了系统的精度和鲁棒性，从而提高了IMU/GPS组合导航系统的应用价值。
[0095] 显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。
