2. 北京航空航天大学 仪器科学与光电工程学院, 北京 100083
2. School of Instrumentation and Optoelectronic Engineering, Beihang University, Beijing 100083, China
高精度位置姿态测量系统(Position and Orientation System, POS)本质是一个SINS/GNSS非线性组合系统，其通过非线性滤波方法，如扩展卡尔曼滤波(Extended Kalman Filter, EKF)、无迹卡尔曼滤波(Unscented Kalman Filter, UKF)，融合来自GNSS和SINS的位置、速度等运动参数，估计SINS的状态误差，进而为载荷提供可靠、高频、高精度的运动信息[1-2]。无论是线性滤波还是非线性滤波，都需要事先精确建立惯导误差模型和确定噪声统计特性[3-4]。然而，这存在2个严重问题：①精确建立误差模型几乎不可能，因为系统复杂时变；②已知噪声统计特性会随着系统的工作状态变化而变化。众所周知，由于后处理平滑利用了所有可观测的信息，理论上其具有最高的估计精度[5-6]。因此，一方面，后处理平滑结果可作为基准数据；另一方面，滤波估计方法仍有提升的空间。然而，怎样提升滤波估计精度是一个很有挑战的难题。
随着机器学习的发展，该问题得以解决。尤其是以高斯过程回归(Gaussian Process Regression, GPR)为主的无参数回归方法，具有不确定度输出、非线性映射、随机性预测等优点，很适合与贝叶斯滤波方法进行结合[7-11]。因此，针对UKF不可能把系统的所有误差源都考虑到，导致UKF的预测和估计能力仍有局限性的问题，本文提出了一种增强高斯过程的UKF(Enhanced GP-UKF, EGP-UKF)方法。该方法的核心思想是利用GP学习滤波估计值与后处理平滑估计值之间的残差。
本文详细论述了如何使用非参数GP学习状态模型和量测模型与真实值(平滑)之间的残差，以及如何无缝地把这些残差融合到UKF中。由此产生的EGP-UKF方法具有非参数GP的优点：①EGP-UKF方法取决于已建立的状态模型和量测模型，但不需要完全精确的数学模型，而且也取决于GP模型，EGP-UKF模型及其参数可以从较少的训练数据中学习得到；②EGP-UKF方法考虑了动态模型的不确定性。此外，UKF可避免因没有足够的训练数据引起的EGP-UKF更多不确定性。通过车载实验验证了EGP-UKF方法的估计性能，实验结果表明，该方法可明显提高POS的滤波估计精度。
1 SINS/GNSS组合导航 1.1 非线性惯导误差模型及状态方程本文采用基于角误差模型的非线性姿态误差方程，该非线性误差模型是UKF的基础。首先定义坐标系，i为惯性坐标系，p为平台坐标系，n为导航坐标系，b为载体坐标系，e为地球坐标系。
1.1.1 姿态误差微分方程
|
(1) |
式中：ϕ=[ϕE ϕN ϕU]T描述数学平台坐标系与导航坐标系之间的失准角；Cnp为平台坐标系p到导航坐标系n的方向余弦矩阵；ωinn为投影在导航坐标系n下的导航坐标系n相对于惯性坐标系i的角速度；δωinn为其误差；εb=[εx εy εz]T为陀螺仪随机漂移在载体坐标系b的投影；
|
(2) |
式中：V=[VE VN VU]T为导航坐标系n投影下的速度；δV为其误差；
|
(3) |
式中：δL、δλ和δH分别为纬度、经度和高度误差；RN和RM分别为椭球卯酉圈和子午圈曲率半径。
1.1.4 惯性器件方程惯性测量单元由3个陀螺仪和3个加速度计组成。假设陀螺仪随机误差ε和加速度计随机误差∇包括随机常数εc、∇c和高斯白噪声εg、∇g。虽然随机常值每次启动不同，但启动后是确定的。因此，陀螺仪和加速度计的随机误差如下：
|
(4) |
|
(5) |
SINS/GNSS组合导航系统的状态方程可以通过误差模型(包括姿态误差、速度误差、位置误差、惯性器件陀螺仪和加速度计的误差)获得，其对应的状态方程为式(1)~式(3)和式(5)。
1.2 量测方程量测方程是一个离散的线性方程，通过SINS捷联解算的速度/位置和GNSS的速度/位置差得到。
|
(6) |
|
(7) |
|
(8) |
|
(9) |
式中：xk为POS的状态向量；VGE、VGN和VGU分别为GNSS的东向、北向和天向速度；LGNSS、λGNSS和HGNSS分别为GNSS测量所得的纬度、经度和高度；ZV(k)和HV分别为速度量测信息和速度量测矩阵；ZP(k)和HP分别为位置量测信息和位置量测矩阵；MV和NP分别为GNSS的速度和位置噪声。
|
(10) |
|
(11) |
式中：ϕ为失准角向量；P为位置误差向量；Hk= 
UKF是Sigma点卡尔曼滤波的一种，通过UT非线性变换逼近非线性系统状态的后验均值和协方差，估计精度至少能达到二阶泰勒以上，且不需要计算雅可比矩阵。
在滤波阶段结束后，通过利用存储的状态预测
|
(12) |
由于在最终时刻，滤波和平滑的结果一样，因此有
GP是一组随机变量的集合，且集合中任意数量的随机变量都应满足联合高斯分布的随机过程，由均值函数和协方差函数确定[12-13]。
|
(13) |
式中：xi, xj∈Rd为任意随机变量。
|
(14) |
式中：xi为输入量，yi为输出量，它们分别组成了Xin和Yout；δout2为均值为零的高斯白噪声。
训练集中的Yout和测试点x*的预测值y*的联合分布为
|
(15) |
|
(16) |
|
(17) |
式中：K(Xin, Xin)为对称的协方差矩阵；σn为n维高斯噪声矩阵；In为n维单位矩阵；μ*为均值；Σ*为方差。
为了实现最优的训练效果，通常采用高斯核函数，如下：
|
(18) |
式中：θ={S, σf2, σn2}为超参数，可通过对数极大似然函数求解后验分布获得；σf2为信号方差。一般采用共轭梯度方法求解最优超参数。函数的偏导数如下：
|
(19) |
|
(20) |
式中：C=K(Xin, Xin)+σn2In; α=(K(Xin, Xin)+σn2In)－1Yout。
3 EGP-UKF模型 3.1 学习状态方程和量测方程EGP-UKF是一种融合数学模型和数据模型各自优势的高精度滤波模型。利用GP学习UKF的状态方程和量测方程以及协方差阵Q和R。针对多维误差模型，采用GP学习UKF中的每一个状态估计值和真值之间的差值，且每个GP的训练数据都是一组输入输出关系。采用GP学习状态模型的残差时，首先求解UKF滤波估计值和后处理平滑值；然后将状态模型中当前时刻的状态xks映射到状态变化量Δxk+1s=xk+1s－xks。量测模型由状态xk+1s映射到观测zk+1s，它们分别是由SINS/GNSS组合导航系统后处理平滑和差分GNSS计算得到的。状态模型和量测模型的训练数据集的形式分别表示为
|
(21) |
|
(22) |
式中：xk+1s为后处理平滑估计状态量；zk+1s为捷联惯导解算结果和差分GNSS之差。采用GP学习状态模型和量测模型残差的数学模型如下：
|
(23) |
|
(24) |
|
(25) |
|
(26) |
经过上述分析，EGP-UKF方法由2个阶段组成：学习和估计。在学习残差模型阶段中，采用GP分别进行学习状态模型和量测模型的训练数据 
1) 初始化
|
2) 计算Sigma点
|
3) 时间更新
|
4) 量测更新
|
5) 状态更新
|
式中：
本文提出的EGP-UKF高精度滤波估计方法不仅不需要训练数据包含整个误差状态空间，也不需要精确的数学模型。正如算法中标注“*”的公式所示。EGP-UKF建模可以用来学习状态模型和量测模型，其优于参数模型，特别是当参数模型是不准确的。GP的残差模型由GPf和GPg组成，一旦模型被训练好，预测模型便可得出：
|
(27) |
|
(28) |
为了评价本文EGP-UKF方法的性能，通过车载实验数据验证了UKF、GP-UKF和EGP-UKF的性能。车载所用的SINS/GNSS组合导航系统是北京航空航天大学设计的POS，其由IMU、GNSS和PCS组成，如图 1所示。此外，为了保证后处理平滑的精度，在实验过程中架设了GNSS地面基准站，如图 1(c)所示。POS安装在实验车上，如图 1(b)所示。
|
| 图 1 车载实验 Fig. 1 Vehicle-mounted experiment |
为了保证后处理的精度且更好地评价EGP-UKF的性能，在北京市昌平区的航空博物馆前进行了车载实验。原因如下：①面积开阔，无GNSS信号遮挡；②道路相对平直，约3km长。在实验过程中，SINS/GNSS组合导航系统运行良好，采集所有相关数据，如IMU数据、GNSS数据和实时组合导航数据，时长约为2200s。因此，IMU数据和差分GNSS数据的平滑结果可以作为真值，其中前800s用来训练GP-UKF[14-15]和EGP-UKF，最后1400s评价本文方法的性能。车载实验轨迹如图 1(d)所示。
图 2~图 4给出了EGP-UKF、UKF、GP-UKF在纬度误差、经度误差和高度误差上的对比结果。在图 2~图 4中，本文提出的EGP-UKF方法和另外2种方法在开始阶段的性能几乎相同，一方面，因为在静止部分参数模型与后处理平滑模型基本一致，另一方面在静止部分本文方法的噪声几乎和参数模型一致。如果发生了机动，EGP-UKF精度明显高于UKF和GP-UKF。因为EGP-UKF不仅能自适应调节系统噪声和量测噪声，而且还考虑了模型不确定性，然而UKF未考虑模型不确定性和噪声的影响。尽管GP-UKF考虑了噪声的影响，但该方法严重依赖数据模型，未考虑系统本身的数学模型。机动之后，本文方法也明显优于其他2种方法，因为EGP-UKF在系统误差模型的基础上进一步利用了数据模型，抑制了系统误差模型不确定度的影响。事实上，模型的不确定主要由参数模型和相应的平滑之间的残差组成。在某种程度上，EGP-UKF可以学习和预测这种模型不确定性带来的残差，并减轻其带来的不利影响。
|
| 图 2 纬度误差对比 Fig. 2 Comparison of latitude error |
|
| 图 3 经度误差对比 Fig. 3 Comparison of longitude error |
|
| 图 4 高度误差对比 Fig. 4 Comparison of altitude error |
图 5~图 7表明，在东向速度误差和北向速度误差方面，EGP-UKF比UKF和GP-UKF的精度更高、更稳定。一方面，东向速度误差在图 3所示经度误差一致；另一方面，北向速度误差变化和图 2所示的纬度误差变化一致，符合惯性导航系统的误差传播定律。由图 4和图 7可知，高度误差和天向速度误差几乎相同。
|
| 图 5 东向速度误差对比 Fig. 5 Comparison of east velocity error |
|
| 图 6 北向速度误差对比 Fig. 6 Comparison of north velocity error |
|
| 图 7 天向速度误差对比 Fig. 7 Comparison of up velocity error |
图 8~图 10表明，在航向角误差、俯仰角误差和横滚角误差方面，本文EGP-UKF方法明显优于UKF和GP-UKF。可以看出，EGP-UKF的收敛速度比UKF和GP-UKF更快。此外，在航向角误差方面EGP-UKF与UKF、GP-UKF几乎是相同的。
|
| 图 8 航向角误差对比 Fig. 8 Comparison of heading angle error |
|
| 图 9 俯仰角误差对比 Fig. 9 Comparison of pitch angle error |
|
| 图 10 横滚角误差对比 Fig. 10 Comparison of roll angle error |
本文定量分析了3种方法的性能。相比GP-UKF和UKF，本文EGP-UKF在纬度误差和经度误差分别降低了18.5%、24.6%和9.1%、10.6%。在高度误差方面，这3种方法几乎相同。东向速度误差和北向速度误差分别降低了42.5%、43.3%和22.6%、24.3%。因为天向速度和高度的相关性，天向速度误差几乎是相同的。此外，航向角误差、俯仰角误差和横滚角误差分别降低了10.3%、30.1%、24.6%和3.4%、24.7%、28.2%。总之，EGP-UKF学习了UKF和其后处理平滑之间的残差模型，通过该残差模型提高了POS的测量精度。
在一般情况下，UKF的计算复杂度为状态向量的维数的二次方。然而，GP在学习和预测过程中的计算复杂度分别是训练数据的三次方和二次方，故GP-UKF比UKF运算速度慢，另外由于EGP-UKF融合了GP和UKF共同的优点，所以EGP-UKF的运算速度比GP-UKF慢。
5 结论1) 针对传统非线性滤波模型不确定性的问题，提出了增强GP的UKF方法。该方法既充分利用了GPR和UKF的优点，又克服了它们自身的缺点。
2) 该方法的核心是通过非参数GP学习UKF的状态方程、量测方程和后处理平滑之间的残差。
3) 车载实验结果表明，与UKF、GP-UKF相比，EGP-UKF具有更高的估计精度，因为EGP-UKF不仅具有更强的不确定度预测能力，而且具有更强的泛化能力。
4) 如何提高EGP-UKF的实时性是未来的一个研究方向。
| [1] |
YE W, LI J L, LI L C. Design and development of a real-time multi-DSPs and FPGA-based DPOS for InSAR applications[J]. IEEE Sensors Journal, 2018, 18(8): 3419-3425. DOI:10.1109/JSEN.2018.2799622 |
| [2] |
YE W, LIU Z C, LI C, et al. Enhanced Kalman filter using noisy input Gaussian process regression for bridging GPS outages in a POS[J]. The Journal of Navigation, 2018, 71(3): 565-584. DOI:10.1017/S0373463317000819 |
| [3] |
RUDOLPH V D. Sigma-point Kalman filters for probabilistic inference in dynamic state-space models[D]. Oregon: Oregon Health and Science University, 2004.
|
| [4] |
CHANG L B, HU B Q, LI A, et al. Transformed unscented Kalman filter[J]. IEEE Transactions on Automatic Control, 2013, 58(1): 252-257. DOI:10.1109/TAC.2012.2204830 |
| [5] |
GONG X L, ZHANG J X, FANG J C. A modified nonlinear two-filter smoothing for high-precision airborne integrated GPS and inertial navigation[J]. IEEE Transactions on Instrumentation and Measurement, 2015, 64(12): 3315-3322. DOI:10.1109/TIM.2015.2454672 |
| [6] |
GREWAL M S, ANDREWS A P.Kalman filtering: Theory and practice using MATLAB[M]. 3rd ed. Hoboken: Wiley, 2008.
|
| [7] |
RASMUSSEN C E, WILLIAMS C K I. Gaussian processes for machine learning(adaptive computation and machine learning)[M]. Cambrige: MIT Press, 2005.
|
| [8] |
XU Z, LI Y, RIZOS C, et al. Novel hybrid of LS-SVM and Kalman filter for GNSS/INS integration[J]. The Journal of Navigation, 2010, 63(2): 289-299. DOI:10.1017/S0373463309990361 |
| [9] |
ROBERTS S, AIGRAIN S. Gaussian processes for time-series modelling[J]. Philosophical Transactions, 2012, 371(1984): 1-25. |
| [10] |
ATIA M M, NOURELDIN A, KORENBERG M. Gaussian process regression approach for bridging GPS outages in integrated navigation systems[J]. Electronics Letters, 2011, 47(1): 52-53. DOI:10.1049/el.2010.7164 |
| [11] |
CHEN H M, CHENG X H, WANG H, et al. Dealing with observation outages within navigation data using Gaussian process regression[J]. The Journal of Navigation, 2014, 67(4): 603-615. DOI:10.1017/S0373463314000010 |
| [12] |
何志昆, 刘光斌, 赵曦晶, 等. 高斯过程回归方法综述[J]. 控制与决策, 2013, 28(8): 1121-1129. HE Z K, LIU G B, ZHAO X J, et al. Review of Gauss process regression methods[J]. Control and Decision, 2013, 28(8): 1121-1129. (in Chinese) |
| [13] |
DEISEBROTH M P, FOX D, RASUSSEN C E. Gaussian processes for data-efficient learning in robotics and control[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2015, 37(2): 408-423. DOI:10.1109/TPAMI.2013.218 |
| [14] |
KO J, FOX D. Learning GP-Bayes filters via Gaussian process latent variable models[J]. Autonomous Robots, 2011, 30(1): 3-23. DOI:10.1007/s10514-010-9213-0 |
| [15] |
李鹏, 宋申民, 陈兴林, 等. 联合高斯回归的平方根UKF方法[J]. 系统工程与电子技术, 2010, 32(6): 1281-1285. LI P, SONG S M, CHEN X L, et al. Combining Gaussian process regression and square root UKF method[J]. Journal of Systems Engineering and Electronics, 2010, 32(6): 1281-1285. DOI:10.3969/j.issn.1001-506X.2010.06.036 (in Chinese) |