基于同步定位与地图构建的色彩追踪智能小车系统

杨 森 张寿明 刘亦兴 刁子健 赵宇豪

（昆明理工大学信息工程与自动化学院）

一般情况下，智能小车可在未知环境下自主运动，感知探究并适应环境，进而对目标进行跟踪与追寻。

使用计算机视觉技术解决智能小车环境感知问题有多种方法，其中，同步定位与地图构建（Simultaneous Localization and Mapping，SLAM）方法［1］凭借着精度高、实时性好及理论依据充足等优点被广泛研究和发展。 视觉SLAM有很多种，如 单 目 视 觉SLAM［2］、双 目 视 觉SLAM 及RGB-D（red-green-blue-depth）SLAM［3～5］等。RGB-D获取像素距离很简单，因此，在计算量和算法复杂度上有非常大的优势。

目标检测是指从不同复杂程度的背景中辨识出运动目标并分离背景，从而完成跟踪、识别等后续任务［6］，从最开始的静态目标检测到目前被广泛使用的动态目标检测，从使用建模方法对图像检测问题描述并解决到通过人工智能和机器学习来解决辨别问题［7］。 路径规划问题的关键在于优化路径，以求得最小的代价［8］，A*搜索算法（A-star Search Algorithm）是一种常见且应用广泛的图搜索和寻径算法，通过使用启发式函数来指导寻径，从而保证高效地找到一条最优路径［9］。

笔者选用RGB-D SLAM方法实现室内环境全局地图的建立与小车的实时定位， 用改进后的ORB（Oriented FAST and Rotated BRIEF）特征提取算法检测特定目标，再用A*算法实现避障与路径规划，以实现智能小车在室内环境中对特点目标进行跟踪，在跟踪丢失时能够自主制定导航策略并重新搜寻目标。

1 视觉SLAM方法研究

1.1 相机模型与图像特征

1.1.1 相机模型

相机模型用于描述图像传感器的感知数学模型，并且最终因为透镜的存在，在投影时，点与点间的投影产生畸变。 为了便于研究与分析，笔者采用针孔相机模型与畸变模型来描述这种映射关系。

1.1.2 针孔相机模型

现实世界的空间点Ρ， 经过相机光心Ο投影后，落在物理成像平面上，成像为Ρ′。 针孔相机模型如图1所示。

图1 针孔相机模型示意图

针孔相机模型中涉及4个坐标系： 世界坐标系、相机坐标系、图像坐标系和像素坐标系。 假设点Ρ在世界坐标系下的齐次坐标ΡW为（xW，yW，zW，1）， 经过旋转变换和平移变换后可得点Ρ在相机坐标系下的坐标，再根据光学投影关系可以获取成像点在图像坐标系中的坐标，最后经过采样和量化得到对应的像素坐标系下的齐次坐标为（u，v，1）。 完整的针孔相机模型［10］可表示为：

其中，s为缩放因子；K为相机的内参矩阵，包含相机的固有属性参数［11］；T为相机的外参矩阵，包含相机位姿变换的旋转矩阵R″和平移向量t；fx，fy，cx，cy表示相机的内参，在制造相机时已经确定。

1.1.3 畸变模型

滤镜并不严格与成像平面平行，导致图像产生畸变。 径向畸变可用多项式矫正：

其中，xrad，yrad是径向畸变前的位置坐标；x，y是横纵坐标；k1、k2、k3是五维向量；r2=x2+y2。

切向畸变的矫正：

其中，xtan，ytan是切向畸变前的位置坐标；p1，p2是五维向量。

综合以上两种畸变， 得到相机的畸变模型，即去畸变后的图像像素坐标系的坐标：

1.2 图像特征提取与匹配

特征点的信息包括关键点（Key-Point）和描述子（Descriptor）。 最经典的尺度不变特征（Scale Invariant Feature Transform，SIFT） 巨大的计算量普通计算机和实时系统难以接受。 改进后的ORB特征提取算法［12］使用FAST关键点，比起SIFT特征适当降低了精度和健壮性，但是很大程度地提升了性能。 ORB特征提取算法的特点是速度快，在添加了尺度和旋转的描述后在一定程度上不受图像噪声像素和图像变换的影响。

FAST关键点属于角点，依靠检测明暗变化明显的小区域而得，FAST检测的基本思想是： 如果一个像素与临近区域的像素灰度数值差别较大，那它可能是一个角点。 其算法过程如下：

a. 在图像中选定像素Q，计算其亮度IQ；

b. 设置一定的阈值T′；

c. 将Q作为中心，选取其距离中心点距离为3的周围16个像素点；

d. 如果其16个像素点中的9个点的亮度全部大于IQ+T′或者全部小于IQ-T′，那么判定Q为特征点，此点则为FAST-9特征点；

e. 对图像中每一个点执行上述操作，并忽略距离边缘像素3个像素单位以下的像素点。

FAST关键点中存在尺度问题，相机距离更近或者更远时， 角点的特征将发生尺度缩放变换，从而产生无法重新识别的问题。 所以针对尺度和方向问题，ORB特征添加了对尺度变换和旋转变换的描述， 从而实现特征具有旋转和尺度不变性。 对于尺度不变性，ORB特征使用图像金字塔；对于旋转不变性， 加入灰度质心法（Intensity Centroid）：

a. 取一关键点图像区域B， 定义此图像块的矩med=xeydI（x，y），e，d={0，1}，x，y表示像素坐标，I（x，y）表示此像素坐标的灰度值；

d. 针对每一关键点进行上述步骤。

BRIEF是一种特征描述子， 对已经检测到的特征进行描述，是一种二进制编码描述子。 经过改进的FAST关键点也弥补了描述子不具旋转不变的缺点。 ORB特征提取如图2所示。

图2 ORB特征提取

匹配点数量过多时， 快速近似最邻近（Fast Library for Approximate Nearest Neighbors，FLANN）算法［13，14］能大幅提升匹配速度。

设当前帧中的特征点描述子X=（x1，x2，…，xn），上一帧Y=（y1，y2，…，yn），则两个特征点间的欧氏距离定义如下：

相邻帧间的特征匹配如图3所示。

图3 相邻帧间的特征匹配

由于ORB特征采用BRIEF描述子描述特征点信息，每个特征点的信息是128维的二值向量，层次聚类树匹配方式将会更加有效， 算法流程如下：

a. 在当前图像的特征点中建立树，针对每一分支随机选一个中心；

b. 从Root开始遍历，计算当前特征点与上一张图像特征点的欧氏距离；

c. 计算Root下所有叶子节点与上一张图像特征间的欧氏距离，记录其中距离最小的节点L；

d. 若L没有叶子节点，使L指向Root，聚类结束；

e. 若L有叶子节点，迭代Root为L并跳转到步骤c；

f. 若聚类结束，Root即为匹配点；

g. 对上一张图像所有特征点进行步骤a～f。

1.3 相机运动估计

利用两帧像素点间的对应，恢复出两帧间相机的运动。 通过深度相机能取到相机与每个像素的距离，所以运动估计问题具体如下。

假设通过图像的RGB信息和Depth信息得到了一组匹配成功的3D点：

其中，A和A′是相邻两帧图像中匹配成功的3D点的集合，要想求得相机的运动，即找到一个欧氏变换R′，t′使得：

此问题为ICP（Iterative Closest Point）问题。使用 奇异值分 解 （Singular Value Decomposition，SVD）进行代数求解后进行ICP过程：

c. 根据R′计算t′，t*=a-R′a′。

相邻两帧色彩图像和相邻两帧深度图像分别如图4、5所示。

图4 相邻两帧色彩图像

图5 相邻两帧深度图像

2 目标识别理论与算法研究

2.1 颜色阈值分割

RGB颜色空间是一种人类理解较为直观的颜色模型，采用RGB888格式存储数据。 HSV颜色空间在人们的视觉中表现比较直观，因此选用此模型对图像进行处理。 因相机数据为RGB888格式，而根据人视觉图像处理的直观性，需使用HSV色彩空间描述像素，因此需要转换颜色空间。

假设RGB颜色空间描述的某个像素的值A″u，v=（R，G，B）， 此像素对应的HSV颜色空间描述的像素点P ″u，v=（H，S，V）。 二者的转换关系如下：

RGB颜色（A）空间转换到HSV颜色（B）空间如图6所示。

图6 色彩空间转换

设HSV模型下图像的像素值为f（u，v），分割特征的低阈值和高阈值为D和D′，RGB模型下分割图像后的像素点值为g（u，v），于是有：

阈值分割如图7所示。

图7 阈值分割

2.2 边缘检测

针对经过阈值分割后的二值图像，图像的腐蚀用来滤除较小的图像区域的干扰，图像的膨胀用来恢复腐蚀后的图像。

图像的膨胀运算为⊕，设a为一幅二值图像，使用卷积模板b来对a图像进行膨胀，执行该操作时，有：

图像的腐蚀运算为-，同上，执行图像腐蚀操作时，有：

腐蚀与膨胀对像素体积小于模板的黑色区域进行剔除，对目标图像进行腐蚀与膨胀，从而使得目标特征更加明显。 图像腐蚀与膨胀如图8所示。

图8 图像腐蚀与膨胀

图像中的边缘信息主要集中在高频段。 在对目标色彩进行阈值分割、腐蚀和膨胀后，高频段的噪声将显著减小，图像的信息将变得更加利于使用，因此使用Canny算法检测边缘［15］。 该算法检测的步骤是：

a. 对图像进行高斯模糊；

b. 计算梯度；

c. 非极大值抑制；

d. 连接边缘。

根据边界跟踪算法来对边缘点进行最终筛选。 如果弱边缘点和强边缘点相连，则均保存，否则删除弱边缘点。 算法从第1个点开始搜索：

a. 如果此点为弱边界且没有被标记，将其标记放入栈与队列中并进入下一步，若此点已经被标记或者不是弱边界，则重复此过程以遍历整幅图像。

b. 从栈中取出元素并遍历周围的8个点，如果其中一像素点是弱边缘且没有被标记过，则标记这个像素点，并加入栈与队列中。 如果其中一个像素点是强边缘，则表示此弱边缘点和强边缘点连通，保留标记。 重复此过程，以遍历栈中的所有元素， 若是不存在强边缘点则清空栈与队列，然后使用下一个像素执行步骤a。

高斯模糊与边缘检测如图9所示。

图9 高斯模糊与边缘检测

2.3 位姿估计滤波器

其中，状态转换矩阵M将系统因果体现出来，在这里M阵为一个两行两列的常数阵。

图像中目标的运动是任意的， 所以在x轴方向和y轴方向上不存在耦合关系，故：

进行以下迭代步骤以预测：

滤波器输入与滤波器效果分别如图10、11所示。

图10 滤波器输入

图11 滤波器效果

3 简单色彩目标追踪的实现研究

3.1 基于ROS的小车导航

要在ROS（Robot Operating System）上实现小车的导航，就必须研究ROS提供的接口［16］。ROS导航的基本框架，即navigation框架，其中最主要的是Move_base节点，简化该节点的输入输出如图12所示， 其中，Map是之前建立好的室内环境的地图；Point Cloud是RGB-D相机的信息；Odometry即里程计， 是使用车轮上的编码器来实现的；Transforms表示小车主体、相机和车轮之间的中心坐标变换关系；Localization即定位信息；Goal Pose是要设定的目标点； 输出信息为Velocity Commands，即速度控制信息，这部分输出信息直接发送给小车底层的驱动板用于驱动小车运动。

图12 Move_base输入输出特性

3.2 路径规划

A*算法根据代价函数优化，将搜索区域划分成简单的二维向量，向量中每个元素对应一个搜索区域节点，使用启发函数来计算路径代价并优化以降低此代价。 A*算法演示如图13所示。

图13 A*算法演示

A*算法的具体流程如下：

a. 将起点加入集合O，O用于存放待检测的搜索区域节点， 而集合C用来存放已经检测过的搜索区域节点。

b. 对每一个O中的节点pi（i=1，2，…，n）检查引起最小的F数值的子节点pi， 将子该节点pi移动到C中。

c. 对pi周围的8个节点pq（q=1，2，…，8）分别检查。 若不可抵达或在C中则忽略。 在其他情况下， 若不在O中则加入O中并将pi设置为pn的父节点；若已在O中，检测pi到pn的F值是否更小，如果更近则设置pn的父节点为pi，并重新计算I和F值。

d. 若将终点放到了O中，则算法结束。若查找重点失败且O为空，则算法结束。

e. 从终点搜索区域节点开始，沿着指向父节点的路径一直到初始搜索节点，即为最优路径。

其中F=I+J，I表示历史代价；J表示启发函数，可使用欧氏距离。

ROS 中 主 要 使 用 动 态 窗 口 法［17］（Dynamic Window Approach，DWA）， 使用多种速度模拟一段时间得到轨迹，对轨迹进行评价，选取最优的轨迹对应的速度来驱动小车运行。 步骤如下：

a. 建立小车的运动模型；

b. 建立速度采样模型；

c. 评价函数。

DWA算法中使用的评价函数为：

其中，heading（v′，w′）为当前轨迹结束时智能小车与目标角度差；dist（v′，w′）为当前轨迹与障碍物的距离；velocity（v′，w′）为当前轨迹的速度；σ，α，β，γ为常数。

DWA局部路径效果如图14所示， 图14a为DWA算法在阻碍区域进行局部路径规划的结果，图14b为DWA算法在开阔区域进行局部路径规划的结果。 其中，*表示半径为0.3的障碍物。

图14 DWA局部路径效果

3.3 点巡航仿真实验

根据ORB SLAM建立的稀疏地图如图15所示，其中标点为环境的角点特征。

图15 定位与建图过程中的特征提取匹配

假设地图在之前的过程中已经建好，二维地图如图16所示。

图16 完整地图

根据小车的传感器信息和定位信息，设定小车在地图上6个点之间导航，小车按照6个定点进行巡航，用于搜索目标，巡航定点应设置在视野开阔处，由路径规划控制器输出导航信息。 使用Rviz数据可视化工具可查看小车导航和路径规划的情况。 在全局目标的规划下，导航算法已经开始为小车规划好了全局路径，而图17中小车前的黑线则为局部路径规划算法所做的模拟轨迹，最终，算法会选择一条评价函数最优的轨迹输出。

图17 局部路径规划轨迹模拟

3.4 目标追踪仿真

3.4.1 识别算法检测

图18a为相机图像， 进行边缘检测后得到的结果如图18b所示，可以看出，此时目标的特征已经十分突出。

图18 目标识别

3.4.2 目标追踪检测

有了理论依据和小车自主导航的实现，只需设置一个节点运行目标检测跟踪算法，便可实现简单色彩目标跟踪。 运行效果如图19所示，可以看出，简单色彩目标在视野中已经被智能小车识别到。

图19 追踪小车演示

4 结束语

针对室内智能小车对色彩目标追踪的任务，在对SLAM问题研究的基础上， 提出使用RGB-D相机基于ORB特征构建SLAM，用于定位与建图，并对检测结果进行估计，最后根据检测算法定位信息，实现智能小车对室内场景下色彩目标的识别与追踪。 实验结果表明，将图像检测算法与路径规划算法用在基于ROS的智能小车上， 可以实现追踪与跟随色彩目标。 基于同步定位与地图构建的室内色彩追踪智能小车可以代替人类探索危险未知的陌生室内环境，并对该室内陌生环境中有简单色彩的事物进行追踪与跟随，这对于有效保障人类自身的安全和探索未知环境具有一定价值。