双目视觉辅助GNSS在恶劣环境下导航定位

冯祎,涂锐,3,韩军强,侯福荣,洪菊,刘金海,王星星

(1.中国科学院国家授时中心,陕西 西安 710600;2. 中国科学院大学,北京 100049;3. 中国科学院精密导航定位与定时技术重点实验室,陕西 西安 710600)

0 引 言

随着全球卫星导航产业的快速发展,全球卫星导航系统(GNSS)因可提供精密的位置、速度和时间信息得到了广泛的应用．当在室外开阔的环境中时,GNSS具有高可用性和很好的精度,但是在城市峡谷、隧道等信号受到遮挡的恶劣环境中,GNSS位置估计误差会明显增大,结果甚至变得不可用．以往,学者们通过惯性导航系统(INS)辅助来改善GNSS的定位精度和可用性[1],随着计算机图形处理技术以及同步定位与建图(SLAM)技术的快速发展,视觉里程计(VO)导航也成为一种新的导航定位方式,在机器人、无人机和自动驾驶车辆的多传感器融合导航定位技术中得到了广泛的应用．视觉里程计通过单目相机、双目相机和深度相机以及组合导航系统获取载体运动状态,实现导航定位功能．但是长时间单独使用视觉里程计误差较大,因此常与INS和GNSS等导航技术融合进行导航[2]．视觉里程计在导航过程中还能感知周围的环境,识别周围环境中的标志点,构建周围环境的3D地图,用于自动驾驶车辆和无人机等导航[3]．因此许多学者研究视觉里程计和GNSS组合系统,Dusha[4]用类似经典的GNSS/INS松耦合滤波器提出了GNSS和单目相机的松耦合模型,并使用一个俯瞰的飞行器实验结果证实了其可行性．Schleicher[5]利用低成本GNSS和双目相机实现了重复轨迹下实时的自动车辆导引．Aumayer[6]证实了在恶劣环境下GNSS定位精度变差时,双目相机对GNSS定位精度具有明显的改善作用．

V-SLAM(Visual SLAM)是指通过相机获得的图像估计自身的位姿,同时构建周围环境地图的技术．V-SLAM根据前端的跟踪方法,可以分为直接法和特征点法:直接法根据图像的某些像素或者全部像素不变求取帧间的位姿变化,经典算法有SVO[7]算法和LSD-SLAM[8]算法;特征点法提取图像的特征点,并通过匹配前后图像的特征点来估计位姿变化,常用算法有PTAM[9]算法和ORB-SLAM[10]算法．视觉里程计在不需要初始环境信息下,能够在小范围和短时间内实现自我运动轨迹的高精度估计．但是在复杂环境和长时间情况下其位置估计会有较大误差,特别是在高速运动或者旋转运动时．

本文介绍了GNSS和视觉里程计的松组合模型和方法,并通过KITTI[11]标准数据集模拟验证了当GNSS信号中断时,利用视觉里程计短时间高精度的特性进行持续导航定位,本文延续前人的工作,研究了GNSS信号中断时组合导航系统的持续导航性能,验证了组合系统在恶劣环境下能够持续地提供导航定位．

1 系统整体框架及松组合滤波器设计

1.1 视觉里程计导航定位原理

相机属于光学传感器,其采集得到的图像会产生失真的效果,需要经过畸变校正后才能进行视觉前端处理,本文采用特征点法的视觉前端处理方法．提取图像的ORB[12]特征点进行跟踪,经过剔除误匹配点之后,利用匹配成功的特征点进行位姿R和t的估计．位姿估计中涉及到三个坐标系,其中像素坐标系原点位于图像左上角,u轴和v轴方向分别向右和向下;相机坐标系原点位于相机光心,Z轴向前,X轴和Y轴方向分别与u轴和v轴平行;世界坐标系原点位于相机起始位置的光心,坐标系方向与相机坐标系起始时相同,在求得特征点在真实世界中的深度后,就可以确定3D点在世界坐标系下的坐标．像素坐标系下的点(u,v)和相机坐标系下的点(X,Y,Z)的关系以及世界坐标系下的点Pw到像素坐标的投影关系如式(1)～(2)所示,其中K为相机内参矩阵,fx和fy是与相机焦距相关的参数,可以通过相机标定得到．cx和cy为图像中间位置的像素坐标,P为相机坐标系下的坐标,T为写成齐次形式的变换矩阵．

(1)

(2)

同时在这个过程中还进行着地图构建和回环检测,地图构建模块不断地插入关键帧,生成利用三角化产生的新的3D点并进行筛选,然后加入到局部地图中,用于后续的特征点跟踪和优化估计部分．回环检测模块通过Bag of Words(BOW)算法,检测是否为闭环,对结果进行优化,提高SLAM算法的精度和鲁棒性．

1.2 视觉里程计和GNSS结果处理策略

在相机运动过程中,由于特征点太少或者特征点变化剧烈等原因,视觉里程计会产生位姿误差,并随着时间积累．GNSS常用的定位方法有伪距单点定位技术,精密单点定位技术和实时动态差分(RTK)定位技术．其中RTK能够得到毫米级定位精度并且误差不会随着时间积累,但是在恶劣环境多路径效应明显时,精度会衰减到分米级甚至米级．式(3)为单频RTK模型,

(3)

视觉里程计短时间内具有较高的精度,基于GNSS和视觉里程计优劣互补的特性,假设位置时间序列:

UG(t)=u(t)+EG(t),

(4)

UV(t)=u(t)+EV(t).

(5)

式中：UG(t)和UV(t)分别为RTK和视觉里程计得到的位置时间序列；u(t)为真实的位置时间序列；EG(t)和EV(t)分别为RTK和视觉里程计的位置结果误差．由于RTK能够达到厘米甚至毫米的精度,对于不要求特别高精度的情况下,省略掉EG(t)．通过将UG(t)和UV(t)作差,

Udif(t)=UV(t)-UG(t)

=EV(t)-EG(t)≈EV(t).

(6)

因为EG(t)≈0,在松组合系统中将差值Udif(t)作为确定EV(t)的观测值,EV(t)能够根据其时间序列进行插值或者进行外推,当GNSS信号受到遮挡或者中断时,可以通过式(7)来估计实际的位置序列u(t)．

u(t)=UV(t)-EV(t).

(7)

1.3 松组合系统设计

设计一个松组合卡尔曼滤波器来组合GNSS和视觉里程计的位置速度结果,如图1所示,流动站数据为载体上的接收机采集的GNSS观测值,通过与附近已知的基准站的观测值差分处理,使用RTK定位技术得到载体的位置速度信息．

图1 松组合系统流程图

视觉里程计自运动估计是以相机光心为原点,通过估计前后帧之间的相对位姿变化得到的整体位姿,在双目相机中位姿估计原点位于双目相机光心的连线中点．为了将视觉里程计和RTK的估计结果进行组合,将视觉里程计的定位结果通过迭代最近点(ICP)算法变换到和RTK同一东北天(ENU)坐标系下．

根据运动学模型来预测新的状态,设系统的运动误差状态向量为

(8)

式中：δrE、δrN和δrU分别表示东北天方向上的位置误差；δVE、δVN和δVU分别表示东北天方向上的速度误差.系统模型定义为

(9)

式中:G(t)为噪声驱动阵,给定为6×6的单位矩阵;w(t)为过程激励噪声,其统计特性为

E[w(t)]=0,

(10)

E[w(t)wT(τ)]=Q(t)δ(t-τ).

(11)

式中：Q(t)为过程激励噪声的协方差矩阵,tk时刻离散值记为Qk；F(t)根据运动学模型表示为

(12)

推导得到k到k+1时刻的状态转移矩阵Φk+1,k为

(13)

式中,Δt为相机采集图像的时间间隔.

观测方程为

Zk=Hkxk+Dk.

(14)

通过将同一坐标系下的RTK结果和视觉里程计结果相减作为观测值,其中Dk为观测误差,其统计特性满足:

E[Dk]=0，

(15)

(16)

式中，Rk为观测噪声协方差矩阵,取为KITTI数据集中给出的RTK位置和速度精度．观测值向量Z和观测设计矩阵H分别为

(17)

(18)

式中：r和V分别表示位置和速度；上标G和V分别表示GNSS和视觉里程计的结果．

因此k-1到k时刻的观测方程为

(19)

状态一步预测方程为

(20)

一步预测后的状态协方差矩阵:

(21)

卡尔曼滤波增益为

(22)

因此状态估计方程为

(23)

式中,上标-和+表示量测更新前后,经过量测更新后的状态协方差矩阵为

(24)

2 模拟实验

2.1 实验平台

KITTI数据集采集车的传感器包括有两对平行对齐并朝前方的灰点相机,可以分别采集灰度图像和彩色图像,灰度图像的采集频率为10 Hz,分辨率为1242像素×375像素．数据采集车在住宅区和高速公路等区域行驶,采集了多段原始数据,我们采用较长和有代表性的四段数据进行模拟实验．

2.2 实验结果演示与分析

本文使用了上述采集车采集的两段住宅区和两段高速公路数据．为了说明实验过程和效果,选定一段住宅区的数据,其中采集车行驶了110 s大约1 233 m;为了展示组合系统在恶劣环境的持续导航性能,模拟了两个GNSS信号中断的实验:1)在结尾处(80 s-110 s)GNSS信号中断了30 s,利用卡尔曼滤波算法和组合系统进行持续导航；2)中途(30 s-60 s)信号中断30 s后GNSS信号又恢复,利用卡尔曼滤波平滑算法和组合系统进行导航．本节的末尾还展示了其余三组数据视觉里程计定位结果误差和模拟实验结果．

将视觉里程计,松组合结果与RTK位置结果在采集车整个行驶路段上进行做差比较,统计其误差特性．图2示出了视觉里程计和RTK参考值的定位结果误差,可以看到误差随着时间积累迅速,可能是汽车行驶速度太快,特征点变化剧烈导致视觉里程计的误差较大．观察松组合结果看到,开始几秒收敛后,松组合结果和参考值相差在分米级．在模拟GNSS信号中断后(假定在80 s-110 s共600多米路径中无GNSS观测值),利用松组合状态方程继续推导视觉里程计的误差,然后利用视觉里程计的短时间定位结果来推导实际的位置．可以看出,组合系统不仅满足持续导航,而且性能相比单视觉里程计结果得到显著提升,U方向误差从原来的8.5 m降低到0.2 m,E方向误差从4.3 m降低到0.5 m,N方向误差从9.5 m降低到0.5 m．其中N方向比E方向的视觉里程计定位误差大,这是由于采集车朝着N方向行驶了约500 m,而只朝着E方向上行驶约49 m,N方向相比E方向速度快,且N方向上的特征点变化迅速且明显,在左右图像、前后帧特征点匹配时,N方向上就会多发生误匹配而造成N方向上误差明显大于N方向．中途30 s-60 s GNSS信号中断时,考虑到中断两端的GNSS信号是完好的,使用卡尔曼滤波平滑算法,从左侧向右侧计算后再从右侧平滑到左侧,经过平滑后,误差呈现出两头小中间大的趋势,并且在30 s大约358 m的路径中E方向误差最大约2.7 m,N方向误差最大约2.0 m,U方向误差最大约3.2 m,显著优于单视觉里程计结果．

(a) 视觉里程计定位 (b)GNSS信号不中断时松组合

(c)80 s-110 s GNSS信号中断时松组合 (d)30 s-60 s GNSS信号中断时松组合

图3示出了其余三组的实验结果,其中第一组为住宅区的结果,后两组为高速公路的视觉里程计定位误差和模拟GNSS中断时的持续导航结果．总体比较可以看出,组合系统在遮挡条件下可以保持较高精度的持续导航,同时可以看到除U方向上的误差较大外,E和N方向上的持续定位能力在米级精度．此外,与住宅区相比,高速道路的特征点较少,且大多数是树木及防护栏,容易发生误匹配造成较大定位误差,并且在高速道路上汽车行驶速度较快,特征点变化剧烈,也会造成视觉里程计较大定位误差,而组合系统刚好可以相互补充,实现高精度的持续导航．

图3 视觉里程计定位误差(顶端)和GNSS中断时组合导航的定位误差(中部、底端)

3 结 语

本文使用KITTI采集车采集的4段校正后的图片和RTK结果数据,通过ORB-SLAM算法计算了视觉里程计的定位结果,并和RTK结果进行比较说明了组合系统的持续导航定位能力．还利用视觉里程计和RTK结果进行松组合估计了视觉里程计随着时间的误差变化,并模拟了GNSS信号中断时,利用状态方程外推视觉里程计误差,然后利用视觉里程计的定位结果估计实际的定位结果．实验表明,视觉里程计的定位结果误差较大,GNSS和视觉里程计的组合系统能够很好地处理恶劣环境下GNSS信号遮挡或中断的问题,但是长时间的导航定位GNSS系统还是必需的．本文的松组合系统使用了GNSS的RTK结果,但是GNSS得到的常常是单点定位结果,精度只能达到米级且恶劣环境下会有几十米误差,此时视觉里程计能短时间很好地改善GNSS的定位结果,在载体周围特征点明显和速度不快时能较好地实现持续定位功能．将来,考虑使用GNSS原始观测值,组建一个紧组合系统利用视觉里程计结果改善单点定位的定位精度然后实现恶劣环境下的持续定位．

致谢:感谢卡尔斯鲁厄理工学院开源的KITTI数据．