基于控制方向角改进势场法的移动机器人路径规划∗

耿双乐 管 启

（上海理工大学光电信息与计算机工程学院 上海 200082）

1 引言

移动机器人的路径规划是智能机器人研究的热点之一。路径规划就是机器人根据自身传感器感知周围环境，规划出一条安全无碰撞的路线［1］。根据是否获得环境信息，将在完全已知环境下的规划称为全局规划。只获得部分环境信息下的规划称为局部规划。全局规划是在完全已知的环境中进行规划，不具有实时性。常见的全局算法有Floyd、栅格法等。人工势场法［2］是由Khatib提出的一种路径规划方法，因其优秀的避障性能和良好的路径平滑性，成为最常用的规划方法之一。传统势场法引入物理上引力和斥力的概念。利用其目标点对移动机器人产生引力，障碍物对机器人产生斥力的这种方法进行规划。但传统势场法在目标点周围存在障碍物时，由于受到周围障碍物斥力作用，会存在无法到达目标点的情况［3］；并且容易陷入局部最优点，导致无法到达目标点。

2 传统势场原理

势场法是将整个运动空间抽象成一个势场，目标点对移动机器人会形成一个引力场，运动空间内所存在的障碍物又会在一定范围内对机器人形成一个斥力场，机器人所在的空间即为引力场和斥力场所叠加的合力场。机器人始终沿着势场梯度下降的方向运动。将斥力场的负梯度作为斥力虚拟力，引力场的负梯度作为引力虚拟力。因此，移动机器人在空间中运动受到引力和斥力的合力作用。设机器人在运动空间中实时位置为X=[x，y]，式（1）～（2）中构建了引力场和斥力场关于机器人位置变化的函数。

其中Uatt表示移动机器人所受到目标位置的引力场，katt是自定义的引力场系数， ||X-XG表示机器人与目标点之间的欧几里得距离。XG表示目标点的位置。η为可自定义的斥力场系数，ρo为自定义的障碍物斥力场影响范围，Xob为障碍物所在的空间位置。那么目标与障碍物所产生的负梯度即引力和斥力如式（3）～（4）:

传统势场法将机器人与障碍物和目标点的距离信息转化为引力和斥力进行规划。但是当目标点周围存在障碍物时，随着移动机器人靠近目标点，引力会逐渐变小，斥力逐渐变大。当所受到的斥力大于引力时，此时就会造成目标点不可达的情况。

文献［2］提出在斥力场函数中引入距离因子，即如式（5）所示:

此时，所受到的斥力如式（6）:

3 改进算法

3.1 引力方向模型

在二维空间内设置机器人起点位置为X0=[0，0]，机器人的实时位置为 X=[x，y]，目标点位置为 XG=[xg，yg]。设坐标正方向为力的正方向。机器人的实时位置与目标点之间的位置关系为Δx=xg-x，Δy=yg-y。则机器人与目标点间实时距离可为机器人引力方向根据位置参数变化，在运动空间中存在如图1四种方向状态。

图1中四种位置关系下的引力始终与横轴方向有如图示夹角。角度范围为(0，2π)。每种方向状态下与横轴的夹角均可使得引力的正弦分量分解到Y轴方向，引力的余弦分量分解到X轴方向。角度的余弦正负决定了引力横向分力的正反，正弦的正负值决定了引力纵向分力的正反。因此四种位置关系下的角度可描述引力在运动空间中的所有方向状态。由于正弦余弦角度的周期性，此时可以引入一个角度 θatt=sign(Δy)arcos(Δx/ratt)可统一表示上面的四个角度，即称为引力角。

图1 引力四种方向状态

3.2 斥力方向模型

再对移动机器人与障碍物之间的传统斥力模型进行方向状态分析。设定障碍物的位置坐标为Xob=[xob，yob]。可统一规定移动机器人与障碍物之间的实时位置坐标变化的关系为Δxob=xob-x，Δyob=yob-y，则移动机器人与障碍物间实时位置可表示为斥力方向根据移动机器人与障碍物位置变化的关系，会存在如图2四种方向状态。

沥青混凝土中的集料是可以称作是它的骨架，集料的质量能够决定混凝土的整体强度，对混凝土的支撑能力有重要的影响，在集料的质量控制中首先应该保证集料的外形上接近立方体，保证其有良好的稳定性，另外由于细集料和粗集料有不同的用处，例如细集料是为了对粗集料中间的空隙进行填充，因此还应该对集料的体积大小进行严格的把控。

图2 斥力四种方向状态

根据移动机器人与障碍物间存在的四种位置关系，可分别求出四种方向状态下引力与横轴方向的夹角。都可将斥力的余弦分量分解到横轴方向。图中所标注的四种位置关系下的角度可以描述斥力在空间中的方向状态。则引入θrep=pi+sign(Δxob)arcos(Δxob/rrep)作为斥力方向的斥力角，统一表示以上四种状态下的角度。

3.3 合力方向模型

再将引力通过引力角分解到横纵方向得到

横纵坐标方向受到引力与斥力的共同作用，也就是所受到的总合力在横纵坐标方向的分量为

根据横纵方向分力存在的四种情况，则合力方向存在如图3所示四种方向状态

图3 合力的四种方向状态

图3 中四种方向状态下的合力与横轴方向存在如图所标注的夹角。夹角的大小则由各分力求得，夹角也可描述出合力在运动空间中的具体方向状态。通过各合力分量之可获得移动机器人所受合力的方向。此时可以根据分量求得合力角来表示图中所标注的四个角度。合力角表示如下:

合力角可实时反应出合力的方向状态。此时，对于传统人工势场虚拟力控制即可转化为角度方向的控制。

4 匀速环境下规划运动

通过如上势场模型分析，已将势场虚拟力模型转化为合力角、引力角和斥力角的控制。下面通过在运动环境中设定匀速环境，设定移动机器人在单位时间内行驶固定步长L。下个时间段行驶的方向则不断通过合力角度变化而变化。设当前时刻移动机器人的实时位置为Xk=[xk，yk]，下一时刻位置为 Xk+1=[xk+1，yk+1]。通过人工势场力所转化的角度控制，可得到下一时刻的位置为

整个运动规划过程就是不断迭代更新控制角度完成。当前时刻移动机器人的位置由上个时刻的合力角所决定。当目标点周围存在障碍物时，就会出现传统势场所固有的缺陷即无法达到目标点。

下面通过在目标点周围设定半圆阈值检测，转换方向控制角来解决目标点不可达的问题。如图4所示。

图4 设定半圆阈值

假定移动机器人从左侧开始运动，则如图4所示在目标点为中心画左侧半圆，设定半圆检测半径为2L。在检测阈值范围之外，始终是合力角不断牵引规划运动。当移动机器人开始进入阈值范围之内，可实时检测阈值之内障碍物到移动机器人与目标点之间连线的距离。将左半圆作为阈值检测范围，即可忽略右侧障碍物到移动机器人与目标点之间连线距离的影响。可设定一个安全距离0.5L，如若检测到的距离满足安全距离要求，则转换合力角为引力角直接导向目标点［10］，即如下迭代所示。

如果不满足安全距离要求，则继续使用合力角导航直到检测左边障碍物满足安全距离要求。

采用左侧半圆检测，满足安全距离时，此时采用引力角牵引，也就是忽略了目标点周围障碍物斥力的影响。阈值范围内左侧的障碍物看作移动机器人是否满足安全距离的影响因素。直到满足安全距离要求时，即可忽略左侧障碍物的斥力作用，直接通过变化合力角为引力角规划的方式无碰撞地直达目标点。以下通过仿真验证了此种方法的可行性，并且相对于原有算法和加距离因子方法均具有改良效果。

5 仿真实验

本次实验将通过阈值处理改进的算法与传统算法、加距离因子改进的算法进行对比。对于三种方式均设定相同系数和相同障碍物，最后用Matlab显示三种方法的最终效果。统一设定引力系数为11，步长L=0.5，斥力为1，斥力影响范围为2。如图5～7分别表示传统算法、加距离因子方法和半圆阈值处理改进的方法在Matlab仿真下的效果图。

图5 传统算法规划效果图

图6 加距离因子算法规划效果图

图7 阈值处理规划效果图

通过图5、6、7中可以看出三种方法的最终规划效果。图5中传统算法在周围存在障碍物的情况下，未能到达目标点。图6是通过加距离因子的方法，可看出规划的路径相对传统方法规划的路径较长，并且不平滑。图7所示，是通过本次改进算法所得到的结果，相对加距离因子方法，路径平滑，并且规划路程较短，又能够在目标点周围存在障碍物的情况下，到达目标点。

6 结语

本文提出的一种匀速模型下在目标点周围设定阈值的方式来解决传统算法所固有的缺陷。通过将传统虚拟力的控制，转化为方向角度控制。进而在目标点周围设定合适阈值，检测阈值内的障碍物到移动机器人与目标点连线之间的距离，直到满足所设定的安全距离，则可将合力角转化为引力角的方式进行路径规划。当移动机器人在阈值范围内时，如满足安全距离要求，则忽略了目标点周围的障碍物影响，并且可以安全到达目标点。此种方法可解决传统算法在目标点周围存在障碍物时，移动机器人无法到达目标点的情况。本文提出的改进算法与加距离因子的方法相比较，既可保证规划路径的平滑性也可缩短路径长度。