一种能够灵敏检测故障的IMU/GPS组合导航方法技术

本发明专利技术公开了一种能够灵敏检测故障的IMU/GPS组合导航方法，包括步骤如下：采用UTM投影算法将WGS84坐标系下的GPS测量信息投影到UTM平面；引入卡方检验算法，建立基于预积分理论的GPS故障检测模型，基于IMU的预积分值将作为基于GPS的预积分值的期望值；将基于IMU的预积分值与基于GPS的预积分值的偏差转化成卡方值用于反映GPS测量的异常程度；当系统通过卡方检验算法检测出最新一次GPS测量异常时，首先将该GPS测量点剔除，并使用IMU测量传播作为该时刻的系统状态估计；采用M

【技术实现步骤摘要】
一种能够灵敏检测故障的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测量对应的时间戳为t
I
，第k+1个IMU测量的前一个GPS测量数据G1对应的时间戳为第k+1
个IMU测量的后一个GPS测量数据G2对应的时间戳为所述的线性插值的过程可表达为：
[0018][0019]其中，G
k+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
和a
t
是角速度和加速度的真实值；b
t
表示偏置；n
本文档来自技高网...

【技术保护点】

【技术特征摘要】
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组合导航方法。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测量对应的时间戳为t
I
，第k+1个IMU测量的前一个GPS测量数据G1对应的时间戳为t
G1
，第k+1个IMU测量的后一个GPS测量数据G2对应的时间戳为t
G2
；所述的线性插值的过程可表达为：
其中，G
k+1
表示第k+1个GPS测量数据；至此，GPS测量与IMU测量的时间戳对齐。4.根据权利要求3所述的能够灵敏检测故障的IMU/GPS组合导航方法，其特征在于：步骤S2，具体步骤如下：S201：定义IMU的角速度和加速度的测量模型；S202：使用IMU的测量模型来推断系统在t+Δt时刻世界坐标系下的速度、位移和旋转：S203：根据式(4)，基于IMU测量计算系统在时刻i到时刻j间的预积分值的测量值；S204：根据式(4)，基于GPS测量计算得到系统在时刻i到时刻j间的预积分值的估计值；S205：基于残差卡方检验，残差值r被定义为基于IMU的...

【专利技术属性】
技术研发人员：赵家俊，吴元清，鲁仁全，黄政杰，
申请(专利权)人：广东工业大学，
类型：发明
国别省市：