一种结合改进ORB视觉特征的智能汽车激光SLAM算法

李奎镇，王金刚，张小俊

一种结合改进ORB视觉特征的智能汽车激光SLAM算法

李奎镇，王金刚，张小俊*

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

针对地下车库场景中SLAM定位不准的问题，文章提出一种基于视觉和雷达信息融合的SLAM算法，将三维ICP算法IMLS-ICP（Implicit Moving Least Square-Iterative Closest Point）进行二维化，并融入入PL-ICP（Point to Line-Iterative Closest Point）和N-ICP（Normal-Iterative Closest Point）的内容，使得IMLS-ICP更加适合于二维环境，获得更好的点云匹配效果。然后，从图像中提取ORB（Oriented FAST and Rotated BRIEF）特征点进行特征匹配，并针对于地下车库场景亮度不稳定的特点，对图像进行了直方图均衡化滤波处理，并用BRIEF（Binary Robust Independent Elementary Feature）描述子进行约束，使得ORB特征点的提取与匹配更具精确性和鲁棒性，并对得到的结果进行了局部的BA（Bundle Adjustment）优化。应用加权最小二乘法进行位姿融合。通过实验对位姿进行了分析，与主流2DSLAM算法进行对比，验证文章算法的有效性。

SLAM；IMLS-ICP；ORB；直方图均衡化滤波；加权最小二乘法

前言

智能汽车SLAM问题可以描述为：智能汽车在未知环境中，在移动过程中通过传感器信息对智能汽车的位姿进行估计，进而建立增量式地图，使智能汽车获得了定位信息与高精度地图。SLAM技术的发展会极大地影响智能汽车的发展，研究智能汽车SLAM具有一定的实际意义。

多位学者对SLAM进行研究，武二永将Rao-Blackwel- lised粒子滤波器的理论融入SLAM算法中[1]，采用改进的粒子分布预测函数，提高了粒子滤波器的性能。陈白帆[2]和周武[3]等人都是对FastSLAM进行改进，陈白帆将粒子群优化加入更新预测粒子的过程中，使得粒子在实际姿态区域更集中，提高了定位精度；周武将遗传算法与FastSLAM相结合，解决了重采样样本枯竭的问题，提高算法性能。朱磊[4]等人将人工鱼群算法应用到预测粒子上，提高了粒子的定位精度。浙江大学的彭勃[5]和李永佳[6]都对Harris特征进行了分析，彭勃提出了一种基于RANSAC的视觉里程计方案，采用了Harris特征，提高了计算效率。李永佳将Harris特征与尺度不变特征相结合，构建出视觉里程计方案，具有较好的计算精度。李宇波[7]对特征区域网格化并将特征点聚类，提高了视觉里程计的鲁棒性，并通过实验平台进行了验证。北京理工大学的江燕华[8]，通过增加车辆俯仰和倾斜约束，提高了视觉里程计的精度，并进行了实车验证。乔明起[9]将神经网络理论加入视觉里程计中，设计了一种基于光流信息的位姿估计模型，提高了系统性能，通过仿真实验验证了理论的有效性。Shao-Hung Chan等人对激光的Gmapping算法和视觉的ORB-SLAM2进行融合[10]，通过雷达与相机的数据融合来提高了室内环境下的定位精度。

单线激光雷达在处理地下车库场景时，由于点云信息不够丰富且场景存在动态物体，容易发生误匹配，使得智能汽车定位发生偏移进而使得SLAM过程失败。采用视觉和雷达信息融合方法，将视觉传感器信息嵌入到雷达SLAM框架中，让车辆在地下车库的场景实现精确的建图和定位。

1 激光点云匹配算法研究

1.1 运动畸变去除

运动畸变是由于激光雷达的运动而产生的，旋转一周的时间记为，激光收发元件在时间内按照次数发送和接受激光束如图1(a)，理论上运动的激光雷达的点云，认为激光的一帧所有点的激光发射位置与激光接受位置是同一位置。实际情况如图1(b)所示，激光雷达在运动过程中激光收发元件即旋转也在平移，这样导致激光雷达一帧点云数据的起始点和终止点不在同一处，会根据内的移动距离大小偏移，所获取的点云数据会对后续计算产生较大误差。

图1 激光雷达点云示意图

已知知激光帧开始时间t和终止时间t，两个激光束间的时间为△，里程计数据存储在一个队列中，并按照时间排序，队首的时间最早，队尾的时间最晚。队首的里程计数据的时间戳小于t，队尾的里程计数据的时间戳大于t，保证了里程计数据的时间域要包含当前帧的时间域。算法流程如下：

一帧激光数据的每个点都对应了一个汽车位姿，对这些位姿进行求解，即求解｛t，t+△，…，t}时刻的汽车位姿。首先求解t和t时刻对应的位姿p和p。我们将里程计数据和激光雷达数据同步即对齐时间戳，假设里程计数据队列中，时刻t和t的数据分别为第数据和第数据：

p=[] （1）

p=[] （2）

里程计数据队列中的时间戳和激光雷达数据的时间戳并不是一一对应的，如果在雷达的数据的时间戳没有里程计的数据，则进行线性差值，假设在l，时刻有位姿且l<<，可以得到：

p=[] （3）

p=[] （4）

在当前帧激光数据中，假设汽车在这段时间内做匀加速运动，则汽车的位姿与时间构成了二次函数，假设中间时刻为t，且l＜＜则：

已知p，p，p，对数据进行差值，差值方程为二次曲线方程：

将二次曲线用分段函数表示，如果分段数大于4，可以忽略误差。在t和t时间段内，提取个线性差值汽车位姿｛p，p+1，…，p+k-2，p}，假设p和p+1之间存在个汽车位姿｛p，p1，…，p（-2），p+1}则：

经过差值计算得到一帧激光数据，包含个激光点与其对应的位姿｛1，2，…，p}。将位姿坐标转换，统一到同一坐标系中：

公式（10）中，x为转化前坐标系的坐标，x’为转换后坐标系的坐标。

将得到的位姿转换为激光数据，并通过ROS话题机制进行发布：

通过激光点云的运动畸变去除，有效降低了由于雷达运动引起的点云误差。

1.2 二维场景点云IMLS-ICP匹配

雷达点云匹配采用IMLS-ICP匹配算法进行匹配[11]。筛选具有代表性的点，减少计算量提升计算速度。在点云中抽象出曲线，合理地模拟出点云的状态，进行帧间匹配。相比于其他ICP匹配算法，IMLS-ICP匹配算法结合了N-ICP[12]和PL-ICP[13]。二维情况下有着更好的匹配效果。

通过KNN算法，应用N-ICP理论的法向量提取方法切点对应点法向量，假设点p在半径内所有点为V，计算V的均值和均方差矩阵U：

对U进行特征值分解：

法向量为最小特征值的特征向量。其对应去率为：

构建点云集合p的曲线：

点x在曲线上的投影y为：

y=x−(x) n（19）

得到点x和点y的对应关系，并通过PL-ICP求解方法进行求解：

对二维下的IMLS-ICP进行验证，采用雷达数据包作为验证的数据集，将IMLS-ICP与PL-ICP进行比较，如图2所示，图中可以看出PL-ICP在第二次转弯后发生了匹配失误，误差越来越大直接偏离路径，而IMLS-ICP能够较好的呈现路径。但是累积误差不断累积，存在了较大的偏移。

2 基于直方图滤波的ORB特征匹配

2.1 直方图均衡化滤波

地下车库场景下由于灯光存在使得亮度忽明忽暗，而亮度影响图像的灰度。图像的灰度分布主要集中于较窄的区域，造成图像的对比度低，会使特征点提取很困难，进而影响特征点匹配结果，为使在不同的亮度条件下都有比较好的效果，采用直方图均衡化滤波，将原图像的直方图修改为在整个灰度区间内大致均匀分布，对集中的灰度分布进行平坦化，增强图像的对比度。直方图均衡化滤波算法的主要流程为：

（1）计算图像的灰度直方图：

（3）种次号可以反映图书的进馆时间，小号的图书都是进馆比较早的书，可以了解图书出版的先后时间顺序，剔旧可以先从种次号数字较小的图书开始剔，这样图书的剔旧工作也解决了。

（2）对原始图像进行累积直方图计算：

（3）得到滤波后图线的像素值：

公式（23）中，D是滤波后图像的像素，（S）表示灰度的图像的累积分布，为最大灰度级，最大值为255。

2.2 ORB特征匹配

假设空间中有个三维空间点P，其对应投影p和深度信信息z，三维空间点对应于世界坐标系下的齐次坐标为P= [X,Y,Z，1]，对应投影U的像素齐次坐标为[u,v，1]。未知量则是相机的位姿,。相应地有：

实践中，由于噪声的原因，上式并不严格成立，即等式存在误差。目标就是最小化误差的平方和：

这里的误差项是观测所得的像素坐标和对应空间点的三维位置根据当前估计的位姿投影到像片上的位置之间的差异，即重投影误差，如图3所示，空间点P在第一张照片上的投影为1，在第二张照片中的投影为2，由于误差的存在，投影在2’处。调整相机位姿，最小化虚线距离。忽略齐次坐标的最后一维，这个误差项是一个二维向量。此外，我们最小化的是所有点对重投影误差的平方和，所以最后得到的是一个总体误差最小的结果，而单个点的误差并不会精确为零。并对最终位姿结果应用BA优化。

图3 重投影误差示意图

图4 地下车库场景ORB特征匹配

如图4和图5所示，对地下车库场景进行ORB特征匹配。经过BRIEF描述子筛选的特征点能够更好地进行ORB特征匹配，没有出现误匹配的情况。对于直方图均衡化滤波的地下车库场景，匹配的次数明显增多，且均匀分布在图像中，有了更多更精准的ORB特征匹配。

图5 直方图均衡化滤波后的地下车库场景ORB特征匹配

3 基于最小二乘法的信息融合算法

雷达位姿和相机位姿都是描述了汽车的运动轨迹，轨迹趋势是大体一致的，两种位姿具有线性相关性。最小二乘理论融合的优点是速度快，非常适合实时环境。加入亮度权重，并对最小二乘计算的矫正矩阵进行更新，以此实现滑动，根据最近的数据来进行计算，来进行位姿融合。融合方式保留了激光的原有特性并加入了视觉的丰富信息协助定位，使得改进的SLAM系统在不同亮度条件下的对称的结构化环境中能够减少定位误差。

用雷达位姿与相机位姿构建直接线性方程：

公式（27）中，为矫正矩阵，为权重系数。

将公式27展开：

转换为矩阵：

化简：

构建超静定方程，其中一共B组数据：

得到：

将C转换为矫正矩阵。通过还进行的更新，转换为最终位姿：

p=Cp（34）

根据最终位姿与点云数据建立栅格地图。

4 实验与结果分析

4.1 定位精度对比实验

本章主要利用奇瑞eQ1小蚂蚁300汽车作为移动平台进行实验，搭载了思岚科技的RPLIDAR A1 360 度激光扫描测距雷达、乐视三合一体感摄像头和里程计等传感器，以太网、usb数据线、can总线等数据传输方式进行数据传输。通过搭载的盈驰微型电脑进行传感器的数据处理和算法运算，处理结果由副驾驶的显示器进行显示，如图6所示。

图6 奇瑞eQ1小蚂蚁300汽车

对两种状态下的位姿进行对比，一种是只应用雷达点云数据进行匹配得到的位姿laser_pose，即原始SLAM位姿，另一种是在雷达点云数据匹配的基础上加入了视觉定位和回环检测得到的位姿fusion_pose，即改进SLAM位姿。真实位姿用reference表示。实验结果如下图7所示：

图7 算法的轨迹对比图

如图8对laser_pose和fusion_pose进行APE评测：针对真实位姿，原始SLAM位姿的ape误差小于5 m，改进SLAM位姿的ape误差小于1 m。可以看出改进SLAM系统性能明显要高于原始SLAM。

图8 fusion_pose与laser_pose之间的APE统

4.2 与主流开源2DSLAM对比实验

为了验证本算法的建图质量，与主流的开源2D激光SLAM进行对比分析。主要对比的算法Gmapping、Hector和Cartographer。如图9所示。

图9 地图对比

统一比较了四种算法的性能，如表1所示。

表1 算法性能对比

算法名称帧间匹配方法优化器大范围精度使用里程计 本文算法IMLS-ICP+ORBg2o较好是 Gmapping粒子滤波无一般是 Hector势能模型无较差否 Cartographer分支定界ceres较好是

5 结束语

针对单一传感器在地下车库场景中定位不准的问题。将雷达位姿和深度相机位姿进行了线性关联，并采用加权最小二乘法去求解，并以一定的频率对结果进行更新。结合地下车库场景的特殊性，提出一种基于直方图滤波的ORB特征匹配算法。针对地下车库场景亮度变化较大这一特点，对图像进行了直方图均衡化滤波，增强了图像在地下车库场景的对比度，使得ORB特征点的提取匹配更加容易，对BRIEF进行约束，滤掉了大量误匹配，使得整个ORB特征匹配流程更鲁棒性。对于获得的位姿结果，进行BA优化，获得更好的定位效果，提高了SLAM系统的有效性。

[1] 武二永,项志宇,沈敏一,等.大规模环境下基于激光雷达的机器人SLAM算法[J].浙江大学学报(工学版), 2007(12):1982-1986.

[2] 陈白帆,蔡自兴,袁成.基于粒子群优化的移动机器人SLAM方法[J].机器人,2009,31(6): 513-517.

[3] 周武,赵春霞.一种基于遗传算法的FastSLAM 2.0算法[J].机器人, 2009,31(1): 25-32.

[4] 朱磊,樊继壮,赵杰,等.未知环境下的移动机器人SLAM方法[J].华中科技大学学报(自然科学版), 2011,39(7):9-13.

[5] 彭勃.立体视觉里程计关键技术与应用研究[D].杭州:浙江大学, 2008.

[6] 李永佳.基于视觉的机器人定位研究[D].杭州:浙江大学,2011.

[7] 李宇波.户外环境下移动机器人视觉里程计技术研究[D].长沙:国防科学技术大学,2012.

[8] 江燕华.车辆运动特性约束的智能车辆视觉里程计系统研究[D].北京:北京理工大学,2014.

[9] 乔明起.基于深度神经网络的视觉位姿估计方法研究[D].合肥:中国科学技术大学,2018.

[10] Shao-Hung Chan, Ping-Tsang Wu,Li-Chen Fu.Robust 2D Indoor Localization Through Laser SLAM and Visual SLAM Fusion[C]// Miyazaki, Japan,Japan:IEEE International Conference on Systems, Man,and Cybernetics (SMC),2018.

[11] Deschaud J E.IMLS-SLAM:scan-to-model matching based on 3D data[C]//2018:2480-2485.

[12] SERAFIN J,GRISETTI G. NICP:Dense Normal Based Point Cloud Registration[C]//IEEE/RSJ International Conference on Intelligent Robots & Systems. IEEE, 2015:742-749.

[13] ANDREA CENSI.An ICP variant using a point-to-line met ric [C]//2008 IEEE International Conference on Robotics and Auto- mation (ICRA 2008), vol.1.2008:19-25.

A Laser SLAM Algorithm Combining ORB Characteristics

LI Kuizhen, WANG Jin'gang, ZHANG Xiaojun*

（School of Mechanical Engineering, Hebei University of Technology, Tianjin 300130）

To solve the problem of inaccurate localization of SLAM in underground garage scenes, a new SLAM algorithm based on vision and radar information fusion is proposed, which uses the three-dimensional ICP algorithm IMLS-ICP (Implicit Moving Least Square-Iterative Closest Point) to work out the two-dimensional expression.At the same time, it integrates PL-ICP (Point to line-Iterative Closest Point) and N-ICP (normal-Iterative Closest Point) expression to make IMLS-ICP more suitable for two-dimensional environment and obtain better Point cloud matching efect.ORB(Oriented Fast and Rotated Brief) feature points were extracted from the image for feature matching. In view of the unstable brightness of the scene of underground garage, histogram equalization filtering was carried out on the image. Binary Robust Independent Elementary feature (Brief) descriptor was used for constraint to make the extraction and matching of ORB feature points more accurate and Robust.Local BA (Bundle Adjustment) optimization was carried out for the results obtained.Weighted least square method was used for pose fusion and raster map was built.The pose is analyzed through experiments and compared with the mainstream 2DSLAM algorithm to verify the effectiveness of the proposed algorithm.

SLAM; IMLS-ICP; ORB; Histogram equalization filtering; Weighted least squares

10.16638/j.cnki.1671-7988.2021.021.008

U495

A

1671-7988(2021)21-31-06

U495

A

1671-7988(2021)21-31-06

李奎镇（1995.11—），男，硕士研究生，河北工业大学机械工程学院学生，主要研究方向为SLAM。

张小俊，河北工业大学机械工程学院。

天津市新一代人工智能科技重大专项（编号：18ZXZNGX00230）。