基于GNSS/SINS/双目视觉里程计的车载导航系统分析与设计

冯黎 郭承军

（电子科技大学，电子科学技术研究院，成都 611731）

主题词：多传感器融合 无人车 双目视觉里程计 冗余 相关性未知 协方差交叉 分散式架构

1 前言

捷联式惯性导航系统（Strap-down Inertial Navigation System，SINS）具有短期定位精度与数据采样频率较高的特点，可以与全球导航卫星系统（Global Navigation Satellite System，GNSS）形成组合导航系统。但基于GNSS 和惯性测量单元（Inertial Measurement Unit，IMU）的传感器融合定位仍达不到无人车高精度定位的要求。除了GNSS 和IMU，无人车一般还配备有激光雷达、相机和微波雷达等多个传感器，通过冗余配置可有效提高导航信息的准确性和可靠性，如何利用多个传感器资源，进行无人车多传感器融合导航定位是目前研究的热点。文献[1]实现了基于GPS/航位推算（Dead Reckoning，DR）/视觉的无人车导航，当GPS信号受到遮挡时，采用视觉导航，当GPS 信号正常时，采用GPS/DR组合导航。Suhr 等基于粒子滤波器融合数字地图以及GNSS、IMU、轮速传感器和单个前置摄像头的输出，实现城市环境下的高精度自定位[2]。

现有的方案没有考虑多个传感器数据的相关性，例如，安装在同一车辆上的一组导航传感器的观测噪声可能由于车辆运动的变化而彼此相关，但现有研究尚未识别出这种相关性。在无人车即时定位与地图构建（Simultaneous Localization And Mapping，SLAM）中，车辆估计其环境中的地标位置时，同时使用这些地标更新其自身的位置估计，车辆和地标的位置估计变得高度相关[3]。此外，现有的方案也没有可以扩展多个传感器的无人车多传感器融合架构，这一架构应该是分散式的[4]，单个节点的丢失或错误不会妨碍整体运行，并且可以即插即用地配置传感器节点。同时，需考虑各节点的相关性，基于各节点的融合结果应具有一致性[5]。

针对上述问题，本文设计基于SINS、GNSS 和双目视觉里程计的无人车分散式融合架构，并在此基础上，根据节点信息的相关性，在卡尔曼（kalman）滤波框架下，采用相应的协方差交叉算法融合其他节点信息，并基于KITTI数据集进行仿真分析。

2 无人车分散式融合架构算法

2.1 卡尔曼滤波算法与协方差交叉算法

系统状态空间模型为：

式中，Xk为系统在k时刻的状态；Zk为对应状态的观测值；Wk为输入的白噪声；Vk为观测噪声；A为状态转移矩阵；B为噪声驱动矩阵；H为观测矩阵。

卡尔曼滤波器的操作包括两个阶段：时间更新与测量更新。时间更新方程与测量更新方程分别为[6]：

式中，Q为系统噪声方差阵；R为测量噪声方差阵；I为适维单位矩阵；Pk为Xk的协方差矩阵；Xˆk为状态估计值；Kk为增益矩阵。

考虑一类非常普遍的数据融合问题：将2条数据信息A和B融合，得到输出C，并且A和B真实统计数据及互协方差未知，唯一可用的信息是A和B的均值和协方差的估计。对此，Simon Julier和Jeffery Uhlmann 提出了避免计算未知的互协方差的协方差交叉（Covariance Intersection，CI）算法。协方差交叉融合给出融合估计实际方差的一个上界，即保证融合估计比经典Kalman方法更具有一致性和鲁棒性，提高了融合的精度，避免融合估值器的发散[7]。

相关性已知的相关估计量最优融合为[8]：

相关性未知的相关估计量最优融合是协方差的凸组合：

式中，a、b和c分别为A、B和C的均值；Paa、Pbb和Pcc分别为A、B和C的协方差；Pab和Pba为A、B的互协方差；ω∈[0,1]。

2.2 无人车分散式融合架构算法

以SINS为参考系统，分别与GNSS和视觉里程计构成节点，节点间通过通信链路相连，每个节点设计1个局部滤波器和主滤波器，具体结构如图1所示。局部卡尔曼滤波器1采用SINS与GNSS的位置和速度之差作为观测值，局部卡尔曼滤波器2采用SINS与视觉里程计解算的位置和速度作为观测值，使用标准的卡尔曼滤波器得到估计值并传播到其他节点。对于每个节点的主滤波器，在卡尔曼滤波框架下，根据信息的相关性进行CI融合，再将融合结果进行观测更新，进而求得最优估计。

图1 多传感器融合架构

考虑节点1从k时刻到(k+1)时刻的融合算法，其融合过程为：节点2主滤波器得到k时刻节点2的状态估计和协方差；节点2的局部滤波器更新得到(k+1)时刻的状态估计和协方差，将其传播至节点1；节点1主滤波器使用卡尔曼滤波器预测方程得到状态估计和协方差；由于上2个步骤的状态量使用同一个SINS的输出值，具有相关性，选用相关性未知的CI算法融合上2个步骤的状态估计和协方差（否则选用相关性已知的CI算法）；根据节点1的GNSS的观测值使用卡尔曼更新方程对上一步的结果进行更新，得到节点1在(k+1)时刻的状态估计。

由上述过程可知，每个节点先将其他节点信息与本地预测进行CI融合，再对融合结果进行观测更新，这样保证了每个节点都利用了全局的信息，由此得到全局的估计。因此，本文设计的多传感器分散式融合架构中，主滤波器1和主滤波器2均为系统的滤波输出，这种滤波结构相比于集中式滤波有更高的灵活性和容错能力。

3 状态模型与观测模型

3.1 双目视觉里程计导航解算

传统的里程计在不光滑的表面运动时，结果往往不可靠，且误差随时间的累积逐渐增大。视觉里程计通过对图像特征点提取、匹配与跟踪，进而估计出运载体的位姿信息的过程不受道路表面的影响，并且具有更高的灵活性和精度。视觉里程计可以分为单目视觉里程计、双目视觉里程计和多目视觉里程计。单目视觉里程计计算相对简单，但其缺少绝对尺度信息。双目视觉里程计利用不同位置的2个摄像机模拟人类的双眼，可以计算出图像中物体的三维信息。普遍采用的双目视觉里程计计算流程如图2所示。

图2 双目视觉里程计计算流程

基于双目视觉里程计[9]解算位置和速度的流程为：

a.获取图像对Ik、Ik+1，并对图像进行畸变校准。

b.计算时刻k和(k+1)的视差图。

c.使用加速分割测试特征（Features from Accelerated Segment Test，FAST）算法检测图像的特征并进行特征匹配。

d.选取使得所有匹配均互相兼容的点云数据（上一步中匹配特征点的3D坐标值）。

e.为了确定旋转矩阵R和平移向量t，使用列文伯格-马夸尔特（Levenberg-Marquardt，L-M）非线性优化方法最小化重投影误差ε：

式中，fk、fk+1分别为k时刻和(k+1)时刻左图像中的特征；jk、jk+1分别为fk、fk+1的二维齐次坐标；wk、wk+1分别为fk、fk+1的三维齐次坐标；P为3×4左摄像机投影矩阵；T为4×4齐次变换矩阵；ε为重投影误差。

f.双目视觉姿态估计、位置估计、速度估计分别可以表示为[10]：

3.2 SINS、GNSS及双目视觉里程计多源融合模型

惯性导航系统误差方程由平台误差角方程、位置和速度误差方程及陀螺仪和加速度计误差方程组成。综合这些误差方程，可以得到SINS的状态方程：

式中，W=[wgx wgy wgz wbx wby wbz wax way waz]T为噪声矢量；wgx、wgy、wgz为陀螺仪白噪声；wbx、wby、wbz为陀螺仪随机常值漂移噪声；wax、way、waz为加速度计随机噪声；F为状态转移矩阵，其选取参考文献[11]；G为误差系数矩阵，其选取参考文献[11]；ΨE、ΨN、ΨU分别为平台东向、北向、天向的误差角；ζvE、ζvN、ζvU分别为载体在东向、北向、天向的速度误差；ζL、ζλ、ζη分别为纬度、经度和高度误差；εbx、εby、εbz分别为陀螺仪的随机常数漂移；εrx、εry、εrz为马尔科夫漂移；Δx、Δy、Δz为加速度一阶马尔科夫漂移。

取SINS 与GNSS 输出的速度和位置之差作为量测量，得到SINS与GNSS组合的量测方程：

式中，V1(t)为GNSS量测的位置和速度误差；Zp1为位置量测矢量；Zv1为速度量测矢量；观测矩阵H1为：

式中，RM为量测点沿北方向的曲率半径；RN为量测点沿东西方向的曲率半径；L为量测点的纬度。

取视觉里程计解算位置、速度与SINS 输出的位置和速度之差作为量测量，量测方程为：

式中，V2(t)为双目视觉里程计量测的位置和速度误差；Zp2为位置量测矢量；Zv2为速度量测矢量；观测矩阵H2为：

4 仿真验证分析

KITTI 数据集由德国卡尔斯鲁厄理工学院和丰田美国技术研究院联合创办，是目前国际上最大的自动驾驶场景下的计算机视觉算法评测数据集。KITTI 的原始数据（Raw Data）提供以10 Hz 同步的GPS、IMU 数据（位置、速度、加速度等）和双目图像序列，各传感器坐标系如图3所示。

图3 KITTI车载传感器安装示意[12]

本文选用数据集住宅集中道路中的一段数据进行验证，该组数据中含有803帧图像，分辨率为1242像素×375 像素。以KITTI 数据集提供的GPS 经纬度为真值，分别在经度和纬度的真值上加入标准差为5 m的白噪声作为GPS量测位置值。在KITTI数据集提供的东向速度和北向速度各加入标准差为0.1 m/s 的白噪声作为GPS量测的速度值。图4所示为该组数据的真实轨迹。

图4 数据集中自主车的真实运动轨迹

4.1 双目视觉里程计验证

首先进行双目视觉里程计的解算，图像序列左、右2个相机各有803帧图像。图5给出了双目视觉里程计得到的内点数的分布图，由图5可知，绝大多数分图像均能得到50个以上内点，双目视觉里程计具有良好的性能。

4.2 无人车分散式融合架构算法验证

为了验证本文设计的基于GNSS、SINS 和双目视觉里程计的无人车多传感器融合架构算法，本文将该算法与惯性导航解算算法和视觉里程计解算算法得到的位置和速度进行比较。惯性导航解算算法将前一时刻的导航参数（姿态、速度和位置）作为初值，利用前一时刻至当前时刻的惯性器件采样输出，解算当前时刻的导航参数，作为下一时刻惯导解算的初值，如此反复不断。本文的融合算法由于采用分散式的架构，节点1（GNSS与SINS融合）与节点2（视觉里程计与SINS融合）均会得到全局的融合结果，这里取节点1的结果进行对比。图6～图8所示分别为真实轨迹的经纬度、真实的速度，图9～图10为各种解算方式的东向和北向的位置和速度误差。

图5 双目视觉里程计得到的内点数的分布结果

图6 运动轨迹

图7 真实东向运动速度

由图9可以看出，基于惯性导航解算和双目视觉里程计解算的轨迹在运动起始的一段时间内偏差较小，但是随着运动的继续，偏差逐渐增大。由图10可以看出，在有一定数量的特征点时，双目视觉里程计解算的速度具有较高的精度，在第53 s左右由于特征点不足产生了较大误差，这是视觉里程计普遍存在的问题。因此，单一的导航系统难以为无人车提供稳定、高精度的导航。

图8 真实北向运动速度

图9 位置误差

图10 速度误差

根据图9 和图10 的误差曲线，表1 统计了SINS 解算、双目视觉里程计解算和本文融合算法解算的最大误差对比。表2给出了节点1和节点2的位置和速度的均方根误差，可见2 个节点都具有良好的性能，能得到相似的全局估计，也验证了分散式融合架构的有效性。

从上述仿真结果可以看出，基于GNSS、SINS 和双目视觉里程计的无人车多传感器融合架构算法解算的导航信息更加精确，并具有更好的鲁棒性。

表1 各种解算方法最大误差对比

表2 节点估计均方根误差

5 结束语

本文实现了双目视觉里程计的位置和速度解算，利用双目视觉里程计和GNSS量测的位置和速度，推导了SINS、GNSS和双目视觉里程计的多源融合模型，并设计了分散式融合架构算法。同时，针对分散式融合节点信息的相关性未知的问题，在卡尔曼滤波框架下，采用协方差交叉算法融合其他节点信息，提升导航系统的鲁棒性。在基于KITTI数据集上的验证结果表明，本文设计的分散式融合架构算法具有更高的导航精度和鲁棒性。