基于改进人工势场法双向规划的双机械臂避碰路径规划

邱 博，单 梁，常 路，王志强，屈 艺

(南京理工大学 自动化学院，南京 210094)

作为机器人的常见类型，机械臂被广泛应用于工业以及服务领域，类似人类手臂的形态使机械臂具备足够的灵活性来完成复杂的任务.尽管如此，单机械臂操作仍存在局限性，有些任务需要双机械臂协同工作才能完成，例如零件的装配以及大件物品的搬运.然而双机械臂在运动过程中，往往存在工作空间的重叠，双臂运动时，不仅要避开环境中的障碍，还要避免机械臂之间发生碰撞，因此有必要对双机械臂进行避碰路径规划.

国内外学者对双机械臂路径规划做了大量研究.文献[1]将机械臂连杆间的最短距离作为避碰距离指标对冗余双臂机器人进行协调避碰.文献[2]从能量的角度规划双机械臂的路径，在双臂之间设置一个虚拟的弹簧，弹簧受到弹力会迫使双臂互相远离.文献[3]使用动态神经网络算法规划具有物理约束的双机械臂避碰路径.但是只考虑双臂间的避碰并不能实现双臂协同运动，还要同时考虑机械臂的路径规划.

机械臂的避障路径规划方法主要有两种：自由空间法和人工势场法[4-5].自由空间法需要预先得到全局的信息，算法开销较大，如A*算法[6]以及快速搜索随机树[7]等.相较于自由空间法，人工势场法不需要计算全局信息，其结构简单，算法效率高，更适合处理多关节机械臂的路径规划.近年来学者们一直在研究人工势场法的改进算法.对于人工势场法易陷入局部极小点的问题，文献[8]通过引入快速扩展随机算法跳出局部极小值点.有学者则通过加入虚拟障碍[9]或者虚拟目标点[10]使人工势场算法跳出局部极小点.但随机算法和添加虚拟点的方式都增加了算法的计算量，并增大了路径振荡的可能.

针对这些问题，文中提出了一种基于改进人工势场法双向规划的避障算法，从改善振荡问题、提升算法效率以及避免局部极小点3个方面对人工势场法进行改进：引入了斥力变增益系数，采用了自适应步长策略，提出了双向规划算法.将改进后的算法应用于主从模式下的双机械臂路径规划，先针对环境障碍规划主臂路径，然后根据主臂每个时刻点的位姿得到主臂的动态碰撞模型，将其作为障碍规划从臂的路径，最终实现双机械臂避碰路径规划.

1 双机械臂建模

以ABB公司生产的IRB 120型六轴机械臂为对象建立双机械臂模型，对其进行运动学分析，并以此来研究双机械臂避碰路径规划.

1.1 机械臂运动学分析

采用D-H方法[11]建立机械臂的运动学模型.IRB120型机械臂尺寸参数及简化模型如图1、2.

图1 ABB IRB 120型机械臂(单位:mm)

图2 简化模型

根据机械臂的简化模型可以得到该机械臂的D-H参数，如表1.

表1 D-H参数

由机械臂D-H参数，可以通过坐标变换得到相邻两个关节之间的转换矩阵为：

(1)

由此，可进一步得到机械臂末端与机械臂基座之间的转换矩阵为：

(2)

综上，可以通过机械臂的关节角度，得到机械臂末端的位姿，即正运动学；反之，也可以反解公式(2)通过末端位姿求得机械臂关节量，即逆运动学.

1.2 双机械臂位姿描述

文中两台机械臂分为主臂和从臂，两机械臂具有相同的初始姿态，并关于X轴对称，通过MATLAB中的Robotics System Toolbox建立图3双机械臂模型.

图3 双机械臂模型

从臂可以看作是主臂平移而来，从臂的基坐标原点Xslaver满足如下转换公式：

Xslaver=Xmaster·Ttrans

(3)

式中：Xmaster为主臂的基坐标原点；Ttrans为转换矩阵.

2 基于改进人工势场法的双向规划算法

人工势场法被广泛应用于移动机器人的路径规划[12-14]，其基本思想就是将机器人视为质点，在其工作空间中建立由引力场Uatt(q)和斥力场Urep(q)构成的人工势场，两者产生的引力Fatt(q)和斥力Frep(q)的作用在机器人上趋近目标点.人工势场法原理如图4.

设机器人当前位置为q，人工势场施加在机器人上的合力为：

F(q)=Fatt(q)+Frep(q)=

-∇Uatt(q)-∇Urep(q)

(4)

与移动机器人相比，人工势场法规划机械臂路径时，不仅要保证机械臂末端到达目标点，还要考虑机械臂各连杆与障碍物之间可能发生的碰撞.文中在机械臂的工作空间中建立了合适的人工势场，并从振荡问题、算法效率以及局部极小点等3个方面对人工势场法进行了改进.

图4 人工势场法原理

2.1 机械臂的引力场与斥力场

为了更好地将人工势场法应用于机械臂的路径规划，分别在关节空间以及笛卡尔空间中建立引力场和斥力场.

2.1.1 关节空间中建立引力场

引力场存在于机械臂与目标位置之间，离目标位置越远，引力越大，产生的引力牵引机械臂到达目标位置.

在笛卡尔空间中规划多自由度机械臂路径时，需要不断进行逆运动学解算，常常导致奇异解的出现.为了避免此问题，在机械臂的关节空间内建立引力场Uatt(q)[15].

(5)

式中：q=(θ1,θ2,θ3,θ4,θ5,θ6)T为机械臂当前位姿；qgoal为目标位姿；ζ为引力增益.设置距离阈值d，防止产生引力过大导致避障失败.

对引力场Uatt(q)求负梯度，可得引力：

(6)

2.1.2 笛卡尔空间中建立斥力场

斥力场存在于机械臂与障碍物之间，产生的斥力Frep(q)使机械臂远离障碍物，其大小与机械臂到障碍物最短的距离ρ(q)成反比.

在关节空间内建立斥力场十分复杂，需要在发生碰撞的所有机械臂姿态Qobs中计算出最小的ρ(q).因此，在更容易计算ρ(q)的笛卡尔空间中建立机械臂的斥力场[15].

由于不能将机械臂看作质点，在机械臂上定义几个控制点作为斥力场中机械臂的受力点.选择机械臂的3、5和6关节所在坐标原点O1、O2、O3作为控制点，如图5.

图5 机械臂控制点

设ai(q)为位姿q时控制点Oi的坐标，对于控制点Oi有斥力势能：

(7)

式中：ηi为控制点Oi斥力增益;ρ(ai(q))为Oi到障碍物的最短距离;ρ0为障碍物对Oi产生斥力的最大距离.

进一步可得施加在Oi上的斥力为:

(8)

式中：∇ρ(ai(q))=(ai(q)-Os)/‖ai(q)-Os‖，其中Os为障碍物上离控制点Oi最近的点.

(9)

式中：Jv,i(q)为机械臂雅克比矩阵中线速度的部分.

2.2 引入斥力变增益系数

机械臂在斥力作用区的某些位置，会发生Fatt(q)>Frep(q)和Fatt(q)

对此，引入斥力变增益系数λ，使斥力增益在振荡发生阶段呈递减趋势，减小斥力增益与引力增益的比值，避免合力方向的突变，可以实现较为平滑的轨迹，具体如下：

ηi=λ·η0

(10)

式中：η0为初始斥力增；ρs为合力方向突变处到障碍物的最短距离；Δρ为λ作用范围阈值；κ为(0,1]之间的常数.

图6为引入斥力变增益系数前后机械臂末端在Z轴上的运动，可以看出，引入斥力变增益系数后，机械臂在障碍物附近的振荡情况明显改善.

图6 机械臂末端在Z轴上的运动

2.3 引入自适应步长策略

为了进一步提升算法的搜索效率，在路径搜索的过程中引入自适应步长策略.

机械臂下一步的位姿qnext通过梯度下降方向来确定，设机械臂当前位姿为q，有：

(11)

式中：α为搜索步长；ò为最大允许误差.

算法根据控制点与障碍物的距离自适应修改步长α.当障碍物较远时，采用大步长提高搜索效率；当障碍物较近时，缩小步长，保障算法精度，具体如下：

(12)

式中：α0为初始步长；τi为控制点Oi的权重系数，τ3>τ2>τ1>0.

2.4 提出双向规划算法

针对人工势场法规划路径时容易陷入局部极小点的问题，提出基于人工势场法的双向规划算法.局部极小点具体表现为：当机械臂在进行路径搜索时，在某个位置上机械臂受到的引力与斥力的合力恰好处于一个极小值点，此时任意关节角度的变化都将导致合力变大，机械臂将在此位置反复运动，机械臂陷入局部极小，算法将会失效.

双向规划就是用人工势场法从初始点和目标点分别规划出两条反向的路径，每次搜索两段路径的目标点都会更新，机械臂受到的引力也会随之变化，一定程度上避免了局部极小点的出现.最后将这两条路径拼接成最终路径.

双向规划的具体步骤如下：

步骤1：由机械臂末端在笛卡尔空间中的初始点以及目标点pstart、pgoal得到机械臂在关节空间内的初始值qstart以及目标值qgoal.

步骤2：从qstart开始作正向规划，初始目标为qgoal；从qgoal作逆向规划，初始目标为qstart.

(13)

(14)

步骤4：重复步骤3，直到经过j次搜索后，正向和逆向规划得到结果满足以下条件，算法结束.

(15)

然而，即便满足式(15)，仍然有可能存在两端路径拼接处不平滑的情况，所以需要对双向规划的拼接处作平滑处理.

对单个关节，设有待处理数据{(xk,φk) |k=1,2…,n}，xk为路径时刻点，φk为关节角度，三次光滑样条曲线函数f满足以下最优条件：

(16)

式中：μ≥0为曲线平滑系数，μ越大，生成的曲线越平滑.最终的平滑效果如图7(以关节4、5为例).

图7 拼接处平滑

3 双臂避碰路径规划

对于工作空间存在重叠的双机械臂，避碰路径规划十分必要，因此基于改进人工势场法的双向规划算法应用于双机械臂路径规划，双臂采用主从运动模式，先针对环境障碍物规划主臂路径，然后将主臂作为动态障碍规划从臂的路径.

3.1 双机械臂主从运动模式

双臂协作方式主要有3种：双臂共同完成一个任务且机械臂末端无相对运动；双臂共同完成一个任务且机械臂末端有相对运动；双臂分别完成独立的任务.文中双臂分别执行不同的任务，有不同的起始点与目标点，并且机械臂末端存在相对运动.

为了简化规划算法，双臂采用主从模式运动，将机械臂分为主臂和从臂，先对主臂进行路径规划，然后将主臂视为动态障碍规划从臂路径.

主臂路径的规划比较简单，与常规的单机械臂避障路径规划相同，只需要考虑避开环境中的障碍物，使用提出的基于改进人工势场法的双向规划算法对主臂进行避障规划，具体流程如图8.

图8 主从运动规划流程

3.2 基于主臂动态碰撞模型的从臂路径规划

规划从臂路径时情况要复杂一些，不仅要考虑环境障碍物，还要将主臂作为一个运动的障碍物进行避碰.不同于静态障碍物，需要对主臂建立比较准确的动态碰撞模型.

由于主臂路径已知，相当于主臂每个时刻的运动姿态都已经得到，可以据此建立主臂的动态碰撞模型. 通常都是用胶囊体几何结构来简化机械臂模型，即用球来代替机械臂关节，圆柱代替连杆.通过此方式建立的碰撞模型虽然精度较高，但是若将机械臂视为障碍，那么某些位姿下建立的碰撞模型可能是非凸的 (nonconvex)[17]，从而导致人工势场法失效.

为了建立有效的动态模型，取各关节坐标原点处横截面最小外接圆Oi(i=1,2…,6)构成点集υ，然后以υ创建凸包作为主臂的碰撞模型.这样在保证算法有效性的同时也减少了计算开销，图9为[0.75,1,-1,0,1.57,0]T姿态下的主臂碰撞模型.

图9 主臂碰撞模型

在规划从臂路径时，按照双向规划从路径两端到中间的策略，依次更新主臂碰撞模型，并将其均匀的分布在从臂的规划过程中.算法在最后收敛阶段步长较小，搜索次数较多，此时保持主臂模型为中间状态，直至从臂路径规划完成.

4 算法仿真

基于MATLAB软件构建仿真环境，通过仿真验证改进人工势场法双向规划算法的有效性以及双臂避碰路径规划的可实现性.

4.1 主臂路径规划结果及算法验证

首先规划主臂路径，并与传统人工势场法对比验证提出算法的有效性.

设主臂基坐标为[0.350, 0, 0]，从臂基坐标为[-0.350, 0, 0].障碍物为两个半径0.1 m的球体，球心坐标分别为[-0.100, 0.200, 0.750]和[-0.1, -0.03, 0.95].

主臂初始关节量为[0,0,0,0,0,0]T，末端坐标为[0.350, 0.380, 0.620].设主臂末端目标点为[-0.100, -0.200, 0.750]，初始搜索步长α0=0.05，距离阈值d=0.4，引力增益系数ζ=1 000，斥力产生最小距离ρ0=0.4，斥力初始增益η0=1 000.

首先采用传统(未改进)人工势场法进行路径规划，经过43次左右的搜索后，主臂会在点[0.033, 0.100, 0.783]附近陷入局部极小点，无法完成路径规划，已规划的部分也存在障碍物附近振荡的问题.具体结果如图10、11.

图10 主臂路径(传统人工势场法)

图11 主臂末端位置(传统人工势场法)

而使用双向规划可以成功规划主臂避障路径，正向和逆向规划搜索次数各为31次，分别收敛在点[0.031,0.173,0.815]和[0.038,0.162,0.826]，仿真结果如图12.

图12 主臂路径(双向规划)

主臂在关节空间以及笛卡尔空间内的轨迹如图13、14.

图13 主臂关节角

图14 主臂末端位置

为了进一步验证所提算法，在[-0.100, -0.200, 0.750]附近取了15个不同的点作为目标点，分别使用传统人工势场法和文中算法对主臂进行路径规划，最终得到实验结果如表2.

表2 规划结果对比

综上，相较于传统单向的人工势场法路径规划，基于改进人工势场法的双向规划算法够避免局部极小值以及振荡现象的发生，以较高的成功率实现避障路径规划.生成的路径无论在关节空间内还是在任务空间内都更加平滑，机械臂在运动过程中不会出现奇异解.

4.2 从臂路径规划结果

根据规划出的主臂路径得到主臂的动态碰撞模型，将其作为障碍规划从臂的路径.

设从臂末端初始点为[-0.350, 0.380, 0.620]，目标点为[0.100, -0.400, 0.300]，使用双向规划算法对从臂进行路径规划，正向和逆向规划搜索次数各为39次，分别收敛在点[0.158, 0.124, 0.150]和[0.169, 0.112, 0.149]，从臂在关节空间以及笛卡尔空间内的轨迹如图15、16.

图15 从臂关节角

图16 从臂末端位置

同时执行主臂和从臂路径，双臂避碰路径仿真结果如图17.

图17 双臂避碰路径

仿真结果显示，双机械臂在运动过程中没有发生碰撞，基于改进人工势场法的双向规划算法可以实现双机械臂的避碰路径规划.

5 结论

文中对工作空间存在重叠的双机械臂避碰路径规划问题进行了研究，首先提出了基于改进人工势场法的双向规划算法，从振荡问题、算法效率以及局部极小点等3个方面对传统人工势场法进行了改进.双机械臂在主从运动模式下，先使用改进算法规划主臂路径，然后根据主臂路径得到主臂的动态碰撞模型，将其作为障碍规划从臂路径.仿真结果显示，提出的规划算法能够实现双机械臂的避碰路径规划，并且有着较高的路径质量.后续将进一步提升算法在未知动态环境下的规划能力.