基于RGB-D SLAM的智能车自主避障与路径规划试验研究*

李兆凯 李龙勇 李泽晖 孔德成 宋绪丁

（1.长安大学，汽车学院，西安 710064；2.长安大学，道路施工技术与装备教育部重点实验室，西安 710064；3.一汽解放汽车有限公司，长春 130011；4.吉林大学，长春 130025）

主题词：RGB-D SLAM 自主移动小车 避障试验 路径规划 无人驾驶

1 前言

智能移动机器人能够根据外界环境及任务目标自主移动，近年来在工业领域获得了广泛应用。受应用场景的限制，机器人的定位与避障问题较难解决[1-2]。无人驾驶车辆作为典型的自主式移动平台，解决其定位与避障问题具有重要意义。

即时定位与地图构建（Simultaneous Localization and Mapping，SLAM）技术被广泛应用于机器人领域进行建图与定位工作[3-4]。根据数据源的不同，SLAM可以分为基于距离的SLAM 与基于视觉的SLAM。视觉SLAM 通过处理机器人（相机）拍摄的照片构建环境地图，成本低、场景表达能力强，被认为是未来SLAM的主要发展方向[5]。视觉SLAM 可分为单目SLAM、双目SLAM与RGB-D（Red-Green-Blue-Depth）SLAM 3种，其中，RGB-D SLAM 可以直接获取图像中物体的深度[6]。P.Henry等人在2010年首次提出RGB-D SLAM方法，并成功构建出全局一致的环境地图[7]；F.Endres 等人在2012 年提出使用Octomap 结构将环境地图转化为可用于机器人导航的栅格地图的RGB-D SLAM 方法[8]，在2014年提出生成三维地图的建图方法[9]；悉尼科技大学的Hu等人提出RGB-D SLAM 改进方法，使用光束平差法发展了一种快速建图的新方法[10]。

路径规划作为无人驾驶的关键技术之一，是自主移动机器人的一项重要功能，分为全局路径规划和局部路径规划2 类方法。A*算法作为常用的全局路径规划算法，结合了启发式方法与图搜索算法，能快速规划一条避障的最优路径[11]。动态A*算法考虑实际应用场景下障碍物的动态变化，实时规划路径，避免全局重新规划[12]，但存在着增加不必要转弯的缺点，而使用平滑的地图作为路径规划可以解决这一问题[13]。使用跳点搜索方法减少环境地图中的扩展节点，可提高动态A*算法搜索效率[14]。

本文使用RGB-D SLAM 的方法实现环境全局地图的建立与小车实时定位的功能，在全局地图的基础上应用动态A*算法实现避障与路径规划，以期实现无人小车的自主移动与避障功能。

2 算法理论基础

2.1 RGB-D SLAM

RGB-D SLAM的计算流程如图1所示，可分为前端与后端2个部分。前端进行信号采集与控制，对相机捕获的图像进行特征提取与匹配，在估算小车位姿的同时进行地图构建，前端的视觉里程计通过估计相邻图像间的关系来估计运动，并采用特征点法计算出小车的运动。后端用来处理噪声，并尽可能地估计出当前状态的不确定性（最大后验概率），所有被认为在前端可能产生噪声的数据，均可通过后端进行处理，以得到全局一致的轨迹。

在搭建前端的过程中会不可避免地出现漂移现象，但这种累积漂移可通过算法进行优化，即后端优化与回环检测。后端对全局状态进行估计与优化，主要目的是处理传感器信号所带来的噪声问题。回环检测通过小车重复经过同一位置时地图及位姿的匹配对相机轨迹和地图进行全局一致性估计，从而消除漂移量。

使用小孔成像原理对相机进行建模，通过投影变换得到相机的内参矩阵K与外参矩阵H，进而得到世界坐标系、相机坐标系及图像坐标系之间的转换关系：

式中，Xw、Yw、Zw为特征点在世界坐标系中的坐标；u、v为该点在图像坐标系中的投影坐标；Fw(X)为映射函数。

特征点是图像中能够反映本质的像素点，包含图像之间的匹配关系，是SLAM的基础支撑。本文使用ORB（Oriented FAST and Rotated BRIEF）特征点进行计算，ORB 特征点算子结合了加速分割测试特征（Features from Accelerated Segment Test，FAST）角点检测算法与二进制鲁棒独立基本特征（Binary Robust Independent Elementary Features，BRIEF）描述子算法，具备旋转、平移与光照等不变性[15-17]。

FAST 角点检测算法对每个像素点p周围各点与该点灰度值之差的绝对值与阈值εd进行比较，并统计前者大于后者的数量N，当N＞12个时，则标记p为角点：

式中，I(i)、I(p)分别为第i点、临近点的灰度值。

ORB 算子通过灰度质心法为其加入了方向特性，通过对图像金字塔上每一层图像进行角点检测，使其具有尺度不变性。BRIEF 描述子通过比较测试点对的灰度值来描述图像，其表达式为：

式中，fnd(p)为P点描述子的值，其结果的二进制形式等价于将式（4）得到的值串联形成的向量；nd为特征点周围按照选定方法挑选的“点对”的数量；xj、yj为以特征点为中心的邻域窗口中选取的随机点。

通过将测试点对与对应的旋转矩阵相乘，使得特征描述子具有旋转不变性。本文选择汉明距离作为特征之间的相似性度量，增加了筛选错误匹配的条件，利用次优匹配来进一步剔除错误的匹配。经前后帧的特征点匹配后，采用迭代最近点（Iterative Closest Point，ICP）算法进行相机的位姿估计。ICP算法对2帧图像中匹配的特征点建立欧拉变换公式，对每组图像建立最小二乘问题：

式中，J为待优化目标；pi、为匹配的特征点；R为2帧图像的旋转矩阵；t为平移向量；n为运动中检测到的特征点数量。

使用奇异值分解（Singular Value Decomposition，SVD）算法求解该问题可得到2帧图像对应的位姿变化。

本文根据相机位姿的距离及2 帧照片之间的时间差选择采集的部分图像作为关键帧，减小后端优化计算量，构建局部地图。图像间的距离为：

式中，Ra、Rb分别为2个图像帧的旋转矩阵；ta、tb分别为2个图像帧的平移向量；r为权重系数，默认取1，本文中由于旋转对应的相机位姿变化较大，取1.6。

如果当前帧被判定为关键帧，根据位姿估计得到每个关键帧对应的相机位姿及运动轨迹，将该关键帧对应的三维点云地图加入到局部地图中，并对局部地图中的相机位姿与路标地图点进行优化。通过对关键帧的检测，识别之前访问过的场景，增加图模型的回环约束，消除累积误差，得到全局一致的地图。

2.2 动态A*算法

A*算法是一种基于状态空间的、广泛应用于机器人路径规划的启发式搜索算法，能够根据起始点、目标点以及障碍物位置进行启发式搜索，得到一条避免碰撞的最短路径。其基本数学模型为：

式中，F(n)为当前节点的代价估计函数，即经过节点n的最优路径消耗的总能量；H(n)为从n点到达终点的最优路径消耗的能量；G(n)为从起始点到n点的最优路径消耗的能量。

其中选择n点到终点的欧式距离作为最优路径消耗的能量：

式中，tx、ty为目标点坐标；nx、ny为当前节点n的坐标。

从起始节点出发，依次对当前节点的代价函数进行计算，用子节点中权重最小者对当前节点进行更新，直到遍历所有节点，将障碍物的代价函数赋值为无穷大。对路网中所有节点赋值后，从目标点反向寻找最优路径，算法每次在扩展时，均选取F(n)值最小的节点作为最优路径上的下一个节点。特殊地，如果H(n)=0，则表示无任何可利用的当前节点与终点的信息，此时，A*算法退化为非启发式的迪杰斯特拉（Dijkstra）算法，其搜索空间随之变大，待处理节点的数量增多，搜索时间相应变长。例如，运用迪杰斯特拉搜索算法的可视图法计算每个节点到其他所有节点的最短路径，主要特点是以起始点为中心向外层层扩展，直到终点为止，由于它遍历计算的节点很多，因而效率很低[18]。

A*算法在全局网络中，依据扩展节点，选择当前“代价”最低的节点进行下一步搜索，直到搜索到终点，从而规划出“成本”最低的路径。在整个路径规划过程中，由于省略了大量无谓的搜索路径，因而在计算效率方面具有一定优势。

此外，不同于基本A*算法，动态A*算法通过调整路径规划范围，针对场景地图变化需重新规划路径时不再进行全局规划，而是保留已通过路径，并以当前位置为起始点进行二次规划，从而减少了计算量。

3 自主移动小车设计

3.1 硬件设计

小车采用四轮式底盘，其中前部为2 个独立驱动轮，实现驱动与差速转向功能，后部为2个相互独立、释放转动自由度的万向轮，避免在车辆移动过程中的滑移量干扰。选用额定功率为300 W的8寸轮毂电机（车轮直径约200 mm）作为驱动电机，采用无刷直流（Brushless Direct Current，BLDC）驱动器驱动车轮转动。选择STM32F103ZET6 作为底层控制器，采用1 000 线AB 两相编码器测速。自主移动小车的结构如图2所示。

图2 自主移动小车

小车搭载Kinect v2 深度相机作为主传感器，该相机能够同时获取环境的色彩及深度信息，如图3所示。

图3 自主移动小车主传感器

3.2 差速运动数学模型

自主小车的底盘采用双轮毂电机驱动与转向控制方案，因此，差速运动数学模型成为小车实现精确控制的基础。图4为小车差速运动模型。

图4 小车差速运动模型

图4中，A(xl,yl)、B(xr,yr)、M(x,y)分别为左轮、右轮及左右轮轴心连线中点的坐标，ω、vM分别为小车角速度、线速度，vl、vr分别为左、右轮线速度，常量R为车轮半径，θ1为位置1与位置2左、右轮轴心连线延长线的夹角，θ2为从位置1 运动到位置2 后的左、右轮轴线连线与x轴的夹角，θ3为相邻时刻航向角增量，l为轮距。

在微小时段Δt内，小车左轮驶过弧长为：

小车右轮驶过弧长为：

小车车体转角增量为：

若Δt足够小，可认为：

若Δt非足够小，则有：

假设初始状态下，A、B点连线（向量AB）与x轴的夹角为β，则t时刻小车行进方向与x轴夹角θ为：

且有：

小车车体转动角速度为：

由于M为A、B点连线中点，因而在任意时刻，小车M点运动线速度为：

小车M点运动半径为：

图示时刻A、B点连线与x轴平行，假设AB转过一角度θ后变为A1B1，定义逆时针旋转为正方向，在任意时刻，由几何关系可得：

式中，xl、xr与yl、yr分别为小车左、右轮心的横坐标与纵坐标。

定义BC为B点的速度，定义γ为x轴单位向量旋转至与BC平行所得夹角，以逆时针旋转为正方向，由几何关系分析可得，任意时刻下：

因而，

定义AD为A点的速度，定义φ为x轴单位向量旋转至与AD平行所得夹角，同理，有：

因而，

根据式（17）将式（21）、式（23）的速度矢量写为分量形式，有：

假设初始时刻M点坐标为M(x1,y1)，因而M(x,y)运动轨迹参数方程为：

3.3 下位机程序设计

下位机程序面向自主小车的底层控制，在Keil环境下编写，主要实现小车行进的闭环控制、数据采集以及数据通信功能。主程序流程如图5所示。

图5 下位机主程序流程

STM32作为控制器，接收控制信息，控制小车行进，并通过其自带的编码器模式，利用定时器获取传回的脉冲信号，对脉冲信号进行滤波处理，再对单个脉冲所代表的路程进行分析，尽可能精确地反馈小车的实际运动，单片机将得到的里程计数据进行处理之后，通过串口发送给上位机。底层的程序主要包括CAN总线的通用异步收发传输器（Universal Asynchronous Receiver∕Transmitter，UART）的串口通信、模数转换器（Analogto-Digital Converter，ADC）、数模转换器（Digital-to-Analog Converter，DAC）及脉冲宽度调制（Pulse Width Modulation，PWM）方波输出、定时器的输入捕捉和对编码器的正交解码以及对IO口的控制。通过对这些底层资源的操作，实现了小车的闭环控制、传感器的数据采集和传感器相互间以及与上位机之间的数据通信功能。

3.4 整车软件系统设计

小车的上位机搭载Ubuntu 16.04系统，Ubuntu系统内安装的机器人操作系统（Robot Operating System，ROS）作为小车上位机程序的运行平台。在ROS中实现用户界面（User Interface，UI）、人机交互、图像处理、SLAM、路径规划、运动决策等功能并通过控制串口收发实现对小车运动的控制与传感器信号的获取。小车的下位机包括电机控制、编码器数据采集、里程计数据融合等模块。小车的上、下位机子模块及其相互关系如图6所示。

图6 自主移动小车功能模块及其关系

图7 所示为本文无人小车基于特征点法搭建的RGB-D SLAM系统流程。

图7 基于特征点法的系统流程

其中，Kinect v2传感器采集周围环境信息数据并通过USB 3.0 串口传到上位机，上位机通过驱动软件Freenect 获取景深数据。景深摄像传感器最先获取点云信息，将其处理后，通过颜色显示出实际的2D深度信息，作为景深摄像头的原始数据（见图8），为后期建图提供必要的数据支持。

图8 传感器数据获取

在获取点云数据后，结合之前的里程计信息与数据的后处理工作进行地图的实时构建。同时，得到原始数据后，在对数据进行后处理的基础上完成回环检测，实现小车的实时定位与建图，如图9所示。

图9 建图过程

获取地图后，通过全局与局部的方式进行路径规划，获取小车最佳行进路线，完成避障。

4 试验研究

4.1 算法可靠性验证

将无人小车置于开阔的室外环境，验证RGB-D SLAM 及动态A*算法的可靠性。设定小车的目标地点为前方路口，正前方路径上的灌木丛为障碍物。试验中RGB-D SLAM 处理后的图片如图10 所示，结果表明，RGB-D SLAM能够建立未知环境的稀疏点云地图，为小车提供定位与地图信息。

图10 RGB-D SLAM验证试验

4.2 自主避障试验

简单场景下的自主避障试验在室外环境进行，选定灌木丛为障碍物，试验中小车的避障路径如图11 所示。小车未观测到障碍物时，朝着正前方直线运动，见图11a、图11b；小车检测到前方障碍物后，开始向右转弯，避开障碍物，见图11c、图11d；绕过障碍物后小车继续沿直线行驶，见图11e、图11f。

图11 自主避障试验

简单场景下的自主避障试验表明：RGB-D SLAM与A*算法结合能够实现未知环境的地图构建与无人小车的实时定位功能，并能够根据地图中出现的障碍物实时改变运动路径，实现自主避障功能。

4.3 综合试验

以上试验表明小车具有自主行驶与避障能力。综合试验在更加复杂的场景下进行，对无人小车的路径规划能力进行测试。综合试验分为2 个部分：首先，在没有先验环境信息的条件下进行路径规划试验，考察小车完成自主建图、定位及路径规划任务的能力；第二，在首次试验建立的场景栅格地图基础上，进行小车在已有路网信息下的定位及全局路径规划试验。

4.3.1 无先验环境信息

测试环境为十字路口场景，道路的前方与左侧存在车辆（视为障碍物）。为考察小车在未知场景下自主捕获环境信息、进行场景分析及路径规划的能力，试验中，将无人小车置于正对路口处，测试其能否避开障碍物并从右侧路口驶离。

试验过程中小车的运动轨迹如图12所示。

如图12a、图12b所示，小车驶入未知环境后沿着直线行驶，建立的局部地图中出现障碍物1（直线行进方向的前车）后，小车退回到左侧入口处，执行左转策略，驶入左侧出口，避开障碍物1，如图12c、图12d所示。小车在左侧通道内继续行驶，发现障碍物2（左侧车）后，执行调头策略，返回到点云地图上第1个路口的右侧出口，通过右侧出口驶离试验场景，如图12e、图12f所示。

图12 未知场景下小车自主行驶试验

由试验过程可见，小车在移动中不断进行建图与定位，在全局地图未知的情况下，根据已有的局部地图规划出最优路径，实现了复杂场景下的建图、定位及路径规划工作。在行进过程中建立的增量式点云地图实现了小车的实时定位；根据所建地图，规划出最短的驶离路线。当障碍物1出现后，动态A*算法在保留已有路径规划的基础上，在当前点重新规划出最短路径，驶向左侧通道；之后，建立的点云地图上出现障碍物2，小车继续重新规划路径，掉头后驶向右侧通道，最终驶离试验环境。

试验结果表明，RGB-D SLAM 能够实时构建局部地图以确定小车的位置，并能够在建立的点云地图上检测到障碍物位置，规划出最短路径。试验过程中小车进行了3 次动态路径规划，每次基于当前点规划最短路径，最终驶离试验场景，由此可见，动态A*算法能够应对障碍物动态出现的场景，并能二次规划出最短路径。

4.3.2 有先验环境信息

为测试动态A*算法的全局规划能力，比较全局规划与局部规划的差异，相同场景下，利用第1 次试验建立的全局地图进行全局路径规划试验。试验中，小车的行驶路线如图13 所示。在开始阶段，小车沿着直线运动，见图13a、图13b；运动至路口左侧附近，直接执行了右转策略，见图13c、图13d；进入右侧通道后驶离，见图13e、图13f。

图13 已知场景下小车自主行驶试验

试验中小车基于全局地图只进行1次路径规划，在左侧路口附近直接右转驶离。与未知环境下路径规划相比，小车不需要进行地图构建、障碍检测工作，减少了路径规划的次数与运动路径的长度。

综合试验表明，RGB-D SLAM 与动态A*算法能够满足未知环境下小车自主运动及动态路径规划的要求。2 次试验的对比表明，与局部路径规划相比，有先验环境信息的全局路径规划能够一次规划出最优路径，基于RGB-D SLAM 建立的点云地图可很好地为全局路径规划提供地图信息。

5 结束语

本文基于RGB-D SLAM与动态A*算法设计搭建了自主式无人小车硬件及软件系统，并进行了试验研究。结果表明：采用RGB-D SLAM与A*算法相结合的方法，可实现无人小车在未知环境下的地图构建与实时定位功能，完成小车运动路径随地图中所现障碍物的实时改变，使得小车具备自主行驶与避障能力，该方法也在一定程度上解决了GPS 信号缺失条件下的无人小车定位与导航问题；采用动态A*算法，无人小车能够很好地应对动态出现的障碍物，并能二次规划出最短路径，实现了复杂场景下的避障与路径规划功能；具有先验环境信息的无人小车可一次规划出全局最优路径，减少了规划次数、缩短了运动长度、提高了执行效率。