北斗卫星定位系统与VSLAM融合的目标定位方法

吕思男,黄家才,高芳征,李毅搏,陈 田

(1. 南京工程学院工业中心、创新创业学院, 江苏 南京 211167;2. 南京工程学院自动化学院, 江苏 南京 211167;3. 南京工程学院机器人与系统集成研究所, 江苏 南京 211167)

随着国内外四足机械狗研究的发展,仿生机械狗在军事储备、警戒巡检、抗震救灾、科学研究等领域的作用备受关注[1].目前国内对机械狗的运动控制研究趋成熟,但对其定位研究相对较少,足式里程计定位误差较大.因此,本文通过融合定位提高机械狗定位精度,在此基础上,再实现目标定位.不仅可以获取机械狗的位置坐标,还能获取机械狗视觉中目标物体的坐标,更好地实现仿生机械狗在抗震救灾、警戒巡检等场景中的应用[2-3].

同步定位与建图(simultaneous localization and mapping,SLAM)技术自1986年开始,先后经历了贝叶斯估计理论、统计学框架描述观测路标与特征点的相关性以及基于图优化等方法以实现在未知环境中定位到自身位置[4-7],但是在大范围环境中存在累计误差.卫星定位技术没有累积误差,其中 全球定位系统(GPS)定位精度为10～15 m、北斗卫星导航系统(BDS)定位的实测精度(按中误差算)为4～5 m,且BDS的覆盖范围更广阔[8].但在有遮挡或者商场等环境,卫星定位系统无法进行精准定位.卫星定位与SLAM 的定位方法具有鲜明的互补性.文献[9]提出GPS 与 VSLAM 紧耦合的低成本定位方案,在GPS信号差的区域依然能保持系统的鲁棒性,但其定位精度不高;文献[10]引入 GPS信号辅助SLAM进行重定位,减少SLAM定位的误差,但是该方法未将两者的定位信息进行融合,以至于GPS定位信息对SLAM的定位精度提高效果不明显.

基于以上问题,本文提出一种VSLAM与机载北斗导航数据相融合的机械狗视觉目标定位方法,通过分析机械狗移动过程中视觉关键帧图像的特征点数据,将得到的机械狗位姿移动信息与BDS得到的实时地理信息进行融合处理;分析图像帧与帧之间提取的特征点,运用三角化等手段获取三维空间的RT矩阵,与BDS每段时间返回的地理路标数据作信息融合,得到实际的移动距离以及转角方向,最终实现机械狗视觉中感兴趣目标点的查询.在卫星定位信号微弱的环境中可以实现机械狗较高精度的定位,同时解决没有融合其他信息源的情况下基于视觉的SLAM方法无尺度信息的问题,使其定位信息可以应用于真实尺度的环境中.

1 算法设计

算法设计主要步骤流程为(见图1):

1) 单目相机作为主要传感器,采用VSLAM算法对每帧图像进行特征点提取匹配,进行三角化、PNP等运算[11-12],将二维图像信息转换为所需要的三维空间信息;

2) 将从BDS获取到的实时经纬度信息进行数据处理得到每段时间的位移信息;

3) 将单目相机获取的估计距离和BDS获取到的位移信息进行融合处理,得到CGCS2000坐标系下的相机位姿;

4) 对相机中采集到的感兴趣目标点,可以在相邻的关键帧中通过极线搜索匹配算法寻找到相似的特征点,再通过反投影手段获取感兴趣目标点在空间坐标系下的实际坐标,最后利用BDS查询该点的经纬度坐标.

图1 算法设计流程图

1.1 机械狗视觉目标定位的基本原理

相机跟踪过程中,相机位姿定义为欧氏空间中的变换矩阵,旋转矩阵集合Rwc和变换矩阵twc可以构成群:

(1)

式中:SE(3)为在空间上一个连续的群,是一种李群;twc为相机在欧氏空间中的位置,twc=[X,Y,Z]T;Rwc为实数域内的矩阵,Rwc∈R3×3,是相机在欧氏空间中的姿态.

设t0时刻的相机位姿为:

(2)

(3)

考虑到变换前后要保持两点间距离不变,为提高计算速度,可直接进行帧间位姿估计,将李群SE(3)转换为对应的李代数形式:

ξ=[ω1,ω2,ω3,v1,v2,v3]T=[ω,v]T

(4)

式中:ξ为一个六维向量,ξ∈R6;v为相机的平移向量,v=[v1,v2,v3]T;ω为相机的旋转向量,ω=[ω1,ω2,ω3]T.

通过幂指数映射规则把李代数中的元素映射到相应的李群中[13],有:

(5)

将空间中某点由相机坐标系pc=[Xc,Yc,Zc]T映射为像素坐标的投影函数为:

(6)

式中:fx为x轴的相机焦距;fy为y轴的相机焦距;u0、v0为相机主点中心.

给定像素坐标[u,v]T,机械狗视觉内的相机坐标系中,在Zc=1平面上对应的相机坐标系函数为:

(7)

设λ为pc三维坐标点沿行进方向与相机的距离(深度),则相机坐标系pc所对应的有真实尺度的空间坐标系为λpc.

1.2 实时相机位姿估计

由单目相机实现的VSLAM技术中较为重要的有初始位姿估计以及稀疏点云图生成,是视觉里程计的关键工作,其重要组成部分为检测图像序列、相机追踪、运动估计以及定位优化(捆集约束).在初始位姿估计过程中,选取重投影误差小的关键帧矩阵作为两帧图像间的变换.位姿的初始化数值选取t0时刻的关键帧,用于估计相机的初始姿态,再将相邻关键帧的变换连成一条曲线构成机械狗的行走路径,达到定位目的.在相机推算路径阶段,e为当前帧的像素坐标与特征点坐标的差值,则相机的位姿为:

(8)

1.3 关键帧处理与生成

特征点匮乏以及误匹配点的增加会使视觉里程计的误差增加,所以初始选择关键帧时,要选择像素点明显的关键帧并且去除冗余关键帧,相机位姿旋转与平移的权重组合为两帧之间特征点的距离[14],即:

(9)

式中,W为一个对角权重参数矩阵,对定位精度有重要影响.

在关键帧处理过程中,两特征点之间的匹配距离超过权重距离会产生误匹配点,影响相机跟踪轨迹的精度.为减小误差,需要将当前图像帧标记为新关键帧,联合其他关键帧通过BoW加速匹配[15],并由单独的构图线程进行处理,将错位关键帧重新插入到闭环关键帧中.对新关键帧的处理步骤为:

1) 提取新关键帧的特征描述符,将新关键帧插入到回环帧中,计算特征点的BoW关系,用于后续的关键帧快速检索;

2) 将插入的关键帧与相邻关键帧进行特征匹配,验证插入位置;

3) 利用三角法生成新的地图空间点;

4) 联合相机位姿状态对极线匹配进行过滤和联合局部优化,设与当前关键帧特征点相似帧的集合为KL,当前关键帧对应的特征点集合为pL,则联合局部优化公式为:

(10)

1.4 特征点均匀分布策略

图2 特征点均匀分布策略流程

特征点检测大多在形状突变的位置,纹理弱的区域特征点少,这就导致了环境描述子过于单调,感知能力弱,不利于后续对于感兴趣目标点的查询.因此,本文提出一种特征点均匀分布策略,利用四叉树结构改进特征点的检测方法,具体流程如图2所示(图2中L为图像金字塔中图像的层数,W为图像的宽度,H为图像的高度).该策略在提取特征点时,利用四叉树来管理从金字塔图像中提取的特征点,设定可提取特征点的数量阈值Na,按照Harris响应选取数量为Na的特征点[16].

本算法设计了2个特征点检测阈值:每个格子中的初始特征点数量(Vini)和每个格子中的最少特征点数量(Vmin).如果每个格子中的特征点数量小于Vini,则认为该区域纹理较弱,此时按照Vmin检测特征点.Vmin的设置可以保证在纹理弱的区域也能提取到特征点.

1.5 VSLAM与BDS融合

在没有融合其他信息源的情况下,基于视觉的SLAM方法没有尺度信息,这就使得其定位信息无法应用于真实尺度的环境中;而BDS只能获得相机位置.针对两种方法单独使用时的局限性,本文将BDS信息与VSLAM位姿估计融合,帮助SLAM进行闭环检测,获得更精确的位姿来提高其目标定位精度,融合步骤如图3所示.

图3 BDS与VSLAM融合流程图

受BDS更新频率影响,BDS设备与图像帧之间存在时间差tD,将BDS信号触发时间与tD相加实现BDS与VSLAM系统的时间同步,以获取任意图像帧对应的CGCS2000坐标系下粗略的BDS导航坐标.初始化时,选取第1帧图像坐标对应的BDS导航坐标为原点,则北斗导航第j个时刻的位置估计融合误差为:

(11)

对构建地图中相机估计位姿与BDS融合的联合优化为:

(12)

式中:KL为VSLAM地图中的关键帧集合;Λ为两个关键帧间的协方差矩阵.

利用优化结果提高特征匹配精度,得到融合后的相机位姿为:

(13)

继而得到 CGCS2000坐标系下的相机位姿.

1.6 极线匹配与北斗导航查询

对图像中感兴趣目标点进行查询,需要选取融合位姿估计中有同一物体的两帧图像,通过联合三角测量的信息和相机的运动估计来确定该像素点对应的空间坐标[17].在相机运动估计误差一定的情况下,为减少特征点匹配的误差,首先严格剔除冗余的关键帧和特征点,再通过GMS算法将匹配点转换为高质量的匹配点[18],对应像素点与目标点匹配的一致,通过三角运算确定其在三维空间中的北斗导航坐标.

2 试验与结果

选用宇树科技机械狗加强版、搭载英特尔D435i高清实感相机进行试验.机械狗视觉获取当前环境地图信息,对获取的地图进行里程计算、回环检测等处理,获得相机的相对位姿估计.

通过估计得到的相机位姿可以清晰地再现机械狗的行走轨迹.将北斗导航与VSLAM的对应帧时间对齐后,得到对应关键帧的CGCS2000坐标,从而使相机估计轨迹得到真实尺度信息.图4为单目VSLAM估计的相机轨迹与参考轨迹(虚线),相机轨迹与参考轨迹对比的绝对位姿误差如图4所示;机械狗的北斗导航融合轨迹与参考轨迹的绝对位姿误差如图5所示.

图4 单目VSLAM估计的相机轨迹与参考轨迹(m)

图5 相机轨迹和北斗导航轨迹融合与参考轨迹(m)

由图4、图5可知,由相机估计得到的轨迹与对应的北斗导航轨迹具有相似轨迹路线.由于融合了北斗导航坐标的尺度信息,绝对位姿误差绝对值由0.134下降到0.049,轨迹精度得到提高.将北斗导航信息与VSLAM融合(Camera+BDS),拟合后轨迹与文献[10]中算法的拟合轨迹(Camera Trajectory)相比，更靠近参考轨迹,如图6所示.

图6 拟合轨迹对比图(m)

由图6可知,拟合后北斗导航轨迹与参考轨迹几乎重合,定位精度得到提高,可使后续的路标定位准确度更高.融合北斗导航信息后,给单目VSLAM在地图中的估计轨迹增加了坐标信息.在单目SLAM所摄地图中标定60个北斗导航路标,通过映射变换到CGCS2000坐标系中,将融合后的像素坐标叠加显示在机械狗视觉中的图像帧上,将三角化新的坐标点放到跟踪模块,达到手动标记路标与机械狗视觉地图叠加显示的目的,以实时查询路标.

对在SLAM地图中手工标定的路标进行反投影操作,得到CGCS2000坐标系下坐标,对比北斗导航坐标系与融合坐标点之间的误差来验证本算法的精度.北斗导航路标平面误差如图7所示.

图7 北斗导航路标平面误差

在北斗导航采用的CGCS2000坐标系下,本文定位方法的尺度漂移和全局优化纠正度更高,对感兴趣目标点的查询精度可以达到0.8 m以内.地图自动初始化估计路标点的3D位置和高精度回环检测是本文算法的关键.对地图构建(更新坐标点和插入关键帧)和主线程相机跟踪(相机位姿和关键帧)进行记录,运行效率结果如表1所示.

表1 性能测试结果 ms

由表1见,在视觉跟踪阶段,每秒可以处理69帧的关键帧,处理帧数完全满足误差优化函数的运行速度,实现相机位姿的实时轨迹跟踪,从而保障后续回环检测的纠正速度.

3 结语

将相机跟踪的位姿信息与北斗导航坐标系下的标定路标进行融合,将融合得到的坐标系投影到同一个球平面,通过三角运算得到新的地图点;利用融合后的相机位姿估计标定路标的坐标位置,实现机械狗视觉中像素点与北斗导航的叠加显示,从而得到地图中标定路标的坐标.本文提供了一种基于视觉SLAM的北斗导航路标定位并融合了北斗导航来进行位置查询的一种方法,为仿生机械狗在警戒巡检、抗震救灾等的目标查询提供了一种可用方法.通过北斗坐标和SLAM坐标的融合,绝对位姿误差由0.134 m下降到0.049 m,使得巡检或救灾场景中目标点的查询精度可以达到0.8 m以内.