室内移动机器人自主导航系统设计与方法

翟敬梅，刘 坤，徐 晓

(华南理工大学 机械与汽车工程学院，广东 广州 510640)

0 引言

移动机器人自主导航在智能制造工厂、智能仓储、车间装配和物流运输等领域有着广泛的应用[1]。移动机器人自主导航主要涉及到环境检测、动态目标识别和跟踪，以及路径规划与优化问题。

目前室内移动机器人进行环境感知及障碍物检测时搭载的传感器主要有超声波、红外、激光雷达和视觉等，或上述的组合。环境检测有局部的，也有全局的。视觉传感器最大限度获取环境图像信息，提供更为丰富的输入数据，且其价格、功耗较低，在移动机器人平台上得到了广泛运用[2]。障碍物检测方面，基于颜色、纹理、特征点等图像特征将图像分割为障碍物、地面和背景区域，相比单目检测只能进行平面测量，立体视觉检测可基于三维点云或视差计算获取障碍物位置信息[3]。

目标跟踪包括初始化、运动模型、特征提取、外观模型和更新机制5个部分。目标初始化有人为和自动两种方式。运动模型描述连续帧间的运动关系，典型模型有均值漂移、滑动窗口和粒子滤波。目标跟踪时特征描述及其提取直接决定了跟踪的效果，目前采用的特征有人工特征和学习特征。人工特征包括灰度、颜色、梯度和纹理等外观特征和运动特征，运动特征运用较为广泛的是光流法，光流法与其他外观特征结合使用更具鲁棒性。随着机器学习的发展，特征可以在模型训练中自主学习，称为学习特征。深度学习通过搭建深层次神经网络模型，运用训练好的网络模型在各层中依次获取颜色、纹理、梯度等特征[4-5]。然而深度学习需要大量相似的样本来训练，为保证跟踪模型的准确性，往往还提供一定数量的跟踪目标样本信息，在实际应用中有一定的局限性。深度学习模型比较复杂，通常是基于经验和试验，而且计算量远远大于传统跟踪算法，需要图形处理器(Graphics Processing Unit, GPU)或神经网络处理器(Neural-network Process Unit, NPU)等支持，获取高效准确的学习模型是一项很大的挑战[5-6]。外观模型在跟踪算法中处于核心位置，目前产生式模型和判别式模型是两种主流分类形式。产生式模型只针对目标外观特性，常用算法有基于模板模型、基于子空间模型、基于稀疏表示模型等算法。判别式模型充分利用背景和前景信息，其算法可以看成一个二分类问题，因此依赖于训练样本的数量和质量[7]，主要算法有支持向量模型、Boosting模型和朴素贝叶斯模型等。两类算法各有优劣，当训练的样本数量较多时，判别式模型具有较好的跟踪结果，但训练过程会降低跟踪效率；当有限数据可用时，产生式模型具有更好的泛化能力。

不同于上述两类方法，时空上下文方法[8]对目标位置的确定使用了时间和空间的局部相关性辅助信息，通过计算目标及其周围区域的低阶特征进行统计建模，利用置信图进行最大似然估计，预测出目标位置。算法本身计算量较小，且在应对短时部分遮挡时具有较高的稳定性。目标跟踪过程中，目标姿态和环境信息在时刻发生变化，如目标旋转、背景干扰，目标遮挡、光线变化等情况，为了适应目标外观和环境变化，需要对外观模型进行实时更新。

移动机器人路径规划方法主要分为全局路径规划和局部路径规划。全局路径规划方法主要有自由空间法、栅格法、可视图法等，这些方法只适用于环境先验信息已知的场景[9]；局部路径规划算法主要有人工势场法和快速扩展随机树算法等，这些方法适用于环境未知或部分未知的场景，障碍物位置、尺寸等信息由各传感器实时获取，能够实时进行动态路径规划。快速扩展随机树算法基于随机采样点生成路径，使得路径不平滑、非最优和缺乏确定性[10]。人工势场法操作简单、易于实现，但存在局部极小值等问题。近年来，一些智能算法如遗传算法、模糊逻辑算法、粒子群优化算法和蚁群优化算法等与传统路径规划算法结合，在优化最优路径求解的同时提高了路径规划的效率[11]。

本文设计了一个室内环境下移动机器人自主导航系统平台。基于单目和双目视觉信息融合，构建全局实时地图；提出了特征点辅助的改进时空上下文目标定位方法；改进了速度向量场路径规划及优化方法。

1 机器人自主导航系统框架

移动机器人自主导航中需要：①障碍物尺寸及实时位置信息;②移动机器人实时位置和姿态信息;③目标实时位置信息。

在大的室内场景下，移动机器人要感知周围环境信息，检测障碍物并进行避障。避障的同时需要获取目标位置信息，由于视觉传感器视野范围有限，移动机器人在向目标点行进和避碰过程中可能使目标离开视野范围。因此，本文设计了“双目视觉+单目视觉”组合式的环境信息获取方案，系统框架如图1所示。

(1)双目视觉系统 双目相机以Eye-in-hand方式水平放置于移动机器人平台上，获取更精确的移动机器人周边环境信息，以此获取障碍物尺寸、位置，以及运动可行路面信息。

(2)单目视觉系统 广角摄像头以Eye-to-hand方式置于移动机器人工作区域上方，获取目标和移动机器人信息。基于该信息，可进行目标、移动机器人的识别和定位，以及动态目标跟踪。

2 地图构建

双目视觉系统检测移动机器人周边环境，获取障碍物相对于机器人的位置和尺寸信息，从而构建局部地图。单目视觉系统实现室内环境中机器人和目标体的识别和实时定位。通过坐标转化，将两者信息有机融合，可以获取全局环境下障碍物、目标和机器人空间位置、尺寸信息，构建出全局地图，为机器人路径规划提供必要的信息。

2.1 双目视觉下障碍物检测与测量

2.1.1 融合色彩饱和度信息的Retinex阴影消除

障碍物检测时，往往存在地面阴影等干扰。单尺度Retinex算法[12]基于图像增强原理消除阴影，运用高斯滤波后图像近似预估光照分量L(x,y)，得到物体本质色彩信息，以此得到阴影消除后图像R(x,y)。然而，人为选择模糊尺度时，阴影消除中易出现过度增强，导致地面与障碍物灰度差较小而影响后续障碍物的提取。

将图像色彩饱和度信息S(x,y)与R(x,y)相融合，消除阴影的同时增强障碍物与地面的灰度差。通常，光照较大时阴影较为严重，因此引入图像环境亮度信息V(x,y)，计算融合权重，自适应地调节光照不同对阴影消除的影响。算法流程如表1所示，图2所示为室内3个障碍物阴影去除过程。

表1 阴影去除算法

续表1

2.1.2 区域分割与障碍物检测

对融合图像I(x,y)进行种子填充，获得图3a中的填充地面。运用二值化和闭合运算去除较小的噪声区域，再提取地面和障碍物的边缘获取地面和障碍物区域，如图3b所示。如图3c所示为环境信息检测结果，将场景分割为背景区域(灰色)、障碍物区域(白色)和地面区域(黑色)，障碍物和背景为非可行区域，地面区域为可行区域范围。

双目相机进行标定和立体矫正后，以障碍物区域块为模板在右相机图像中进行匹配，计算出障碍物中心点三维坐标和障碍物尺寸[13]。移动机器人导航中障碍物距离和宽度信息最为重要，表2列出了图3所示障碍物在机器人当前视野下的宽度和相对距离测量结果，以及检测时间(core i5处理器，主频2.50 GHz环境下)，结果满足了移动机器人避障导航精度和实时性的要求。

表2 障碍物测量信息

2.2 基于单目视觉的移动机器人定位

为了识别机器人并对其定位，设计机器人靶标如图4a所示。在白色的A4纸内设置一个粉红色矩形框，将图4a转化为HSV颜色空间，根据H分量下设定的先验阈值分割获取粉红色区域如图4b，获取靶标区域掩码如图4d，同时由S分量分割获得图4c区域。在掩码区域对图4c进一步精确分割得到图4e的A4纸区域图像。计算图4e白色A4纸区域面积得到重心点，则图4a中离重心点最近的C点设为原点，点C与离重心第二近的点B构成X轴，点C与离重心第三近的点D构成Y轴。由单目测量方法[15]可得B、C、D三点在单目测量坐标系XcOcYc下二维坐标Ol、Px和Py，以Ol为原点，OlPx为X轴，OlPy为Y轴，建立图4f所示的机器人靶标坐标系XlOlYl，各坐标系示意图如图5所示。

图5中取单目测量坐标系XcOcYc的Y轴单位向量为y=(0,1)，则XlOlYl坐标系与XcOcYc坐标系的夹角θ为：

(1)

至此，获取到了机器人位置Ol和姿态倾角θ。

2.3 改进的时空上下文动态目标定位算法

时空上下文跟踪算法[8]以固定更新率更新模型，在背景光照干扰和目标长时间遮挡时，易引入错误背景信息造成目标漂移，在目标快速运动时易跟踪失败。基于时空上下文跟踪算法，本文提出一种特征点辅助的动态目标跟踪算法。

2.3.1 光流追踪和特征点匹配的目标位置预估

2.3.2 基于特征点变化率的时空上下文模型更新

对于目标位置x和目标位置中心x*，建立上下文特征集Xc={v(z)=(I(z),z)|z∈Ωc(x*)}，其中I(z)表示在z点的灰度值，Ωc(x*)表示x*为中心的局部区域。

目标与其相邻区域的空间上下文关系模型hsc(x)[8]：

(2)

式中：b为归一化系数；α是尺度参数；β是形状参数；ωδ(·)表示权重函数；F(·)表示快速傅里叶变换；F-1(·)表示傅里叶逆变换。

(3)

2.3.3 基于时空上下文信息的目标精确定位

以t+1帧时的目标位置置信图中极大值的位置作为该帧目标位置，目标精确位置为：

(4)

在文献[15]分别展现了背景干扰、全局和部分遮挡、快速运动与运动模糊、目标旋转等情况下展现本文算法的跟踪效果，并与传统时空上下文 (Spatio-temporal Context learning, STC)以及当前比较热门的快速压缩跟踪(fast Compressive Tracking, CT)算法 、跟踪学习检测(Tracking Learning Detection, TLD)算法 、在线增强视觉(On-line Boosting and vision, OAB )跟踪算法作比较，结果表明本文跟踪算法综合测试指标中跟踪效果最佳，具有较好的鲁棒性和跟踪精度。

2.4 视觉信息融合的二维地图实时构建

(5)

至此，获取了目标体、机器人和障碍物在单目测量坐标系XcOcYc下的位置等信息，构建如图6b所示的全局地图。

3 基于速度向量场的路径规划算法

速度向量场方法由吸引速度va、排斥速度vr和切向速度vt控制机器人避障运动。机器人在这3个速度综合作用下实现避碰运动，原理如图7所示。图中O、G和R分别为当前障碍物、目标和机器人位置；dOG、dOR和dRG分别代表各位置之间的距离；robs为障碍物半径；s为避碰环半径；rd为障碍物排斥场作用半径。

传统速度向量场方法路径规划时存在以下问题[16]：①机器人吸引速度过大，到达目标点时会引起来回震荡；②机器人进入排斥场时由于排斥速度突然变化，产生较大的轨迹波动；③当障碍物处于目标点附近，此时排斥速度场大于吸引速度场而使得目标不可到达；④机器人运行速度较大时在排斥场边缘处易造成轨迹抖动；⑤未考虑动态障碍物下切向速度方向优化。

3.1 衰减吸引速度函数设计

目标点区域设置一个半径为ra的缓冲带，吸引

速度va定义如下：

(6)

(7)

其中：RG为R指向G的向量；M为吸引速度系数，为了使机器人较快到达目标点，M通常取较大值。

当机器人运行在目标缓冲区域外时，机器人以较快的速度运行。当进入目标缓冲区域，机器人以递减速度接近目标点，避免了目标点附近震荡现象。

3.2 对数形式排斥速度函数设计

为解决传统速度向量场方法中轨迹波动、目标不可到达等问题，设计了如下对数形式排斥速度vr：

(8)

(9)

其中：OR表示O指向R的向量，β表示排斥速度系数。

机器人刚进入排斥场时排斥速度较小，越接近障碍物排斥速度越大，避免了速度突变。另一方面，机器人速度较大时排斥场边缘处易产生轨迹抖动，在式(8)的排斥速度中引入机器人与障碍物距离的导数(dOR-robs)′，其代表机器人与障碍物相对距离变化速度，变化越快则接近速度越大，此时排斥速度相应变大，避免了轨迹波动。

当障碍物接近目标点时，一旦出现排斥速度大于吸引速度，机器人无法到达目标点。为此，当障碍物和目标点距离小于rd时，在排斥速度中引入式(9)的排斥速度衰减策略，使得机器人接近目标时，排斥速度以比例形式逐渐衰减，保证机器人最终到达目标点。

3.3 切向速度函数优化设计

为确保机器人运行路径最短，如图8a中对于静态障碍物，两个切向速度vt和v1t选择切向速度vt与吸引速度va夹角较小的vt。对于动态障碍物，需进一步考虑障碍物运动方向，机器人的绕行方向应与障碍物移动方向vobs相反，具体如图8b和图8c所示。

速度形式与排斥速度类似，引入切向速度向量场作用半径rt，vt定义如下：

(10)

(11)

其中：OR′表示向量OR的垂直向量，γ表示切向速度系数。

改进的切向速度可有效避免轨迹波动等问题，同时基于静态障碍物下绕行距离最短和动态障碍物下避障最安全原则，设计的切向速度可有效避免机器人陷入局部极小值，加速绕开障碍物。

3.4 机器人避碰运动速度计算

在机器人运动场景中，若存在n个障碍物，则机器人避碰运动速度合速度v计算如下：

(12)

3.5 路径规划算法仿真与性能分析

(1)多种吸引速度对比仿真

图9a为衰减吸引速度与常数形式吸引速度[17]和比例形式吸引速度[18]对比效果图。常数吸引速度下，机器人接近目标点时，冲过目标点而后折返，在图中表现为距离上下波动，始终无法收敛到目标位置；比例吸引速度避免了机器人在目标点附近震荡，但其收敛速度较慢，在5 s时才完全停下；本文的衰减吸引速度约在1.5 s时已收敛到达目标点。

(2)多种排斥速度对比仿真

图9b所示为本文提出的对数形式排斥速度与目前采用的排斥速度模型(常数形式[17]、一次反比形式[18]、二次反比形式[19]、指数形式[20]和虚拟弹簧阻尼形式[16])的仿真对比结果。结果表明：①排斥速度为常数时，机器人进入排斥场后速度发生突变，轨迹抖动剧烈；②一次反比函数排斥速度改善了轨迹抖动程度；③二次反比形式排斥场，总体轨迹相对平滑，但离开排斥场区域段仍存在轨迹抖动；④指数形式排斥场下，机器人轨迹基本平滑，但进入和离开排斥场时轨迹仍未达到平滑过渡；⑤虚拟弹簧阻尼形式排斥场得到较为理想的轨迹，但需要设定多个弹簧阻尼系数，轨迹平滑度很大程度受弹簧阻尼系数选取的影响。本文设计的对数形式速度排斥场轨迹最为平滑，可操作性强，且路径最短。

(3)目标处于障碍物排斥场内的路径规划仿真

目标处于障碍物排斥场内，图9c未加入排斥衰减策略，机器人始终围绕目标绕行，无法到达目标点。图9d加入排斥速度衰减策略后，机器人能够始终朝目标点运动，并在较短时间内抵达目标点。

(4)动态路径规划仿真

4 实验与分析

实验场景如图11所示，一个移动机器人在一个有障碍物Ob1、Ob2和Ob3的室内环境下跟踪一个扫地机器人。扫地机器人作为动态目标，其运动轨迹是随机的，与障碍物碰撞时，其运动随机改变，同时也改变障碍物位置。实验系统框架如图12所示。

移动机器人和扫地机器人具有不同高度，需要在对应高度上进行相应测量矩阵的求取[15]，外参数标定结果如表3所示。

表3 单目测量外参数标定结果

图13为实验过程中移动机器人和扫地机器人在地图中的运动轨迹，图中同一颜色或灰色对应同一时刻位置，移动机器人轨迹中的ABCDEFGHI点分别对应扫地机器人轨迹的A′B′C′D′E′F′G′H′I′点。

(1)A位置(第31帧)

机器人到达A位置，获取了障碍物Ob1信息。此时机器人未处于Ob1排斥场内，从A到B运动时，机器人向目标做最短的直线运动。

(2)B位置(第64帧)

机器人到达B，进入Ob1排斥场内，双目视觉系统检测到障碍Ob2、Ob3，将它们信息添加到地图中。机器人由B向C运动，向目标运动同时还进行Ob1避碰运动。

(3)C位置(第97帧)

机器人到达C，这时扫地机器人碰到Ob2，致其位置发生变化，Ob2新位置更新至地图。

(4)D位置(第119帧)

机器人到达D，在Ob2排斥场作用下继续由D向E运动，图13中虚线为Ob2未移动时理论轨迹，对比可知Ob2移动后使移动机器人更贴近Ob2的排斥场，轨迹发生偏移。

(5)F位置(第183帧)

机器人到达F，此时扫地机器人到达F′点，与边界挡板发生碰撞，之后运动方向改变，此时移动机器人不受障碍物排斥场作用，以最短路径从F向G运动。

(6)H位置(第258帧)

移动机器人从G点向H点运动过程避碰Ob3，到达H点。

(7)I位置(第304帧)

机器人在Ob3排斥场下继续向I点运动，最终移动机器人和扫地机器人分别到达I点和I′点，完成了跟踪任务。

实验中障碍物位置信息测量结果如表4所示，最大、最小相对误差分别为2.1%和0.4%，平均相对误差为1.55%，满足移动机器人导航需要。

表4 地图构建中障碍物位置测量结果

5 结束语

本文提出了一种基于“Eye-in-hand双目视觉”和“Eye-to-hand单目视觉”组合的移动机器人自主导航方法。在障碍物检测时，将图像色彩饱和度信息和图像环境亮度信息引入单尺度Retinex算法，自适应不同光照下阴影的消除和障碍物的有效提取；动态目标跟踪定位方面，将目标特征点及其变化信息引入时空上下文目标跟踪算法，自适应地调节时空上下文模型的更新率，较为有效地防止引入错误背景信息造成目标漂移和目标快速运动易跟踪失败等问题，在保证较好实时性的同时达到了稳定的目标跟踪效果；上述视觉信息处理模型的改进，一方面减少了模型依赖于人为经验或试验数据，另一方面可自适应不同的环境。在机器人避障路径实时规划方面，对基于速度矢量场路径规划算法进行了一系列改进，避免了机器人在目标附近震荡，解决了轨迹抖动、动态障碍物逼近和目标处于障碍物排斥场不可到达问题，以及摆脱局部极小值并以最短安全路径绕行等问题。通过仿真和实验验证了本文算法的有效性和实际应用的可行性。

今后可以结合迁移学习等方法，进一步改进模型在复杂场景下的自适应性和稳定性；另外，可以采用强化学习思想，研究机器人自学习进行室内自主导航，这些将是未来的研究方向。