服务机器人导航与路径规划技术研究

何 宁，赵 彬，毛 欣

（1.重庆水利电力职业技术学院，重庆 永川 402160；2.沈阳新松机器人自动化股份有限公司，辽宁 沈阳 110168；3.国网重庆市电力公司永川供电分公司，重庆 永川 402160）

1 引言

随着机器人技术和信息技术的结合，面向家庭应用的智能服务机器人必将成为未来数字家庭的主导。移动机器人已经逐步走入到我们的日常生活中，其最具典型的应用功能包含：服务、清洁、卫生、房屋维修、家庭服务、病人护理和商业宣传等[1-2]。随着相关技术的进一步发展，为了能够满足社会生活和生产的需求，独立决策、移动机器人感知和最优控制等功能越来越受到国内外学者的关注。动态环境实时建模和导航技术是家用智能服务机器人研究和开发的关键技术，是机器人在家庭环境安全可靠运行、自主完成任务的基础和保障。

移动机器人依靠机器人核心信息处理单元，主要功能提供多个传感器信息融合技术、自主学习技术和最优规划控制技术。移动机器人是由多个传感器组成的，其中包含本身和其周边地区各种各样的信息，信息经过数据融合进行智能处理。自主移动机器人还可以感知位置环境信息，从而帮助它的运动和控制决策等。国内外学者对移动机器人也进行了深入的研究。例如：研究文献[3]对基于网格的移动机器人地图构建进行了深入的研究，它阐述了移动机器人地图构建的详细过程的。研究文献[4]介绍了基于卫星图像拓扑的智能移动机器人地图构建方法。文献[5]提出了一种基于视觉的方式来移动机器人的导航，并使用激光投影建地图，重建了三维地图，可以用于未知环境的导航控制[5-6]。结合我国对服务机器人产业发展的重大需求，瞄准智能服务机器人技术发展的前沿，探索适合于在家庭动态环境下应用的服务机器人导航系统体系结构，以解决目前国内在动态非结构环境下的家用服务机器人的实时环境建模和导航技术中存在的问题和不足。

2 运动控制

2.1 移动机器人平台

为移动机器人平台，并且承载激光导航，如图1所示。移动机器人平台采用的是双轮差动方式结构的移动底盘平台，该底盘平台能够适应各种复杂地形，在小区域能够行走自如。从而使得导航控制更为灵活，无运动控制障碍。

图1 移动机器人平台Fig.1 The Mobile Robot Platform

一般来说移动机器人的运动模式：目前规划三种运动模式，分别是平移运动，原地旋转和弧线运动。平移运动，角速度W为0，根据V和V_theta分解出x方向线速度和y方向线速度，进行速度，运动过程中，机器人的theta值不变（朝向不变）。原地旋转，V为0，根据W值进行旋转。弧线运动，同时存在线速度和角速度，进行圆弧运动。

2.2 差动控制算法

采用双轮差动方式运动的移动机器人，其左右轮轮速分别为 Vl，Vr，车体宽度为 d，，整车运动轨迹的圆半径为 r，在 Vl＜=Vr的情况下，由左右轮差速所构造的运动轨迹分析如下。

左轮若当做质点运动有：

右轮若当做质点运动有：

在实际中，左右轮位于机器人本体上，运动的角速度相同，所以有wl=wr，车体质点运动速度为：

当 Vl＞Vr时，情况与此相同，则公式变为趋向正负无穷的时候，车走直线，r=0时车原地旋转，r＞0时车体向左侧旋转，r＜0时车体向右侧旋转。以上为计算码盘位姿公式，根据读取的码盘值，计算出位姿中各个成员的增量。每次对地图类中的各个数据进行更新。

3 移动机器人导航定位

导航系统最为关键的技术包括定位技术、环境建模技术和路径规划技术[7-9]。环境建模与定位是移动机器人导航研究的基础，也是移动机器人真正实现自主的最重要的。导航系统的功能是让机器人能够感知自己的位置和周围的环境信息，并且能够按照某一性能指标搜索出一条从起始状态到目标状态的最优或次最优的无碰路径。

3.1 可靠环境感知方法

环境感知是地图创建的基础。二维激光雷达对环境离散化程度高，受遮蔽等影响大。面向定位的激光雷达数据通用特征提取方法利用环境的先验知识可以很容易的解决这些问题，提高特征提取的精度，但在环境不符合先验知识的情况下，将产生大量不稳定的特征，严重影响定位系统的鲁棒性。为了解决这些问题，我们在对数据进行滤波、分割的基础上提取这些离散点构成的网格的结构张量。这些信息稳定且可以广泛的应用于多边形、弧形及丛状物体（Blob）的激光雷达观测之中。本系统中特征提取方法的特征重复度＞90%，位置误差＜0.05m（平均误差），不确定度估计准确度＞80%（统计误差）。

3.2 鲁棒数据关联方法

经典的联合相容性分支界定树数据关联方法关联率相对较低，计算复杂度高，要求构建全协方差矩阵[9]。构建式（1），基于后验概率的联合相容性验证函数可以有效解决经典方法所存在的问题。

式中：IHt，S—状态变量估计位置与关联位置之间的距离（新息）；IHt，O—激光雷达特征估计位置与关联位置之间的距离（新息）；Pt—状态变量的协方差矩阵；Put—投影矩阵估计值的不确定度矩阵；V—激光雷达特征的协方差矩阵；J1及J2是投影函数f（·）对视觉系统位置和投影距离的偏导数。

定义：S—投影函数所构建的协方差矩阵；H—f（·）的Hessian矩阵，K=PHTS-1则为卡尔曼滤波增益矩阵；G—将激光雷达特征投影到视觉传感器坐标系下的投影矩阵对激光雷达坐标系的Hessian矩阵；I—单位矩阵，则有：

图2 错误数据关联对地图精度的影响Fig.2 Effect of Incorrect Data Association on Map Accuracy

错误数据关联对地图精度的影响。错误的数据关联会严重影响创建地图的精度。图2（a）和图2（b）分别为对地图进行优化时，状态变量链接关系图中包含十次和一百次错误关联所产生的地图。由此可见，由于卡尔曼滤波极小化了均方误差，因此，式（1）所定义的成本函数可以极小化关联误差。本系统采用的数据关联方法正确关联率＞90%，错误关联率＜0.2%。

3.3 定位与地图匹配

地图匹配与合并可以将创建的局部地图合并入全局地图中，或将多个局部地图合并创建全局地图。地图匹配与合并的核心是计算正确的地图转移量。在二维环境中，这个地图转移量包含在X和Y方向的平移距离以及一个旋转角。地图转移量的计算需要找到地图之间的相似目标，进而验证地图根据该相似目标进行合并后的似然函数。

图3 特征地图合并Fig.3 Merge Feature Map

给出了一个地图合并的例子，图 3（a），图 3（b）包含同样特征物体的两个地图，图3（c）为地图合并后的结果。地图合并的核心在于有效特征的提取与数据关联。本部分的算法内容与地图创建中的特征提取与数据关联基本一致，在此不再赘述。

4 移动机器人路径规划

4.1 动态环境下的移动机器人路径规划

将全局路径规划方法和局部路径规划方法相结合、将基于反应的行为规划和基于慎思的行为规划相结合，解决路径规划算法的合理性、完备性、最优性、实时性以及对环境变化的适应性问题，同时保证机器人在动态不确定环境下能够在满足自身运动学约束的前提下以最短路径代价运动到目标点。

4.2 Wavefront法

由于室内环境规模较小，环境内障碍物相对较多，一般室内环境都采用栅格地图或拓扑度量地图标示。本系统拟采用栅格地图表征环境。在栅格地图中，采用Wavefront方法进行路径规划是一种简洁、可靠的选择。Wavefront方法示意图，如图4所示。

图4 Wavefront路径规划方法示意图Fig.4 Wavefront Path Planning Method

Wavefront方法将地图看做一个可导热的物体，环境中的障碍物为热绝缘体；出发点被看做热源，热量从热源散布开去。当热量到达目标点时，热量停止扩散，同时产生有效路径。然而，由于计算复杂度的原因，Wavefront方法不适用于大规模环境之中。利用这种方法，可以确保有效路径的探测率达到100%，同时，可以根据应用需求，在安全性和能量效率之间进行平衡，产生定义的最优路径。

5 功能测试

功能测试包含在测试环境内对激光传感器测试，对定位精度测试和路径规划性能测试。首先指定机器人起始点，命令机器人导航运动至目标点。使用尺、量角器进行实际测量，测出机器人实际的位置与朝向角与起始点的偏差。同时通过远程桌面实时监控是否丢失位置，是否拾取路径。

5.1 激光传感器测试

激光传感器的参数比对，如表1所示。（1）对激光传感器的测量：获得激光传感器在离障碍物不同距离时的测量值，并与实际值（50cm间隔，共测10点）进行比对，从而获得测距精度结果，并确定传感器的可用量程。（2）不同介质测量：使激光传感器对玻璃材质（纯玻璃、贴胶玻璃）的障碍物与非玻璃材质的障碍物进行测距，并比对结果。

表1 激光传感器的参数比对Tab.1 Parameters of Laser Sensor

5.2 基于激光传感器自定位算法的精度测试

建立好地图后，遥控机器人运动到某处后，运行软件得出机器人在全局地图中的位姿。使用尺、量角器进行实际测量，测出机器人实际的位置与朝向角。计算偏差。测试10次，取平均值。着重对以下方面进行测试：（1）对于走廊区域等环境类似场景的定位能力测试（2）对于“绑架”问题的处理能力（3）目前软件算法能适应的最大环境面积，如果通过降低栅格精度的方法增加导航范围，则要再测试新配置下的定位精度。为选取一个固定位置，通过PAD操作创建地图。地图说明：地图左侧为B3楼3楼大厅正门侧，地图右侧为电梯侧。左右全长约136m，上下宽约34m。地图创建完成后，手动遥控实验平台运动，并使用PC端软件的地图显示功能实时监控实验平台运行，确认实验平台实际运行位置和地图上显示位置是否一致。如图5所示。为测试出的B3楼的地图，经过测试证明，导航效果较为理想。

图5 实验平台创建的B3楼地图Fig.5 Experimental Platform Create B3 Floor Map

6 结论

首先系统介绍了自主移动机器人双轮差动运动控制问题。其次利用网格的结构张量解决了环境不符合先验知识的情况下，将产生大量不稳定的特征，实时精准获取当前的二维地图信息。再次在栅格地图表征环境，采用Wavefront方法进行路径规划。完整的解决了移动机器人三个基本问题。最后进行了一系列测试，测试机器人通过狭窄区域的能力、测试导航性能和测试导航效果。实验结果表明，该导航算法和路径规划算法具有较好的稳定性、实用性和战略意义。