基于激光视觉融合的帧间匹配方法

何仲伟，张小俊，张明路

基于激光视觉融合的帧间匹配方法

何仲伟，张小俊，张明路

（河北工业大学 机械工程学院，天津 300132）

同步定位与地图构建（Simultaneous Location and Mapping, SLAM）是机器人在未知环境中实现自我导航能力的重要保证。目前SLAM算法使用的主传感器基本是激光雷达或视觉相机。二者各具优劣，激光雷达能更精确地进行测距，视觉相机能反映环境丰富的纹理信息。与使用单一传感器相比，将二者融合的SLAM算法能够获得更多环境信息，达到更好的定位和建图效果。文章提出一种融合激光雷达和视觉相机的帧间匹配方法，通过在SLAM帧间匹配过程中加入地面约束以及视觉特征约束，提高帧间匹配过程精度，增强算法鲁棒性，从而提升SLAM算法整体效果。文章最后利用采集的地下停车库数据进行结果验证，与开源算法A-LOAM进行对比。结果表明，相比A-LOAM的帧间匹配方法，文章提出的方法相对位姿误差提升约30%。

SLAM；多传感器融合；地面提取；迭代最近点；随机一致性采样；Fast特征；光流法；帧间匹配

引言

同步定位与地图构建（Simultaneous Location and Mapping, SLAM）能够让处于未知环境中的机器人进行自身定位和获取周遭信息，进而实现路径规划和自主导航。SLAM算法中使用的主传感器有激光雷达和视觉相机，其中激光雷达能够获取精确的环境距离信息，但因其具有一定的角分辨率，所以在空旷环境中获取的数据较为稀疏；视觉相机能够得到丰富的纹理信息，相比激光雷达数据更加连续致密，但对环境光照变化敏感[1]。因此，融合二者的SLAM系统更能适应复杂多变的环境。

目前，已有多位学者对激光视觉融合SLAM算法进行了研究。J. Zhang等利用视觉里程计估计机器的运动，高速率低精度地对准点云，然后基于激光雷达来优化运动估计和进行高精度点云对准[2]。Y. Xu等提出了一种基于EKF的RGB-D相机与激光雷达融合的SLAM方法用于处理相机匹配失败，即当相机匹配失败时使用激光雷达对相机3D点云数据进行补充并生成地图[3]，该方法没有真正地融合两种传感器数据，只是进行了传感器的切换。Y. Shin等提出了一种利用了雷达提供的稀疏深度，基于单目相机直接法的SLAM框架[4]，为了保证实时性，文章提出了利用滑动窗口进行追踪，严格的位姿边缘化和深度的大场景帧间匹配方法。欧阳毅利用激光数据为视觉图像增加了深度信息，提高了图像特征的提取效率，完成了对特征点的深度估计，且基于PnP算法完成了对机器人的运动估计，并采用光束平差法（Bundle Adjustment, BA）完成了局部优化[5]。J.Graeter等开源了SLAM方案LIMO，利用单目相机和激光雷达来模拟超广角深度相机，将提取到的视觉特征点转换为三维数据点，利用PnP估计相机帧间运动，但整体采用VIO的策略[6]。

上述算法虽进行了激光雷达和视觉相机数据融合，但在解算过程仅采用单一传感器进行运动估计，而非采用紧耦合的融合方式。与这些方法不同，本文提出了一种融合激光雷达和相机的紧耦合的帧间配准方法，将点云特征和视觉特征共同加入帧间配准误差方程进行求解。同时为了增加程序鲁棒性，在进行点云预处理过程中，本文提出了一种基于随机一致性采样（Random Sample Consensus, RAN- SAC）的地面点云提取方法，并将地面点云作为一种约束加入帧间配准误差方程。改进的帧间配准算法流程如图1所示。

图1 算法流程图

1 算法框架

该流程主要包括预处理部分和运动估计部分。点云预处理部分包括利用惯性导航单元（Inertial Measurement Unit, IMU）去除激光点云畸变、提取地面点云、依曲率提取边角点云和平面点云。视觉预处理部分包括提取Fast特征和光流跟踪。完成上述过程后，利用预处理得到的数据进行紧耦合的运动估计。

1.1 点云预处理

1.1.1分割地面点云

地面提取算法流程如图2所示，在实际场景中，地面并非完美平面而是具有一定的弧度，但为方便配准方程的构建，仍采用平面模型进行地面提取。场景中的墙面也会对平面模型提取过程造成干扰，因此首先依据雷达实际安装高度利用直通滤波提取候选点云。为增加提取速度，直通滤波通常区间长度通常较小。当地面本身具有坡度或激光雷达安装误差问题，将导致地面与激光雷达坐标系的平面具有一定夹角，该情况使得候选点云中包含较大比重的非地面点云，影响地面提取效果。为解决上述问题，本文逐渐增加候选点云数量获得多个平面模型，利用模型筛选单帧点云中符合该模型的内点数据。若相邻两次内点数据变化幅度不大，则认为提取到了正确的地面模型。

平面提取过程利用的是随机一致性采样（Random Sample Consensus, RANSAC）算法，该算法是从一组包含异常数据的观测数据集中估计其数学模型参数的迭代方法[8]，是一种不确定算法，通过重复选择数据中的一组随机子集来达成目标。

图2为地面点云提取效果，图中显示的是单帧点云数据，其中彩色点云为地面点云，可以看出该方法成功提取到大部分地面点，能够为运动估计阶段提供正确的地面特征。

图2 地面点云提取效果

1.1.2提取点云特征

特征点云分为边角点和平面点，分别计算每个点的曲率，式（1）为曲率计算公式。()代表第帧点云中第个数据点，代表()的邻近点集。

依据曲率阈值将所有点分为边角点和平面点。为使数据分布均匀，将点云划分为多个子区域，每个子区域中提取固定数量的特征边角点和特征平面点。若某个数据点被选为特征点，其邻域内的点则不予考虑，避免特征点过于集中，使运动估计过程陷入局部最优的状况。

1.2 提取视觉特征

采集单帧图片中的FAST（Features from Accele- rated Segmentst）特征，利用LK光流法在帧间进行特征像素追踪。

LK光流[9]算法原理基于三个假设：（1）亮度恒定，即连续的前后帧图片中相同物体的像素值大小不发生变化；（2）时间连续，即在连续前后帧的图片中物体的运动状态是缓慢变化的；（3）空间均匀，即图片场景中某一像素点的运动状态的变化和它相邻区域像素点的运动状态变化相同。

假设在时刻，图像中（,）像素点亮度为(,,),经过短时间∆后，其像素坐标变为(+∆,+∆)，亮度变为(+∆,+∆,+∆)，根据亮度恒定假设，得到公式（2）。

(+∆,+∆,+∆)=(,,) （2）

对上式进行一阶泰勒展开，由于∆很小，忽略高阶项，得到公式（3）。

(+∆,+∆,+∆)

（3）

将公式（2）和公式（3）联立，等式两边同时除以∆，得到公式（4）。

根据光度不变假设，等号左侧为零。根据空间均匀假设，利用特征像素的多个邻近像素构造超定方程，求方程的最小二乘解得到像素速率，实现特征点的追踪。

1.3 构建误差方程

1.3.1特征点ICP约束

传统ICP算法需已知两帧点云中数据点的对应关系，将前帧点云利用帧间位姿变换到后帧坐标系下，以对应两点间的距离作为误差项，迭代求解变换矩阵。

在实际场景中，没有数据点之间的对应关系，同时高线数激光雷达数据量庞大，用全部数据点构建误差方程进行帧间匹配难以达到实时性要求。因此，该部分采用经典激光SLAM算法LOAM中的特征点ICP匹配。将点云预处理阶段的特征点利用帧间位姿变换到目标坐标系下，在目标坐标系中使用k-维树[10]（k-dimensional tree, kd-tree）搜索边角特征的两个最近边角点和平面特征的三个最近平面点，得到特征之间的匹配关系。以两帧特征间的距离1(T+1,k)作为误差项，变换矩阵T+1,k作为待优化变量，进行迭代优化计算，见公式（5），其中为边角特征到对应边角线的距离，为平面特征到对应平面的距离。

1.3.2地面约束

在较为空旷的环境中，点云数量会大幅减少。此外，由激光雷达存在水平角分辨率，导致单线距离雷达较远数据点间有更大的间距，从而影响数据点曲率的计算，可能导致错误的数据点分类，降低特征点ICP匹配的精度。相比提取到的特征点，地面点云在两帧间连续，更具稳定性。因此，将地面点云作为约束加入误差方程，能降低错误匹配特征点带来的影响，使匹配算法更加精确、鲁棒，见公式（6），式中J代表点到平面距离误差，P代表第帧中提取的地面点云，F,k+1代表从+1帧地面点云中提取的平面模型。

2(T1,k)=min‖J(P,F+1)‖ （6）

1.3.3视觉特征约束

由于激光雷达角分辨率的缘故，实际点云并不连续，随着雷达线数降低越发稀疏。而相机图像携带的场景信息更加连续致密，大部分像素无法利用外参和激光数据获得深度。因此，本文利用对极约束和两帧间特征像素的对应关系解算出不含尺度信息的位姿变换，将该变换中的旋转加入误差方程中进行约束，加快结果收敛速度。公式（7）中T1,k和T1,k分别代表视觉特征获得的帧间旋转和待求的角度旋转，J(P,P)代表两种旋转间对应欧拉角的差值。

3(T1,k)min‖J(T1,k,T1,k)（7）

联立以上各式，构建帧间位姿变换误差方程，待求变量为T1,k，利用高斯牛顿法对其进行迭代求解，完成紧耦合的运动估计过程。相比于A-LOAM中单纯利用特征点作为运动估计的约束，本文算法额外添加了地面约束和视觉旋转约束，能够对旋转和轴向位移做出更严格的限制，从而在有限的迭代轮数中获得更精确的前端配准结果。

2 实验与结果分析

本文的实验平台为Inter Core i5-4210H, 2.90 GHz，4 GB内存，操作系统Ubuntu 16.04。以上述算法替换A-LOAM中的前端配准模块得到改进算法，完成智能车位姿估计并构建地下停车库点云地图。利用SLAM领域开源工具EVO绘制智能车轨迹，计算智能车的绝对位姿误差和相对位姿误差，并与改进前的算法A-LOAM进行精度对比。

图4为本文算法与A-LOAM的轨迹对比图，其中深色线条为A-LOAM算法轨迹，浅色线条为改进后算法轨迹。从图中可以看出在路径前半部分两种算法得到的结果相差无几。但在后半程中，A-LOAM算法出现了明显的漂移，而本文算法表现相对更好。

图4 轨迹对比图

表1为两种算法相对位姿误差的结果对比，该项指标与绝对误差不同，其计算两帧间的位姿变换并与真值进行结果对比。即便某次帧间变换误差过大，并不影响后续误差的计算，能够对全局位姿估计误差作出更为公正的判断，评价结果更全面。因此，本文将相对位姿误差作为算法精度的判定标准。该项指标中，最大值下降约59.67%，平均值下降约29.09%，中位数下降约27.66%，标准差下降约31.71%，均方误差下降约30.43%。

表1 相对位姿误差

绝对误差A-LOAM本文算法 最大值0.041 90.016 9 平均值0.005 50.003 9 中位数0.004 70.003 4 标准差0.004 10.002 8 均方误差0.006 90.004 8

SLAM算法框架的前端部分实时性要求较高且需要进行传感器数据预处理，较长时间的解算过程将会影响后续传感器数据的读取，因此前端配准精度一般较低。而算法框架的后端优化部分在前端配准结果的基础上进行点云帧到局部点云地图的高精度位姿估计，更为准确的前端结果能有效加快算法的收敛速度，从而在相同的迭代次数下获得更准确的结果。

3 结语

本文提出了一种基于激光视觉融合的SLAM前端帧间配准方法，通过耦合多种数据，添加约束使配准结果更加精确。实验结果证明，改进算法相比改进前精度提升约30%。同时，添加的视觉信息能够保证在一些退化环境中仍得到正确的旋转约束，提升了前端帧间配准算法的鲁棒性。

[1] 王锦凯,贾旭.视觉与激光融合SLAM研究综述[J].辽宁工业大学学报(自然科学版),2020,40(06):356-361.

[2] J.Zhang,S.Singh.Visual-lidar odometry and mapping: Low- drift, robust, and fast[C].2015 IEEE International Conference on Robotics and Automation (ICRA),2015,2174-2181.

[3] Y. Xu, Y.Ou and T. Xu. SLAM of Robot based on the Fusion of Vision and LIDAR[C].2018 IEEE International Confe- rence on Cyborg and Bionic Systems (CBS), Shenzhen,2018, pp.121-126.

[4] Y.Shin,Y.S.Park and A.Kim.Direct Visual SLAM Using Sparse Depth for Camera-LiDAR System[C].2018 IEEE Internati- onal Conference on Robotics and Automation (ICRA), Brisb- ane,QLD, 2018, pp. 5144-5151.

[5] 欧阳毅.基于激光雷达与视觉融合的环境感知与自主定位系统[D].哈尔滨:哈尔滨工业大学,2019.

[6] Graeter J, Wilczynski A, Lauer M. LIMO: Lidar-Monocular Visual Odometry[C], IEEE/RSJ International Conference on Interlligent Robots and Systems(IROS), 2018 7872-7879.

[7] J.Zhang, S. Singh. LOAM: Lidar Odometry and Mapping in Real-time[C]. Robotics:Science and Systems Conference.2014.

[8] Fischler M A,Bolles R C.Random Sample Consensus:A Para- digm for Model Fitting with Applications to Image Analysis and Automated Cartography-ScienceDirect[J].Readings in Computer Vision, 1987:726-740.

[9] Lucas B D,Kanade T.An Iterative Image Registration Techniquewith an Application to Stereo Vision[C].Proceedings of the 7th International Joint Conference on Artificial Intelligence. Morgan Karfmann Publishers Inc.1997.

[10] Bentley J L,Friedman J H.Data structures for range searching [J]. ACM Computing Survyes,1979,11(4):397-409.

Frame-matching Method Based on Laser and Vision Fusion

HE Zhongwei, ZHANG Xiaojun, ZHANG Minglu

( School of Mechanical Engineering of Hebei University of Technology, Tianjin 300132 )

Simultaneous location and mapping (SLAM) is an important guarantee for robot to realize self navigation ability in unknown environment. At present, the main sensors used in SLAM algorithm are lidar or vision camera. Lidar can range more accurately, and visual camera can reflect the rich texture information of the environment. Compared with using a single sensor, SLAM algorithm can obtain more environmental information and achieve better positioning and mapping effect. In this paper, an inter frame matching method combining lidar and vision camera is proposed. By adding ground constraints and visual feature constraints in the process of slam inter frame matching, the accuracy of inter frame matching process is improved, the robustness of algorithm is enhanced, and the overall effect of SLAM algorithm is improved. Finally, the paper uses the collected underground parking data to verify the results, and compares with the open source algorithm A-LOAM. The results show that the relative pose error of the proposed method is improved by about 30% compared with A-LOAM.

SLAM; Laser and vision fusion; Ground extraction; ICP; RANSAC; FAST; Optical flow method; Inter frame match

A

1671-7988(2022)01-19-05

TP 24

A

1671-7988(2022)01-19-05

CLC NO.:TP 24

何仲伟，男，硕士研究生，河北工业大学机械工程学院，主要研究方向为智能车环境感知。

10.16638/j.cnki.1671-7988.2022.001.005