机器人行驶路径分层规划系统设计

易春求，孔丽丽，易春回，徐朋朋，张静妮

（1.奥的斯机电电梯有限公司，杭州 310019；2.中国计量大学，杭州 310018；3.桂林智源电力电子有限公司，广西桂林 541004；4.滨江兄弟信息技术（杭州）有限公司，杭州 310052）

0 引言

在一些工矿、隧道施工等高强度、高粉尘、大噪声的恶劣环境中，操作人员面临劳动强度大、易疲劳、易受伤等身体问题。这种特殊的工作环境不便于操作人员直接进入工作现场，采用机器人代替人工操作是解决上述问题的理想方案。路径规划是机器人自主行驶的核心问题之一，是指机器人按照某一性能指标搜索一条从起始点到目标点的可行无碰最优路径[1]。

目前，对移动机器人路径规划算法的研究已经取得了大量成果，常用的路径规划算法主要有拓扑法、可视图法、切线图法、栅格法、惩罚函数法、人工势场法、启发式搜索法、RRT（快速随机扩展树）算法、A*算法、模糊逻辑算法、遗传算法、人工神经网络、模拟退火算法、蚁群优化启发式算法、粒子群算法等[2-4]。近几年，还提出了基于滚动窗口的规划方法、基于滚动的RRT算法、Bi-RRT算法等方法[5-10]。如果起始点到目标点的距离远且环境复杂，拓扑法、栅格法、启发式搜索法等存在搜索时间长、效率低等问题，因此这一类方法更适合在较大的、全局范围内进行一条粗略初始路径的选择；另一方面，这些方法仅仅提出了如何规划一条路径，对于如何使机器人更好地跟踪已规划好的路径研究较少。

许多路径规划算法已能得到令人满意的效果，但目前的研究多是集中在单一或者某种改进算法的研究与仿真，对路径规划系统整体设计的研究相对较少，工程化应用较少。本文以轮式移动机器人为载体，提出了一种用于移动机器人的分层路径规划方法。设计了机器人自主行驶规划系统架构，并根据机器人行驶速度慢、工作环境复杂、环境地图非结构化等特点，介绍了分层路径规划系统中每一层的算法及实现过程，并给出了仿真实验结果和工程实车实验结果。

1 分层路径规划系统

如图1 所示，规划系统按结构可以分为3 层：全局规划、局部规划和路径跟踪。

全局路径规划包括环境地图建立、全局路径搜索生成两个子模块，是基于已知或部分已知数字地图的路径规划，目的是根据环境模型，采用优越的搜索策略找出从起始点到目标点的可行最优路径。

局部路径规划由两个子模块构成：障碍物检测和路径重规划。利用机器人所安装的雷达、视觉等环境感知传感器所提供的障碍物的尺寸、形状、位置信息，进行障碍物规避。在局部规划中，环境信息是未知的，周围障碍物信息完全由环境感知传感器获取。局部路径规划的目的是在具有障碍物的环境中，重规划一条避障路径[11-12]。路径跟踪实现车辆的纵向、横向控制，目的是控制车辆能按照已规划的路径行驶。

图1 分层路径规划系统架

2 全局路径规划

全局路径规划的步骤包括：（1）轮式机器人环境地图建立；（2）栅格地图划分；（3）形成栅格地图所对应的搜索空间；（4）在搜索空间上利用搜索算法进行路径搜索。

2.1 栅格地图划分

采用栅格法进行工作空间划分。栅格法是用一定尺寸的栅格，将机器人的二维工作空间进行均匀栅格划分，基本元素位于自由区取值为0，表示该栅格为可行栅格，用白色表示。处在障碍物区为1，表示该栅格为障碍栅格，用黑色表示。在计算机建立的二维栅格地图如图2所示。

图2 栅格地图模型

2.2 启发式A＊算法

栅格地图创建完之后，要在栅格节点之间建立一定的联系，才能从起始点搜索到目标点的有效路径。常用的是8 连接，其表示从当前栅格可以达到与之相邻的8 个栅格节点。如图3 所示，a 为当前栅格，周围1～8为与之相邻的8个栅格。

应用启发式A算法[13-14]进行相邻栅格节点搜索，该算法相较于其他搜索算法具有更好的重构性和实时性。具体的搜索过程是：首先构造一个估价函数，在栅格a 周围的8个栅格中找到一个使得估价函数值最小的栅格b；然后把a 标注为已走过的栅格，画出从栅格a 到栅格b 之间的路径；然后再以栅格b为中心栅格，重复上述过程，直至到达终点。构造其估价函数为：

式中：k为a 周围的8 个栅格中的某一个；g(k)为起始点到k 栅格的路径长度，定义为实际代价，是已知的；h(k)是k 栅格到目标点的估计路径长度，定义为启发函数，是未知的。

图3 全局路径节点选取

为了保证搜索路径的最优性，选用以下公式作为启发函数，令：

式中：（xk,yk）是k 栅格的中心点坐标；（xend，yend）是目标点坐标。

估价函数最小的判断条件是：一个栅格与障碍物栅格的距离越大、且与目标点的距离越小，估价函数的值越小，此栅格将更容易被选中。其实现流程如图4所示。

选择估价函数最小的栅格，将其存入到close 列表中，当扩展搜索到目标点时，即将目标点存入到close列表中时，结束搜索。在列表中，搜索到最小f ( k )的栅格节点可以形成一条从起始点到目标点的最优路径。当close 为空时，说明路径不存在。全局规划的结果是一系列离散轨迹点。

图4 启发式A*算法流程图

3 局部路径规划

局部路径规划重点解决：当环境感知传感器检测到新的障碍物或动态障碍物时，需要改变原路径，重新规划一条供机器人行驶的无障碍路径，以实现规避障碍物。环境感知传感器检测障碍物的尺寸、形状和位置。

3.1 机器人状态空间模型

轮式机器人行驶需满足非完整运动学约束，在状态空间中车辆状态X=(x,y,u)，其中(x,y)为车辆位置坐标，u为输入。θ为车体航向角，L为轴距，Φ 为前轮转角，车辆微分约束方程表示为则其状态转移方程为：

3.2 局部路径规划方法

局部规划算法有很多种，快速随机扩展树算法（RRT 算法），因其具有快速高效的特点，在机器人路径规划中得到广泛应用。而且RRT 算法还综合考虑了车辆非完整运动学约束等约束条件，该算法更适合于机器人非结构化工作环境下的局部路径规划[15]。

在局部范围内定义机器人的状态空间C，自由空间Cfree，起始状态Xstart，目标状态Xgoal，Xgoal∈Cfree。如图5所示，其以状态空间C中的起始状态Xstart为根节点构建随机搜索树T，通过迭代随机采样方式选择状态节点Xrand，遍历T 找出距Xrand最近的节点Xnear。然后通过控制输入u，驱动机器人沿着Xnear到Xrand。对式（2）在一定时间Δt上进行积分便可得到Xnew，并将其作为新的叶节点添加到随机扩展树上。重复上述过程，直至到达目标状态，随机树构建结束。

图5 快速随机扩展树模型构建

从起始状态点出发，构建随机搜索树，直至到达目标状态点。这样就规划出了机器人从起始状态点到达目标状态点满足微分约束的路径，以及在每一时刻的控制输入参数，为下一步的路径跟踪提供依据。因为在搜索过程中充分考虑了机器人客观存在的微分约束方程，因而RRT 算法规划出来的路径具有很强的合理性。图6 所示为RRT 算法局部规划流程图。

图6 RRT算法局部规划流程图

4 路径跟踪

4.1 机器人车辆模型

轮式机器人等效成图7 所示模型。后轴驱动，其行驶速度为v，前轮转角为Φ。后驱动轴安装编码器用于采集当前速度值，前轮安装转角传感器负责采集方向盘转角值。

图7 机器人车辆模型

4.2 横向控制和纵向控制

轮式机器人对已规划路径的跟踪效果由机器人控制层决定。轮式机器人的控制包括两个方面：速度控制和方向控制。将路径跟踪分为横向控制和纵向控制。横向控制指方向控制，纵向控制指速度控制。如图8所示，机器人的路径规划层负责计算出期望速度和期望方向，传感器获取机器人当前的速度和方向信息。设计纵向控制器，能根据车辆期望速度和当前速度的偏差Δv计算出油门和刹车的控制量。设计横向控制器，能根据车辆期望方向和当前方向的偏差ΔΦ 计算出方向盘转角的控制量。控制轮式机器人的行驶速度和方向始终与期望值偏差为0，使机器人能按照规划的路径行驶。

图8 路径跟踪控制系统框图

5 实验分析

基于上述的路径规划系统架构及路径跟踪控制器的设计，实现了机器人的规划系统。仿真实验时，根据机器人的最大长宽尺寸，将其等效为一个矩形。随机生成若干障碍物，设定起始点和目标点，进行路径规划。共设置了20 20栅格地图，在地图中随机设置障碍物，图9给出了两种随机放置障碍物的路径搜索结果。

图9 仿真实验结果

实车实验是将轮式机器人应用在实际工程实践中，如图10所示。图10（a）中曲线是全局规划路径。根据环境感知传感器获取的周围环境信息，进行全局路径规划。图10（b）是实车开始行驶时，对规划路径的跟踪效果。当检测到前方有障碍物或动态障碍物出现时，根据局部路径规划进行避障规划。将机器人以一长方形等效显示，长方形区域为实际走过的轨迹。可以看出机器人的实际行驶轨迹对规划路径的跟踪效果良好。

图10 工程实验结果

6 结束语

本文针对轮式机器人的路径规划问题，设计并实现了一种分层路径规划系统。在全局规划中采用栅格法创建地图信息，采用启发式A*算法搜索到一条全局路径点，为下一步的局部规划提供导向约束。在局部规划中建立机器人车辆的微分约束方程，选用快速RRT 算法进行局部路径规划。路径跟踪时，设计横向控制器和纵向控制器分别实现机器人车辆的横向控制和纵向控制。仿真实验和实车试验表明：这种分层的路径规划系统可以全面地考虑各种因素的影响，层次架构清晰。算法的搜索速度适宜，完全满足实验中所采用的慢速机器人的行驶速度要求。不同层次采用不同的规划算法，使得规划的路径合理、平顺、稳定，实际可执行性强。这种系统级的研究相比较单一算法的理论研究，更具有工程实际应用价值，而且这种分层路径规划架构通用性强，适用范围广。