基于SINS/BDS的抗差组合导航技术研究

温哲君 陈安升 陈帅 薄煜明

1.南京理工大学，南京210000 2.北京自动化控制设备研究所，北京100854

无人车在城市峡谷环境下，BDS信号因高楼遮挡往往出现断续、延迟及干扰等异常状况，惯组也容易因震荡而造成野值输出，进而导致导航结果不准确。

捷联惯性导航系统(SINS)完全自主、刷新率快、短时精度高，可实现全天候的自主三维定位测速，但是误差会随时间累计增大[1]。中国北斗卫星导航系统(BDS)能够在亚太地区提供实时的速度位置和时间信息(PVT)，且精度不随时间变化[2]。由于BDS和SINS具有很强的互补性，采用SINS/BDS的组合导航模式对SINS误差进行实时校正，可以获得较于单个子系统更好的导航性能[3]。

但是SINS/BDS导航系统在无人车的实际应用中也存在着一些问题，例如卫星信号干扰情况下系统的抗干扰性、故障检测能力差，噪声突变导致导航精度下降、鲁棒性变弱等。本文以此为出发点，提出一种基于SINS/BDS在城市峡谷等复杂干扰环境下可应用于无人车的自适应组合导航抗差算法，可有效提高系统的鲁棒性、抗干扰能力及导航精度。

1 基于新息的自适应卡尔曼滤波

采用速度/位置的松组合卡尔曼滤波方式，选取东北天坐标系作为导航坐标系，将IMU和BDS数据进行数据融合。此外，在常规的卡尔曼滤波基础上引入基于新息的自适应卡尔曼滤波算法[4]，通过调整噪声R的统计协方差矩阵的大小以实现对新息序列方差的在线调节，并据此调节滤波增益矩阵的计算，大大减小异常噪声对滤波的污染，有效控制滤波的发散，从而增强系统的鲁棒性和稳定性。

1.1 卡尔曼滤波(KF)

常规的卡尔曼滤波器对SINS误差模型进行建模，采用15维的状态变量，具体如下：

(1)

根据SINS误差传播模型和惯性器件误差传播方程可得到系统的状态方程为：

(2)

其中状态转移矩阵F(t)为：

(3)

FN(t)为惯导模型的误差矩阵，矩阵各项具体见参考文献[5]；FS(t)、FM(t)为惯性器件的误差转换矩阵，具体如下：

(4)

FM(t)=06×6

(5)

系统噪声驱动矩阵G(t)和系统噪声矩阵W(t)分别为：

(6)

(7)

对进行系统离散化后，状态方程和量测方程为：

(8)

常规集中式卡尔曼滤波器主要有以下5个公式构成[6]，分别为：

状态一步预测：

(9)

状态一步预测均方误差阵：

(10)

滤波增益：

(11)

状态后验估值：

(12)

后验均方误差阵：

(13)

1.2 基于新息的自适应滤波算法

(14)

根据文献[7]，新息r(t)为零均值的白噪声过程，有协方差阵：

(15)

所以考虑采用新息序列的方差来衡量新息的变化，新息的理论方差为：

(16)

本文采用一种长度为L的滑动窗口(L一般小于100)对新息的方差进行实时估计，即

(17)

滤波的统计新息的更新方程变为：

(18)

(19)

将上式代入滤波增益矩阵的更新方程(5)中，得到自适应卡尔曼滤波的增益K*为：

(20)

基于新息的方差匹配自适应滤波方法主要解决外部噪声发生异常变化时系统无法做出正确响应的问题。其可通过在线实时调节噪声R的协方差大小以达到对新息方差的调整，并因此根据噪声变化实时动态调整滤波器的增益，可以有效控制滤波的发散。

2 抗差容错算法设计

当无人车因路况较差导致强烈振动时，IMU也许出现异常数据，代入积分运算后速度、位置将会随时间累计。为了确保整个系统在惯组短时间数据异常情况下仍可解算出正确的定位导航信息，需要对惯组数据进行检验。当无人车在城市峡谷复杂环境下运动时，卫星信号因高楼遮挡或干扰，往往会出现断续延迟或异常，所以需要对BDS数据的有效性进行判断[8]。本文分别对IMU/BDS数据分别进行检验，并针对异常情况提出处理策略。

2.1 惯组数据异常检验

无人车在城市中正常行驶，不会出现剧烈加速、侧滑和跳跃的现象，根据惯组的量程和无人车运动环境综合考虑设置阈值。当惯组的陀螺仪或加速度计输出数据超出阈值，则当前时刻IMU数据异常。例如，惯组陀螺仪和加速度计某个轴的阈值为ωgate和fgate，对惯组输出的每一包数据检验是否满足(21)式，若满足则继续进行下一级检验；否则为异常，进入异常处理策略。

(21)

其次，根据惯组每个轴的数据输出进行多项式拟合，从而预测下一次惯组输出，将预测值乘以比例因子kIMU，与真实输出值进行对比[9]。

综合考虑计算量与结果精度选择进行五阶多项式拟合。以惯组的加速度计某个轴为例有：

f(t+τ)=a+bτ+cτ2+dτ3+eτ4+
fτ5(0<τ

(22)

将t、t+T、t+2T、t+3T、t+4T时刻采样的角速率测量值代入上式可得：

(23)

式中，T为惯组输出间隔，a、b、c、d、e和f为多项式系数。通过矩阵运算可求出各个拟合系数。将拟合的预测值乘以比例因子，与惯组实际输出值进行比较，若超出则认为惯组输出异常，进入异常处理，否则正常。

|f-ffit|

(24)

2.2 惯组数据异常处理策略

若惯组数据通过上述二级检验，则认为正常。否则当某级检验为异常时，将拟合预测值与上一时刻数据求平均代替当前数据输出，进而与BDS数据进行信息融合。

(25)

2.3 BDS数据异常检验

2.3.1 第一级检验

根据卫星导航原理，对BDS数据进行收星数和PDOP、VDOP和HDOP检验[10]，若满足阈值条件则BDS数据正确，继续进行下一级检验；否则为异常进入异常处理。

2.3.2 第二级检验

由于城市峡谷环境中BDS信号可能因干扰而存在较大误差或因高楼遮挡出现断续延迟等状况，所以可以利用惯性导航短时精度较高的优势对BDS数据进行判断。

当BDS数据更新时，将BDS与SINS解算出的绝对位置与绝对速度进行比较。

(26)

其次，在滤波时刻，将BDS与SINS解算出的位置和速度在滤波周期内的增量进行比较，，若均小于阈值，则认为BDS数据正常；否则为异常。将速度和位置分别检验如下：

(27)

式中，PBDS为北斗接收机输出的位置值，PSINS为SINS解算出的位置值，VBDS为北斗接收机输出的速度值，VSINS为SINS解算出的速度值，ΔP1为北斗接收机位置RMS误差，ΔP2为惯性器件位置误差，ΔP3为组合导航的位置误差，ΔV1为北斗接收机速度RMS误差，ΔV2为惯性器件速度误差，ΔV3为组合导航的速度误差，Kp和Kv分别为位置比例因子和速度比例因子。

若BDS数据通过上述二级检验，则判定BDS数据正常，否则进入BDS数据异常处理流程。

2.4 BDS数据异常处理策略

根据实际情况，本文将BDS数据异常分为长时间异常和短时间异常。

1)BDS数据短时间异常时，根据对系统的建模，正常进行滤波的状态预测和预测均方误差的更新，利用状态预测值对导航结果进行校正，具体如下式所示：

(28)

2)若BDS数据异常时间较长，重新收到有效的BDS数据时，从系统实时性与快速性的角度考虑，用BDS数据重置卡尔曼滤波器，同时重置组合导航位置、速度值。

3)若SINS/BDS数据均异常，SINS和BDS解算出的定位结果势必存在较大的误差，组合导航也因此失去意义，系统无法提供精确的导航结果。此时可以采取航位推算(Dead Reckoning，DR)等算法进行导航[11]，本文不做详细分析。

3 延迟校正补偿

3.1 数据传输造成时延补偿

由于数据的传输和接收机软件的解算需要耗费一定的时间，即从接收机射频前端收到信号到解算完成传输至滤波器存在一定的时间差，且不同硬件平台传输和解算耗时也不同。若选择BDS数据更新时刻与SINS进行数据融合，会造成二者实时性不匹配，滤波的量测信息有一定误差。因此，需要对时延造成的误差进行相应补偿。

文中采用的接收机刷新率为1Hz，时统为GPS秒，整秒时刻收到PPS秒脉冲。因为惯性导航短时精度较高，故将上述时间段内的SINS速度/位置增量补偿到整秒时刻的BDS速度/位置上，然后再与惯性导航解算结果进行数据融合。

(29)

3.2 滤波造成时间延迟误差补偿

实际应用中，滤波的计算过程需要消耗一定的时间，得到的滤波结果也会存在一定的滞后，如果直接代入反馈校正过程，会造成一定的系统误差。根据之前所述的滤波方程，状态转移矩阵描述了状态变量随时间变换的过程[12]，故可以通过使用状态转移矩阵来减小滤波计算时间对校正结果的影响，具体过程如图1：

利用状态转移矩阵将计算得到的滤波结果递推到当前时间，然后再完成对状态量的校正过程。系统整体解算流程为：在整秒tk时刻进行滤波得到系统状态量为Xtk，读取当前时间并计算时间延迟td得到离散化后的状态转移矩阵Φtk+td|tk。利用tk+td时刻的状态转移矩阵Φtk+td|tk将tk时刻的状态变量估计Xtk递推至tk+td时刻，得到当前时刻状态变量的最优估计Xtk+td，再对系统校正。

4 试验与分析

为了验证本文所述系统和算法的抗差容错性能和导航精度，进行了半实物仿真试验。试验以DSP/FPGA为硬件平台，利用轨迹发生器产生一条轨迹模拟无人车的运动。将惯组置于三轴转台上实时模拟载体角度变化，仿真管理接口子系统将惯组发送的实时角速度与轨迹发生器生成的比力信息进行拼包后发送给硬件平台。试验轨迹如图2。

图1 状态转移校正流程

图2 半实物试验轨迹图

试验中惯组刷新率为200Hz，陀螺仪的零偏和零偏稳定性均为1(°)/h，设置加速度计的零偏和零偏稳定性为0.5mg和0.15mg，卫星接收机的速误差为0.15m/s(RMS)，位置误差为10m(RMS)，刷新率为1Hz。半实物仿真试验实物图如图3。

图3 半实物试验实物图

考虑无人车的实际运动环境和惯组的实际量程，惯组数据异常检验阈值fgate和ωgate设置为8m/s2和25(°)/s，比例因子kIMU设为1.5；BDS数据异常检验中，收星数阈值为5，DOP值阈值为5，比例因子Kp和Kv均取2。

半实物试验过程中，将本文所述的自适应抗差卡尔曼滤波算法与常规卡尔曼滤波进行比较，并设置了2种检验场景：1)无人车经过隧道、城市峡谷等场景时的丢星状态；2)外界噪声突变引起的滤波不稳定进而导致导航精度下降。

1)在BDS数据中100s～150s时间段设置DOP值过大，这时BDS数据无法通过前文所述的二级检验，从而进入丢星状态。以东北天坐标系的北向误差进行说明，试验结果如图4～5。

图4 位置误差曲线

图5 速度误差曲线

在丢星的50s中，常规组合导航算法失效，只能依靠纯惯性导航(实线1)，位置误差为13.08(RMSE)，速度误差为1.16m/s(RMSE)。而本文所述的抗差导航算法(虚线2)根据惯性系统的建模对系统进行实时修正，位置误差为6.84m(RMSE)，速度误差为0.15m/s(RMSE)。从统计结果可以看出，本文所述的算法可在卫星失效情况下大大提升导航的精度。

2)在BDS数据中100s～150s时间段设置位置、速度噪声增大为正常情况的5倍。以东北天坐标系的北向误差进行说明，试验结果如图6～7。

图6 位置误差曲线

图7 速度误差曲线

在噪声异常的50s中，常规的组合导航算法(实线1)由于无法敏感外界噪声变化，结果出现较大的跳变，呈发散趋势，其中位置误差为8.64(RMSE)，速度误差为0.41m/s(RMSE)。本文所述的自适应抗差导航算法(虚线2)可对噪声变化进行统计并实时调节滤波中的增益阵，减小量测量的置信度，其中位置误差为5.01m(RMSE)，速度误差为0.089m/s(RMSE)。从统计结果可以看出，本文所述的算法可在噪声情况下大大提升导航的精度，增强系统的鲁棒性。

5 结论

对于常规的集中式卡尔曼滤波器(KF)在卫星或惯性数据异常时，导航结果变差甚至滤波发散，导致系统崩溃的问题，设计了一种基于SINS/BDS的自适应抗差容错导航系统(SINS/BDS-based adaptive fault-tolerant navigation system)，对SINS和BDS数据进行有效性检验，并针对异常值进行剔除，做出相应处理措施。同时引入基于新息的自适应算法以减小噪声导致的系统异常。由于无人车在城市峡谷中行驶时IMU/BDS导航源易受干扰，故其可采用本文所述的自适应抗差导航算法，保证导航精度的同时提高了系统的鲁棒性和稳定性。最后通过半实物试验验证了整个系统的导航精度和可靠性，信息融合抗差算法设计合理。