[发明专利]一种基于图优化的组合导航系统融合定位方法

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

通过所述坐标转换矩阵将所述卫星位置从ECEF坐标系转换到东北天坐标系。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

其中，x_k以三维行向量进行表示。