复杂环境下移动机器人路径规划与跟随

王 影，杨家鑫，刘 麒，徐姣姣

(吉林化工学院 信息与控制工程学院，吉林 吉林 132022)

移动，对于人类来说不过是从一个地方到达另一个地方的简单任务，但对于机器人来说这却是一个非常大的挑战[1].近几十年来，越来越多的机器人走进人类的生活中，涉及农业、工业、服务、军事等方面[2].鉴于机器人在生活中起到的作用越来越重要，利用人工智能算法，设计一套行之有效的控制方法解决机器人最基本的移动问题显得尤为重要[3].例如，在地震或者火灾这类危险的环境中，需要机器人预先进入其中搜索受害者并采集受害者的身体信息，但在这类复杂的环境中，往往外部通信设备欠佳，无法对机器人进行实时有效的控制，这就要求机器人拥有自主移动的能力.机器人想要拥有自主移动的能力需要经过两个过程，一是，需要从起点规划出一条到达终点的路径，这条路径需要拥有一定的评判标准且是可行的，这一过程称之为路径规划；二是，在已知路径的情况下，通过控制机器人的速度和方向使得机器人能够沿着这条路径行走，这个过程叫作路径跟随.

近年来，相关学者对移动机器人的路径规划与跟随做了大量的研究.在路径规划方面，陈继清等人[4]为解决障碍物分布对生成路径的影响，将传统的A*和人工势场法进行结合，提高了传统A*算法的搜索能力.邱硕涵等人[5]研究了巡检机器人在D*和Bug2两种算法下的路径规划效果，得出D*算法在运算速度和环境适应性上的优越性.Iram Noreen等人[6]为提高机器人的路径规划速度，在RRT*的基础上改变随机采样的边界，使得原算法变得更具有目的性.同样的，在路径跟随方面，张旋武等人[7]介绍了目前较为常用的6种路径跟随控制方法，强调了分层控制与强化学习等控制方法在未来路径跟随方面的重要性.马艳彤等人[8]提出了一种基于水下机器人的双层PID控制器，跟随效果达到了分米级.张立伟等人[9]设计了一种适用于无轨列车的PID控制算法，在曲率突变的情况下跟踪效果较为优异.

在前人的所有研究中，路径规划和跟随被当作两个独立的问题.本文利用路径规划生成期望路径，利用路径跟随通过控制器控制机器人的姿态生成实际路径，这样就将其统一为一个控制问题.同样，在前人的研究中，其路径规划和跟随研究的都是较为简单的环境，复杂环境下的研究较少，而日常生活中却存在着非常多的复杂环境，如仓储物流、港口运输.因此本文将研究重点放在复杂的环境下.

本文的其余部分组织如下：第1节基于A*算法研究了移动机器人的路径规划问题；第2节建立了机器人运动学模型并设计了控制器；第3节设计了实验并对实验结果进行了分析总结；第4节对论文进行了总结.

1 路径规划

目标是在障碍物已知的二维栅格地图上找到一条从起点到终点的无碰撞路径，其评判标准是符合实际情况、最少的路径转折点以及高平滑性.首先进行环境建模，之后在现有的地图环境中进行路径规划，最后对规划的路径进行平滑处理.

1.1 环境建模

一张完整的栅格地图上包含起点、终点、可行区域、不可行区域、边界这5个基本要素.在传统的栅格法建模过程中，将整个环境空间划分为具有一定分辨率的小栅格，其中可行区域用1来表示，不可行区域用0来表示，但是由于大部分路径考虑的都是数学上的最小值，即最短路径，这样就导致规划出来的路径往往距离障碍物非常近，甚至是紧挨着障碍物的.这种规划的路径在实际情况中是不存在的.因此，本文采用扩展障碍物的方法在传统的建模方法上进行改进，其改进思路如下：

通过提前设定一个安全阈值r来对环境中的障碍物进行拓展.在开始的时候，只有原障碍物所在的栅格值为0，之后以单位栅格的膨胀速度从障碍物的边界开始对障碍物进行拓展，拓展范围内的栅格值设为0，拓展的距离记为d.如果d

1.2 算法改进

采用A*算法来进行路径规划.就原始的A*算法而言，其规划出来的路径存在着许多的路径转折点，而有些转折点在实际情况中是非必要的，同时转折点的增多无疑增加了移动机器人控制的复杂性，针对这个问题，对传统的A*算法进行了改进.其改进思路如下：

步骤一：直线判断.对非终点和非起点的路径节点进行判断，假设当前节点为Qi(xi,yi)，取其上次迭代的节点Qi-1(xi-1,yi-1)和上上次迭代的节点Qi-2(xi-2,yi-2).根据公式(1)进行直线判断，如果k=1，说明当前节点和前两个迭代的节点在一条直线上，反之k=0.

(1)

其中i≥2，xi和yi为第i个节点的横坐标和纵坐标.

步骤二：增加权重因子W.在前一个步骤的基础上，引入一个权重因子W.若此时k=1，说明当前的路径是直线的，较为理想，评价函数为：

f(n)=g(n)+h(n).

(2)

若k=0，说明当前路径进行了转折，可能存在非必要的转折点，评价函数为：

f(n)=g(n)+h(n)+W.

(3)

其中，0

算法2多转折点优化输入:遍历后的最优节点信息Ni,遍历的节点个数p输出:权重W1:fori=2:p2: k←lineJudge(Ni,Ni-1) % 判断当前节点和上一节点是否在同一条直线上3: if k ≠ 14: W =0.25: else6: W = 07: end if8:end for

现对上述改进A*算法的方法进行实验验证.如图1所示的是在不规则障碍物下的原始A*算法和改进后的A*算法进行规划所产生的路径.如图1(a)所示的路径第1个关键转折点A的坐标为[7，1]；如图1(b)所示的路径第1个关键转折点C的坐标为[9，1]，因为原始的A*算法规划的路径的过早转折，所以在后续的路径中产生了4个非必要的转折点.同样的，在第2个关键转折B点也是因为规划路径的过早转折，造成后续产生了两个非必要的转折点，而改进后的算法则没有这样的缺陷.相同的路径长度下，原始A*算法规划的路径共有8个路径转折点，而改进后的A*算法只有2个路径转折点，从中可以说明改进后的A*算法在减少路径转折点方面的优越性.

1.3 路径优化

移动机器人在运动过程中遇到障碍物要进行规避时，在某个指定的位置停下转向后再进行避障的情况是非常少的.一般情况下是在行走的过程中进行避障，所以折线形的路径并不适合机器人后期的路径跟随，因此本文使用四阶三次均匀B样条曲线对路径进行优化.

由n+1个控制点p0，p1，…，Pn可以确定的四阶三次均匀B样条曲线是[10-11]：

(4)

其中，0≤u≤1，i=0，1，…，n-3.

在平面的曲线规划中，机器人的坐标可以表示为：

(5)

其中，0≤s≤1，s为归一化路程.

(6)

速度可以表示为：

(7)

加速度可以表示为：

(8)

直接用四阶三次均匀B样条曲线进行规划，其产生的路径并不经过起点与终点，因此采用拓展控制点的方法对四阶三次均匀B样条曲线进行调整.具体方法如下：

先将起点S(x,y)拓展为3个节点s1、s2、s3，其中s1=S-VsL，s2=S,s3=S+VsL.其中，Vs表示移动机器人过起点时的速度，L为常数.终点拓展同理.

为说明上述方法的正确性，现对其进行证明.证明如下：

设与起点S连续的控制点为C，那么对起点进行拓展后的曲线方程为：

在同一个属性条件下，第k个方案和第i个方案之间的排序关系可由决策矩阵中元素的大小关系来表示。ak等价于ai⟺rkj=rij;ak优于ai⟺rkj>rij;ak劣于ai⟺rkj

(9)

(10)

(11)

将s=0带入公式(9)、(10)、(11)中，可得：

(12)

(13)

(14)

得证起点经节点扩展后得到的曲线过起点，同理可证，终点经节点扩展后得到的曲线过终点.

2 路径跟随

本节的目标是在已经规划好的路径上使得移动机器人能够沿着这条规划的路径行走.首先对两轮差速移动机器人进行建模，之后设计控制器使得移动机器人能够沿着既定的路径行走.

2.1 机器人运动学建模

(15)

考虑到对移动机器人进行控制时，使用单积分器速度命令会更加直观，但由于单积分器模型是全向机器人的简化模型[12]，普通的两轮差速机器人存在着运动学约束，其无法直接遵循单积分器命令(即无法侧向行驶).这个问题的一个有效的解决方案是在单积分器模型和移动机器人运动学模型之间使用近似恒等微分同胚(NID)[13].具体实现步骤如下[14]：

(16)

其中，0<|∈R.

前视点的瞬时速度不受差速驱动平台非完整约束的限制.为了直观表达，考虑机器人的右手局部坐标系，其中X方向与图2中的线速度矢量v在相同的方向上.给定一个纯旋转运动的速度命令，前视点将在局部坐标系的Y方向上运动.同样，如果给定一个纯直线运动的速度命令，前视点将在局部坐标系的X方向上运动，如此就符合单积分器动力学.

对p′求导，得到基于前视点的移动机器人运动学方程：

(17)

2.2 控制器设计

ex=xr-x-lcosθ，

(18)

Y轴方向的误差：

ey=yr-y-lsinθ，

(19)

可得位置误差追踪方程：

(20)

(21)

(22)

(23)

设计PID控制器:

(24)

(25)

可得：

(26)

(27)

故可得控制律：

(28)

3 仿真实验

本节在MATLAB中共设计了4个实验来验证前文的理论论述.实验3.1对应第1.1、1.2小节；实验3.2对应第1.3小节；实验3.3对应第2节；实验3.4验证了在本文的理论基础上两轮差速移动机器人在复杂环境下路径规划与跟随的效果.

3.1 路径规划实验

设计了6张不同的地图来验证所提出的算法.每张地图上都分布着不同的障碍物，且栅格大小统一设置为500×500.在前5张地图中，将起点和终点统一设置为[10，10]和[490，490].第6张地图中起点设置为[250，250]，终点设置为[490，300].由于目前栅格地图搜索方法以4邻域、8邻域、24邻域为主，故就这几种搜索方法分别进行实验，用来说明改进后的A*算法在处理多拐点问题方面的优越性.代价用欧式距离进行计算，算法评判标准为：

(29)

其中，Mi代表第i个转折点.

通过比较3种不同搜索方式下A*算法和改进A*算法在6张地图中的不同路径，可以很清晰地看出改进后的算法在处理多拐点方面的优异性和普适性.如表1所示，在不同邻域搜索下改进A*算法产生的路径相对于A*算法距离障碍物拥有更大的安全距离，转折点更少.如图3所示，在4邻域搜索方式下，改进后的算法相较于原算法转折点减少了45.9%.在8邻域搜索方式下，改进后的算法相较于原算法转折点减少了41.8%.在24邻域搜索方式下，改进后的算法相较于原算法转折点减少了21.7%.从以上实验结果中可以看出，改进后的算法在3种搜索方式下均取得了一定的效果.

表1 3种不同搜索方式下算法在6张地图中的生成的路线

3.2 曲线优化仿真实验

为验证四阶三次均匀B样条曲线对改进A*算法规划路径的优化效果，在改进A*算法规划出的路径基础上进行了实验，仿真实验结果见图4.

如图4所示，当规划路径出现转弯时，使用四阶三次均匀B样条曲线优化可以生成一条平滑的曲线进行转弯，甚至规划路径出现两次或者3次连续转弯时，经曲线优化后的路径仍可以保证路径是连续且光滑的.

3.3 路径跟随仿真实验

如图5所示，其中左侧路径为改进A*算法生成的路径，右侧路径为移动机器人实际走过的路径.从中可以看出机器人能够很好地沿着规划好的路径行进，从而验证了基于前视点模型所设计的PID控制器路径跟随能力的优越性.

3.4 综合实验

如图6所示，利用改进A*算法和四阶三次均匀B样条曲线融合的方法规划出的一条从起点到终点的较优平滑路径，路径距离障碍物有很大的安全距离，不仅有利于后续移动机器人的跟随，而且提高了路径的安全系数.如图7所示，左侧路径为提前规划好的路径，右侧路径为移动机器人的实际行走路径，从实验结果来看，无论是在直线路径还是在连续的转弯路径上，移动机器人都能对提前规划的路径做到较好的跟随.

4 结 论

根据以往的研究可知，本文是第一个提出复杂环境下的移动机器人路径规划和跟随的综合研究.在MATLAB中分项测试了提出的所有理论研究，并通过一个仿真的两轮差速机器人完成了复杂迷宫环境下的路径规划和跟随.基于改进A*算法的路径规划使得生成的路径转折点更少，经过四阶三次均匀B样条曲线优化后的路径更加平滑，更合适移动机器人进行路径跟随.仿真的两轮差速机器人通过前视点建模的方法，采用PID控制能够对预期路径做到完美跟随，并且从迷宫入口无碰撞到达迷宫出口，验证了本文所提出的方法可以实现复杂环境下的移动机器人路径规划和跟随.