一种GNSS/SINS组合导航系统的因子图优化方法

本发明属于卫星导航，尤其涉及一种gnss/sins组合导航系统的因子图优化方法。

背景技术：

1、车载导航系统的定位技术是实现导航和自动驾驶关键技术之一。gnss/sins组合导航系统通过gnss和sins传感器信息融合可有效提高系统的整体性能和环境适应能力，能实现卫星导航信号受干扰、遮挡等各类复杂环境下的稳定可靠定位。在gnss/sins组合导航系统算法方面，因子图能够解决系统中测量值的异步传输、即插即用、多速率数据的融合问题，同时也能解决状态方程或观测方程非线性的问题，具有良好的扩展性和灵活性，是近年研究的热点。但是，随着观测量的不断增加，因子图中产生的变量节点也会逐渐增多，这给实时系统带来巨大的计算和存储负担，对处理器的处理能力和存储能力都是重大挑战。

技术实现思路

1、针对上述技术问题，本发明提出了一种gnss/sins组合导航系统的因子图优化方案。

2、本发明第一方面提出一种gnss/sins组合导航系统的因子图优化方法。所述gnss/sins组合导航系统为由全球卫星导航系统gnss和捷联惯性导航系统sins组合而成的导航系统，所述捷联惯性导航系统sins包括测量惯性单元imu；所述方法包括：

3、步骤s1、利用gnss接收机的基带信号处理器中输入码延迟锁定环路dll的相干积分值来构造信号误差鉴别函数；

4、步骤s2、利用所述imu输出的检测统计量构造零速检测统计量，并利用所述零速检测统计量确定载体处于静止状态；

5、步骤s3、构造所述gnss/sins组合导航系统的因子图，所述因子图包括过程模型因子节点和测量模型因子节点；

6、其中，所述过程模型因子节点为sins节点，所述测量模型因子节点为gnss节点；

7、步骤s4、基于所述gnss/sins组合导航系统的因子图构造零速修正因子节点，并计算载体的状态量；

8、步骤s5、对所述因子图进行计算，以获得所述载体的三维位置、三维速度、三维姿态角、载体本地时钟误差和时钟误差飘移率；

9、其中，所述载体为车辆。

10、根据本发明第一方面的方法，在所述步骤s1中：

11、所述信号误差鉴别函数为：

12、

13、其中，r(ε)为输入所述码延迟锁定环路dll的相干积分值，ε为伪码估计误差，im(x)表示对x取虚部操作，re(x)表示对x取实部操作；并且：

14、

15、其中，τc为伪码的一个码片宽度，ae和al分别为所述码延迟锁定环路dll中的超前支路斜率和滞后支路斜率，并且：

16、

17、

18、其中，d为相关器间隔，取0.25个码片宽度；

19、将n颗可观测卫星对应的信号误差鉴别函数进行归一化，对于第i颗可观测卫星，其归一化的信号误差鉴别函数值为wi，建立权值矩阵w，且

20、根据本发明第一方面的方法，在所述步骤s2中：

21、将所述imu输出的加速度测量值和角速度测量值作为检测统计量，以构造广义似然比公式，从而得到所述零速检测统计量：

22、

23、其中，n为检测数据的个数，g为地球重力常量，zn为第n次检测的检测统计量集合；

24、其中，所述检测统计量为所述imu在各个测量时刻的测量值，包括第k个测量时刻载体三轴加速度矢量和第k个测量时刻载体三轴角速度矢量表示所述imu的加速度测量方差，表示所述imu的角速度测量方差；

25、其中，ωn为检测窗口内的测量数据集合，为检测窗口长度内的加速度平均值，且表示的二范数运算，且

26、其中，当所t(zn)小于设定门限γ时，判定所述载体处于静止状态。

27、根据本发明第一方面的方法，在所述步骤s3中，计算所述过程模型因子节点具体包括：

28、在测量时刻i，所述imu在所述载体的坐标系的输出测量值为加速度和角速度所述载体的坐标系为b系，所述imu的测量值为

29、为所述imu在b系三个坐标轴上的加速度计测量偏差，εbi为所述imu在b系三个坐标轴上的陀螺仪测量偏差，所述imu输出所述测量值的时间间隔为δt；

30、所述sins节点的更新周期为时间间隔t，且t＝m·δt，m≥1，所述时间间隔t的大小与所述gnss的接收机输出的测量值的时间间隔一致，在所述时间间隔t内对所述imu的测量值在b系下的速度、姿态角变化量分别进行预积分，得到所述imu的测量值在b系下的速度、姿态角在所述时间间隔t内的变化量和

31、在测量时刻i+1，多源融合导航系统的速度估计值为位置估计值为姿态角估计值为加速度计测量偏差估计值为陀螺仪测量偏差估计值为并且：

32、

33、

34、

35、

36、

37、其中,分别为组合导航系统在地球地固坐标系下测量时刻i时的位置、速度、姿态角，所述地球地固坐标系为e系；表示从测量时刻i到测量时刻i+1由b系变换到e系的旋转矩阵，ge表示重力矢量，在所述时间间隔t内为常量，测量时刻i到测量时刻i+1的时间间隔为t；

38、所述sins节点的状态量为所述r15为包括15维状态变量的空间，则测量时刻i+1的sins节点的状态量的估计值为所述sins节点的状态方程为：

39、

40、根据本发明第一方面的方法，在所述步骤s3中，计算所述测量模型因子节点具体包括：

41、在测量时刻i，所述gnss的接收机的测量值为ρi＝(ρi,1,ρi,2,…,ρi,j…,ρi,j)t，ρi,j为接收机在测量时刻i对第j颗可观测卫星的伪距测量值，为ρi,j对应的伪距率，j为可观测卫星的最大数量；

42、测量时刻i的第j颗卫星在e系下的位置为对应的速度为则测量时刻i所有可观测卫星的位置矢量和速度矢量分别为和

43、所述gnss的接收机在测量时刻i的状态量为r8为包括8维状态变量的空间，δi为所述gnss的接收机的本地时间和所述gnss的系统时间的差值，为δi的变化率；

44、则所述gnss的接收机的测量方程为：

45、

46、其中，c表示光速，表示所述gnss的接收机与可观测卫星在视线方向上的距离大小，表示所述gnss的接收机与可观测卫星在视线方向上的速度大小，所述gnss的接收机的测量方程的计算结果为伪距与伪距率。

47、根据本发明第一方面的方法，在所述步骤s4中：

48、在所述载体处于零速状态的情况下，所述载体的车体坐标系m系下三个方向的速度为零，vm＝0，所述sins在测量时刻i由所述imu的测量值实际计算得到的载体速度不为零，b系及e系下的速度矢量存在如下关系：

49、

50、其中，所述载体在测量时刻i的e系下的速度由测量时刻i-1的速度计算得到；

51、所述载体在导航坐标系下的姿态矢量为该导航坐标系为n系，αi、βi、γi分别为测量时刻i所述载体的航向角、横滚角、俯仰角，n系姿态矢量到e系姿态矢量的转换关系为：

52、

53、其中，表示所述载体在测量时刻i由e系变换到n系的旋转矩阵，在时间间隔δt内航向角的变化量δαi与姿态矢量各元素的关系为：

54、

55、其中，为所述载体在惯性系的角速度在δt内的变化量在b系下的投影，当载体静止时，在航向角上的变化量为零，并约束所述imu输出的角速度零偏值；

56、载体静止时所述sins节点的状态方程为：

57、

58、其中，为对应的的测量值，hzupt_sins(x)表示对x进行计算的非线性函数；

59、在测量时刻i观测质量最好的卫星的伪距率测量方程为：

60、

61、其中，为接收机与卫星之间的几何距离变化率，卫星的钟差变化率由星历计算得到，ξi为测量残差，为包括对流层电离层在内的传播误差随时间的变化率，接收机与卫星之间的几何距离变化率为卫星的速度在卫星与接收机径向方向的大小，则所述载体静止时所述gnss节点对应的测量方程为：

62、

63、其中，为对应的的测量值；

64、所述载体处于静止状态的情况下，所述因子图中不增加新的因子节点，对应矩阵的大小维持不变。

65、根据本发明第一方面的方法，在所述步骤s5中：

66、在测量时刻i+1，所述因子图的状态量xi+1的最大后验概率值xi+1map的表达式为：

67、

68、

69、其中，表示求矩阵x的马氏距离的平方，σ为x对应的协方差矩阵；

70、所述载体处于静止状态的情况下，利用所述零速修正因子节点计算状态量的最大后验概率值xi+1map：

71、

72、从而得到所述载体的三维位置、三维速度、三维姿态角、载体本地时钟误差和时钟误差飘移率。

73、本发明第二方面提出一种gnss/sins组合导航系统的因子图优化装置。所述gnss/sins组合导航系统为由全球卫星导航系统gnss和捷联惯性导航系统sins组合而成的导航系统，所述捷联惯性导航系统sins包括测量惯性单元imu；所述装置包括：

74、第一处理单元，被配置为：利用gnss接收机的基带信号处理器中输入码延迟锁定环路dll的相干积分值来构造信号误差鉴别函数；

75、第二处理单元，被配置为：利用所述imu输出的检测统计量构造零速检测统计量，并利用所述零速检测统计量确定载体处于静止状态；

76、第三处理单元，被配置为：构造所述gnss/sins组合导航系统的因子图，所述因子图包括过程模型因子节点和测量模型因子节点；

77、其中，所述过程模型因子节点为sins节点，所述测量模型因子节点为gnss节点；

78、第四处理单元，被配置为：基于所述gnss/sins组合导航系统的因子图构造零速修正因子节点，并计算载体的状态量；

79、第五处理单元，被配置为：对所述因子图进行计算，以获得所述载体的三维位置、三维速度、三维姿态角、载体本地时钟误差和时钟误差飘移率；

80、其中，所述载体为车辆。

81、本发明第三方面公开了一种电子设备。电子设备包括存储器和处理器，存储器存储有计算机程序，处理器执行计算机程序时，实现本公开第一方面的一种gnss/sins组合导航系统的因子图优化方法。

82、本发明第四方面公开了一种计算机可读存储介质。计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时，实现本公开第一方面的一种gnss/sins组合导航系统的因子图优化方法。

83、可见，本发明在扩展的增量平滑与建图方法(incremental smoothing andmapping 2,isam2)isam2的基础上，结合gnss和sins构成组合导航系统，将因子图算法进行优化。其中，因子图的概念于2001年由kschischang等人提出，它是概率图模型的一种表示方式，用来表示全局函数和局部函数关系的模型图，被广泛应用于人工智能、信号处理、数字通信等领域，能对各种复杂概率问题进行求解。本发明采用因子图建模，其主要目的是将复杂的概率问题进行因式化分解，即将复杂的全局函数分解为多个局部函数相乘的形式，当已知局部函数之间的相关性时，只需要分析每个局部函数就可以得到全局复杂概率问题的解。因子图中含有因子节点与变量节点，两种节点通过局部函数相连构成双向图。因子图通过各传感器的观测量建立当前系统状态和前一状态的联系，并构造代价函数，通过求解系统状态量后验概率的全局或者局部最大值，获得当前状态最优估计值。因子图能够解决多源融合系统中测量值的异步传输、即插即用、多速率数据的融合问题，同时也能解决状态方程或观测方程非线性的问题，具有良好的扩展性和灵活性。