由于定位精度高、全天候使用、通信快捷和运营成本低廉等特点,卫星导航已广泛应用于城市环境的车辆导航。特别是随着我国自主研发的BDS系统的逐步完善,将更进一步拓展卫星导航的应用范围。但是在城市环境中,卫星信号会因建筑物的反射或散射形成多路径(multipath, MP)或非视距(non-line-of-sight, NLOS)信号,造成的误差可达百米,这将使得传统滤波算法产生定位偏差,甚至发散[1-2]。
为削弱MP和NLOS等观测异常的干扰,一方面,研究人员采用抗差估计的方法来削弱其对导航定位的影响[3-7]。文献[4]分析了基于最小二乘(least square, LS)的等价权估计方法,表明良好的参数初值是保证等价权抗差效果的前提。文献[5]基于新息向量构造抗差Kalman滤波(Kalman filter, KF)并应用于室内导航,结果表明该方法可提高NLOS环境的导航解精度和稳定性。文献[6-7]采用较扩展卡尔曼(extended Kalman filter, EKF)截断误差更小的无迹卡尔曼滤波(unscented Kalman filter, UKF)进行目标跟踪,并对观测异常采用开窗的方式进行在线估计。试验表明该方法可有效提高观测噪声统计不准确时的非线性目标跟踪精度。但是合理确定移动窗口的大小是该算法的难点,特别是过大的窗口会降低其对MP和NLOS快速变化的灵敏度。
另一方面,有学者提出多传感器融合抑制MP和NLOS干扰的方法[8-14]。文献[8]利用鱼眼相机的图像分析实时导航环境,并通过方位判断干扰信号。文献[9]基于城市的3D模型预测MP和NLOS的误差边界,在滤波估计中予以考虑。但是由于复杂性,这些方法难以应用于低价GNSS(global navigation satellite systems)接收机的实时导航。文献[10-13]研究了GNSS/INS(inertial navigation system)组合的导航算法,极大地削弱GNSS信号异常对导航解的影响。但是这些方法增加了传感器负荷,且未知参数较多,计算复杂。
另外,有学者根据载体的运动学规律,提出具有运动信息约束的稳健方法[15-17]。文献[15]利用侧向和天向速度为零的条件提高了低价INS的导航参数精度。文献[16]采用前一历元高程变化作为当前高程约束的方差,结果表明可有效改进高程常值约束的不足。文献[17]通过构建载体的垂向运动模型约束高程的解,实测结果表明该方法明显改善高程的精度。总体上,以上方法多考虑高程向的约束,对运动方向约束的研究相对并不充分。
为此,本文研究一种在不增加传感器负担情况下,在利用新息向量较优抗差的基础上,附加运动方向和高程约束的UKF导航算法。基于实测的车载数据,通过星间单差分析MP和NLOS信号的特性,然后与传统迭代最小二乘解比较,验证本文算法在多MP和NLOS环境中的导航精度和稳定性。
1 抗差UKF模型 1.1 标准UKF常用BDS卫星动态导航的观测模型和动力学模型一般为
式中,Lk为k时刻观测卫星的伪距向量,一般采用B1I频率上的码伪距,此处的伪距已经进行了电离层延迟、对流层延迟、地球自转改正以及硬件延迟改正[18];Xk为状态参数向量,包括地心地固坐标系(Earth-centered Earth-fixed, ECEF)中的3个位置分量,3个速度分量,一个接收机钟差和一个接收机钟漂;h表示观测方程的非线性关系;Vk为观测向量噪声;Fk为状态转移矩阵;Wk为系统噪声向量。一般情况下,Vk和Wk是互不相关的零均值高斯白噪声,即满足
(1) 状态参数初值及其协方差按下式给出
式中,
(2) 计算Sigma点。UKF基于无迹变换,实现概率分布的逼近。无迹变换的采样策略与Sigma点的位置、个数和对应权息息相关[21-22]。通常采用基于Cholesky分解的对称采样策略
式中,χk-1为2n+1个列向量;
式中,ωim和ωic分别为均值和方差的权值;β为非线性高阶信息的参数,对于高斯分布,取β=2。
(3) 基于动力学模型的一步预测表示为
式中,Xk为状态向量预测值;Pk为预测状态向量的协方差阵。
(4) 基于观测模型的测量更新
式中,Lk为预测观测值;Pyy为预测观测值的协方差阵。
(5) 状态参数的修正
式中,Kk为增益矩阵;
标准UKF的观测噪声服从零均值高斯分布,但MP和NLOS信号会污染观测噪声的分布特性,造成标准UKF出现定位偏差,甚至发散。为此,有必要研究城市复杂环境中的MP和NLOS特性,以便给出合理的抗差方法。假设具有最高高度角的观测卫星(>80°)Sb,基本不受MP和NLOS信号干扰[2], 其他卫星Si受到的干扰为δ。首先将式(1)的观测方程表示为显示形式
式中,Rb、Ri分别为测站与卫星Sb、Si的几何距离,当测站坐标已知时,可准确计算其值;δt为接收机钟差;δ为卫星Si的MP/NLOS误差;V为观测噪声。星间单差可消除接收机钟差,即有
式中,ΔLb, i=Li-Lb为星间伪距单差。通过式(10),即可计算MP和NLOS误差。
1.3 抗差模型由于基于位置和接收机钟差的一步预测值与观测值做差,新息向量具有较好的抗差初值,其表示为
基于新息向量,构造抗差因子,从而调节观测信息对滤波结果的贡献,抗差因子可采用IGGIII的表达形式[3]
式中,Ri为等价方差阵的第i个对角线元素;k0、k1为常数阈值,通常k0取1.5~2.0,k1取3.0~8.5,本文取k0=2, k1=4;ui为标准化残差,可基于中位数计算的方差因子得到[3, 10]
式中,median为取中位数算子;Pi, yy为新息向量协方差阵的第i个对角线元素。把观测噪声的等价协方差阵R代入式(7),可得具有抗差特性的UKF。
2 运动学约束模型 2.1 运动学约束在城市主干路行驶时,载体短时间近似在一个比较确定的方向上运动,如图 1所述。考虑约束方法的物理意义,本文将ECEF坐标转换至以基站为坐标原点的站心坐标系(East/North/Up, ENU)。首先,通过车载导航地图获得当前行驶道路的节点和路段信息。然后,通过节点坐标(转换至ENU坐标系)确定中心道路的方向,并根据路段信息判别交叉路口转弯的情况。根据上述方法确定的运动方向,可表达近似的约束为
式中,θ为近似的运动方向;(E, N)为载体在ENU坐标系中的东坐标和北坐标;b为约束的观测理论值。由于本文采用道路中心的理论观测值,则可根据车道宽度及车道数确定该观测值的误差qb为
式中,N、width分别为车道数及车道宽度。
另外,城市的道路相对平坦,短时间内高程方向不会发生较大变化,为此可以进行近似高程约束[16-17]。考虑实际行驶中前后历元高程基本一致,约束方程为
式中,Uk为当前历元的非约束的高程;Uk-1为上一历元的高程。同时,根据上一历元的状态估计的方差阵,给出Uk-1的误差为qU, k-1,这表达了高程约束的精度,有效抑制前一历元高程异常引起的约束扰动。将式(14)与式(16)表示成矩阵表示形式
式中,Xk=[[Ek Nk Uk]T为ENU下位置和高程的值;Bk=[b 0]T。另外,根据式(15)及qU, k-1,约束观测的方差阵可表达为
文献[23-25]给出了分步约束的KF滤波过程:第1步是没有约束的标准KF;第2步是基于约束条件的修正。类似地,本文给出附加运动学约束的UKF的第2步[26]。然后,可得约束UKF的递推过程。
(1) 基于无约束解的Sigma采样
(2) 基于约束模型的约束更新
式中,Bk为预测的约束观测值;
(3)状态参数的修正
式中,
本试验采用车载方式,采集地点为东京主城区,道路两侧环境复杂,特别是超高建筑较多。数据采集时,截止卫星高度角设置为10°,采样频率为5 Hz。同时,在一空旷地设置基准站,采用高精度商业软件IE解算的实时动态RTK结果作为参考基准[27]。试验的运动轨迹如图 2所示。
图 3给出车载试验中可用卫星的高度角变化。可以看出,C11卫星的高度角最高,基本在80°以上。因此,以C11作为参考星,采用星间单差的方法计算MP和NLOS误差,如图 4所示。可以看出,C12的最大误差可达200 m,C01、C13有部分误差大于50 m,因此这些误差将严重影响滤波效果。另外,可以看出C08的误差最小,呈现一定的高度角相关性,但是C12相较于C02的大误差更多,表明仅考虑高度角相关是不足的。表 1给出MP和NLOS误差的统计结果,包括最大值、最小值和平均值。可以看出,MP与NLOS误差表现出一定的随机性,并不完全依赖于高度角。
卫星 | 最大值 | 最小值 | 平均值 |
C01 | 44.4 | -7.1 | 2.6 |
C02 | 20.7 | -19.5 | -1.5 |
C03 | 123.2 | -10.8 | 0.1 |
C06 | 24.6 | -11.9 | 1.4 |
C08 | 3.3 | -4.4 | 1.0 |
C12 | 182.7 | -25.9 | 14.6 |
C13 | 84.3 | -11.2 | 7.4 |
C14 | 11.0 | -11.4 | -1.5 |
进一步分析位置精度因子(position dilution of precision, PDOP)与MP/NLOS信号的关系,如图 5所示。可以看出,PDOP与可见卫星数(Sat_Num)相关。卫星可见数变化较小时,PDOP比较稳定,如红色框①所示;卫星可见数变化较大时,PDOP的变化也较大,如红色框②所示。另外,在高频采样时,卫星可见数的跳变可反映观测环境的复杂,表明卫星信号受到遮挡较多,可能产生的MP/NLOS信号也会较多。因此,PDOP受到的扰动与MP/NLOS信号相关,这与文献[28]模拟分析的PDOP与NLOS关系类似。
为评估MP和NLOS环境的导航定位效果,设计3种方案对计算的平面位置和高程结果进行对比:①抗差迭代最小二乘ILS;②抗差UKF滤波;③附有运动学约束的抗差UKF滤波,即本文所提算法。表 2给出滤波解算的参数设置。其中,考虑载体行驶中高程变化较小,设置高程的过程方差大于平面的过程方差,本文分别设置为5 m和1 m。滤波过程中,转换ENU坐标系下的噪声方差到空间直角坐标系ECEF。
参数 | 处理策略 |
位置初值及误差 | 最小二乘给出 |
钟差初值及误差 | 最小二乘给出 |
速度初值及误差 | 0 m/s, 10 m/s |
钟漂初值及误差 | 0 m/s, 100 m/s |
位置的过程噪声 | 平面5 m; 高程1 m |
速度的过程噪声 | 平面5 m/s; 高程1 m/s |
钟差/漂的过程噪声 | 100 m, 100 m/s |
钟跳判别阈值 | 1e+3 m |
图 6—图 8分别给出3种方案在东方向、北方向和高程方向的误差序列图。首先,可看出在时刻50、600以及1300附近,PDOP波动较大,对应3个方向的定位精度也较差,表明PDOP能较好反映定位的性能。但在时刻750附近,方案1的东方向和高程方向仍有较大异常扰动,仅依靠PDOP验前确定观测扰动存在不足。而方案2基于新息向量的等价权进行抗差,较好地抑制了MP/NLOS影响。
另外,总体上,方案2和方案3较方案1,在3个方向上的波动更少,误差基本在±15 m左右;方案3较方案2,东方向600时刻附近,北方向1300时刻附近,改善明显。这主要因为顾及过紧约束可能引起的滤波异常扰动,方案3的方向约束的方差较大,主要改善出现明显滤波发散的情况。
进一步对比实际场景下3种方案的导航定位效果,如图 9所示。可以看出,在两侧具有高耸建筑物时,由于MP和NLOS信号的干扰,方案1出现较明显的不规则扰动。方案2基于新息向量抗差,基本维持较稳定的行车轨迹,但在图 9(b)中出现部分滤波偏离现象,这可能是由于过分依赖动力学模型,而方案3顾及了行驶方向的松约束,有效抑制了滤波的偏离误差。
表 3给出3种方案平面位置和高程误差的均方根。可以看出,方案2较方案1,东方向和北方向分别改善约9.2 m和2.4 m,高程方向改善约12.7 m;方案3较方案2,东方向和北方向进一步分别改善约1.8 m和0.4 m,高程方向改善约0.5 m,表明本文算法抗差性能更强,可靠性更优。
4 结论
城市复杂环境中,单一BDS导航会受到MP和NLOS信号干扰,严重影响导航精度和可靠性。针对此问题,本文基于实测车载数据,验证了附加运动学约束的抗差UKF滤波算法,得到以下结论:
(1) 城市复杂环境中,MP和NLOS信号引起的测距误差可达上百米,且误差表现出一定的随机性,并不完全依赖于卫星高度角。
(2) 基于新息向量构造的抗差UKF算法克服初值不准引起的抗差性能下降问题,相较于传统迭代最小二乘法,其具有更优的抗差性。
(3) 附加约束的抗差UKF通过顾及运动学的信息,可有效抑制因过分依赖动力学模型的局部滤波发散,提高了导航的精度和稳定性。
[1] |
JIANG Ziyi, GROVES P. GNSS NLOS and multipath error mitigation using advanced multi-constellation consistency checking with height aiding[C]//Proceedings of the 25th International Technical Meeting of the Satellite Division of the Institute of Navigation (ION GNSS 2012). Nashville, TN: ION, 2012: 79-88.
|
[2] |
TOMINAGA T, KUBO N. Adaptive estimation of measurement noise to improve the performance of GNSS single point positioning in dense urban environment[J]. Journal of the Institute of Positioning Navigation and Timing of Japan, 2017, 8(1): 1-8. DOI:10.5266/ipntj.8.1 |
[3] |
杨元喜. 自适应动态导航定位[M]. 北京: 测绘出版社, 2006. YANG Yuanxi. Adaptive navigation and kinematic positioning[M]. Beijing: Surveying and Mapping Press, 2006. |
[4] |
杨玲, 沈云中, 楼立志. 基于中位参数初值的等价权抗差估计方法[J]. 测绘学报, 2011, 40(1): 28-32. YANG Ling, SHEN Yunzhong, LOU Lizhi. Equivalent weight robust estimation method based on median parameter estimates[J]. Acta Geodaetica et Cartographica Sinica, 2011, 40(1): 28-32. |
[5] |
刘韬, 徐爱功, 隋心, 等. 新息向量的抗差Kalman滤波方法及其在UWB室内导航中的应用[J]. 武汉大学学报(信息科学版), 2019, 44(2): 233-239. LIU Tao, XU Aigong, SUI Xin, et al. An improved robust Kalman filtering method based on innovation and its application in UWB indoor navigation[J]. Geomatics and Information Science of Wuhan University, 2019, 44(2): 233-239. |
[6] |
胡高歌, 高社生, 赵岩. 一种新的自适应UKF算法及其在组合导航中的应用[J]. 中国惯性技术学报, 2014, 22(3): 357-361, 367. HU Gaoge, GAO Shesheng, ZHAO Yan. Novel adaptive UKF and its application in integrated navigation[J]. Journal of Chinese Inertial Technology, 2014, 22(3): 357-361, 367. |
[7] |
刘光明, 徐帆江. 抗差自适应UKF算法在地基光学跟踪空间目标中的应用[J]. 系统工程与电子技术, 2018, 40(3): 623-629. LIU Guangming, XU Fanjiang. Application of robustly adaptive UKF algorithm in ground-based bearings-only tracking for space targets[J]. Systems Engineering and Electronics, 2018, 40(3): 623-629. |
[8] |
SUZUKI T, KUBO N. N-LOS GNSS signal detection using fish-eye camera for vehicle navigation in urban environments[C]//Proceedings of the 27th International Technical Meetings of the Satellite Division of the Institute of Navigation. Tampa: Institute of Navigation, 2014: 1897-1906.
|
[9] |
KBAYER N, SAHMOUDI M, CHAUMETTE E. Robust GNSS navigation in urban environments by bounding NLOS bias of GNSS pseudoranges using a 3D city model[C]//Proceedings of the 28th International Technical Meeting of the Satellite Division of the Institute of Navigation. Tampa: Institute of Navigation, 2015: 2410-2420.
|
[10] |
谭兴龙, 王坚, 李增科. 基于SVD的改进抗差UKF算法及在组合导航中的应用[J]. 控制与决策, 2014, 29(10): 1744-1750. TAN Xinglong, WANG Jian, LI Zengke. SVD aided improved robust UKF algorithm and its application for integration navigation[J]. Control and Decision, 2014, 29(10): 1744-1750. |
[11] |
李彦杰, 杨元喜, 何海波. 附加约束条件对GNSS/INS组合导航结果的影响分析[J]. 武汉大学学报(信息科学版), 2017, 42(9): 1249-1255. LI Yanjie, YANG Yuanxi, HE Haibo. Effects analysis of constraints on GNSS/INS integrated navigation[J]. Geomatics and Information Science of Wuhan University, 2017, 42(9): 1249-1255. |
[12] |
胡杰, 严勇杰, 王子卉. 基于速度约束与模糊自适应滤波的车载组合导航[J]. 兵工学报, 2020, 41(2): 231-238. HU Jie, YAN Yongjie, WANG Zihui. Vehicle integrated navigation based on velocity constraint and fuzzy adaptive filtering[J]. Acta Armamentarii, 2020, 41(2): 231-238. |
[13] |
YANG Cheng, SHI Wenzhong, CHEN Wu. Robust M-M unscented Kalman filtering for GPS/IMU navigation[J]. Journal of Geodesy, 2019, 93(8): 1093-1104. DOI:10.1007/s00190-018-01227-5 |
[14] |
JIANG Chen, ZHANG Shubi, CAO Yizhi, et al. A robust fault detection algorithm for the GNSS/INS integrated navigation systems[J]. Journal of Geodesy and Geoinformation Science, 2020, 3(1): 12-24. |
[15] |
DISSANAYAKE G, SUKKARIEH S, NEBOT E, et al. The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications[J]. IEEE Transactions on Robotics and Automation, 2001, 17(5): 731-747. DOI:10.1109/70.964672 |
[16] |
柴艳菊, 阳仁贵, 王海涛, 等. 附加运动约束提高GPS/INS导航参数估计精度[J]. 中国惯性技术学报, 2011, 19(1): 28-32. CHAI Yanju, YANG Rengui, WANG Haitao, et al. Improving the navigation accuracy of GPS/INS by adding motion information constraints[J]. Journal of Chinese Inertial Technology, 2011, 19(1): 28-32. |
[17] |
YANG Jun, MA Jinfeng, LIU Xinning, et al. A height constrained adaptive Kalman filtering based on climbing motion model for GNSS positioning[J]. IEEE Sensors Journal, 2017, 17(21): 7105-7113. DOI:10.1109/JSEN.2017.2752751 |
[18] |
GUO Fei, ZHANG Xiaohong, WANG Jinling. Timing group delay and differential code bias corrections for BeiDou positioning[J]. Journal of Geodesy, 2015, 89(5): 427-445. DOI:10.1007/s00190-015-0788-2 |
[19] |
JULIER S J, UHLMANN J K, DURRANT-WHYTE H F. A new approach for filtering nonlinear systems[C]//Proceedings of 1995 American Control Conference-ACC'95. Seattle, WA: IEEE, 1995: 1628-1632
|
[20] |
JULIER S J, UHLMANN J K, DURRANT-WHYTE H F. A new method for the nonlinear transformation of means and covariances in filters and estimators[J]. IEEE Transactions on Automatic Control, 2000, 45(3): 477-482. DOI:10.1109/9.847726 |
[21] |
JULIER S J. The scaled unscented transformation[C]//Proceedings of 2002 American Control Conference. Anchorage, AK: IEEE, 2002: 4555-4559.
|
[22] |
GROSS J, GU Yu, GURURAJAN S, et al. A comparison of extended Kalman filter, sigma-point Kalman filter, and particle filter in GPS/INS sensor fusion[C]//Proceedings of AIAA Guidance, Navigation, and Control Conference. Toronto: AIAA, 2010.
|
[23] |
CRASSIDIS J L, JUNKINS J L. Optimal estimation of dynamic systems[M]. 2nd ed. Boca Raton: Chapman & Hall/CRC, 2012.
|
[24] |
胡丛玮, 刘大杰, 姚连璧. 带约束条件的自适应滤波及其在GPS定位中的应用[J]. 测绘学报, 2002, 31(S1): 39-44. HU Congwei, LIU Dajie, YAO Lianbi. Adaptive filtering with constraint for GPS positioning[J]. Acta Geodaetica et Cartographica Sinica, 2002, 31(S1): 39-44. DOI:10.3321/j.issn:1001-1595.2002.z1.009 |
[25] |
YANG Yuanxi, GAO W, ZHANG X. Robust Kalman filtering with constraints:a case study for integrated navigation[J]. Journal of Geodesy, 2010, 84(6): 373-381. DOI:10.1007/s00190-010-0374-6 |
[26] |
SIMON D, CHIA T L. Kalman filtering with state equality constraints[J]. IEEE Transactions on Aerospace and Electronic Systems, 2002, 38(1): 128-136. DOI:10.1109/7.993234 |
[27] |
DING Weidong, WANG Jinling. Precise velocity estimation with a stand-alone GPS receiver[J]. The Journal of Navigation, 2011, 64(2): 311-325. DOI:10.1017/S0373463310000482 |
[28] |
HONDA J, KAKUBARI Y, KOGA T. DOP simulation considering LOS/NLOS for aircraft positioning on airport surface[C]//Proceedings of 2015 IEEE Asia-Pacific Conference on Synthetic Aperture Radar (APSAR). Singapore: IEEE, 2015: 773-776.
|