一种车载GNSS/INS组合导航零速检测及增广零速约束方法

本发明涉及组合导航定位，尤其是一种车载gnss/ins组合导航零速检测及增广零速约束方法。

背景技术：

1、近年来随着自动驾驶、智能无人车等领域的快速发展，对车载导航定位技术的要求也日益提高。当前基于惯性导航系统和全球卫星导航系统的组合导航系统仍是实现车载导航的主要技术手段。然而，在城市环境下卫星信号易受干扰和遮挡，惯性导航系统存在长时间误差累积，使得gnss/ins组合导航定位结果的稳定性和可靠性通常并不尽如人意。目前提高导航精度的技术手段之一是基于运动约束辅助的组合导航方法，该方法可以在不增加外部传感器设备的条件下，有效解决imu漂移和gnss中断后定位发散的问题。运动约束是根据车辆的动力学特性和运动状态，通过挖掘隐含的约束条件，从而为组合导航系统提供虚拟的外部观测校正。零速修正是一种典型的速度约束，利用车辆停止时的速度为零作为约束条件对速度误差进行修正。

2、零速修正包括零速检测和零速约束两个部分。传统的零速检测方法抗干扰能力较差，而基于机器学习的零速方法又需要大量的数据训练，目前需要一种适合工程应用的综合检测方法。同时常规的零速约束仅利用零速作为运动约束条件，虽然能够有效提升速度精度，但是对姿态精度的提升效果不明显，无法解决航向发散的问题，因此还需要提出一种能够全面提高导航精度的零速约束方法。

技术实现思路

1、本发明所要解决的技术问题在于，提供一种车载gnss/ins组合导航零速检测及增广零速约束方法，能够解决传统车载紧组合导航航向发散和定位不准的问题。

2、为解决上述技术问题，本发明提供一种车载gnss/ins组合导航零速检测及增广零速约束方法，包括如下两个部分：

3、(一)零速检测方法通过综合陀螺仪模值检测统计量和组合导航速度检测统计量实现车辆零速状态的检测；

4、(二)增广零速约束方法根据车辆在零速状态下的速度为零、位置和姿态、加速度和陀螺仪零偏不发生变化的隐含约束条件，将零速状态下线速度和角速度为零以及前后历元时刻姿态变化为零作为组合导航的观测约束条件对车辆运动状态进行约束，从而提高车辆静止时速度和姿态解算精度，实现定位性能的整体提升。

5、优选的，第一部分零速检测方法中，零速判定的表达式为：

6、

7、式中，t(ωk)为k历元时刻陀螺仪模值检验统计量，ωi为i时刻n表示时间窗内采样个数，||·||表示2范数。t(vhor)为水平速度检验统计量，vhor为载体当前时刻水平速度，为车辆处于零速状态时一段时间内的水平速度均值。

8、优选的，陀螺仪检验统计量阈值的计算公式为：

9、

10、式中，λg为角速度模值的检验阈值，k为膨胀系数，m表示车辆启动前内采样个数，||·||表示2范数，通常取车辆启动前取2～5分钟的陀螺仪原始数据的统计量均值的2-5倍作为检验阈值，即k取2～5。

11、优选的，水平速度检验统计量的计算公式为：

12、λv＝kσv                            (3)

13、式中，σv为标准偏差，可取车辆启动前取2～5分钟的导航结果计算水平速度标准偏差，通常取水平速度标准偏差的0.5倍作为检验阈值。

14、优选的，第一部分中，确认阈值后需要根据综合零速检测方法实时输出车辆运动状态，判定准则为：

15、准则1：如果在一个gnss周期内的陀螺仪检验统计量小于陀螺仪检测阈值，则判定车辆在该周期内处于零速状态；

16、准则2：如果在一个gnss周期内的水平速度检验统计量小于水平速度检测阈值，则判定车辆该周期内处于零速状态。

17、优选的，第二部分增广零速约束中，根据零速检测结果采用增广零速约束具体为：

18、基于零速检测方法的检测结果，对检测为零速的状态添加增广零速约束，运用零速度约束、零角速度约束和姿态约束修正静止状态下的车辆导航结果。

19、优选的，增广零速约束是将可以采纳的运动约束量作为额外的观测量添加到紧组合导航的量测方程中，紧组合的状态方程和量测方程如下：

20、选取位置误差、速度误差、姿态误差、陀螺仪和加速度计零偏以及接收机钟差和钟漂构造紧组合导航17维状态向量。

21、

22、其中，n表示导航坐标系(采用“东北天”)，b表示载体坐标系(采用“右前上”)，δrn＝[δe δn δu]分别表示东向、北向、天向误差，δvn＝[δve δvn δvu]分别表示东向、北向和天向速度误差，φ＝[φe φn φu]分别表示俯仰角、横滚角和航向角误差，表示陀螺仪在x，y，z方向的零偏，表示加速度计在x，y，z方向的零偏，bclk和dclk分别表示钟差和钟漂。

23、紧组合导航系统的状态方程可表示为：

24、

25、式中，f(t)为状态转移矩阵，g(t)为系统噪声矩阵，w(t)为系统噪声向量。

26、状态转移矩阵f(t)的表达式为：

27、

28、其中，

29、

30、

31、

32、

33、

34、其中，rm和rn分别为子午圈及卯酉圈曲率半径，ωie为地速，l为当地纬度，vn，ve为北向和东向速度，h为高程，fn＝[fe，fn，fu]分别为东北天方向的比力。符号[×]表示反对称矩阵，为b系到n系的方向余弦矩阵。

35、系统噪声矩阵g(t)和系统噪声向量w(t)的表达式为：

36、

37、

38、对于紧组合导航，采用gnss测量得到的伪距和伪距率与ins推算的伪距和伪距率与之差作为观测量，其量测方程为：

39、z(t)＝h(t)x(t)+v(t)            (14)

40、式中，z(t)为观测向量，h(t)为观测矩阵，v(t)为测量噪声向量。

41、观测向量其中δρ(t)＝[δρ1，...δρn]t为伪距观测量，为伪距率观测量，伪距和伪距率的表达式为

42、δρi＝e1iδx+e2iδy+e3iδz+bclk-cdts+t+i+wρi           (15)

43、

44、式中，(xi，yi，zi)为gnss第i颗卫星在地心地固定坐标系中的位置，(x，y，z)为载体在地心地固定坐标系中的位置，c为光速，dtr和dts分别为接收机和卫星钟差，t为对流层延迟，i为电离层延迟。(δx，δy，δz)和分别为载体的位置偏差和变化率，它们与状态参数δrn和δvn的关系如下：

45、

46、观测矩阵h(t)的表达式为

47、

48、

49、式中，为n系到地心地固坐标系转换矩阵。

50、在以上伪距、伪距率量测方程的基础上考虑车辆静止状态。车辆处于静止时，车辆的速度应当为0，三轴陀螺仪的输出应当为0且车辆的水平姿态角变化应当为0(即俯仰角和横滚角保持不变)，将三种运动约束作为额外观测量添加到原伪距、伪距率量测方程中，则约束观测模型分别为：

51、

52、

53、

54、式中表示ins系统输出的三维速度矢量，εv表示速度量测白噪声；表示三轴陀螺仪上个导航时间输出的三维角速度矢量，εg表示陀螺仪量测白噪声；为当前k时刻的俯仰角和横滚角，为前一时刻的俯仰角和横滚角，为俯仰角陀螺仪量测白噪声，为横滚角陀螺仪量测白噪声。

55、根据以上状态方程和量测方程进行卡尔曼滤波：

56、首先将上述状态方程和量测方程离散化，可得

57、xk＝φk|k-1xk-1+γk-1wk-1                                   (23)

58、zk＝hkxk+vk                                       (24)

59、式中，xk为k时刻的n维状态向量，也是被估计向量；zk为k时刻的m维量测向量；φk|k-1为k-1到k时刻的系统一步转移矩阵，wk-1为k-1时刻的系统噪声；γk-1为系统噪声矩阵，hk为k时刻的量测矩阵；vk为k时刻的m维量测噪声。卡尔曼滤波要求{wk)和{vk}是互不相关的零均值白噪声序列，即

60、

61、式中，qk为系统噪声的对称非负正定矩阵，rk为量测噪声的对称正定矩阵，δkj为克罗内克函数，满足以下关系：

62、

63、状态一步预测方程：

64、

65、状态估值方程：

66、

67、滤波增益方程：

68、

69、一步预测均方方程：

70、

71、估计均方误差方程：pk＝(i-kkhk)pk/k-1pk＝(i-kkhk)pk/k-1          (31)

72、此外，在使用卡尔曼滤波器前应对其进行初始化，预先设置好初始值和p0|0，其中初始状态估计均方误差p0|0一般设置为对角矩阵，计算公式下：

73、

74、根据以上时间修正方程和量测修正方程可以得到估值将估值反馈到惯性导航系统内，对误差状态进行修正，从而得到精确的导航结果。

75、本发明的有益效果为：本发明综合考虑了传统零速检测方法的优劣，融合水平速度检验统计量和三轴陀螺仪幅值检验统计量进行零速检测，即使在复杂多变的环境条件下，如强干扰、断续运动等情况下也可以保持较高的检测率，为进一步的零速约束提供了良好的基础。同时，多种运动约束下的增广零速约束能够显著提高导航精度，优化系统性能。本发明形成的增广零速约束辅助的gnss/ins紧组合导航算法能够提高车载导航系统的鲁棒性和精确性，为各种应用场景提供了可靠的导航解决方案。