一种多无人机协同定位与稠密地图构建算法

2024-01-02唐嘉宁陈伟陈云浩李玉亭胡敏森许俊锋

科学技术与工程 2023年35期

唐嘉宁, 陈伟, 陈云浩*, 李玉亭, 胡敏森, 许俊锋

(1.云南民族大学电气信息工程学院, 昆明 650000; 2.云南民族大学无人自主系统研究所, 昆明 650000)

同时定位与地图构建(simultaneous localization and mapping, SLAM)指的是无人机在未知环境中运动时增量地构建自身周围的环境地图,同时使用已构建好的地图来评估无人机自身的位姿[1],该技术在增强现实(augmented reality,AR)、三维重建、无人机、无人车等领域得到越来越广泛的应用[2-3]。视觉SLAM(visual SLAM,VSLAM)指的是以摄像头作为无人机的传感器,相比于传统的激光雷达等传感器,摄像头以其价格便宜、重量较轻等优点受到广大研究者的青睐[4]。

VSLAM比较成熟的算法主要包括ORB-SLAM2[5](oriented fast and rotated brief-SLAM2)、VINS-Mono[6](Monoculor visual-inertial state) 、Kimera[7]、ORB-SLAM3[8]等,这些算法在单无人机的应用中表现出了较高的精度,但多数尚未应用到多无人机协同定位与地图构建中。其中,VINS-MONO在众多的SLAM算法中具有定位精度较高、支持相机与imu融合、支持回环检测与重定位等优点,但也存在着建图效率不高、构建的稀疏点云地图对环境的描述信息较少的缺点。

在复杂的环境下,单无人机工作的效率和鲁棒性往往达不到要求,此时需要多无人机协同完成工作,因此多无人机SALM逐渐成为研究热点[9]。CCM-SLAM[10]系统采用集中式的架构,服务器与客户端通过无线网络进行连接,每个无人机作为客户端可独立进行定位与建图,直到识别到重叠区域,服务器才进行地图融合、全局优化等工作。CORB-SLAM[11]系统采用集中式框架,每个机器人都会单独执行ORB-SLAM2算法来建立各自的局部地图并传送给中央处理器。中央处理器会融合来自机器人的局部地图,然后把融合后的全局一致性地图传送回每个机器人。Schmuck 等[12]提出了基于ORB-SLAM3的中心化协同SLAM框架,可以用于多达十二架无人机。以上多无人机SLAM算法在定位精度方面显著提升,但是其构建的稀疏点云地图并不能满足无人机的自主导航等需求。

稀疏点云地图对环境的描述在一些数据集中可以满足无人机的定位要求,但是无法应用于无人机的自主导航。稠密点云地图虽可以满足无人机的自主导航的要求,但是稠密点占用的存储过大,并且没有距离信息。欧几里得符号距离场(euclidean signed distance functions,ESDF)地图中的每个体素都包含距离信息和梯度信息,在无人机在线运动规划中具有重要作用。Voxblox[13]算法中提出了一种从TSDFs(truncated signed distance function)增量的构建ESDFs((euclidean signed distance functions)的方法,其在单个CPU上验证了方法的实时性。Bartolomei等[14]提出了一种分层的多无人机全局规划算法,其将Voxblox算法做出改进并应用在所提出的多无人机全局规划算法中。

针对单无人机工作效率低、鲁棒性低且多无人机SLAM系统构建的稀疏点云无法用于自主导航问题,现提出一种集中式多无人机协同定位与稠密地图构建算法(多无人机协同SLAM算法)。算法以VINS-MONO算法作为多无人机协同SLAM算法框架的客户端,以Voxblox建图模块和地图融合与位姿图优化模块组成多无人机协同SLAM算法框架的服务端。为了建立各个客户端之间的连接,建立了统一的世界坐标系,并使用回环闭合获得了多无人机之间的额外约束。提出使用双向检测匹配法对关键帧的特征点做匹配,并使用PROSAC算法剔除特征点的误匹配,从而提高回环闭合中特征匹配的准确度,以提升多无人机之间的定位精度。进一步结合Voxblox算法,构建了可供五架无人机协同导航的全局稠密地图。

1 VINS-MONO算法

VINS-MONO算法是由Qin等[6]提出的一种鲁棒且通用单目视觉惯性状态估计器,算法框架如图1所示。算法主要包括四个大方面:初始化模块、观测预处理模块、视觉惯性里程计(visual inertial odometry,VIO)融合模块和全局优化模块。观测预处理模块是同时对单目相机和惯性测量单元(intermediate maintenance unit,IMU)进行观测数据的预处理,单目视觉预处理是利用光流法对上一帧与当前帧之间的特征点追踪,并于当前帧提取新的特征点和挑选新的关键帧,IMU预处理是对IMU预积分进行计算。初始化模块主要分为视觉惯导对齐和纯视觉初始化。VIO融合模块是当初始化模块完成后,用输入的观测数据构建新的约束,即局部建图过程,同时采用滑窗机制来管理局部建图的节点(新节点加入,最早进入的节点将被删除)。全局优化模块是对算法在连续建图过程累计的误差进行消除。

2 改进的多无人机协同SLAM算法

2.1 系统总体框架

算法的系统架构由多个无人机代理和一个中央服务器组成,如图2所示。每个代理均为携带相机和IMU传感器的无人机,可以分别运行VINS-MONO算法,同时立体摄像机可以构建立体点云。为了减轻向服务器传输立体点云的带宽需求,通过体素滤波融合从后续帧获取的立体点云以对立体点云进行压缩。然后,将里程计、关键帧和关键帧中的三维地标以及立体点云一起发送给服务器。服务器负责部分计算成本更高的任务,如基于全局优化的传感器融合、使用Voxblox模块构建全局稠密地图等任务。

2.2 地图融合与位姿图优化

对于每个代理,都有独立的地图坐标系,且在其独立坐标系下执行SLAM过程,当检测到回环闭合时执行全局优化。为了建立多个代理之间的联系,使用四种不同类型的坐标系来表示坐标转换,如图3所示。图3中世界坐标系在所有代理中都是唯一不变的,对于每个代理,有一个局部坐标系和里程计坐标系,用于VIO的漂移校正。

图1 VINS-MONO算法框架图Fig.1 VINS-MONO algorithm block diagram

图2 系统总体框架图Fig.2 System overall framework diagram

图3 坐标变换图Fig.3 Coordinate transformation diagram

2.2.1 残差计算

为了优化坐标系之间的变换,根据变换表达测量值,以形成残差。算法中使用的第一类残差为先验残差,其定义式为

(1)

(2)

算法中使用的第二类残差是相对测量约束,其代表两个关键帧之间相对测量位姿与基于状态变量的预测转换之间的误差,定义式为

(3)

式(3)中:TMS为关键帧坐标系到局部地图坐标系的转换;TOS为关键帧坐标系到视觉惯性里程计坐标系的约束;j、p为不同关键帧。关键帧位姿与视觉惯性里程计位姿之间的关系为

(4)

式(4)中:TMO为视觉惯性里程计坐标系到局部地图坐标系的转换。

2.2.2 回环闭合

为了在代理与代理之间轨迹建立额外的约束,利用视觉词袋模型(bags of binary words,BoW)[15]执行视觉回环闭合检测。为了可以在代理与代理之间执行回环闭合检测,需要创建一个在所有代理之间共享单词的数据库,通过查询数据库中相似的候选对象来检测新的循环闭包,并对最佳候选对象进行基于特征(binary robust indenpendent elementary features,BRIEF)描述符的双向检测匹配法进行匹配,并使用渐进一致采样法(progressive sampling consensus,PROSAC)算法剔除误匹配。如果找到足够的内点数,则在不同代理之间进行回路闭合检测的情况下,通过最小化已建立对应的重投影误差来优化相对位姿。

2.2.3 改进的多无人机协同全局优化

针对多无人机协同工作时的全局优化,提出当在多个代理之间的轨迹检测到回环闭合时,全局优化会优化所有连接的位姿和地图转换。如在一个包含三个代理的算法中,如果在代理1和代理2之间的轨迹中检测到回环闭合,则与代理1和代理2相关的所有变换都会得到优化,而代理3内的变换保持独立。全局优化的目标表达式为

(5)

式(5)针对室外环境,在视觉的基础上融合了全球定位系统(global positioning system,GPS)以增加定位精度。本文算法主要针对室内环境下的定位与建图,室内环境下无GPS信号,考虑到增加GPS后在室内环境下并不能增加定位精度并增加计算量,对公式做出了改进,剔除掉融合的GPS部分减少计算量,公式为

(6)

优化变量x表示为

x=[x1,x2,…,x|Z|]

(7)

每个x包含如下转换

(8)

式(8)中:W代表世界坐标系;M代表地图坐标系;S代表关键帧坐标系。回环闭合约束的残差计算公式为

(9)

2.3 多无人机协同建图

立体点云计算模块接收数据集的图像后,将计算得到的立体点云传输给数据处理模块,数据处理模块负责将立体点云压缩,并将VINS-MONO算法得到的关键帧和位姿传输给服务端,服务端的地图融合与位姿图优化模块对位姿进行优化,之后Voxblox模块利用立体点云构建全局一致的TSDF地图和ESDF地图。

3 改进的回环闭合算法

传统的回环闭合的方案使用单向检测的匹配算法加随机一致性采样(random sample consensus,RANSAC)算法对特征点进行处理,其准确性不够高。因此提出使用双向检测匹配法对特征点进行匹配,并结合PROSAC算法剔除误匹配,来提高特征点匹配的准确度,以提升回环闭合的准确性。

第一步：利用双向检测匹配法对特征点进行匹配。首先是前向匹配:计算第一帧图像中某一个特征点xi描述子与第二帧图像所有特征点y=(y1,y2,…,ym)描述子之间的汉明距离,对所有的距离进行排序,取距离最近的一个作为匹配点。然后是后向匹配:计算第二帧图像的某一个特征点yi与第一帧图像所有特征点x=(x1,x2,…,xn)描述子之间的汉明距离,对所有的距离进行排序,取距离最近的一个作为匹配点。

只有当前向匹配和后向匹配得到的为同一点对才认为匹配正确。计算两帧图像的描述符之间的汉明距离,其表达式为

(10)

式(10)中:a、b代表两帧图像的所有特征点描述子;⊕为计算特征描述子之间的直线距离。

第二步：利用PROSAC算法剔除特征点的误匹配。PROSAC算法是对RANSAC(random sample consensus)算法的优化,不同于RANSAC算法,PROSAC算法会对样本点进行降序排序,然后选取四组匹配质量最高的匹配点,计算内点数量,因为样本点为相似性更高的点,所以得到的更可能为内点,并且采样点的减少使得算法计算速度更快。算法引入质量函数q(uj),数据点集合为uN,数据点降序满足公式

ui,uj∈uN,i

(11)

式(11)中:“:”代表并列,即ui、uj属于集合uN且i

数据点为内点的概率p(uj)公式为

p(ui)≥p(uj)

(12)

设β为匹配的特征点描述子欧式距离的比值,对质量函数进行评估,表达式为

(13)

式(13)中:D1代表特征点描述子欧式距离最小值;D2代表特征点描述子欧氏距离次小值。比值越小,代表匹配的准确度越高。

通过引入质量因子γ来衡量匹配点对的质量好坏,其值越大,代表匹配质量越高,其计算公式为

(14)

改进后的回环闭合算法流程图如图4所示,首先使用视觉词袋模型进行视觉回环闭合检测,然后判断是否满足预先设置的阈值,满足阈值以后进行特征描述符的计算,使用双向检测匹配算法进行特征点匹配,其次使用PROSAC算法进行特征点误匹配的剔除,最后执行回环闭合优化,整个算法流程不断循环,直到系统结束。

图4 改进后的回环闭合算法流程图Fig.4 Improved loop closure algorithm flow chart

4 实验结果分析

实验在Ubuntu18.04环境下进行,所用计算机CPU为2.30 GHz Intel i7,运行内存为16 GB。实验数据集来自瑞士苏黎世联邦理工学院制作的EuRoc数据集,取其中MH_01_easy、MH_02_easy、MH_03_medium、MH_04_difficult和MH_05_difficult序列。在实验中对提出的多无人机协同SLAM算法中的特征点匹配做出分析,并且与VINS-MONO的定位精度做了对比,同时分析了多无人机协同SLAM算法构建的TSDF地图与ESDF地图的效果。

4.1 特征点匹配分析

实验选取了EuRoc数据集中的MH_01_easy序列中的两幅图片,分别比较了不同情况下匹配效果,如图5所示倾斜绿色匹配线代表匹配错误的点。

从图5中看出匹配错误的线逐渐减少,说明匹配的准确率逐渐上升,在四种方法中使用双向检测匹配法加PROSAC算法匹配的结果达到最优,匹配的准确率最高。

图5 不同算法的匹配结果图Fig.5 The matching result chart of different algorithms

4.2 定位精度分析

为了验证所提算法定位精度的提升,在EuRoc数据集下的五个序列中与VINS-MONO算法做了多次对比定位精度实验,取其中一组对比实验进行分析,结果如图6、图7所示。

图6 两种算法的轨迹对比图Fig.6 The trajectory comparison chart of the two algorithms

从图6可以看出,相对于图6(a)与图6(b)的轨迹,图6(c)中多无人机协同SLAM算法在MH_03_medium数据集下的轨迹与真实轨迹更加贴近。

评估算法定位精度,可以采用真实轨迹与实际运动轨迹的绝对轨迹误差(absolute trajectory error,ATE)进行比较。ATE越小,算法的精度越高,计算式为

(15)

将算法估计的轨迹与数据集真实轨迹对比,使用EVO工具绘制绝对轨迹误差图如图7所示,图中APE代表绝对位姿误差。

由图7可以看出,多无人机协同SLAM算法相较于VINS-MONO算法的定位精度有了明显提升。利用绝对轨迹误差进行比较,表1为两种算法在五个不同数据集下的最大轨迹误差对比,其中最大绝对轨迹误差对应绝对轨迹误差图中折线的最高值。

定位精度提升公式为

(16)

式(16)中:Z1为VINS-MONO算法在有回环闭合时的运行轨迹误差;Z2为多无人机协同SLAM算法在有回环闭合时的运行轨迹误差。

表1 最大绝对轨迹误差Table 1 Maximum absolute trajectory error

从表1中可以看出,VINS-MONO算法在有回环闭合的情况下比无回环闭合的情况的最大绝对轨迹误差减小,这说明在有回环闭合时VINS-MONO算法的定位精度更高,而多无人机协同SLAM算法相较于VINS-MONO算法在有回环闭合时的最大绝对轨迹误差较小,这表示多无人机协同SLAM算法的定位精度较VINS-MONO算法精度更高。

根据式(16)计算,多无人机协同SLAM算法相比于VINS-MONO算法在有回环闭合时,在五个数据集下的误差皆减小,在MH_01_easy、MH_02_easy、MH_03_medium、MH_04_difficult和MH_05_difficult数据集下,误差分别减小了34%、26%、42%、24%、19%。

4.3 多无人机协同稠密地图构建对比

图8中所有图像均来自Ubuntu18.04环境下的ROS(机器人操作系统)中的Rviz(三维可视化平台)。

本文算法使用五个无人机代理协同构建稠密地图,与文献[14]采用三个无人机代理协同构建稠密地图进行对比分析,其中三个代理采用MH_01-MH_03的三个数据集进行实验,五个代理采用MH_01-MH_05的五个数据集进行实验,如图8所示。

从图8(f)左图与图8(f)右图的虚线框对比中可以看到,增加代理后,图8(f)右图中构建的TSDF地图和ESDF地图具有更高的完整度。图8(f)右图中在构建TSDF地图和ESDF地图时增加了代理四与代理五,所构建的地图可满足五架无人机协同导航需求,虽然相较于三个代理在构建TSDF地图和ESDF地图时的时间增加了3%左右,但仍可以满足在构建全局一致的TSDF地图与ESDF地图时的实时性。