基于视觉激光惯性相结合的机器人SLAM算法

王立玲，朱旭阳，马东，王洪瑞

(1.河北大学电子信息工程学院，河北保定 071002；2.河北大学机器人技术研究中心，河北保定 071002)

0 前言

近几年来随着智能驾驶、机器人避障导航和自动泊车等技术的不断发展，摄像头作为重要的视觉传感器被广泛应用于自动驾驶等领域。通过视觉捕获机器人周围环境相关信息，作为下一步判断与决策的依据，因此，视觉传感器进行机器人同时定位与地图构建(Simultaneous Localization and Mapping，SLAM)技术成为研究热点。

相比于单传感器视觉SLAM，多传感器融合进行机器人定位和绘图精度大大提高。单目相机和单线激光雷达是当前室内移动机器人SLAM领域最具代表性、应用范围最广的两类传感器。刘诤轩等[1]融合双目和2D激光雷达进行室外定位，克服雷达点云的稀疏性，取得了较好效果。王祝等人[2]通过障碍物点云聚类分割提高雷达测距和定位稳定性。姜祚鹏、梅天灿[3]通过不同点云匹配方式设计两种激光里程计进行失效矫正和融合，实现高鲁棒的定位。MU等[4]设计2D激光雷达融合深度相机，在保证实时性的基础上实现精准定位。CHEN等[5]提出基于单目视觉和2D激光雷达的城市机器人搜救系统，实现精准建图。但国内外对单目融合单线雷达研究较少，在弱纹理环境(存在大面积特征较为单一环境)中激光雷达可以估计精准位置，但只能获得部分信息[6]。与激光雷达相比，相机可以获得更丰富信息，相机和雷达结合实现完美互补。因此，本文作者提出一种单目视觉、2D激光雷达、惯性信息融合策略，使用3种传感器系统用于移动机器人同时定位与地图构建，并展开研究。

本文作者针对弱纹理环境下单一传感器机器人定位和建图性能较差，使用视觉、激光雷达和IMU惯性元件多传感器融合进行机器人SLAM。针对雷达运动估计存在激光点漂移、运动畸变问题，使用改进PL-ICP算法(An Iterative Closest Point Variant Using a Point-to-Line Metric)，采用激光点到其最近两个点连线的距离，并使用IMU和轮速计去除雷达运动畸变。在视觉激光惯性融合方法中，采取如下策略，有利于实现精准定位和建图：

(1)在视觉与雷达预处理阶段，视觉提取点线特征，同时雷达帧间匹配过程采用激光点到其最近两个点连线的距离构建误差方程，实现精度更高的匹配效果。

(2)针对单线雷达运动畸变问题，采用惯性传感器与轮速里程计进行雷达运动畸变校正[7]，同时雷达估计的信息为单目点线特征三角化提供良好的深度值。

(3)针对单一传感器进行机器人SLAM稳定性较差，将视觉特征定位信息融入到栅格地图，利用多传感器融合[8-10]提高机器人SLAM精确度。

1 系统设计

1.1 视觉点线特征提取

视觉部分，构建点线视觉里程计用于机器人定位。传统基于点特征机器人定位算法，在弱纹理环境中单目视觉采集不到足够特征，导致算法在机器人移动过程中定位能力减弱。利用单目视觉作为里程计提供相对位姿，将一个局部时间窗口内多个时刻得到的2D激光雷达数据融合成一个局部子图。其中，视觉使用两种特征结合方式，位置估计时获得更多环境信息，雷达和惯性传感器对单目视觉点线特征三角化深度信息进行校准，视觉为雷达提供更多有用的信息，采用多传感器融合的方式进行机器人定位，实现精准的机器人定位。

1.2 雷达信息预处理

1.2.1 运动畸变模型

机器人采集激光点云数据不是瞬间获得的，发射一束激光到接收激光数据的时间间隔伴随着机器人运动和环境噪声影响，当激光帧率较低时，机器人运动产生雷达运动畸变不能被忽略。

假设激光雷达的扫描帧率为10 Hz，机器人运动时一帧数据首尾时间差100 ms，若机器人以0.5 m/s的速度向前移动，扫描前面的障碍物，那么100 ms后尾部的测量距离和首部的测量距离在x方向上相差5 cm，如图1所示。

图1 激光雷达运动畸变原理

绿色箭头表示激光雷达旋转方向，黑色箭头表示激光雷达运动方向。显然，可以看出每个激光束距离数据都是在不同位姿下采集而来，如橙色、灰色、蓝色点位姿所示。然而，一般激光雷达驱动封装数据时，默认一帧激光雷达数据的所有激光束是在同一位姿下瞬时获得的，也就是所有激光束都是从橙色点获得的数据，这样实际使用的数据和真实数据有明显差距，如5 cm。所以，对于低帧率激光雷达，运动畸变明显是个需要被解决的问题。

1.2.2 运动畸变补偿

机器人采集到激光雷达数据同时里程计及IMU惯性信息也会被发送到数据处理中心，将3个数据信息进行滤波处理，时间同步，数据对齐，构建紧耦合模型。对里程计位姿分段线性近似及线性插值，最后将里程计及IMU坐标系变换到基准坐标系，实现雷达信息精准预处理。本文作者使用IMU和轮速里程计时时校准雷达运动过程中产生的畸变，其中运动畸变校正传感器对齐原理如图2所示。

图2 雷达运动畸变去除原理

假设对第n个激光雷达扫描点进行畸变校正。如图2所示，机器人雷达数据帧开始时刻为tlidar，采样周期为 Δt，则对应扫描点采样时刻为tn=tlidar+(n-1)Δt。经查询知该扫描点相邻两帧里程计及IMU数据采样时刻与姿态分别为tm、qm、tm-1、qm-1，则计算可得旋转四元数之间平滑差值系数：

(1)

根据球面线性插值公式可以得到qn=Slerp(qm,qm-1,μ)，已知该雷达扫描点相对于激光雷达坐标系的空间坐标pn=[xn,yn,zn]T，并将四元数q0、qn分别转换至R0、Rn。最后，通过系统坐标变换公式计算得到该扫描点校正后的坐标：

(2)

1.3 雷达点云匹配

激光雷达点云匹配算法为了匹配机器人在三维空间采集到的两帧点云数据，计算其前后的累计位姿差形成机器人激光雷达里程计数据。文中相较于传统算法，采用某一激光点云到其最近两个点连线的距离构建最优匹配参数模型[11]，使用分段线性的方法对雷达扫描的点云曲面进行近似处理，计算鲁棒的帧间匹配误差系数，实现基于PL-ICP的激光雷达里程计。

机器人激光雷达扫描得到激光点为真实环境曲面离散采样，最优距离误差为激光点到实际曲面距离构建误差方程，PL-ICP匹配误差方程更贴近于真实情况。如图3所示，构建激光点pi和pj匹配误差方程为

图3 改进雷达点云匹配

(3)

式中：qk+1为下一时刻位置转换矩阵；Ck为点到线的对应关系；R(θk+1)为对应旋转角度θk+1的旋转矩阵；tk+1为平移矩阵。

2 基于视觉激光惯性的SLAM算法

2.1 雷达里程计

在采集到雷达数据后，使用雷达点云信息每一帧之间的累计位姿构建雷达里程计。传统轮速里程计是基于安装在机器人车轮上编码器来跟踪每个车轮转数，转数被整合到车辆运动模型中，以确定机器人相对于起始点的当前位置。在机器人长期移动过程中，由于轨迹漂移可能造成较大的累积误差。代替轮速里程计使用激光雷达的点云深度值构建雷达里程计为视觉特征点提供深度信息，可以提供更高精度的位姿估计和更高质量的地图。

2.2 信息融合

此算法采用雷达及惯性信息辅助视觉深度提取、融合补偿进行机器人定位和建图，将雷达采集点云信息映射到视觉图像特征中，并基于此特征融合进行机器人定位。雷达帧间匹配对传感器信息初始值更敏感，将单目相机采集到的特征信息与雷达融合有效避免了雷达因初始值不理想陷入局部循环的问题，通过构建最小化误差方程实现更加精准的轨迹定位。视觉雷达惯性加权融合[12]，构建视觉雷达惯性混合残差：

(4)

式中：rC为相机传感器对应残差函数；rL为雷达传感器对应残差函数；rB为IMU传感器对应残差函数。三者为传感器采集误差与机器人状态矩阵X相关的函数。通过最小化混合残差实现位姿优化，其中视觉里程计约束：使用点线视觉里程计，计算每个关键帧位姿，进而得到相对位姿。激光里程计约束：使用PL-ICP匹配雷达里程计，计算每个关键帧位姿，进而得到相对位姿[13]。IMU约束：在上一个关键帧的基础上，进行惯性积分，从而得到两个关键帧相对位姿信息。

2.3 改进栅格地图的地图构建

传统栅格地图在应用雷达信息建图时被广泛使用，机器人进行同时定位与地图构建时可以构建基于雷达信息栅格地图，此过程由下式表示：

Mgrid={mg(x,y)}

(5)

机器人建图时雷达栅格信息grid(x,y)被占用不确定性用mg(x,y)表示。为了简便，记mg(x,y)取值为0或1，0代表机器人移动时此栅格没有被占用，反之1代表栅格信息已被占用。

基于单目视觉获取图像中点线特征地图，可以用下面表达式来表示：

Mfeature={f(x,y,z)}

(6)

其中点线特征信息用f(x,y,z)表示，利用PNP求解进行位姿估计。

作者将视觉采集到的点线信息、二维激光雷达和IMU惯性单元的深度信息整合到一张地图中，视觉为雷达提供良好初值，IMU、雷达信息校准视觉深度信息，构建栅格地图。可以用下列式子描述这个过程：

M={m(x,y)}

(7)

m(x,y)={mg(x,y),feature_list(x,y)}

(8)

有了栅格地图的辅助，移动机器人定位时不断对准地图区域坐标信息及栅格地图识别的障碍物信息。视觉特征信息融入到栅格地图，系统拥有更多且更丰富的信息用于回环检测，减少系统累积误差，也可以对导航和避障产生很大的帮助。

3 改进算法流程

在移动机器人SLAM过程中，改进算法主要分为两个部分，图4为改进算法流程框架。第一部分：通过视觉提取点线特征构建单目视觉里程计，达到弱纹理环境机器人定位稳定性效果；第二部分：通过改进雷达点云匹配构建雷达惯性融合生成局部子图；第三部分：通过先验地图、视觉局部子图和激光惯性局部子图融合[14]、轨迹对齐实现位姿校正，轨迹更新。采用多传感器融合方法，视觉为雷达信息定位提供良好初值，雷达、惯性传感器深度信息为视觉点线特征三角化提供深度对照[15]，实现弱纹理环境鲁棒轨迹定位和建图。

图4 改进算法流程

4 实验结果与分析

为验证此改进算法的有效性，本文作者分别在Gazebo构建仿真环境和移动机器人上进行实验。通过弱纹理环境下机器人定位和建图精确度来评价算法的性能。

4.1 雷达点云提取与匹配实验

图5是分别用传统雷达点云信息提取算法使用PL-ICP点云提取与雷达运动畸变补偿后点云提取效果对比。

图5 雷达点云去畸变结果

从图5可以看出：在场景(a)中，受机器人移动影响，传统雷达点云检测算法存在较大运动畸变及激光漂移等问题，长时间累计误差过大导致机器人定位失败；在场景(b)中，使用惯性元件和轮速里程计对雷达进行运动畸变补偿，通过点云分段线性近似、线性插值，找到激光束原点近似里程计位姿和近似激光雷达坐标系角度。将所有激光束从里程计及惯性坐标系变换到基准坐标系，实现里程计、惯性传感器辅助单线雷达去除运动畸变。

根据以上实验理论分析，分别对3种传统雷达帧间匹配算法和改进后融合惯导、里程计数据的 IOPL-ICP(An Iterative Closest Point variant using a point-to-line metric，IMU and odometer)雷达匹配算法进行实验对比和数据分析，并完成激光雷达帧间匹配实验，雷达匹配耗时及精度如表1所示。

表1 4种雷达匹配算法的单帧匹配耗时对比

结果表明：文中改进雷达匹配算法相较于其他3种算法整体性能最好，耗时最少情况下能够更加完整、精准地估计机器人位置，并且在不同场景下机器人定位和建图准确率都能维持在一个较高的水平，具有较强的适应能力。

4.2 仿真环境实验

为验证算法在弱纹理环境下的稳定性与鲁棒性，建立基于Gazebo仿真实验。实验采用Cartgrapher的 SLAM 算法，对移动机器人采集激光点数据，根据改进PL-ICP算法进行帧间点云匹配求解位姿，然后通过局部子图融合进行局部和全局位姿优化，减少位姿误差，并解决定位不准确而导致 SLAM 构建栅格地图发生畸变的问题。雷达融合视觉里程计信息、IMU惯性信息进行定位，采用地图回环检测进行轨迹更新、地图更新，最终定位轨迹精度测评和构建栅格地图如图6所示。

图6 仿真实验

图6(c)(d)分别为仅使用雷达与视觉雷达惯性融合建图效果对比，如图6(c)中红绿色圈出假墙和假影，图6(d)融合后被有效去除。

4.3 真实实验

为验证文中改进算法对机器人SLAM的可行性，进行弱纹理环境实验，如图7所示。以麦克纳姆轮式机器人作为硬件平台，如图7(a)所示。采用机器人单目摄像头、单线激光雷达和IMU惯性测量单元融合进行实验。调用Ubuntu系统的ROS(Robot Operating System)机器人操作系统，将实时定位和建图数据发送到远程可视化界面进行处理。

图7 算法融合前后对比

实验一：验证算法稳定性。进行弱纹理环境建图实验，改进的雷达去除运动畸变建图效果对比如图7(c)(d)所示，融合惯导、里程计信息进行机器人建图能提高系统稳定性，优质的先验地图有助于机器人定位。

实验二：验证定位精确性。进行弱纹理环境定位实验，将视觉里程计、雷达里程计、视觉雷达惯性3种定位方法进行对比。如图8可以看出：单一传感器进行机器人定位精确性较差，改进算法能够有效克服弱纹理环境单一传感器机器人SLAM鲁棒性较差的问题，采用多传感器融合的方式进行定位大大提高了系统鲁棒性。

图8 融合前后定位精度对比

实验三：验证建图准确性。进行楼道走廊实验，在弱纹理大厅环境下对机器人算法融合前后建图精度进行对比，如图9所示。加入了视觉里程计后，融合算法有效提高弱纹理环境长走廊建图效果，实现鲁棒的机器人SLAM，提高算法精确性。

图9 融合前(a)、后(b)建图精度对比

5 结论

提出一种基于视觉激光惯性融合的机器人定位算法，并且将所提方法在Gazebo仿真环境和真实弱纹理环境中分别进行了测试和实验。对比实验结果表明：改进算法能够快速准确地进行机器人同时定位与地图构建，有效克服了弱纹理环境单一传感器进行机器人SLAM鲁棒性较差的问题，在保证实时性的同时对弱纹理环境具有较强的适应性，定位和建图精度满足实际需求，为后续机器人避障导航提供了有力保障。