基于人工势场法的机器人路径规划改进方法的研究

李晓凡，席浩哲，尹思佳，邸若彤，高 强，秦 景

(1.河北建筑工程学院 信息工程学院，河北 张家口 075132；2.天津理工大学 海运学院，天津 西青 300384；3.燕山大学 信息科学与工程学院，河北 秦皇岛 066004；4.河北建筑工程学院 能源工程系，河北 张家口 075132)

0 引 言

机器人自主导航的核心技术是机器人路径规划，而全局路径规划算法和局部路径规划算法在路径规划中发挥着重要作用。前者关键作用是寻找全图最优导航路线，而后者主要作用于实时的避障导航[1]。全局规划算法有Dijkstra算法[2]、启发式A*算法[3]和蚁群算法[4-7]等，全局规划算法是为机器人从起始点到目标点规划出一条可行的路线。而自主导航更多需要依靠局部规划算法，如人工势场算法[8]、模拟退火算法[9]、粒子群算法[10]等，局部路径规划算法是在全局路径规划算法规划出可行路线的基础上结合机器人的线速度和角速度指导机器人完成自主导航。

魏先勇等[11]克服了目标周围有障碍物无法到达的问题，提出了构建一个考虑质数因子的斥力场函数，消除了局部极小点，解决了当障碍物较多时，机器人将会出现在障碍物前停滞不前的问题。温素芳等[12]采用障碍物影响范围缩小的方法，解决了障碍物靠近目标点时无法到达目标的问题。同时，采用叠加障碍物效应的思想，解决局部极小点造成的陷阱问题，使机器人顺利到达目标点。陈倩等[13]提出对障碍物的连通性进行了分析，通过几何拓扑得到的可行解域并进行路径预规划，解决了人工势场法陷入局部极小点的陷阱的问题。同时在势场公式中考虑了速度因素，使机器人能够避开动态障碍物。张鹏彬等[14]提出了使用了虚拟目标点代替目标点使其摆脱局部极小点。张建英等[15]针对局部极小点的陷阱问题，提出了一种增加附加控制力的方法，即当障碍物对机器人的排斥力与目标点对机器人的拉力在一条直线上时。添加与障碍物相关的控制力以消除局部最小值。Abdall等[16]结合人工势场法和模糊控制，将模糊逻辑控制器的隶属函数结合粒子群算法进行优化，用于移动机器人的运动控制，解决了局部极小点问题。

人工势场法是将障碍物在地图上的位置和目标点在环境中的位置结合点的势能值标记出，使环境势场的值也能反映拓扑结构和环境结构(图1～2)。障碍物边界的势场很大，并且随着距离障碍物的距离增加而减小。目标点周围形成势场值最小点。人工势场法的核心思想：将人工势场放置在目标机器人的工作范围内，目标机器人在势场的影响下移动到势场值最小的地方。

图1 势能场 图2 势能场的障碍物

1 传统人工势场法

传统的人工势场法引导机器人导航是由Khatib[17]提出的。由传统人工势场理论指出，势场的负梯度作为作用在小车上的虚拟力，虚拟力分为虚拟引力场和虚拟斥力场的叠加，在任何一个目标导向下，引力场为机器人提供引力，随着机器人靠近目标点，机器人的势能降低。当机器人接近障碍物时，障碍物表面会产生排斥力。从而对机器人施加排斥力，机器人的势能升高。在组合虚拟力的影响下，机器人从势能高的地方，向势能低的地方移动。

目标点的吸引力遍布在地图的，机器人可以感受到目标点的吸引力，并在地图中任意一点移动到目标点。障碍物的斥力仅在障碍物周围才会产生，只有当机器人移动到距离障碍物一定距离时才需要对障碍物进行躲避。

1.1 引力函数

将机器人所在的空间视为二维空间，用P(x0,y0)表示机器人的位置，用P0(xgoal,ygoal)来表示目标点的位置，用Uttr来表示目标点在机器人处产生的引力势能。在引力场中，P(x0,y0)距离P0(xgoal,ygoal)越远势场越大，而P(x0,y0)等于P0(xgoal,ygoal)时，势场值等于零。因此，目标点的引力与机器人与目标点的距离成正比，距离越大，目标点施加的引力越强。相反，距离越短，引力越小，当机器人到达目标点时，引力变为零。故传统人工势场引力公式为

Uttr(q)=0.5K(P(x0,y0)-P0(xgoal,ygoal))2

(1)

其中K为位置增益函数，(P(x0,y0)-P0)是机器人与目标点之间的相对位置。目标点的引力势场函数相对于机器人引力的负梯度为：

Fttr(q)=-Uttr(q)=-k(P(x0,y0)-p0(xgoal,ygoal))

(2)

当机器人到达了目标点时，(p-p0)接近于零，故引力逐渐收敛于零。

1.2 斥力函数

为了使机器人在行驶过程中躲避障碍物，在障碍物表面增加斥力场。斥力场要求对机器人产生排斥而并非远离障碍物，使机器人不断躲避障碍物。同时，当机器人离障碍物较远时，应使障碍物对机器人影响因子降到很小甚至没有影响。因此，定义障碍物对机器人产生的排斥力大小与机器人到障碍物的距离成反比，机器人离障碍物越近，排斥力越大，越远离障碍物，排斥力越小。所以斥力公式为

(3)

其中Urep(q)是障碍物对机器人处产生的斥力，n是障碍物斥力的比例系数，P(x0,y0)-P(q0)为当前机器人与障碍物之间的欧氏距离，p0为障碍物的作用范围。根据公式可以看出，当距离大于p0时，不会有斥力表现出来；当距离小于这个值时，斥力会按照公式给出。当距离小于p0时，障碍物的斥力势场函数相对于机器人排斥力的负梯度为

(4)

1.3 合力函数

当机器人处于有目标点以及障碍物的环境中，机器人会受到排斥力和引力的共同作用(图3)，故合力的公式为

图3 机器人受合力作用的仿真

F=Frep+Fttr

(5)

图3为机器人在目标点吸引和障碍物排斥的联合作用下，机器人避开障碍物，向目标点前进的仿真实验图。

2 人工势场法存在的问题

2.1 局部极小点

局部极小点问题是指机器人在运动过程中，机器人受到的合力为零或者接近于零，导致机器人在运动过程中停滞不前或者震荡。从而误认为局部极小点是目标点，局部最极小问题主要有两种：

第一种：机器人、障碍物和目标点在同一条直线上，当机器人距离目标点较远时，目标点对机器人的吸引力较大。同时障碍物距离目标点更近，且距离机器人也更近，因此，对机器人的斥力较大。导致机器人所受到的合力为零。从而失去对对机器人的引导力，误认为到达目标点。

第二种：机器人与目标点的连线上没有障碍物，但周围有两个或两个以上的障碍物，障碍物提供给机器人的斥力组合为一个合力，合力与目标点对机器人的引力大小相同，方向相反。同时机器人的速度存在，故在合力的作用下，机器人在极小点震荡。第二种在实验中更为常见，如图4。

图4 局部极小点两种方式

2.2 目标不可达

理想状态下，目标和障碍物相距较远，当机器人逼近目标时，障碍物的斥力变得非常小甚至可以忽略不计，机器人将只受到吸引力的影响便可直接达到目标。但在许多实际环境中，往往会有一个甚至多个障碍物与目标点离的很近，在这种情况下，当机器人接近目标时，它也将向障碍物靠近，导致斥力将远大于引力，这样机器人会远离目标点。当距离障碍物较远时，又会被吸引力吸引重新向目标点移动，如此反复振动。因此移动机器人将不可能到达目标。

3 改进人工势场法

由于存在局部极小点和目标不可达等问题，故采用设置暂时中间目标点和重新定义斥力函数的方法。为机器人重新定义一个实例函数，将目标点的势场值作为机器人工作环境中的最小点。同时当机器人陷入局部极小点时，使暂时的中间目标点对机器人产生吸引力，致使机器人远离极小点。待机器人到达暂设置的中间目标点后，撤销中间目标点，继续向真正的目标点行驶。

3.1 对于目标不可达的改进

由于传统人工势场法的斥力场可知，目标点的势场值可能不是机器人工作环境中势场值的全局最小点。为了解决这个问题，需要在斥力场公式中引入目标点与机器人的相对位置，从而构建一个新的斥力场公式：

(6)

其中k是比例系数，P(x0,y0)-P(q0)为机器人与障碍物之间的欧氏距离，ρ0为障碍物的作用范围，m是指机器人与目标点距离的m次方。与之前的斥力公式相比，新的公式加入了机器人与目标点之间的的欧式距离，确保了目标点是处于机器人工作环境中势场值的最小值。

根据重新定义的斥力公式可知公式，当机器人未到达目标点时，斥力可以写为

(7)

其中

(8)

(9)

图5 机器人的受力情况

根据m的不同取值，分析市场函数的不同特征。

1)当0

(10)

(11)

当机器人向目标点靠近时，ρm(q,qgoal)接近于零，故F1会逐渐变小趋近于零，而F2会越来越大，在两个力的组合影响下，机器人向目标点前进。

2)当m=1时，ρ(q0)<ρ0且ρ(q0)≠0。

(12)

(13)

3)当m>1时，机器人在靠近目标点的斥力场可微分，故F1和F2都接近于零，从而使斥力和为零，机器人在吸引力的作用下向目标点移动。

3.2 对于局部极小点的改进

首先对是否陷局部极小点进行检测，由局部极小点的成因指出，机器人受到的引力和斥力很少会出现绝对的方向相反且大小相等的情况，即机器人所受到的合力为零。故假设实验中机器人在陷入陷阱后，在某一圆形邻域内反复震荡，并无法走出该邻域。假设局部极小点的半径为C1，机器人第n次和第n+1次的位姿为Qn和Qn+1(n>=0)，同时设定机器人每隔T段时间进行一次检测，记录下每时刻机器人的位姿。当在T这一时间段内，当|Qn+1-Qn|>C1时，则认为机器人没有陷入目标点，可以继续使用目标点经进行指引。若机器人仍在一个很小的范围内，即当|Qn+1-Qn|

根据上述算法的描述，机器人运行过程中每隔一段时间记录一次当前位姿，若检测到没有处于同一邻域则继续导航并记录位姿。若检测到陷入同一邻域，则开始添加中间目标点进行指引。

当检测到机器人陷入到了局部极小点的陷阱中，需要自动选择一个中间目标点，将机器人牵引出。本文中间目标点的设置采用在障碍物边缘选择中间目标点的方法。

首先将目标点的中心与机器人的中心连线，记作L1。之后以L1为路线，搜索距离机器人的最近的障碍物。以障碍物中心为点做一条垂直于L1的直线，记作L2，将L2周围的障碍物结合障碍物的作用范围(当机器人与障碍物的距离大于这个距离时，斥力视为零)视为一个障碍物集合。在距离障碍物集合两个步长距离的位置，作垂直于L1的直线，记作L3。沿L3进行对直线L3两侧进行搜索。当搜索到直线L3两侧均没有障碍物时，视为找到障碍物边缘，记录障碍物两个边缘到L1的距离，选择距离短的一边设置障碍物，如图6；当搜索到一侧有障碍物，一侧没有障碍物时，则在无障碍物的设置中间目标点，如图7；以距离L2一个步长的位置设置中间目标点。当机器人行驶至中间目标点后，撤销中间目标点，继续使用真正的目标点对机器人进行牵引。若搜索到两边均有障碍物，则将中间目标点设置在距离L1最近的一侧，当到达中间目标点后重新设置其他中间目标点继续进行牵引。

图6 两侧均无障碍物中间目标点的设置 图7 一侧有障碍物中间目标点的设置

之后再设目标点的位姿为Qgoal，目标点邻域范围为C1，当|Qn+1-Qgoal|<=C1,则认为机器人到达目标点。若|Qn+1-Qgoal|>=C1，则认为机器人未到达目标点，继续进行导航并继续经行是否陷入极小点的判断。如图7，为机器人行动过程中对是否到达目标点的判断过程。

图7 检测是否到达目标点

算法步骤如下：

1)判断机器人是否陷入局部极小点的陷阱，若陷入则进入设置局部极小点的算法流程中，否则继续按人工势场法进行导航。

2)计算机器人位姿和目标点位姿的连线L1，根据直线L1和机器人位姿计算出直线L2，根据障碍物的斥力影响范围计算出障碍物合集，并计算出直线L3。

3)沿L3对障碍物进行扫描。

①若侧都无障碍。则计算直线L1到障碍物集合两边的距离并比较，选择距离较短的一边，在距离L2一个步长的位置设置中间目标点，使用中间目标点对障碍物进行牵引(图6)。

②若有一侧没有障碍物，一侧有障碍物。则选择在没有障碍物的一侧，扫描到障碍物的边缘，设置中间目标点，并对机器人进行牵引(图7)。

4)重新检测是否陷入局部极小值的陷阱中，并检测是否到达目标点。若不符合，则继续按照人工势场算法导航。

4 仿真实验与分析

本文通过MATLAB经行仿真分析，分析传统人工势场法与改进人工势场法对机器人导航过程中遇到的目标不可达和局部极小点问题的仿真结果，并进行对比。验证了改进人工势场法的可行性与优越性。

4.1 改进目标不可达的仿真实验

在仿真图(图8～9)中定义网格为1m×1m，设机器人起始位置为(0,0)，目标点为(10,10)，障碍物的位置为[(1.5,1.5)(3,3)(3.2,6)(4,4.5)(6,2)(5.5,6)(8,8)(7.8,9)]其中引力增益系数为15%，斥力增益系数为5%，步长为0.2%。

由图8所示机器人从(0,0)向目标点移动当有移动到障碍物附近时，由于障碍物距离目标点较近，故在传统的人工势场法公式的作用下，机器人在障碍物周围徘徊无法到达目标点。而图九是根据改进后的斥力场公式，机器人顺利到达目标点。

图8 传统斥力场仿真实验 图9 改进后的斥力场仿真实验

4.2 改进局部极小点的仿真实验

在仿真图中定义网格单元为1 m×1 m，设机器人起始位置为(0,0)，目标点为(10,10)，障碍物的位置为[(1,1)(4,6)(6,4)(3,6)(6,2)(5.5,5.5)(8,7)]其中引力增益系数为15%，斥力增益系数为5%，步长为0.2%。

由图10所示，机器人从(0,0)开始驶向目标点(10,10)，在移动到(5,5)附近时，由于(4,6)、(6,4)2个障碍物提供的斥力，与目标点提供的引力，使机器人陷入了局部极小点的陷阱中，停滞不前。而使用改进后的人工势场法，如图11，当机器人陷入局部极小点后，在(6,3)的位置设置了一个中间目标点。使机器人摆脱了陷阱，移动到中间目标点后，撤销中间目标点，继续向真正的目标点移动。由此可见，设置中间目标点可以有效的解决机器人在导航过程中陷入中间目标点的问题。

图10 传统人工势场法仿真 图11 改进人工势场法仿真

5 结 论

本文首先分析了机器人导航过程中可能会遇到的目标不可达和局部极小点的原因，并针对这些问题提出了重新定义斥力场公式和分配中间目标点的改进方法。重新定义斥力场公式，使得目标点成为机器人工作环境中势场值的最小点，防止在势场中因为障碍物距离目标点较近，导致其他势能最低点的出现，使机器人无法到达目标点。在判定机器人陷入局部极小点后，在障碍物集合周围放置一个中间目标点，利用中间目标点的吸引力对其进行引导，从而将机器人拉出局部极小点。机器人到达中间目标点后，取消中间目标点，继使用真正目标点吸引，向真正的目标点吸引。

文章重新定义了斥力公式，设置了中间目标点。在斥力公式中引入了目标点与机器人的相对位置，解决了因障碍物和目标点距离较近导致的目标不可达。增加中间目标点，帮助机器人摆脱陷阱。与传统的人工势场法相比，极大的提高了路径规划的成功率。

使用MATLAB进行了仿真实验，证实了改进的人工势场法的有效性与优越性。