基于运动检测的飞行区车辆差分北斗/ SINS组合导航

胡 杰 王子卉 朱倚娴

1 中国电子科技集团公司第二十八研究所空中交通管理系统与技术国家重点实验室，南京市永智路1号，210014 2 东南大学微惯性仪表与先进导航技术教育部重点实验室，南京市四牌楼2号，210096 3 南通大学机械工程学院，江苏省南通市啬园路9号，226019

建立飞行区车辆管控系统有利于实现车辆的可视化、数字化、精细化管理[1]，车载定位系统能够实时准确地提供车辆行驶态势，是实现车辆监控管理的前提。在车载定位系统中组合使用捷联惯导系统(SINS)与全球卫星导航系统(GNSS)，可以获得车辆行驶速度、位置以及姿态等信息[2-3]。我国北斗导航系统已完成全球组网，正在推进其在民航领域的应用，差分北斗与SINS融合可为飞行区车辆管控系统提供高精度的位置服务。

当卫星信号有效时，SINS与GNSS融合能够获得车辆的行驶速度、位置等参数；当卫星信号受到遮挡时，组合导航系统精度会降低。刘鹏飞[4]提出一种里程计(DR)辅助的高精度GNSS/INS组合导航方法，并建立了GNSS/INS和DR/INS两种组合导航滤波模型，可根据车辆行驶环境切换滤波模式，具有长时间、高精度导航定位功能，但需要安装辅助导航设备，因此使用范围受限。张梦得等[5]针对GPS短时失效的问题提出一种基于偏最小二乘辅助高斯过程回归的SINS/GPS组合导航方法，但该方法的适用性还需进一步验证。Yao等[6]和张伦东等[7]提出一种基于零速修正技术(ZUPT)的组合导航方法，利用车辆停止或行人驻足时速度为0作为约束条件来估计SINS误差。胡杰等[8]提出利用车辆行驶中侧向和天向速度为0构造滤波器量测约束信息，称为动态零速修正技术(DZUPT)，但当车辆急速转弯时，侧向速度为0的假设条件不成立。周俊等[9]对GNSS/SINS组合导航算法进行研究，但未考虑卫星失效时引起系统精度降低的问题。近年来，部分学者[10-11]提出利用机器学习、深度学习等技术辅助SINS导航，但这些算法实现较为复杂，在使用中存在局限性。

本文针对飞行区车辆卫星信号断续或遮挡时导航系统精度降低的问题，利用ZUPT与DZUPT结合的运动学约束技术，提出一种基于车辆运动检测的差分北斗/SINS组合导航方法。针对车辆行驶特征给出组合导航系统框架，建立组合导航滤波状态方程与量测方程，根据SINS输出信息实现车辆运动状态的检测，并对该方法进行实验验证。

1 基于运动检测的差分北斗/SINS组合导航系统框架

图1为本文提出的飞行区车辆差分北斗/SINS组合导航系统框架。当北斗卫星信号有效时，组合导航滤波器利用差分北斗提供的速度、位置信息作为观测量进行滤波器量测更新，并根据估计得到的误差值反馈校正SINS。当北斗卫星信号受到遮挡时，要利用SINS提供的导航信息，首先需通过车辆运动状态检测模块来判断当前的车辆行驶状态(停车状态、直线行驶或拐弯行驶)；然后根据车辆运动学特性构造相应的运动约束信息(ZUPT模式、DZUPT模式或仅时间更新的DZUPT模式)；最后将构造得到的运动学约束辅助信息与SINS相组合，实现系统的误差估计与校正。

图1 组合导航系统框架Fig.1 Frame of integrated navigation system

2 组合导航滤波器设计

本文导航坐标系选取东-北-天地理坐标系，记为n系，惯性坐标系记为i系，载体坐标系记为b系，其中oxb轴沿车体横轴指向右方，oyb轴沿车体纵轴指向前方，ozb轴垂直于车体向上，即构成一个右-前-上坐标系。

2.1 系统状态方程

X=[δLδλδhδvEδvNδvU

φEφNφUεxεyεzxyz]T

(1)

进一步，由SINS误差方程可得到组合导航滤波状态方程为：

(2)

式中，A为系统状态转移矩阵，可根据SINS误差确定；W为系统噪声向量。

2.2 系统量测方程

2.2.1 北斗卫星信号有效时

当卫星信号有效时，将SINS输出速度、位置与北斗接收机输出速度、位置的差值作为Kalman滤波器测量值，此时滤波量测方程可表示为：

Z1=H1X+V1

(3)

式中，Z1为卫星信号有效时的量测向量；V1为系统量测噪声向量；H1为量测矩阵，可表示为：

(4)

式中，I3×3表示3×3维单位矩阵；03×3、03×9分别表示3×3和3×9维零矩阵。

2.2.2 北斗卫星信号失效时

当卫星信号失效且检测到车辆处于停止状态，即组合导航系统处于ZUPT模式时，利用车辆n系3个方向速度为0的特性构造滤波器量测值：

(5)

式中，Z2为卫星信号失效且车辆停止时的量测向量；vE、vN和vU分别为n系3个方向的速度误差；vE、vN和vU分别为n系3个方向的速度值。

由式(5)可得到卫星信号失效且车辆处于停止状态时的量测方程为：

Z2=H2X+V2

(6)

式中，V2为系统量测噪声向量；H2为量测矩阵，可表示为：

(7)

当卫星信号失效且车辆处于直线行驶，即组合导航系统处于DZUPT模式时，利用车辆运动学特性约束SINS误差。假设车辆无侧滑、跳跃等运动，则b系中侧向和天向速度可近似为0，如图2所示，该方法被称为非完整性约束[12]。

图2 非完整性约束Fig.2 Non-holonomic constraint

(8)

式中，vb、vn分别为b系和n系车辆速度。由式(8)可得b系速度误差表达式为：

(9)

式(9)可进一步表示为：

δvb=Hv,1δvn+Hv,2φn

(10)

其中，

(11)

(12)

利用车辆非完整性约束条件可构造滤波器量测值为：

(13)

由式(10)和式(13)可得量测方程为：

Z3=H3X+V3

(14)

式中，V3为系统量测噪声向量；H3为系统量测矩阵，可表示为：

H3=

(15)

式中，Hv,1(1,)、Hv,2(1,)表示矩阵第1行；Hv,1(3,)、Hv,2(3,)表示矩阵第3行；01×3表示1×3维零矩阵。

3 运动状态检测

根据分析可知，车辆状态可分为直线行驶、拐弯行驶和停止3种。当车辆停止时，可采用传统ZUPT进行误差约束；当车辆直线行驶时，利用车体侧向和天向速度为0进行非完整性约束；当车辆急速拐弯时，会产生较大的向心加速度，车体侧向和天向存在速度分量，不满足DZUPT约束条件，但一般情况下车辆拐弯时间较短，此时Kalman滤波算法需暂停量测更新，即组合导航系统处于仅时间更新的DZUPT模式。

假设k时刻IMU输出的车辆角速度和比力分别为ωk=[ωxkωykωzk]T、fk=[fxkfykfzk]T，车辆运动状态检测的目的是根据IMU输出的信息确定当前车辆行驶状态，具体实现过程如下：

1)确定停止状态阈值。在车辆初始对准阶段，采集3～5 min的IMU原始静态数据计算角速度和加速度检测阈值，表达式为：

(16)

式中，λG和λA分别为车辆停止时陀螺和加速度计的检测阈值；N为数据采集时间长度；ωi和fi(i=1, 2,…,N)分别为时间窗内i时刻IMU输出的陀螺和加速度计值；‖·‖表示2范数。

2)判别车辆是否停止。滑动窗口时间取值为T，根据IMU采样频率可求得T时间段内采样值，假设为M。实时计算滑动窗口内角速度和比力平均值，并与式(16)进行比较，以此来判断车辆是否停止：

(17)

(18)

式中，LG(·)和LA(·)分别为根据陀螺和加速度计输出求得的车辆停止检验值；g为当地重力加速度。判断车辆是否停止可认为是假设检验问题，当式(19)成立时，车辆即可被认为处于停止状态：

(19)

3)判别车辆是否拐弯。由牛顿运动定律可确定车辆向心加速度为：

a(t)=V(t)×ωz(t)

(20)

式中，a(t)为t时刻车辆向心加速度；V(t)为车辆行驶速度；ωz(t)为陀螺输出沿垂向拐弯角速度。

如果|a(t)|≥λa，说明车体急速拐弯，此时将误差约束切换至仅时间更新的DZUPT模式，其中检测阈值λa由车辆实际行驶路况及车辆拐弯幅度确定。

4 车载实验

车载验证实验装置由差分北斗/MEMS SINS组合导航系统、北斗差分基准站、北斗接收天线等组成，SINS中采用的MEMS陀螺仪零偏稳定性(1 s平滑，1σ，室温)优于5°/h，加速度计零偏稳定性(1 s平滑，1σ，室温)优于100 μg。采用高精度Septentrio PolaRx5型GNSS接收机保存的星历和观测数据进行事后RTK差分处理，获得车辆参考行驶路线，数据更新频率为1 Hz，位置精度优于0.05 m。

实验地点为某机场飞行区，利用专用移动通信网络实现北斗差分站与行驶车辆以及行驶车辆与监控席位之间的数字通信。图3为车辆行驶轨迹，图4为该时间段内北斗可见卫星个数及其几何精度因子曲线(GDOP)，图5为差分北斗/SINS组合导航系统位置误差曲线，表1为位置误差统计结果。

图3 车辆行驶轨迹Fig.3 Vehicle track

图4 可见卫星个数及其GDOPFig.4 Numbers of visible satellites and their GDOP

图5 差分北斗/SINS组合导航位置误差Fig.5 Position errors of differential Beidou/SINS integrated navigation

表1 组合导航位置误差数值统计

由图4可以看出，该时间段内可见卫星个数最少为14颗，最多为16颗，满足差分定位需求；GDOP最大值为2.35，卫星几何分布优。由图5及表1可以看出，组合导航系统的最大位置误差小于1 m，表明差分北斗与SINS组合后能够为飞行区车辆管控系统提供亚米级精度位置服务。

为进一步模拟车辆在机场停机坪、廊桥等遮挡环境下的组合导航系统定位精度，利用保存的SINS和差分北斗数据，通过人为切断北斗接收机的卫星信号对本文提出的基于运动检测的DZUPT约束组合导航方法进行验证。行驶里程约为2.5 km，时刻为605～919 s，对应图3中A-B行驶路段。该路段车辆行驶状态包括直线行驶、停车以及拐弯行驶等，传统DZUPT约束与本文提出的基于运动检测的DZUPT约束的位置误差对比曲线如图6所示，表2为2种约束方法的最大位置误差对比统计。

图6 卫星信号失效时组合导航位置误差对比Fig.6 Comparison of position errors of integrated navigation under the satellite signal outage

表2 卫星信号失效时组合导航的最大位置误差对比

由图6及表2可以看出，当中断卫星信号并采用传统DZUPT约束辅助时，组合导航系统的经度误差最大为5.21 m，引起误差的主要原因为拐弯时车辆侧向速度不为0，传统DZUPT的约束条件不成立；当采用本文提出的基于运动检测的DZUPT约束辅助时，通过运动状态检测算法能够实时检测车辆的运动状态，当车辆拐弯时则切换滤波器至仅时间更新的DZUPT模式，可隔离车辆拐弯所引起的量测误差，经度最大误差减小为1.35 m，从而提高了组合导航系统精度。

5 结 语

为进一步提高机场飞行区车辆组合导航精度，本文对北斗导航系统卫星信号中断时SINS位置误差随时间积累问题进行分析和研究。提出一种基于运动检测的DZUPT辅助约束SINS组合导航方法，当卫星信号中断时，利用惯性测量单元输出的陀螺和加速度计信息以及SINS输出的车辆速度来实现运动状态在线检测，从而切换组合导航系统至对应的辅助约束模式。

车载组合导航验证实验结果表明，当北斗导航系统卫星信号有效时，差分北斗与SINS组合定位精度优于1 m；当卫星信号中断时，本文提出的基于运动检测的DZUPT辅助约束方法相比于传统DZUPT约束方法定位精度更高。