一种基于图优化的组合导航系统融合定位方法

本发明涉及定位，具体涉及一种基于图优化的组合导航系统融合定位方法。

背景技术：

1、全球导航卫星系统(globalnavigation satellite system，gnss)系统作为传统定位技术手段，在大部分场景都可以提供绝对的位置信息，但在室内，城市峡谷等特殊环境应用受限。惯性传感器(inertial measurementunit，imu)数据可以不受外界信号因素的影响，但在长时间的定位中因为积分解算原理容易形成误差累积。因此，组合导航系统(gnss&imu)融合为当前主流的组合导航方法。

2、传统gnss定位系统在恶劣环境下接收卫星信号数目较少，由于gnss定位系统位置解算原理基于最小二乘原理，卫星信号提供的约束越少而定位精度就会越低。而在一些极端情况由于约束过少甚至会导致gnss系统无法定位的情况，而完全依靠imu系统便会导致定位精度降低。基于滤波的解算方法基于马尔科夫性概率假设，只考虑当前时刻量测信息，历史信息被封存于被统计原理认定为最优估计的“上一时刻”，无法被直接利用，导致传统滤波算法对历史信息利用率较低，造成的误差较大。得益于算力的增长，图优化算法通过建立更多的量测约束，将不同时刻的状态信息进行批量估计，更直接有效地利用了历史存储信息，优化估计结果具有更高的准确性。但直接将不同时刻的gnss定位信息与imu位置轨迹信息进行图优化建模，只是从计算形式上将迭代计算的滤波替换为规模化的最小二乘求解，并没有引入更多的约束信息。同时，gnss的定位结果亦是建立在卫星伪距上的一次估计，其本身是处理后的，非原始的直接测量信息。

3、因此，如何通过增强gnss定位信息与imu位置轨迹信息之间的关联性在算法模型中的作用，以提升组合导航系统解算的精度与有效性，成为目前亟待解决的问题。

技术实现思路

1、有鉴于此，本发明实施例提供了一种基于图优化的组合导航系统融合定位方法，以解决现有技术中的组合导航系统存在gnss定位信息与imu位置轨迹信息之间的关联性利用效率不足，导致定位精度和有效性低的问题。

2、本发明实施例提供了一种基于图优化的组合导航系统融合定位方法，包括：

3、对imu轨迹信息进行解算，获取各时刻下的第一定位载体位置；

4、将各时刻下的第一定位载体位置作为顶点，建立图优化模型；

5、通过gnss数据获取在相邻的第一时刻和第二时刻之间的定位载体的第一位置转移关系，并将第一位置转移关系作为图优化模型的量测；

6、通过imu轨迹信息获取在相邻的第一时刻和第二时刻之间的定位载体的第二位置转移关系，并将第二位置转移关系作为图优化模型的待优化变量；

7、根据量测和待优化变量建立目标函数，并通过莱文贝格－马夸特算法进行优化；

8、当目标函数优化至最小值时，获取对应的误差修正量，对imu数据解算进行修正。

9、可选地，在对imu轨迹信息进行解算，获取各时刻下的第一定位载体位置之前，还包括：

10、通过读取星历数据进行卫星位置匹配，获取各卫星在ecef坐标系下各个时刻对应的卫星位置；

11、通过gnss数据获取各卫星在所有时刻的伪距信息；

12、根据卫星位置和伪距信息，进行坐标转换，获取各卫星在东北天坐标系下的位置。

13、可选地，坐标转换包括：

14、设定东北天坐标系的坐标原点；

15、以坐标原点的东方为x轴，以坐标原点的北方作为y轴，以坐标原点的上方作为z轴建立坐标系；

16、根据坐标转换原理，以enu坐标系坐标原点经纬度建立坐标转换矩阵；

17、通过坐标转换矩阵将卫星位置从ecef坐标系转换到东北天坐标系。

18、可选地，对imu轨迹信息进行解算，获取各时刻下的第一定位载体位置，包括：

19、根据imu轨迹信息中的定位载体的初始速度信息、坐标系中三个方向的加速度和时间信息，通过积分算法获取各个时间段内定位载体的位置变化量和各时刻下的第一定位载体位置。

20、可选地，通过gnss数据获取在相邻的第一时刻和第二时刻之间的定位载体的第一位置转移关系，包括：

21、通过gnss数据获取各时刻对应的各卫星位置；

22、在卫星位置和第一定位载体位置之间进行伪距转换，得到三个伪距；

23、分别获取对应三个伪距下的三个位置坐标；

24、分别根据三个位置坐标和在初始时刻的卫星位置计算出三个距离数据；

25、对三个距离数据求平均值，作为卫星和定位载体之间的新距离；

26、根据新距离和三个伪距建立误差方程，并通过最小二乘法估算出各时刻下的第二定位载体位置。

27、可选地，根据量测和待优化变量建立目标函数，包括：

28、将各时刻对应的待优化变量和量测之间的差值作为误差函数；

29、将每个误差函数进行累计，获得目标函数。

30、可选地，通过imu轨迹信息获取在相邻的第一时刻和第二时刻之间的定位载体的第二位置转移关系，并将第二位置转移关系作为图优化模型的待优化变量，包括：

31、获取第k时刻下imu轨迹信息中的定位载体位置和第k+1时刻下imu轨迹信息中的定位载体位置之间的转移关系：

32、

33、其中，xk以三维行向量进行表示。

34、可选地，还包括：

35、获取第k时刻下的一次融合后解算出的载体位置与k+1时刻之间的转移关系：

36、

37、其中，zk以三维行向量进行表示。

38、可选地，目标函数为：

39、

40、其中，误差函数ek＝xk-zk；ω为协方差矩阵的逆矩阵。

41、可选地，当目标函数优化至最小值时，获取对应的误差修正量，对imu数据解算进行修正，包括：

42、pi(x,y,z)＝pi(x,y,z)+δp(δx,δy,δz)

43、其中，pi为解算后的imu数据；δp为修正量。

44、本发明实施例提供了一种基于图优化的组合导航系统融合定位方法，在gnss与imu的第一次数据融合中，通过对gnss卫星伪距到卫星初始时刻位置与载体之间距离的转换，弥补了特殊情况卫星数目过少，约束不足的情况，提高了gnss的精度与系统的鲁棒性，之后再次通过imu信息建立图，使用第一次融合后的数据再次对图进行整体优化，进行第二次数据融合。相对于卡尔曼滤波不适合大型场景的缺点，图优化由于bundle adjustment稀疏，所以较为适用于大型场景，同时图优化方法是一种全局的优化方法，误差更小。

技术特征：

1.一种基于图优化的组合导航系统融合定位方法，其特征在于，包括：

2.根据权利要求1所述的基于图优化的组合导航系统融合定位方法，其特征在于，在对imu轨迹信息进行解算，获取各时刻下的第一定位载体位置之前，还包括：

3.根据权利要求2所述的基于图优化的组合导航系统融合定位方法，其特征在于，所述坐标转换包括：

4.根据权利要求1所述的基于图优化的组合导航系统融合定位方法，其特征在于，对imu轨迹信息进行解算，获取各时刻下的第一定位载体位置，包括：

5.根据权利要求3所述的基于图优化的组合导航系统融合定位方法，其特征在于，通过所述gnss数据获取在相邻的第一时刻和第二时刻之间的所述定位载体的第一位置转移关系，包括：

6.根据权利要求1所述的基于图优化的组合导航系统融合定位方法，其特征在于，根据所述量测和所述待优化变量建立目标函数，包括：

7.根据权利要求5所述的基于图优化的组合导航系统融合定位方法，其特征在于，通过所述imu轨迹信息获取在相邻的所述第一时刻和所述第二时刻之间的所述定位载体的第二位置转移关系，并将所述第二位置转移关系作为所述图优化模型的待优化变量，包括：

8.根据权利要求7所述的基于图优化的组合导航系统融合定位方法，其特征在于，还包括：

9.根据权利要求8所述的基于图优化的组合导航系统融合定位方法，其特征在于，所述目标函数为：

10.根据权利要求1所述的基于图优化的组合导航系统融合定位方法，其特征在于，当所述目标函数优化至最小值时，获取对应的误差修正量，对imu数据解算进行修正，包括：

技术总结
本发明公开了一种基于图优化的组合导航系统融合定位方法，包括：对IMU轨迹信息进行解算，获取各时刻下的第一定位载体位置；将各时刻下的第一定位载体位置作为顶点，建立图优化模型；通过GNSS数据获取在相邻的第一时刻和第二时刻之间的定位载体的第一位置转移关系，并将第一位置转移关系作为图优化模型的量测；通过IMU轨迹信息获取在相邻的第一时刻和第二时刻之间的定位载体的第二位置转移关系，并将第二位置转移关系作为图优化模型的待优化变量；根据量测和待优化变量建立目标函数，并通过莱文贝格－马夸特算法进行优化；当目标函数优化至最小值时，获取对应的误差修正量，对IMU数据解算进行修正。

技术研发人员：邱海洋,赵云,王慧,智鹏飞,朱志宇,葛慧林
受保护的技术使用者：江苏科技大学
技术研发日：
技术公布日：2024/1/12