基于多星座卫星定位与惯性导航全状态融合的列车组合导航系统

王 剑，凌 浩，姜 维，蔡伯根

(1.北京交通大学 电子信息工程学院,北京 100044；2.北京市轨道交通电磁兼容与卫星导航工程技术研究中心，北京 100044)

随着我国铁路运输的发展，基于全球导航卫星系统(Global Navigation Satellite System, GNSS)和惯性导航系统(Inertial Navigation System, INS)构成的组合导航系统已经成为列车定位的方式之一。然而目前组合导航系统主要应用于列车的测速定位，如何继续改善和提高其性能，并且应用于更多铁路方向成为组合导航系统的主要研究方向。

GNSS/INS组合导航系统多是利用GNSS输出的位置对INS的误差进行校正，但INS位置、速度的解算都需要利用姿态，并且姿态的变化可以反映列车振动的大小和线路的变化。因此针对姿态的长期高精度测量，已经成为国内外学者研究的重点。王冰等[1]对基于基线和模糊度参数的单基线，以及基于姿态和模糊度参数的多基线两种模型进行了实验分析。姜维等[2-3]提出基于仰角约束的最优分布参考卫星选择方法，以提高双差整周模糊度的固定效率，从而实现双天线相对位置的解算。Zhang等[4]提出通过安装多个天线，使用自适应Kalman滤波对欧拉角和角速度进行估计，并通过静态实验和仿真实验进行了验证。梅玲玉等[5]则提出INS/GPS/磁力计三组合，利用磁力计实现航向角测量，利用惯性测量单元中加速度计测得的横向、垂向加速度实现横滚角和俯仰角的测量，进而实现位置、速度、姿态的全状态组合。综上所述，目前针对除惯性导航外的姿态测量方法主要集中在多天线测姿、加速度计和磁力计上，对GNSS/INS全状态组合在列车上的应用研究相对较少。而我国铁路沿线环境复杂，列车运行时间较长，单GPS难以满足使用要求。随着北斗导航系统(BeiDou Navigation Satellite System, BDS)中北斗三代的成功布置，BDS的研究与应用也越来越多。为此本文提出采用BDS和GPS多星座相结合的方式实现双天线测姿，将GPS/BDS双天线测姿系统的长期稳定性和惯性导航短期高精度的姿态测量相结合，从而实现长时间、高精度的姿态输出，进而提高定位测速效果。

1 双天线测姿

1.1 测姿原理

载体的三维姿态是指载体相对当地导航坐标系的3个角。利用卫星导航进行载体姿态测量，是获取多个天线相对主天线在当地导航坐标系下的坐标和在载体坐标系下的坐标，然后利用这两坐标系之间的旋转关系进行姿态的求解。

坐标旋转转换示意见图1，当地导航坐标系(Xn,Yn,Zn)依次绕着Z、X、Y轴进行旋转可以得到载体坐标系(Xb,Yb,Zb)[6]，旋转的3个角度依次为航向角ψ、俯仰角θ和横滚角φ。

图1 坐标旋转转换示意

载体坐标系与当地导航坐标系的原点相同，两坐标系之间存在转换关系为

(1)

(2)

当放置一根基线时，可以根据坐标旋转关系计算出2个姿态角，基线放置方向为横向，垂直于列车运行方向，此时可以计算出航向角ψ和横滚角φ分别为

(3)

(4)

式中：Len为基线长度。

1.2 双差载波相位

在使用GPS/BDS测姿时，一根辅助天线和一根主天线构成一条基线，通过计算基线在载体坐标系和当地导航坐标系下的坐标，再利用坐标转换关系即可求解姿态角。其中，载体坐标系下的坐标可以在安装时通过测量获得，当地导航坐标系下的位置坐标则需要利用双差载波相位进行计算。双差载波相位原理见图2。与伪距相比，载波相位具有更高的精度，而站间差分和星间差分可以消除大部分的误差，故其测量值主要为卫星钟差、电离层和对流层的传播误差，以及接收机钟差。

图2 双差载波相位原理

图2中，点o为主天线M的位置，认为是载体坐标系和当地导航坐标系的原点；点O(X，Y，Z)为辅助天线S的位置。根据载波相位的传播路径，主辅两天线的载波相位观测方程为

(5)

(6)

式中：φ、λ、f分别为载波相位的测量值、波长、频率；ρM、ρS分别为卫星与主天线、辅助天线之间距离的真实值；I为电离层误差；T为对流层误差；tR、ts分别为接收机、卫星的钟差；N为整周模糊度；ε为其他误差；i为接受卫星的编号。

(7)

(8)

(9)

式中：上标i为关于卫星i的站间差分；上标j为关于卫星j的站间差分；上标i_j为卫星i与卫星j之间差；下标M_S为主天线与辅助天线之间差。

(10)

(11)

将式(11)在原点处展开为泰勒级数

(12)

式中：(Xi,Yi,Zi)、(X,Y,Z)分别为卫星i、辅助天线在当地导航坐标系下的位置；ρM为卫星到主天线的距离；(Δx,Δy,Δz)为泰勒展开的偏差。由于卫星到主天线的距离远大于基线长度，可以认为辅助天线的位置即为主天线位置泰勒展开的大约位置[7]，因此(Δx,Δy,Δz)也就是基线坐标。

对于卫星j同样进行泰勒展开

(13)

式中：(Xj,Yj,Zj)为卫星j在当地导航坐标系下的位置。

将系数参数记为

(14)

则式(10)可以简化为

(15)

式中：l(i_j)为系数l(j)与l(j)之差；m(i_j)、d(i_j)含义同l(i_j)。将式(15)代入式(9)，忽略其他误差可得

(16)

式(16)中只有基线坐标和双差整周模糊度未知，当主辅天线收到n颗共同卫星时，双差时GPS和BDS需要各选取一颗主星，所以存在n-2个双差模糊度，其基线坐标和模糊度可以通过Kalman滤波计算。

1.3 多星座融合

相较于单星座系统，多星座系统可以有效增加可视卫星数和改善定位精度因子，提高系统的可用性、稳定性和精度。而要想实现不同导航系统的联合解算，需要将其统一到相同的时空参考框架下，其中空间参考框架的影响较小，本文不进行考虑[8]。对于GPS时间系统(GPS Time System, GPST)的时间TGPS与BDS时间系统(BDS Time System, BDST)的时间TBDS，由于闰秒的原因[9]，两者之间存在一个时间差，其转换关系为

TBDS=TGPS-14

(17)

BDS相较于GPS增加了GEO、IGSO轨道的卫星，IGSO卫星的位置计算与MEO卫星相同，而GEO卫星位置的计算需要进行转换。卫星位置计算流程见图3，具体算法见文献[9-10]。

图3 卫星位置计算流程

1.4 基线坐标解算

基线的计算需要利用Kalman滤波进行，其系统模型为

Xk=FXk-1+Qk

(18)

(19)

(20)

(21)

系统的量测模型为

(22)

(23)

Hk=[l(n-2)×1m(n-2)×1d(n-2)×10(n-2)×3

-λ·I(n-2)×(n-2)]

(24)

为GPS与BDS的双差载波相位，共n-2个。

1.5 模糊度固定

双差载波相位模糊度具有整数特性，而利用Kalman滤波计算的模糊度为浮点解，通过固定整周模糊度可以实现厘米级甚至毫米级的精度，并且由于天线基线长度不变，可以利用基线长度约束提高固定效率和成功率。模糊度固定流程见图4。

图4 模糊度固定流程

模糊度固定首先是人为地以Kalman滤波求解的浮点解和误差协方差矩阵为中心构建一个置信搜索空间，将所有可能的模糊度整数组合包含在内。但模糊度之间存在相关性，搜索空间的最长轴与最短轴相差较大，呈椭圆体，模糊度方差矩阵为非对角阵，因此需要进行去相关处理，将其方差矩阵变为对角阵。处理后搜索空间转为接近球体，这样使得搜索空间减小，提高搜索效率。随后在变换后的搜索空间中进行搜索，并依据一定的约束条件进行校验和回代。

首先定义包含所有可能的整周模糊度组合的整数搜索空间为

(25)

(26)

此时变换后整周模糊度的搜索空间可以表示为

(27)

变换矩阵Z的求解需要对协方差矩阵进行Cholesky分解[11]

(28)

(29)

式中：L1为下三角矩阵；D1为对角阵，其对角线上的元素为di；Q1为变换矩阵，具有对称性和正定性。

(30)

通过矩阵Z进行变换后，置信空间由椭圆体变为近似球体，可以将式(27)展开为[12]

(31)

式(31)就是判断模糊度是否符合要求的不等式，而模糊度搜索是依次对每个模糊度单独搜索，从第n个开始，建立搜索边界，对第n-1个模糊度进行搜索，依次直到第一个模糊度完成搜索。对式(31)进行数学变换确立搜索边界为

(32)

(33)

由于天线基线长度不变，当确定好存在多个整周模糊度集合时，可以将模糊度集合带入式(16)进行验证，并与实际基线长度进行比较，获取最优解。

2 GPS/BDS/INS融合

2.1 惯性导航递推算法

惯性导航系统包含惯性测量部分和导航处理器推算导航系统。惯性测量部分包含三轴加速度计和三轴陀螺仪，可以提供运动物体的运行加速度和角速度。推算导航系统包含姿态、速度、位置更新，可以计算出运动物体的姿态、速度和位置。

(1)在捷联惯性导航系统中，常用四元数来更新姿态矩阵，其微分方程为

(34)

(35)

(36)

(37)

(2)惯性导航系统速度更新的微分方程为

(38)

式中：fn为载体上加速度计测量的比力；gn为当地重力加速度；V为速度。

(3)惯性导航系统的纬度L、经度B、高程H的微分方程为

(39)

(40)

(41)

式中：Vu为天向的速度。

2.2 全状态松组合模型

通过接收机可以直接读取到位置的一维速度，利用双天线测姿结果可以将速度进行分解，从而实现位置、速度、姿态的全状态组合。GPS/BDS/INS全状态组合结构见图5。

图5 GPS/BDS/INS全状态组合结构

将GPS/BDS和INS的计算结果传给松组合Kalman滤波，通过对传感器的系统误差和噪声源进行建模，实现对INS的误差估计，从而减小甚至消除INS的漂移误差[13]。

(42)

对应的系统转移矩阵则是根据捷联惯性导航的误差方程进行设置，位置、姿态、速度误差方程依次为

(43)

(44)

(45)

(46)

(47)

系统的观测向量为

(48)

式中：PI、VI、ψI、φI分别为INS输出的位置、速度、航向角、横滚角；PG、VG、ψG、φG分别为双天线测姿系统输出的位置、速度、航向角、横滚角。

量测矩阵为

(49)

由于惯性导航元器件的误差，惯性系统从载体坐标系到当地导航坐标系之间的转换矩阵在计算时会产生一个平台失准角的误差，状态方程中姿态部分的设置与平台失准角有关[14]，所以量测矩阵中HA为

(50)

全状态组合系统的噪声矩阵QAk为

(51)

式中：bf为加速度计零偏；bω为陀螺仪零偏。

3 实验验证与结果分析

为了验证本文算法的有效性，选用在2020年12月6日朔黄线的一段10 min左右的数据进行实验。其间列车运行状况正常，位于定州东站—定州西站之间，场景开阔，卫星信号良好。

在实验车车头前部横向放置一对天线，两天线相距2.8 m。实验设备包括1台SPAN INS(NovAtel OEM6 GNSS接收机和IMAR-FSAS IMU)、1台UR380接收机和STIM300 INS。其中SPAN是商用高精度GPS/INS组合导航系统，在开阔环境下可以作为结果参考；NovAtel OEM6 GNSS接收机和UR380接收机分别记录两根天线的数据；MEMS惯性导航STIM300是一款较低精度的惯性测量单元，采集频率为125 Hz，用于GPS/BDS/INS全状态组合算法验证。

实验过程中公共可见卫星数见图6。从图6可见，大部分时刻GPS卫星存在5颗，但是其数目变化频繁，在此基础上又选取了5颗高度角大的北斗卫星，以增强双天线测姿系统的稳定性和可用性。

图6 可见卫星数

(1)双天线测姿结果见图7。从图7中可以看出，在卫星数发生变化时，单GPS测姿结果波动较大，横滚角尤为明显；当卫星数过少时，如在600 s之后卫星颗数快速降低，单GPS测姿结果离参考值较大，而采用GPS/BDS联合解算可以有效降低卫星颗数过少带来的误差增大；在卫星数目稳定时，当姿态发生较大变化，如在300 s附近，双天线测姿可以有效跟踪姿态的变化。

图7 双天线测姿结果

双天线测姿误差分析见表1。从表1中可以看出，相较于单GPS，GPS/BDS联合测姿的误差的标准差STD和均方根误差RMS均有较大改善，BDS系统的加入可以有效提高多天线测姿系统的稳定性和精度。

表1 双天线测姿误差分析 (°)

(2)组合测姿结果见图8。从图8(a)～图8(d)可以看出，单位置组合的姿态结果会不断偏离参考值，误差不断增大，而全状态组合的姿态结果相对稳定。由于单位置组合的姿态主要依靠于惯性导航的递推，而全状态组合则利用双天线测姿的结果对惯性导航递推的姿态误差进行估计与修正，从而有效抑制了INS的姿态漂移，航向角、横滚角的RMS也分别从0.310 0°、0.590 1°降至0.111 8°、0.080 0°。在300 s附近列车姿态有较为明显的改变，此时列车正在经过道岔，因此姿态有较大变化，两种组合方式都有效跟踪了姿态的变化趋势，其中全状态组合的效果更为明显，但由于列车在经过道岔时振动较大，导致其组合解算的结果出现了一定的突变。

从图8(e)～图8(j)可以明显看出，全状态组合的位置、速度相较于单位置组合更加平滑，具有更高的稳定性，其中测速精度的改善较为明显。

图8 组合测姿结果

组合测姿的位置误差分析见表2，速度误差分析见表3。从表2、表3中可以看出，全状态组合的位置误差的标准差有明显下降，均方根误差有一定的改善；速度测量有明显提升，其中北向和天向提升较为明显。

表2 位置误差分析 m

表3 速度误差分析 m/s

4 结论

(1)基于GPS/BDS/INS的全状态组合导航系统，可以有效实现INS的姿态校正，从而提高组合导航的整体精度。

(2)为了克服双接收机接收到的GPS共同卫星数目波动频繁的问题，在系统中加入BDS卫星，可以有效提高双天线测姿的稳定性和精度，并在传统的松组合基础上加入速度和姿态实现全状态融合，可以有效抑制INS的发散，使航向角和横滚角的均方根误差分别为0.305 9和0.132 4，有效提高了组合导航系统的精度和稳定性。