杨璐宁, 刘正华, 温 暖
(北京航空航天大学自动化科学与电气工程学院, 北京 100191)
随着无人驾驶车辆的研究不断深化,无人驾驶车辆对导航系统的要求逐渐严格[1]。其中,应用最广泛的导航系统为捷联惯性导航系统(strapdown inertial navigation system, SINS)和全球卫星定位系统(global positioning system, GPS)组成的组合导航系统[2]。在良好的工作环境下SINS/GPS组合导航能提高准确的定位信息[3],但在山洞、隧道等恶劣环境下,GPS信号将受到影响[4],基于惯导、卫导的组合导航方式退化为纯惯导系统[5],致使导航误差累积,定位信息发散[6],无法提供高精度的导航信号[7]。因此,为了保证在GPS中断期间的导航可靠性,研究人员针对多元未知环境中的导航问题展开探索[8]。
为了获得高性能的导航系统,将传统线性滤波估计器转化为无迹卡尔曼滤波(unscented Kalman filter, UKF)估计器进行组合导航[9],防止卡尔曼估计出现的较大偏差[10]。同时,为了克服滤波器的发散性,Geiger等应用自适应卡尔曼滤波提高组合导航精度[11]。基于传感器原始数据,可扩展性强,但不能从根本解决问题[12]。随着人工智能的发展,Zhang提出了一种多层感知机提供伪GPS信号的求解方式[13]。但由于只利用末时刻导航信息造成导航信息浪费现象[14],基于神经网络时间序列预测技术应运而生[15]。Fu等设计了一种混合递归神经网络框架估计无人船的位置[16]。然而,由于在训练期间引入额外输入历史数据,模型变得更加复杂[17]。
高斯过程(Gaussian progress, GP)是一种典型的非参数贝叶斯模型[18],可将输入空间之间的相关性投影到输出空间来估计给定任意输入的输出均值和协方差[19],故GP在预测问题上引起广泛关注[20]。Chen等[21]提出了一种GP/空间网格正交法(spare grid quadrature,SGQ)-卡尔曼滤波(Kalman filter,KF)的方法来处理测量中断。但由于GP模型的能力受到核函数表达能力的限制[22],在GPS长期中断的情况下,GP模型的性能仍有提高的空间[23]。深度GP(deep GP, DGP)是一种基于多元GP的深度模型[24]。与GP不同的是GP只能代表一类有限函数,而DGP是一种多层次模型[25],更适合于复杂的小样本数据关系[26]。同时DGP能自动防止过拟合且在输出上能提供完整的概率分布[27],并对不确定性进行合理量化[28]。简言之,DGP保留了GP原有的属性[29],并克服了其局限性,更适合高度复杂的数据关系[30]。
本文针对多元未知环境中的无人驾驶车辆导航问题,设计了一种基于DGP的组合导航轨迹预测方法,有效解决了GPS长时间失锁的信息融合问题,极大提高了无人驾驶车辆在多元未知环境中的导航定位精度。本文的创新点主要包括:
(1) 提出了一种基于非参数模型DGP的DGP-UKF组合导航框架。采用DGP-UKF框架进行轨迹预测,即使数据已知先验信息较少时,DGP能增加模型复杂程度,针对于数据量较少的情况有效防止过拟合,相对于深度神经网络极大提高了预测精度。同时,对于GPS中断时间较长的轨迹预测,DGP模型预测结果较深度神经网络误差更小,具有更好的鲁棒性和可靠性。
(2) 针对传统深度神经网络可解释性弱的问题,概率模型DGP能通过一种变分推理的方法求解出定位位置的置信区间,同时给出精确的证据下界计算方法,为基于深度学习模型的数据融合预测方法提供了严格的理论推导和可解释性说明。
基于DGP的无人车多元未知环境的导航问题,就是在GPS信号可用时,根据SINS与GPS信号之间的关系进行DGP模型训练[31];在GPS信号失效时,利用网络模型对位置信息进行预测,再将预测的伪位置信息与SINS构成组合导航模型。故无人车导航定位的建模应包括位置解算和组合导航系统位置估计两部分。
在“东-北-天”(简称E-N-U坐标系)坐标系下,无人驾驶车辆的位置计算如下:
(1)
在地理系的比力可由测量比力及旋转矩阵计算得到。
(2)
--------------------
(3)
--------------------
构建组合导航系统,首先建立15维的状态方程与观测方程为
(4)
式中:X为状态向量;F为系统矩阵;G为系统噪声阵;W为过程噪声向量;Z为观测向量;H为观测矩阵;V为观测噪声向量。其中,状态变量X选取为
(5)
(6)
(7)
(8)
实际无人车导航过程中应用离散UKF,故将式(4)离散化:
(9)
设定初始化值为
(10)
根据无迹变换(unscented transform,UT)变换原理,可得到时间更新方程为
(11)
无迹卡尔曼量测更新如下:
(12)
滤波更新方程为
(13)
本节主要建立了无人车导航定位模型,针对位置解算和GPS/INS组合导航位置估计模型应确定与车辆位置存在函数关系的参量,构建非参信息融合GPS信号预测模型,为无人车导航定位提供精准的导航信号。
本节重点介绍了基于DGP的无人车辆导航系统设计,首先搭建基于GP的GPS预测模型,进而将网络叠加构成DGP完成GPS信号预测网络设计。
(14)
式中:p(f,u;X,Z)表示高斯过程的先验概率;p(yi|fi)为似然函数;p(f|u;X,Z)表示输入为目标点X及诱导点Z的u的条件下f的概率且满足p(f|u;X,Z)=N(f|μ,Σ);p(u;Z)表示输入为Z的u的概率且满足p(u)=N(u|m(Z),k(Z,Z)),其中μ,Σ满足:
[μ]i=m(xi)+α(xi)T(u-m(Z))
(15)
[Σ]ij=k(xi,xj)-α(xi)Tk(Z,Z)α(xj)
(16)
式中:α(xi)=k(Z,Z)-1k(Z,xi)。
针对于较大量数据时的高斯过程求解复杂,故采用一种变分推理法进行求解。即通过求解KL(Kullback Leible)散度的最小值,寻找概率q(f,u)近似代替高斯过程的联合概率密度p(y,f,u)。同时根据等价性原则,可将求解KL散度的最小值转化为求解边缘似然函数证据下界的最大值为
(17)
p(y,f,u)求解如式(17)所示,变分后验概率q(f,u)表达式如下:
q(f,u)=p(f|u;X,Z)q(u)
(18)
式中:q(u)满足高斯分布且q(u)=N(u|m,S)。
由于后验变分的两项都服从高斯分布,可将u边缘化得到q(f|m,S;X,Z):
(19)
μm,Z(xi)=m(xi)+αT(xi)(m-m(Z))
(20)
ΣS,Z(xi,xj)=k(xi,xj)-αT(xi)(k(Z,Z)-S)α(xj)
(21)
变分后验边缘概率fi取决于输入xi,故可将边缘概率q(fi|m,S;X,Z)改写为
q(fi|m,S;X,Z)=q(fi|m,S;xi,Z)=
N(fi|μm,z(xi),ΣS,Z(xi,xi))
(22)
使用变分后验概率式(18)和证据下界式(17)可得到简化的证据下界:
(23)
为了进一步提升GPS预测模型精度,弥补单层GP的模型缺陷。在本节中,将第2.1节中GP模型进行叠加,构建多层深度GP,根据GP的y,f,u的联合概率分布式(18),类似地,可以得到DGP的联合概率分布:
(24)
式中:l为第l个GP层。
DGP的层数增加时,无法通过非线性协方差函数求得解析解,假设层与层之间的后验概率分布是可因式分解的,参考GP中的式(18)变分推理求得的DGP后验概率:
(25)
式中:q(Ul)为高斯分布且满足q(Ul)=N(ml,Sl)。
类似单层的GP将每一层的诱导变量边缘化,可以得到在层内与层间完全耦类似单层的GP将每一层合的分布。
(26)
类似GP的证据下界式(17),推断出深度GP的证据下界为
(27)
结合式(24)和式(25),并代入式(27),可得到简化的DGP证据下界为
(28)
总结GPS信号预测的两个模型,绘制GP/DGP-KF算法流程图如图1所示。
图1 GP/DGP-UKF组合导航框架流程图
本文首先采用德国卡尔斯鲁厄理工学院和丰田美国技术研究院联合创办的车辆导航数据集KITTI进行实验[11],KITTI数据集包含了对驾驶车辆的GPS信号及惯性测量单元(inertial measurement unit, IMU)参数,IMU包括三轴加速度计和陀螺仪。安装在车辆中后轮轴中间上方,GPS接收机天线安装在无人车辆车顶。GPS/IMU输出频率为100 Hz。软件开发平台为:Python3.7+Pytorch1.10.2。训练硬件平台为CPU:i7-7700k。
为了进一步验证DGP方法在GPS信号失锁时信号预测的优越性,本节利用GPS有效时的数据进行训练,利用典型深度神经网络长短期记忆(long and short term memory, LSTM)网络进行对比实验,同时构建GP和DGP模型进行验证实验。由于IMU频率较高,时间迭代次数较多,网络层数较多会导致过拟合及训练速度较慢现象发生,因此将LSTM与DGP网络层数设置为2。且为了验证DGP的优势,保证所有参数尽量一致。3种模型的所有网络参数如表1所示。将训练好的GP/DGP及对比LSTM模型离线保存,调用模型进行在线预测验证,模型训练及预测的详细过程如图1所示。
表1 3种模型及模型参数
选取GPS信号中断50 s数据集进行在线测试验证,假定数据集中GPS数据中断50 s,利用GP/DGP及LSTM模型进行GPS数据在线预测,并与数据集中真实GPS信号进行比较,其表现结果如图2~图4所示。同时,考虑模型在线预测时间,理论上GP模型在线预测运算时间主要来自于式(15)和式(16)中矩阵k(Z,Z)求逆运算。由式(19)可知,GP的在线预测运算时间复杂度为O(n3);DGP模型时间复杂度为N×O(n3),其中n为诱导点数目,N为DGP模型深度;实际中测量所用硬件GP/DGP更新一次GPS预测值所需时间。由于GP/DGP两种模型GPS更新速率较快,故测量更新100次GPS值所耗时间tGP=0.49 s,tDGP=0.95 s。由于GPS/IMU数据输出频率为100 Hz,故输出100次GPS量测值共需1 s。根据真实时间测量可知DGP模型的GPS更新频率高于GPS/IMU量测频率,故DGP模型可以在GPS失效时与IMU量测信息构成DGP-KF框架估计车辆导航信息。
图3 GP模型测试效果图
图4 DGP模型测试效果图
将3种模型进行保存,累加预测的经纬度增量,可以得到无人车轨迹的经纬度变化值,实验结果通过4种方法进行对比分析:① 使用纯IMU惯导输出的数据;② 使用LSTM-UKF模型进行预测;③ 使用GP-UKF模型进行预测;④ 使用DGP-UKF模型进行预测。选择采样时间为50 s进行轨迹预测如图5所示。
图5 3种模型预测轨迹与真实轨迹
使用交叉验证的方法重复实验5次,取平均值来验证模型最终拟合结果,将定位误差值累积来度量比较3种定位方法的模型性能。同时,将包含经纬度信息的轨迹值转换成东向和北向误差如图6所示。
图6 3种模型预测东北方向误差
由于DGP误差较小,故截取部分采样点观测误差,通过实验结果可以看出通过IMU直接解算的误差值较大,在出发时间9 s后误差率达到35.34%,相比于由IMU直接解算,采用3种模型可在不同程度上提高整个驾驶车辆过程中车辆导航定位的准确性。
通过实验结果可以看出,INS直接输出数据在出发后误差迅速发散,相比于直接采用惯性元件结算出的导航信号。另外,3种对GPS信号进行估计的方法效果更好。LSTM网络无法根据模型拟合的置信区间来判断模型的可靠性对模型参数进行调整,故在拟合过程中无法保证所有预测值均在预测位置的置信区间带中,模型定位会产生较大误差。GP模型较DGP模型简单,拟合效果有限,无法达到最优的预测效果。DGP网络能增加模型复杂程度,针对于数据量较少的情况有效防止过拟合,同时能保证在车辆运动发生较大变化时预测模型仍在置信区间中,能有效提高精度,相较于LSTM网络定位误差率降低为40.19%。
选取同一起点的不同GPS信号中断时长进行实验,并将其转化为东北方向的误差变化其实验结果如表2所示。
表2 LSTM-UKF/GP-UKF/DGP-UKF3种模型误差比较
将训练好的LSTM-UKF、GP-UKF、DGP-UKF模型保存,并设置GPS信号失效时间不同时间的对比实验验证模型GPS信号失锁较长时间时模型的预测能力。设置GPS分别信号失锁100 s、200 s及500 s,此时3种模型定位误差如表2所示。可以看出,随着GPS信号失锁时间较长,LSTM模型无法利用模型不确定程度进行修正,GP模型拟合能力有限,本文提出的DGP-UKF组合导航框架在GPS失效较长时间内能始终保持在较高精度。随着GPS失锁时间增大,模型误差增大。直至GPS信号失锁500 s时,原有INS/GPS-UKF先验信息缺失最多。由于DGP能够增加模型复杂程度,且LSTM模型拟合能力有限,故DGP-UKF较LSTM-UKF具有较好的预测效果。通过计算可知,相较于LSTM-UKF框架,DGP-UKF定位精度提高40.19%,相对于GP-UKF定位精度提高28.96%。由表1和表2可以看出,GP和DGP区别于LSTM为非参数模型,可以学习任意类型的函数,提高训练精度。对于GPS中断时间较长的轨迹预测,DGP-UKF模型也表现出较好的预测能力,较深度神经网络预测误差更小,具有更好的鲁棒性和可靠性。
本文针对无人车在多元未知环境中的轨迹预测问题,构建了GPS信号的轨迹预测模型,实现了无人车在复杂环境中的精准导航定位。考虑DGP的非参数优势,提出了一种基于DGP的组合导航轨迹预测方法,该方法具有抑制过拟合、精度高、可靠性好的特点,通过仿真说明了提出的DGP-UKF算法可以有效解决无人车面对未知环境的导航问题。引入DGP方法进行辅助导航在预测过程中计算定位轨迹置信区间,为基于深度学习的数据融合组合导航问题提供了严格的理论推导和可解释性支持。通过交叉验证可以得到500 s信号中断时,DGP相较于纯惯导解算精度提升97.32%,相较于LSTM精度提升40.19%,相较于GP精度提升28.96%,能够实现更精确的导航定位,具有更好的鲁棒性和可靠性。