视觉SLAM的空地协同导航方法研究

高宏伟，于 斌，武炎明

(1.沈阳理工大学 自动化与电气工程学院，沈阳 110159；2.沈阳自动化研究所，沈阳 110017)

近年来无人机与无人车协同作业的应用越来越广泛。无人车虽然能够对地面的近距离目标精准定位并执行任务，但其视野范围具有局限性，限制其工作的区域和效率；无人机机动性强、视野范围广，但对地面物体定位的精确度不高，对地面物体操作的灵活性差。二者组成的协同系统能够构建出空地层次的全局地图，实现对环境更精确、更完整的感知[1]。

传统无人机与无人车空地协同实现地面导航的技术核心是将无人机航拍的图像序列拼接为全景图像，无人车经过图像处理将其中的障碍物识别并标记出来，进而开始路径规划并沿着规划路线到达目标地点。Jianwen Huo等[2]提出了一种基于地图匹配的路径规划算法，通过在传统的无人机拼接全局地图方法中加入图像校正来减小误差，同时利用无人车的激光雷达建立局部地图，通过最小二乘法确定地图匹配中的未知值，有效地提高了协同导航精度；但该过程中的无人车无法从拼接图像中确定障碍物的具体位置信息，导致协同导航系统的环境适应性不足。王晨捷等[3]在点线特征同步定位与地图构建(Simultaneous Localization And Mapping，SLAM)方法的基础上加入了跟踪显著闭合边界的线程，通过基于线特征的闭合区域表示障碍物，同时配合无人车的激光雷达构建局部地图更新数据，实现了对无人车的导航；该方法在障碍物较少的环境下取得了不错的效果，但欠缺对于航拍视觉存在遮挡情况的处理。

随着视觉SLAM技术迅速发展，在建图和定位方面取得了不错的效果。目前，视觉SLAM系统按照建图结构可分为稀疏SLAM、半稠密SLAM和稠密SLAM[4]。由于后两者建图时恢复出的信息更加丰富，对系统硬件的要求高，而导航任务中只需要知道障碍物的基本信息，故选用稀疏SLAM方法以提高系统的实时性。基于特征点的单目实时ORB-SLAM2是目前使用最广泛的稀疏SLAM方法[5]。

本文在ORB-SLAM2基础上，将2D线段和3D线段分别匹配，减少线段的误匹配，增加建图的准确性。融合视觉SLAM建图与图像拼接建图构建全局地图，使构建出的全局地图既有前者的准确位置信息又有后者的准确形状信息，提高地图的还原度。地面无人车通过进一步融合激光雷达局部地图的方法实时修正地图，完成路径规划。

1 系统整体框架

本系统可分为两部分，即全局地图的构建和地面机器人的导航。首先通过无人机采集地面航拍图像并进行全局地图构建，地面机器人通过构建好的全局地图进行路径规划，通过自身的激光雷达构建局部地图并对全局地图进行更新，同时对预先规划出的路径进行调整，最后到达指定位置。系统主要流程如图1所示。

2 无人机视觉SLAM的全局地图构建

2.1 线特征SLAM全局地图构建

在航拍图像中地面的绝大多数障碍物都可以近似为一个闭合图形，故采用线特征SLAM的方法构建全局地图。

线特征提取算法(Line Band Discriptor，LBD)[6]首先对图像进行下采样，构建出尺度金字塔，然后对金字塔每层都进行一次线段提取并重构线段的集合，最后通过线段构成的线段支持区域计算描述符，提取出每一帧图像中的线特征。

采用基于特征点的ORB-SLAM2方法用作无人机构建地图的整体框架。因地面导航需要二维平面地图，而物体垂直方向上的边界也会被识别成线特征对地图构建造成干扰，针对这一问题本系统在其原有基础上添加了3D-2D的线特征匹配线程。

3D场景中的线段如图2所示。

图2 3D线段匹配

图2a中O为相机光心；Lc为光轴；K为关键帧；L为空间中直线；α为过光心和直线L的平面；θ为Lc与平面α的夹角。

图2b中Knear为K的相邻关键帧；L′为L在Knear上的投影；β为过光心垂直于L的平面；A、B为L上的端点；a、b为L′上的点。

对于2D场景中的线段，采用LBD算法对其进行匹配。3D-2D的线特征匹配方法是：通过平面β与L′的交点即可确定出端点A在平面上的匹配点a，同理可确定端点B的匹配点b。

基于直线的方向计算每个关键帧中的候选匹配线段l，具体过程为：将当前帧中的线段根据其方向分为36组，对特征点的处理方式和ORB-SLAM2中相同；选择l中具有相同方向的线段并分组，将每组中的全部线段添加到集合SL中作为候选匹配线段；在SL中根据最邻近算法计算出直线的最佳匹配。

由于线段是多维的，所以采用一种点线距离和点点距离结合的方法对线段匹配误差进行优化。线段l和l′的匹配误差优化公式为

d1(l，l′)=α(d(l，p1)+d(l，p2))

(1)

d2(l，l′)=(1-α)(d′(p1，p3)+d′(p2，p4))

(2)

(3)

式中：p1、p2为l′的两个端点；p3、p4为l的两个端点；d1(l，l′)为l与l′的点线距离；d2(l，l′)为l与l′的点点距离；d(l，p1)表示l到p1的距离；d(l，p2)表示l到p2的距离；d′(p1，p3)表示p1到点p3的距离；d′(p2，p4)表示p2到点p4的距离；lave是当前帧所有线长的平均值；δ为权重系数；d(l，l′)为两直线优化后的距离。

由线段之间的匹配关系将空间线段投影到地图中。和空间点的投影不同，线段会随相机运动出现不完整观测的情况，故采用如下解决策略。

(1)首先通过SLAM前端初始化求得的变换矩阵T计算出两个端点相对于当前相机坐标系的坐标；

(2)若两端点均在相机前方且方向相同，则对两点进行投影，绘制两点间线段即可得到线段在地图上的投影；

(3)若只有一个端点在相机前方，则令点的Z轴坐标分量为0，求出空间直线与图像平面交点的坐标；

(4)若两个端点均在相机后方，则不进行投影。

由此可得到空间中直线投影后的地图。

对于线特征SLAM建图过程中出现的线段误匹配和识别错误，通过多次测试分析，将线段长度的阈值设为2.5lave，可将超出阈值范围的线段剔除。同时将三维地图拟合到二维平面，对图像进行多边形近似拟合处理，得到SLAM全局地图。

2.2 图像拼接全局地图构建

在线特征SLAM构建初始地图的同时，启动图像拼接线程。首先对无人机视频序列每隔50帧选中一帧图像作为关键帧，对每个关键帧采用快速提取方法(Speeded Up Robust Features，SURF)进行特征点提取[7]。

SURF方法首先对图像中像素构建海森矩阵，再通过图像金字塔构建图像的尺度空间，然后通过滤波器检测出特征点并确定每个特征点的主方向，最后通过计算8个方向的梯度直方图生成特征点描述子，由此完成特征点的检测与匹配。

为减少误匹配的现象，针对每帧图像检测出的特征点进行阈值筛选。如果两帧图像中匹配成功的特征点数量多于100则视为匹配成功，使用随机抽样一致性(Random Sample Consensus，RANSAC)方法继续筛选可靠的匹配点[8]，求得两个关键帧的变换矩阵，实现图像配准。

配准图像后，将第一个关键帧拷贝到配准图上并对两幅图像拼接处的像素值进行渐入渐出加权融合，具体公式为

X(m，n)=

(4)

(5)

式中：m、n为像素值；x1，x2为待拼接图像；X为拼接后图像；γ为加权因子；w为重叠区域横坐标的差值；wd为重叠区域像素距离。

根据无人机图像序列的特点，采用“U形”递增的顺序拼接图像序列[9]，对拼接后的图像运用二值化、多级边缘检测算法Canny等[10]基本图像处理方法，得到此线程的全局初始地图。

2.3 轮廓相似度融合的全局地图构建

SLAM构建的全局地图能够较好地还原地面的位置信息，但对于障碍物的识别不够清晰；图像拼接线程构建的全局地图能够较好地分辨出障碍物，但拼接过程易产生较大的位置误差。本文对两个线程得到的地图进行融合以构建最终的全局地图。地图融合方法是通过对比两个初始地图中障碍物轮廓特征进行相似度计算完成。

使用Suv表示轮廓u和轮廓v的相似度，基于边界跟踪法构建表示特征轮廓的函数fu、fv，可得

(6)

当Suv=1时，两个轮廓完全相同；当0.7≤Suv≤1时，认为两个轮廓是同一个障碍物，并对两个轮廓进行融合。

(7)

候选融合线段倾斜角差值小于等于阈值θs[11]，则将其合并为一条线段。此处θs取值为10°。

最后对图像中闭合区域进行提取，得到用于地面导航的全局地图[12]。

3 激光雷达的局部地图构建与导航

由于无人机得到的全局地图对于地面细小的障碍感知能力较差，而且当无人机航拍视角存在遮挡时会对地面路径规划造成干扰，对于地面新增障碍物的情况全局地图不能及时更新，因此对地面无人车配备激光雷达构建局部地图，并与全局地图相融合构建导航地图。

树木遮挡情况仿真图与激光雷达建图如图3所示。

图3 树木遮挡情况仿真与建图

当蓝色的无人车被树木遮挡时，航拍的地图无法协助地面感知真实的环境，此时采用激光雷达可以很好地获取周围障碍物的位置信息。

建图过程如下：

(1)将无人机传入的全局地图转换成占据栅格地图，通过视觉标靶[13]的方式确定无人车在地图的位置，采用目前普遍使用的双向快速拓展随机树(Rapidly-exploring Random Tree，RRT)路径规划算法解决复杂约束的路径规划，在全局地图中规划出到达目标点的最优路径[14]。

(2)地面无人车采用2D-SLAM算法Cartographer[15]进行局部地图构建，通过激光雷达实时扫描感知周围环境。

(3)无人车在前进的同时对局部地图和全局地图进行融合，当检测到地图更新后无人车对路径进行重新规划，自主导航到达目标点。

4 仿真结果与分析

系统的仿真实验选择在Ubuntu18.04下的ROS系统中进行，采用Gazebo软件构建仿真环境并对空地协同无人系统进行仿真[16]。

表1 实验环境

无人机模型选用了开源仿真四旋翼无人机RotorS，用于地面导航的无人车选用了配备激光雷达的开源机器人模型mbot。无人机与无人车仿真模型如图4所示。

图4 无人机与无人车仿真模型

为减小建图误差，将相机角度调整为垂直于地面。在场景中模拟了光照、摩擦力和重力等因素以保证和真实环境高度一致。同时在场景中加入了汽车、路桩和多种形状的物体作为地面导航的障碍物，如图5所示。

图5 仿真场景

场景中右上方放置了一个视觉标靶，以辅助无人车定位。

实验完成了如下仿真过程：

(1)控制无人机在仿真场景中全局飞行并对全局环境进行图像采集；

(2)启动SLAM线程对全局地图进行实时构建，并融合拼接线程建图；

(3)本系统无人车选用双向RRT算法作为路径规划方法。

无人机在仿真场景采集图像拼接并处理后得到的效果如图6所示。全局地图构建结果如图7所示。

图6 图像拼接线程建图

图7 全局地图构建

图7a为SLAM线程构建的地图，图7b为融合图6后用于地面导航的全局地图。

对于建图质量的评价，主要通过将建图结果进行二值化，计算图中占据栅格和仿真场景中对应栅格的距离，将所有距离总和除以仿真场景中占据栅格的数量，算出的建图误差为

(8)

式中：ri、ti为第i个栅格在x、y方向上的坐标；k为仿真环境中占据栅格的总数量；r′i、t′i为仿真场景中第i个栅格在x、y方向上的坐标。

对建图效果测试，结果如表2所示。

表2 全局建图效果评价

由表2可见，融合边缘特征的建图方法相较于未融合的SLAM建图方法有效减小了建图的误差。

在实验过程中将本文算法与使用未进行融合的全局地图路径规划的方法进行对比，二者路径规划效果如图8所示。

图8 全局路径规划

测试全局地图路径规划效果的评价如表3所示。

表3 全局地图路径规划效果评价

图9为无人车按照全局规划的路线进行导航，最终到达目标点过程。

图9 无人车导航过程

图10为航拍图像出现遮挡时无人车通过激光雷达局部建图并融合地图进行导航的仿真。

由此可见，加入图像拼接线程后构建的全局地图更精确可靠，通过无人车激光雷达局部建图并融合地图，提高了路径规划效率，能帮助空地协同系统规划出更短的有效路径。

图10 模拟特殊场景

5 结论

提出一种2D、3D线段分别匹配的线特征SLAM方法对无人机获得的连续航拍图像进行定位和建图，并基于轮廓相似度融合图像拼接线程构建的全局地图，获得更加精确的地面导航地图。地面无人车通过路径规划算法以及激光雷达建立局部地图实现路径重新规划，并解决动态障碍物和遮挡情况。通过在仿真环境中完成空地协同导航任务对比实验，表明上述方法可实现空地协同导航任务，对于障碍特征较为明显的场景效果较好。