改进人工势场法的机械臂避障路径规划

王俊龙，张国良，羊帆，敬斌

WANG Junlong1,ZHANG Guoliang1,YANG Fan2,JING Bin1

1.第二炮兵工程大学301教研室，西安 710025

2.宝鸡市150信箱11分箱23号

1.Τeaching and Research Office 301,Τhe Second Artillery Engineering University,Xi’an 710025,China

2.No.23,11 Cases,P.O.Box 150,Baoji

改进人工势场法的机械臂避障路径规划

王俊龙1，张国良1，羊帆2，敬斌1

WANG Junlong1,ZHANG Guoliang1,YANG Fan2,JING Bin1

1.第二炮兵工程大学301教研室，西安 710025

2.宝鸡市150信箱11分箱23号

1.Τeaching and Research Office 301,Τhe Second Artillery Engineering University,Xi’an 710025,China

2.No.23,11 Cases,P.O.Box 150,Baoji

1 引言

机械臂的避障路径规划是指在有障碍的环境下，机械臂按照给定的某一种方法找到从初始状态到末状态的无碰撞的运动轨迹。机械臂常见的路径规划方法有插值法等，典型的机器人路径规划方法有人工势场法、栅格法、A*算法、D*算法[1-3]。

机械臂路径规划的传统方法是利用机械臂末端在空间中进行规划，尔后通过关节之间的约束分解得到各个关节的运动轨迹。但是这种方法只适用于具有特定结构的机械臂，即能够利用解析方法，利用末端位姿，通过运动学反解，得到各关节的角度值的封闭解[4-6]。然而由于机械臂自身多关节、强耦合的特点，即使能够通过解析法机型运动学反解，也存在反解计算量大，甚至得不到与当前角度连续的角度值的问题，即在机械臂末端变动不大的情况下，反解出来的关节角度不连续，产生奇异解问题[4]。

本文所采用的机械臂并不具有这种特殊结构，不能够运用解析的方法进行机械臂运动学反解，更不能够实现基于传统方法机械臂的避障路径规划。

在目标点与障碍已知的情况下，可以利用遗传算法等方法求解得到一组关节值，使得在此关节值组合下，机械臂末端达到目标，同时机械臂远离空间障碍，实现机械臂的反解。

本文提出在得到一组合理的关节值后，在关节空间内进行机械臂路径规划，保证机械臂在运动过程中，末端朝向目标点，最终到达目标，同时避开障碍。本文改进人工势场法的运动控制标准，在机械臂运动过程中，以机械臂所受到的引力势能与斥力势能的合势能最速下降为标准控制机械臂运动。同时提出针对人工势场法局部极小的问题，本文采用在局部极小附近添加空间虚拟障碍、利用虚拟障碍所产生的虚拟斥力势能将机械臂推离局部极小，绕过障碍，到达目标。

2 人工势场法基本原理

人工势场法[7-8]是由Khati提出的一种局部路径规划方法，其基本思想是将移动机器人在环境中运动视为一种机器人在虚拟的人工受力场中运动，障碍物对移动机器人产生斥力，目标点对移动机器人产生引力，斥力和引力的合力作为控制移动机器人向目标点运动的力，由于目标点被设计为合力的全局最小点，因此在合力的作用下，理论上移动机器人可以到达目标点。

假设移动机器人当前时刻的位置X=[xy]Τ，目标点位置Xgoal=[xgoalygoal]Τ，距离移动机器人最近的障碍物位置Xobs=[xobsyobs]Τ，则移动机器人所受的引力场被定义为：

式中ξ为正比例系数；ρ(X，Xgoal)=||X-Xgoal||为机器人X到目标Xgoal的距离。

移动机器人所受的斥力场定义为：

式中η为正比例系数；ρ(X，Xobs)为移动机器人到障碍物的距离；ρ0为不影响移动机器人安全运动的最小安全距离。

3 机械臂模型

3.1 机械臂运动学模型

本文采用标准D-H法[9]建立机械臂运动学模型，结果如下：

图1为D-H模型图，图2为实物图，表1为模型参数说明。

图1 机械臂D-H模型

图2 机械臂实物图

通过运动学正解的方法可以得到在当前关节角下所对应的末端位置。通过求解可以发现，机械臂末端位置是关节1～5的角度的函数，而与关节6无关，关节6只影响末端的姿态。

表1 机械臂D-H模型参数

本文仅考虑机械臂的末端位置，因而以机械臂关节1～5的关节角度为控制目标，不考虑关节6角度的影响。

3.2 机械臂4连杆简化模型

机械臂不同于自主移动机器人，在应用人工势场法时不可看做质点，必须考虑障碍物到机械臂各关节连杆的距离。对六自由机械臂建立简化4连杆模型如图3所示。

图3 机械臂4连杆简化模型

将机械臂的末端定义为关节7。其中：

基座、关节1、关节2构成一条线段，定义为直线1；关节2、关节3、关节4构成一条线段，定义为直线2；关节4、关节5构成一条线段，定义为直线3；关节5、关节6、关节7构成一条线段，定义为直线4。

通过第3.1节机械臂运动学模型，可由机械臂当前关节角组合S=(θi1，θi2，…，θi6)求解出基座、关节2、关节4、关节5和关节7的空间位置，方法如下：

基座：

上式中，pix，piy，piz分别表示关节i在世界坐标系下x，y，z三个轴向上的分量，也就是世界坐标系下的三维直角坐标。通过立体几何方法，由基座、关节2、关节4、关节5和关节7的空间位置求得直线1到直线4的空间解析方程，进一步求解得到空间障碍到各个连杆的距离。

4 改进人工势场法的机械臂路径规划

改进的人工势场法以机械臂各关节角度作为控制和运算的对象。在机械臂由当前关节角度运动到目标关节角度的过程中，实现机械臂的避障路径规划，保证末端接近目标位置，机械臂避开障碍。

4.1 机械臂搜索空间

机械臂的运动并不像自主移动机器人一样可以在空间范围内自主向各个方向方便地移动，机械臂由于其自身多关节、强耦合的特点，任何一个关节运动都会导致其后关节连杆的位置变化。按照传统人工势场法的思想，必须依次求得各关节所受到的力，然后得到一个合力，决定机械臂的移动方向。然而世界坐标空间的运动控制必须经过运动学反解转换到关节坐标系，得到各个关节转动的角度值，这将会使得机械臂频繁地进行反解运算，不仅计算的难度和计算量很大，更有可能得不到前后连续的关节角度值，即奇异解，这在实际应用中是不允许出现的。

本文采用机械臂关节空间作为人工势场法的搜索空间，基本思路为：以当前关节状态S=(θi1，θi2，…，θi5)为初态，遍历当前状态的相邻关节值，求取出各种关节组合所对应的合势能，选择合适的关节组合作为机械臂下一步的运动目标，在此需要设置一个搜索步长λ，在此设置为0.5。

以关节空间作为搜索空间有以下几个优点：

（1）关节空间搜索必连续，不会出现奇异点。

（2）由关节空间通过正解方法得到的机械臂路径必可达。

（3）从关节坐标系到世界坐标系的转换计算难度和计算强度远低于世界坐标系到关节坐标系。

4.2 合势能最速下降的控制标准

在第4.1节的分析中可以发现，采用传统人工势场法产生一个合力来控制机械臂运动的方法是行不通的。

通过分析可以发现，引力势函数与斥力势函数有这样的性质：

（1）引力势函数在整个空间内都是有效的，没有作用范围的限制；斥力势函数仅在斥力场内有效。

（2）机械臂引力势能可以通过机械臂当前关节角度和目标角度的差求取；机械臂斥力势能可以通过机械臂到障碍物的距离计算。

（3）引力势能总是大于等于0的；斥力势能在其作用范围内也是大于等于0的；当机械臂到达目标点后，引力势能与斥力势能同时为零。

机械臂运动的目标应是使机械臂所受到的引力能与斥力势能的合势能减小，当机械臂到达目标点后，引力势能与斥力势能同时为零。若机械臂停止运动，而所受到的合势能不为零，说明机械臂还未到达目标，机械臂陷入局部极小。

以机械臂六个关节角度最终位置为目标，求解得到机械臂所受的引力势能和斥力势能：

其中Eatt、Erep和Ei分别表示机械臂所受引力势能、斥力势能和各关节连杆所受的斥力势能；ka和kr分别表示所受引力和斥力的比例系数；di和d0分别表示各关节到障碍物的距离和障碍物的斥力影响范围。

4.3 添加虚拟障碍

4.3.1 局部极小的判断标准

人工势场法属于局部路径规划，只能考虑局部障碍信息，容易陷入局部极小。在机械臂的应用中，以势能的变化为控制标准，局部极小的表现为：在机械臂进行关节空间搜索时，其中的一个角度使得机械臂的引力势能与斥力势能合势能达到一个极小点，任何角度的变化会使得两者之和增大，机械臂不会继续有关节角度的变化，停止运动。若当前关节不是机械臂运动的目标角度值，此时机械臂陷入当前关节局部极小。

图4 机械臂障碍点与机械臂位置示意图

局部极小的判断可以通过以下方式进行：判断当前关节值与n次之前的角度值绝对值之差，若两者的差值在一个很小的范围，表示机械臂在一个关节角度停止不动或者震荡，则可以认为机械臂陷入了局部极小。

4.3.2 虚拟障碍的位置与基本参数

文献[10]提出一种针对二维平面移动机器人局部极小问题添加虚拟障碍的方法。受其启发，本文针对机械臂局部极小问题，采用在局部极小附近添加空间虚拟障碍，利用虚拟障碍产生的斥力势能将机械臂推出局部极小。

如图4所示虚拟障碍设置于：虚拟障碍点与机械臂到障碍点最近位置P的连线垂直于平面X：由当前角度下关节B、关节C所在位置与目标角度下关节C所在的位置确定。虚拟障碍点到点P的距离可以根据情况设置为一个合适的值。

为了使虚拟障碍在局部极小附近成为主要作用场，保证机械臂脱离局部极小，通常设定虚拟障碍点的斥力系数为一般障碍的1～1 000倍、作用范围为一般障碍物的1.5～3倍[7]。

本文设置虚拟障碍到点P的距离为机械臂的安全距离d0，设置斥力为普通障碍物的10倍，斥力范围为普通障碍的1.5倍。

5 仿真实验

为了说明所提出方法的有效性，进行了仿真实验。实验从当前的角度运动到目标的角度，在运动过程中，避开障碍，当机械臂成功到达目标角度后，说明机械臂成功避开障碍，末端到达目标点。

设定机械臂运动控制参数如下：

初始角度S0=(0，0，0，0，0，0)，目标角度SE=(45，-45，0，45，0，0)，障碍位置PObj=(25，25，430)，运行步长λ=0.5°，安全距离dsafe=10，引力系数ka=30，斥力范围d0=40，斥力系数kr=50，虚拟障碍斥力系数kνr=500，虚拟障碍范围dν0=60。

局部较小的判断标准：运行10次的角度绝对值之差的和小于1.5。

实验1未添加虚拟障碍

未添加虚拟障碍时，机械臂会陷入到局部极小。结果如图5所示。

图5 未添加虚拟障碍机械臂关节运行图

在未添加虚拟障碍时，机械臂陷入局部较小，停止在S=(45，-2.5，0，45，0，0)的关节角组合上。

实验2添加虚拟障碍

实验结果如图6～图8所示。

图6 斥力势能、引力势能、未添加虚拟势能合势能变化图

由图6、图7可以看出，当机械臂在进入到局部极小之前，机械臂的合势能始终下降，机械臂的关节角度不断变化，机械臂处于在合势能最速下降的控制下运动。

当运行到第99步，机械臂利用局部极小判断准则，判断出陷入局部极小。此时添加虚拟障碍。通过虚拟障碍产生的虚拟势能，使得机械臂脱离局部极小，最后机械臂受到的合势能减小为零，机械臂到达目标。

图7 虚拟势能、添加虚拟势能合势能变化图

图8 关节位置轨迹图

6 结论

改进人工势场法在关节空间内、以机械臂所受的引力势能与斥力势能的合势能最速下降为标准，控制机械臂运动的方法，可以有效解决简单障碍环境下机械臂避障路径规划的问题。

该方法主要是针对关节空间内进行，以角度为运算对象，存在的主要不足之处在于该方法的实现必须在通过其他的方法得到一组合理的关节角度解后，以该组角度解作为目标，进而实现避障路径规划。下一步的努力方向在于拓展该方法的适应性，降低该方法对其他方法的依赖。

[1]李爱萍，李元宗.机器人路径规划方法的研究[J].机械工程与自动化，2009（10）：194-197.

[2]朱大奇，颜明重.移动机器人路径规划技术综述[J].控制与决策，2010（7）：961-967.

[3]Gerke M.Genetic path planning for mobile robots[C]//Proc of American Control Conference，San Diego，CA，USA，1999：596-601.

[4]Niku S B.机器人学导论——分析、系统及应用[M].孙富春，朱继红，刘国栋，译.北京：电子工业出版社，2004.

[5]余达太，马香峰.工业机器人应用工程[M].北京：冶金工业出版社，2001.

[6]吴振彪，王正家.工业机器人[M].2版.武汉：华中科技大学出版社，2006.

[7]刘春阳，程亿强，柳长安.基于改进势场法的移动机器人避障路径规划[J].东南大学学报，2009（9）：116-120.

[8]张建英，刘暾.基于人工势场法的移动机器人最优路径规划[J].东南大学学报，2007（8）：183-188.

[9]李宏庆.基于视觉的六自由机械臂控制技术研究[D].南京：南京理工大学，2009.

[10]黄健生.移动机器人的路径规划研究[D].杭州：浙江大学，2008.

Aiming at the shortcoming of traditional method of path planning using merely on typical structure of manipulator,after the result of using a third way as genetic algorithm to get an ideal data,a new method of improved artificial method with combined potential energy steepest descent based on arthrosis space is proposed.Using the method of adding virtual obstacle in the minimum field to produce virtual potential energy in solving the problem of local minimum,the manipulator successfully achieves the goal avoiding the obstacle.Τhe simulation results show that the proposed method is valid and effective in path planning of manipulator.

improved artificial field method;combined potential energy steepest descent;manipulator;path planning;virtual obstacle

针对传统机械臂路径规划只适用于具有特殊构型的机械臂这一不足，提出在利用遗传算法等方法求解得到一组合理的关节值后，运用人工势场法在机械臂关节空间内搜索、采用合势能最速下降为控制标准的机械臂路径规划方法。同时利用在局部极小附近添加虚拟障碍，产生虚拟斥力势能的方法解决了局部极小问题，使机械臂成功绕过障碍到达目标。仿真验证了该方法的正确性和有效性。

改进人工势场法；合势能最速下降；机械臂；路径规划；虚拟障碍

A

ΤP242

10.3778/j.issn.1002-8331.1201-0360

WANG Junlong,ZHANG Guoliang,YANG Fan,et al.Improved artificial field method on obstacle avoidance path planning for manipulator.Computer Engineering and Applications,2013,49（21）：266-270.

王俊龙（1987—），男，硕士，主要研究领域为先进控制理论及应用、机械臂路径规划与运动控制；张国良（1970—），男，博士，教授，主要研究领域为先进控制理论及应用；羊帆（1984—），男，硕士，主要研究领域为先进控制理论及应用、机器人视觉问题；敬斌（1976—），男，讲师，主要研究领域为多传感器信息融合、模式识别。E-mail：junlong87@126.com

2012-01-19

2012-03-05

1002-8331（2013）21-0266-05

CNKI出版日期：2012-06-29http://www.cnki.net/kcms/detail/11.2127.ΤP.20120629.1645.001.html