改进人工势场法的冗余机械臂避障算法

删除或更新信息，请邮件至freekaoyan#163.com(#换成@)

本站小编哈尔滨工业大学/2019-10-24

改进人工势场法的冗余机械臂避障算法

曹博,毕树生,郑晶翔,杨东升,黄国威

(北京航空航天大学机械工程及自动化学院, 北京 100191)

摘要:

针对传统人工势场法应用于串联型冗余机械臂避障时无法约束各关节位姿、陷入局部极小后难以逃离的问题,提出一种改进人工势场法. 建立冗余机械臂运动学模型,采用线段球体包络盒模型进行碰撞检测. 在笛卡尔空间内建立末端引力势场和障碍物斥力势场,在关节空间内建立目标角度引力势场,所有势场共同作用引导机械臂运动. 在关节空间内求解虚拟目标角度并采用高斯函数建立虚拟引力势场处理局部极小问题. 利用七自由度冗余机械臂进行仿真和实验,结果表明:算法可约束各关节位姿,陷入局部极小后可引导机械臂逃离局部极小,最终完成避障；避障结束时各关节角度最大误差为0.8 °,末端平均位置误差和平均姿态误差分别为0.010 m和2.40 °,均小于传统算法；避障过程中各关节运动幅度小于传统算法. 改进算法可引导机械臂逃离局部极小并完成避障,同时提高避障结束时各关节及末端的定位精度,对冗余机械臂的避障研究及应用具有一定的指导意义.

关键词: 冗余机械臂人工势场法局部极小避障运动规划碰撞检测

DOI：10.11918/j.issn.0367-6234.201806121

分类号:TP242.6

文献标识码:A

基金项目:

Obstacle avoidance algorithm for redundant manipulator of improved artificial potential field method

CAO Bo,BI Shusheng,ZHENG Jingxiang,YANG Dongsheng,HUANG Guowei

（School of Machanical Engineering and Automation, Beijing University of Aeronautics and Astronautics, Beijing 100191, China）

Abstract:

An improved artificial potential field method (APFM) is proposed for the problem that only the end of manipulator can be guided but the angle of each joint cannot be constrained, and it is difficult to escape from local minimum when traditional APFM is used to avoid obstacles for redundant manipulator. A kinematic model of the manipulator is established and the line segment sphere enveloping box model is established for collision detection. Attract potential field of the end and obstacle repulsive potential field are established in Cartesian, and attract potential field of the target angle is established in joint space, and they work together to guide the manipulator. A virtual target angle is solved in joint space and the virtual potential field is established using Gaussian function to deal with the local minimum problem. The simulations and experiments on the 7 DOF redundant manipulator show that the algorithm can constraint joint pose and guide the manipulator escape from local minimum when trapped, and finally complete obstacle avoidance. At the end of the obstacle avoidance, the maximum error of each joint angle is 0.8°, and the average position error and attitude error are 0.010 m and 2.40°, which are smaller than the traditional algorithm respectively. The motion amplitude of each joint in the obstacle avoidance process is smaller than the traditional algorithm. The improved algorithm can guide the manipulator escape from the local minimum and complete the obstacle avoidance, as well as improve the positioning accuracy of each joint and the end at the end of the obstacle avoidance. The study has certain guiding significance for research and application of obstacle avoidance for redundant manipulators.

Key words: redundant manipulators artificial potential field local minimum obstacle avoidance motion planning collision detection

曹博, 毕树生, 郑晶翔, 杨东升, 黄国威. 改进人工势场法的冗余机械臂避障算法[J]. 哈尔滨工业大学学报, 2019, 51(7): 184-191. DOI: 10.11918/j.issn.0367-6234.201806121. 复制到剪切板

CAO Bo, BI Shusheng, ZHENG Jingxiang, YANG Dongsheng, HUANG Guowei. Obstacle avoidance algorithm for redundant manipulator of improved artificial potential field method[J]. Journal of Harbin Institute of Technology, 2019, 51(7): 184-191. DOI: 10.11918/j.issn.0367-6234.201806121. 复制到剪切板

作者简介曹博(1993—)，男，硕士研究生;
毕树生(1966—)，男，教授，博士生导师通信作者毕树生，ssbi@buaa.edu.cn 文章历史收稿日期: 2018-06-19

Contents            -->Abstract            Full text            Figures/Tables            PDF

改进人工势场法的冗余机械臂避障算法
曹博, 毕树生, 郑晶翔, 杨东升, 黄国威
北京航空航天大学机械工程及自动化学院, 北京 100191

收稿日期: 2018-06-19
作者简介: 曹博(1993—)，男，硕士研究生;
毕树生(1966—)，男，教授，博士生导师
通信作者: 毕树生，ssbi@buaa.edu.cn

摘要: 针对传统人工势场法应用于串联型冗余机械臂避障时无法约束各关节位姿、陷入局部极小后难以逃离的问题，提出一种改进人工势场法.建立冗余机械臂运动学模型，采用线段球体包络盒模型进行碰撞检测.在笛卡尔空间内建立末端引力势场和障碍物斥力势场，在关节空间内建立目标角度引力势场，所有势场共同作用引导机械臂运动.在关节空间内求解虚拟目标角度并采用高斯函数建立虚拟引力势场处理局部极小问题.利用七自由度冗余机械臂进行仿真和实验，结果表明：算法可约束各关节位姿，陷入局部极小后可引导机械臂逃离局部极小，最终完成避障；避障结束时各关节角度最大误差为0.8 °，末端平均位置误差和平均姿态误差分别为0.010 m和2.40 °，均小于传统算法；避障过程中各关节运动幅度小于传统算法.改进算法可引导机械臂逃离局部极小并完成避障，同时提高避障结束时各关节及末端的定位精度，对冗余机械臂的避障研究及应用具有一定的指导意义.
关键词: 冗余机械臂    人工势场法    局部极小    避障    运动规划    碰撞检测
Obstacle avoidance algorithm for redundant manipulator of improved artificial potential field method
CAO Bo, BI Shusheng, ZHENG Jingxiang, YANG Dongsheng, HUANG Guowei
School of Machanical Engineering and Automation, Beijing University of Aeronautics and Astronautics, Beijing 100191, China

Abstract: An improved artificial potential field method (APFM) is proposed for the problem that only the end of manipulator can be guided but the angle of each joint cannot be constrained, and it is difficult to escape from local minimum when traditional APFM is used to avoid obstacles for redundant manipulator. A kinematic model of the manipulator is established and the line segment sphere enveloping box model is established for collision detection. Attract potential field of the end and obstacle repulsive potential field are established in Cartesian, and attract potential field of the target angle is established in joint space, and they work together to guide the manipulator. A virtual target angle is solved in joint space and the virtual potential field is established using Gaussian function to deal with the local minimum problem. The simulations and experiments on the 7 DOF redundant manipulator show that the algorithm can constraint joint pose and guide the manipulator escape from local minimum when trapped, and finally complete obstacle avoidance. At the end of the obstacle avoidance, the maximum error of each joint angle is 0.8°, and the average position error and attitude error are 0.010 m and 2.40°, which are smaller than the traditional algorithm respectively. The motion amplitude of each joint in the obstacle avoidance process is smaller than the traditional algorithm. The improved algorithm can guide the manipulator escape from the local minimum and complete the obstacle avoidance, as well as improve the positioning accuracy of each joint and the end at the end of the obstacle avoidance. The study has certain guiding significance for research and application of obstacle avoidance for redundant manipulators.
Keywords: redundant manipulators    artificial potential field    local minimum    obstacle avoidance    motion planning    collision detection
避障是冗余机械臂路径规划的基本问题之一，是科学和工程领域中的研究热点^[1].串联型冗余机械臂的雅克比矩阵为欠约束形式，其运动学逆解存在无穷多个，也可能存在奇异点^[2-3]，单独依靠逆运动学求解算法难以满足避障要求.目前冗余机械臂避障的常用算法主要包括改进逆运动学求解法^[4-6]、自由空间法^[7]、遗传算法^[8]、A*算法^[9]、人工势场法^[10-11]等.文献[4-6]对冗余机械臂逆运动学求解算法进行改进，提高求解精度和速度，但仍旧无法有效解决奇异点问题.文献[7]提出基于工作空间密度的平面冗余机械臂避障算法，在生成的工作空间中计算出多个离散解，并将工作空间密度作为评价准则求取最优解.该方法对于处理多障碍物条件下的避障十分有效，但只适用于平面冗余机械臂.

人工势场法在目标点处构建引力势场，障碍物处构建斥力势场，并在两类势场共同作用下，引导机械臂避开障碍物到达目标点^[10].它不需要进行逆运动学求解即可完成避障，具有模型简单、算法高效、适应性强的特点，但容易陷入局部极小且难以逃离^[12].串联型冗余机械臂在给定末端位姿情况下，有无数组关节角度与之对应^[13-14]，而传统人工势场法只在笛卡尔空间中对末端建立势场，难以约束各关节位姿.文献[15]提出通过遗传算法求解关节值，利用人工势场法在关节空间内搜索，在极小点处添加虚拟障碍物处理局部极小问题，该方法应用于非冗余机械臂具有一定效果，但不适用于冗余机械臂.文献[16]提出基于构型的人工势场法，与空间机械臂逆运动学求解相结合进行运动规划，该方法在避障方面具有一定成效，但避障结果误差较大，无法约束关节位姿，在冗余机械臂上应用较为困难.

为此，本文提出改进人工势场法，以七自由度冗余机械臂为例建立其运动学模型和碰撞检测模型，分别在笛卡尔和关节空间内建立势场共同引导机械臂运动，在关节空间内求解虚拟目标角度并建立虚拟引力势场处理局部极小问题，仿真和实验对算法有效性进行验证.

1 机械臂模型 1.1 运动学模型本文以加拿大Kinova公司的Jaco2七自由度冗余机械臂为研究对象.采用D-H法建立机械臂运动学模型如图 1所示，D-H参数见表 1.

Fig. 1
点击查看原图

图 1 机械臂D-H模型 Fig. 1 D-H model of the manipulator

表 1

表 1 机械臂D-H模型参数 Tab. 1 The D-H parameter of the manipulator i α_i-1/(°) a_i-1/m d_i/m θ_i/(°)

1 90 0 -D₁ Q₁-180

2 90 0 0 Q₂

3 90 0 -(D₂+D₃) Q₃

4 90 0 -e₁ Q₄

5 90 0 -(D₄+D₅) Q₅

6 90 0 0 Q₆

7 180 0 -(D₆+D₇) Q₇-90

表 1 机械臂D-H模型参数 Tab. 1 The D-H parameter of the manipulator

表 1中：i(i=1, 2, ..., 7)为机械臂关节序号；Q_i为i关节角度.为简化表达，分别将cos(θ_i)、sin (θ_i)、cos(α_i-1)、sin(α_i-1)记为cθ_i、sθ_i、cα_i-1、sα_i-1，则机械臂x_i-1-z_i-1坐标系到x_i-z_i坐标系的齐次变换矩阵M_i-1可表示为

$\begin{array}{l}{\mathit{\boldsymbol{M}}_{i - 1}} = {\mathit{\boldsymbol{R}}_{i - 1}}\left( {{\theta _i}} \right){\mathit{\boldsymbol{T}}_{i - 1}}\left( {{d_i}} \right){\mathit{\boldsymbol{T}}_{i - 1}}\left( {{a_{i\;\; - 1}}} \right){\mathit{\boldsymbol{R}}_{i - 1}}\left( {{\alpha _{i - 1}}} \right) = \\\;\;\;\left[ {\begin{array}{*{20}{c}}{{\rm{c}}{\theta _i}}&{ - {\rm{c}}{\alpha _{i - 1}}{\rm{s}}{\theta _i}}&{{\rm{s}}{\alpha _{i - 1}}{\rm{s}}{\theta _i}}&{{\alpha _{i - 1}}{\rm{c}}{\theta _i}}\\{{\rm{s}}{\theta _i}}&{{\rm{c}}{\alpha _{i - 1}}{\rm{c}}{\theta _i}}&{ - {\rm{s}}{\alpha _{i - 1}}{\rm{c}}{\theta _i}}&{{\alpha _{i - 1}}{\rm{s}}{\theta _i}}\\0&{{\rm{s}}{\alpha _{i - 1}}}&{{\rm{c}}{\alpha _{i - 1}}}&{{d_i}}\\0&0&0&1\end{array}} \right].\end{array}$

式中：R_i-1(α_i-1)代表将x_i-1-z_i-1坐标系绕x_i-1轴旋转α_i-1的变换矩阵，T_i-1(α_i-1)代表将x_i-1-z_i-1坐标系沿x_i-1轴平移a_i_-1的变换矩阵，T_i-1(d_i)代表将x_i-1-z_i-1坐标系沿z_i-1轴平移d_i的变换矩阵，R_i-1(θ_i)代表将x_i-1-z_i-1坐标系绕z_i-1轴旋转θ_i的变换矩阵.

1.2 碰撞检测模型机械臂与障碍物间的碰撞检测采用线段—球体包络盒模型，如图 2所示.根据图 1建立的机械臂D-H模型，共存在6个不重合的坐标系原点，则碰撞检测过程中将连杆i(i=1, 2, ..., 7)组合并等效为5条线段，如图 2中A₀A₁、A₁A₂、A₂A₃、A₃A₄、A₄A₅所示，同时对障碍物半径等效增大R_h，R_h为线段A_h-1A_h对应连杆最大半径，h(h=1, 2, ..., 5)为等效线段序号.机械臂末端安装9个测距传感器探测障碍物，如图 1所示，其中沿末端圆周方向均布安装6个传感器，用于探测机械臂末端四周信息，在末端正前方安装3个测距传感器，用于探测机械臂末端正前方朝向信息.机械臂在运动过程中，安装有测距传感器的末端位姿不断变化，则传感器探测方向不断发生改变，通过传感器不断进行障碍物探测，即可获得机械臂各连杆周围的障碍物信息.所探测到的第m(m=1, 2, ..., M)个障碍物表面信息可用空间中一点P_m表示，M为避障完成前探测到的障碍物最大数目.为确保多次探测后球体包络盒将障碍物表面基本包络，需给定球体包络盒的安全半径r，r的取值需同时保证机械臂在避障过程中不影响末端执行器的操作，依据末端执行器及工作环境情况选取.避障过程中机械臂中心线与障碍物间最近距离为R_h+r，当R_h+r取值较小时，机械臂可在较小空间范围内避障，但与障碍物发生碰撞的概率增大，而取值较大时，机械臂与障碍物发生碰撞的概率减小，但避障需要较大空间.障碍物模型可用球体包络盒表示为S_m(P_m, R_h+r)，其中P_m为球心，R_h+r为半径.

Fig. 2
点击查看原图

图 2 碰撞检测模型 Fig. 2 Collision detection model

根据图 2建立的碰撞检测模型，端点A_h与x_t－z_t(0≤t≤7)坐标系原点重合，此时A_h在x_t－z_t坐标系中的齐次坐标矩阵为A_h^t=[0, 0, 0, 1]^T，则A_h在x₀－z₀坐标系中的齐次坐标矩阵为

$\mathit{\boldsymbol{A}}_h^0 = \prod\limits_{i = 0}^{t - 1} {{\mathit{\boldsymbol{M}}_i}\mathit{\boldsymbol{A}}_h^t} $

针对每条等效线段A_h-1A_h及障碍物的球体包络盒模型S_m(P_m, R_h+r)，碰撞检测问题可通过求解球心P_m到线段A_h-1A_h的距离L_mh，并与球体半径R_h+r比较解决.

1) 若L_mh≤R_h+r，则发生碰撞.

2) 若L_mh＞R_h+r，则没有发生碰撞.

各条等效线段分别与每个障碍物碰撞检测，即可获得机械臂不发生碰撞条件下的运动空间.

2 改进人工势场法 2.1 势场函数建立在笛卡尔空间内的目标点处建立引力势场U_att、障碍物处建立斥力势场U_rep，在关节空间内的目标位姿处建立引力势场U_joi，所有势场共同作用引导机械臂向势能减小方向运动.

在笛卡尔空间中建立引力势场函数为

${U_{{\rm{att}}}} = \frac{{{K_{\rm{a}}}}}{2} \times \left( {\left\| {\mathit{\boldsymbol{P}}_{\rm{r}}^0 - \mathit{\boldsymbol{P}}_{\rm{g}}^0} \right\|_2^2 + \left\| {{\mathit{\boldsymbol{\varphi }}_{\rm{r}}} - {\mathit{\boldsymbol{\varphi }}_{\rm{g}}}} \right\|_2^2} \right).$

式中：K_a为引力势场参数；P_r⁰、P_g⁰分别为当前和目标位姿下末端坐标系原点在基坐标系x₀－z₀下的坐标向量；$\mathit{\boldsymbol{\varphi }}$_r、$\mathit{\boldsymbol{\varphi }}$_g分别为当前和目标位姿下末端坐标系x₇－z₇的x、y、z轴与基坐标系x₀－z₀对应轴间的夹角组成的向量.目标点对机械臂末端产生适当大小的引力势场，并随两者之间距离增大而迅速增大，引导机械臂末端向目标位姿方向运动.

在笛卡尔空间中，障碍物包络盒S_m与线段A_h-1A_h间的斥力势场函数为

${U_{mh}} = \left\{ \begin{array}{l}\frac{{{K_{\rm{r}}}}}{2}{\left[ {\frac{1}{{{L_{mh}} - \left( {{R_h} + r} \right)}} - \frac{1}{{{L_0} - \left( {{R_h} + r} \right)}}} \right]^2},\\\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;{R_h} + r < {L_{mh}} < {L_0};\\0,\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;{L_{mh}} \ge {L_0}.\end{array} \right.$

式中：K_r为斥力势场参数；L_mh为障碍物包络盒S_m球心到线段A_h-1A_h的距离；L₀为斥力势场作用范围.斥力势场U_mh随L_mh减小而增大，当L_mh趋近于R_h+r时，斥力势能迅速增大并趋于无穷，引导机械臂向远离障碍物方向运动.机械臂的总斥力势能为

${U_{{\rm{rep}}}} = \sum\limits_{m = 1}^M {\sum\limits_{h = 1}^H {{U_{mh}}} } .$

式中:M为当前保存的障碍物表面位置信息最大数目，H为机械臂等效线段条数.

在关节空间内，建立关节引力势场函数为

${U_{{\rm{joi}}}} = \frac{{{K_{\rm{j}}}}}{2}\sum\limits_{i = 1}^{\rm{I}} {{{\left( {{\gamma _i} - {\theta _i}} \right)}^2}} .$

式中：K_j为关节引力势场参数；i(i=1, 2, ..., I)为机械臂关节序号, I=7为关节数目；γ_i为i关节目标角度；θ_i为i关节当前角度. U_joi随γ_i与θ_i之间差值增大而增大，引导i关节向γ_i方向运动.

由于斥力势能太大会影响总势能的下降速度，不利于机械臂向目标位置运动，势场参数K_r、K_a按如下方法调整：

${K_{\rm{r}}} = {K_{\rm{a}}}/MH.$ (1)

式中H=5为机械臂等效线段条数.式(1)可保证随障碍物数量增多，斥力势能不会急剧增大.由于关节引力势能太小不利于引导关节到达目标角度，太大不利于引导末端到达目标位姿，势场参数K_j、K_a按如下方法确定：

${K_{\rm{j}}} = {K_{\rm{a}}}/{S_c}I.$

式中S_c为常量系数.

假设机械臂在第n步运动前第i个关节角为θ_in，定义当前关节角集合：J_n={θ_in, i=1, 2, ..., 7}.机械臂在J_n姿态下，在笛卡尔空间内的引力势能和斥力势能分别记为U_att(J_n)、U_rep(J_n)，在关节空间内的引力势能记为U_joi(J_n)，则机械臂的总势能为

${U_{{\rm{tol}}}}\left( {{J_n}} \right) = {U_{{\rm{att}}}}\left( {{J_n}} \right) + {U_{{\rm{rep}}}}\left( {{J_n}} \right) + {U_{{\rm{joi}}}}\left( {{J_n}} \right).$

2.2 局部极小处理当引力势函数与多个斥力势函数共同作用时，总势能容易陷入局部极小而非全局最小值，不改变势场分布难以引导机械臂继续运动，导致避障任务失败.由于串联型冗余机械臂关节数量较多，当陷入局部极小时，只对末端进行引导难以保证各关节避开障碍物.为此，本文在关节空间内求解虚拟目标角度，同时引导末端和关节运动.

定义机械臂在J_n姿态下的相邻关节角为：C_in={θ_in+λ₀, θ_in, θ_in－λ₀, i=1, 2, ..., 7}，λ₀为初始步长.相邻关节角集合为：C_n={C_in, i=1, 2, ..., 7}.若满足U_tol(J_n)=min(U_tol(C_n))，则判定机械臂陷入局部极小.在关节空间内，虚拟目标角度势函数需在局部极小点处起主要引导作用，并能在逃离局部极小后快速回落至零.机械臂在当前关节角J_n姿态下i关节对应的虚拟目标角度势函数为

${\tilde U_i}\left( {{J_n}} \right) = \frac{{{K_{\rm{v}}}}}{\sigma }{{\rm{e}}^{ - \frac{{{{\left[ {\left( {{{\tilde \gamma }_i} - {\theta _{in}}} \right) - \left( {{{\tilde \gamma }_i} - {\theta _{i1}}} \right)} \right]}^2}}}{{2{\sigma ^2}}}}} - \frac{{{K_{\rm{v}}}}}{\sigma }{{\rm{e}}^{ - \frac{{{{\left( {{{\tilde \gamma }_i} - {\theta _{i1}}} \right)}^2}}}{{2{\sigma ^2}}}}}.$

式中：K_v、σ为关节虚拟引力势场参数；$\tilde \gamma $_i为i关节虚拟目标角度；θ_i1为i关节逃离局部极小的起始运动角度；θ_in为J_n姿态下i关节实际角度.根据高斯分布的3σ准则，${\tilde U}$_i的快速回落区间为[μ－3σ, μ]，对应于自变量$\tilde \gamma $_i－θ_in的回落区间[0, |$\tilde \gamma $_i－θ_i1|].则满足：

$\left\{ \begin{array}{l}\mu - 3\sigma = 0,\\\mu = \left| {\left. {{{\tilde \gamma }_i} - {\theta _{i1}}} \right)} \right|.\end{array} \right.$

解得

$\sigma = \left| {{{\tilde \gamma }_i} - {\theta _{i1}}} \right|/3.$

机械臂在J_n姿态下具有的虚拟总势能${\tilde U}$_tol(J_n)为

${\tilde U_{{\rm{tol}}}}\left( {{J_n}} \right) = {U_{{\rm{att}}}}\left( {{J_n}} \right) + {U_{{\rm{rep}}}}\left( {{J_n}} \right) + \sum\limits_{i = 1}^I {{{\tilde U}_i}\left( {{J_n}} \right)} .$

为保证U_i能够引导机械臂逃离局部极小，机械臂在J_n姿态下具有的虚拟总势能需大于在J_n+1姿态下具有的虚拟总势能.则K_v需满足

${\tilde U_{{\rm{tol}}}}\left( {{J_n}} \right) > {\tilde U_{{\rm{tol}}}}\left( {{J_{n + 1}}} \right).$

Jaco2机械臂的2、4、6关节控制机械臂在包含等效线段的平面内运动，由于这些关节运动对机械臂等效线段位置影响较大，因此仅对它们求取$\tilde \gamma $_i，将其定义为$\tilde \gamma $_i求解关节.如图 3所示，当前位姿i(i=1, 2, ..., 7)关节中心点为T_i，目标位姿i关节中心点为${\tilde T}$_i，障碍物球体包络盒模型为S_m，构建三角形区域T₂

${\tilde T}$

₄T₄、T₄

${\tilde T}$

₄

${\tilde T}$

₆、T₄

${\tilde T}$

₆T₆、T₆

${\tilde T}$

₆

${\tilde T}$

₇、T₆

${\tilde T}$

₇T₇.若球体包络盒模型与多个三角形区域相交，在求解$\tilde \gamma $_i时将其归属于相交面积最大的三角形区域.三角形区域所在平面将包络盒模型切分为两部分，取体积较小的部分所在空间为$\tilde \gamma $_i求解方向.以三角形区域的一个顶点为活动顶点，以该顶点所在关节的上一个$\tilde \gamma $_i求解关节(较小标号方向)为转动关节，关节转动带动活动顶点运动，使得三角形区域所在平面与球体包络盒相切，此时关节角度为$\tilde \gamma $_iA，为减弱障碍物斥力势场的影响，需给定角度安全余量$\tilde \gamma $_iB，使得机械臂向虚拟目标角度方向的运动幅度更大，则

${\tilde \gamma _i} = {\tilde \gamma _{iA}} + {\tilde \gamma _{iB}}.$

Fig. 3
点击查看原图

图 3 虚拟目标角度求解示意图 Fig. 3 Schematic diagram for solving a virtual target angle

三角形区域的三个顶点中，若仅存在一个顶点在当前位姿上，则该顶点为活动顶点，以三角形区域T₄

${\tilde T}$

₄

${\tilde T}$

₆为例，T₄为活动顶点，关节2为$\tilde \gamma $_i求解关节.若存在两个顶点在当前位姿上，则取两个顶点中标号较大的顶点为活动顶点，标号较小的顶点所在关节为$\tilde \gamma $_i求解关节，以三角形区域T₄

${\tilde T}$

₆T₆为例，T₆为活动顶点，关节4为$\tilde \gamma $_i求解关节.除求解关节外的其他关节以逃离局部极小时的起始运动角度为虚拟目标角度，即$\tilde \gamma $_i=θ_i1.

2.3 算法实现改进人工势场法算法主流程如下：

1) 获取机械臂当前位姿的相邻关节角集合C_n，初始步长为λ₀.

2) 对C_n中每个相邻关节角组合进行运动学正解，获得各关节点在基坐标系x₀－z₀下的坐标，求解机械臂等效线段A_h－1A_h方程式和障碍物球体包络盒模型S_m方程式.

3) 将C_n中每个组合对应的各条等效线段A_h－1A_h与所有球体包络盒模型S_m进行碰撞检测，若存在碰撞，将该组合舍弃.

4) 对C_n中每个剩余组合计算总势能，得到总势能最小的一种组合，记为J_min.

5) 判定机械臂是否陷入局部极小.若陷入局部极小，则进入局部极小处理流程.

6) 判定是否稳定于目标位姿附近，满足条件则更改步长为λ₁.

7) 判定是否到达目标位姿或更换步长后仍稳定于目标点附近.满足条件则避障成功，否则机械臂运动到J_min，跳转到第1)步.

局部极小处理流程如下：

1) 计算机械臂进入局部极小总次数w，若w>w_max，则避障失败.

2) 求解虚拟目标角度$\tilde \gamma $_i.

3) 若当前朝γ_i运动，将算法主流程中的所有目标角度γ_i替换为$\tilde \gamma $_i，跳转算法主流程第1)步.

4) 若机械臂到达虚拟目标角度或当前朝$\tilde \gamma $_i运动，则将$\tilde \gamma $_i替换为γ_i，跳转算法主流程第1)步.

3 仿真与实验 3.1 仿真验证利用本文建立的Jaco2七自由度机械臂模型对算法进行仿真验证.算法参数设定：K_a=20 000，S_c=100，λ₀=3 °，λ₁=1 °，$\tilde \gamma $_iB=25 °，R_h=0.06 m，r=0.05 m，L₀=0.14 m.当障碍物形状为凹型且机械臂运动到达凹点附近时，算法容易陷入局部极小.如图 4(a)所示，仿真时采用“8”字型双球体障碍物，通过两个相互垂直平面将空间分为E_k(k= 1, 2, ..., 6)六个区域，其中E₃和E₆在平面上，将末端的起始、终点位置设置在两个不同区域内，共进行了15(C₆²)组仿真实验，其中有7组陷入局部极小，通过采用本文算法均成功逃离，所有仿真实验均避障成功.下面以末端起始、终点位置分别位于E₄、E₆区域的仿真实验为例进行分析.

Fig. 4
点击查看原图

图 4 机械臂运动轨迹 Fig. 4 Trajectory of the manipulator

本组仿真实验中，起始运动关节角度、陷入局部极小时关节角度、虚拟目标角度$\tilde \gamma $_i、跳出局部极小时关节角度、目标角度γ_i、运动终点位姿下关节角度及其与γ_i间的误差如表 2所示.由表 2可知，终点位姿下各关节角度与γ_i间的误差均为零，表明算法可有效引导机械臂到达目标位姿，保证了运动结束时的末端定位精度.

表 2

表 2 不同状态下各关节角度 Tab. 2 Angle of each joint in different states 关节i 起始/(°) 陷入局部极小/(°) ${\tilde y}$_i/(°) 终点/(°) γ_i/(°) 误差/(°)

1 180 210 210 270 270 0

2 270 270 240.8 270 270 0

3 0 0 0 0 0 0

4 90 180 180 180 180 0

5 270 270 270 270 270 0

6 270 171 171 180 180 0

7 0 0 0 0 0 0

表 2 不同状态下各关节角度 Tab. 2 Angle of each joint in different states

仿真过程中各关节角度变化如图 5所示，机械臂运动轨迹依次如图 4(a)、4(b)、4(c)所示. 图 5中，机械臂运动步数为1、35、46、66时的位姿分别与图 4中的起始位姿、陷入局部极小位姿、到达$\tilde \gamma $_i位姿、终点位姿对应. 图 4中，为便于观察机械臂与障碍物间的相对位姿，将观察视角进行了旋转，仿真中两者未发生碰撞，机械臂采用图 2建立的等效线段模型表示，黑色粗线为机械臂在某一时刻位姿，红色细线为机械臂两个特定时刻位姿之间的运动轨迹. 图 4(a)中，机械臂从起始位姿开始运动，在第35步到达双球体障碍物模型附近并陷入局部极小. 图 4(b)中，算法引导机械臂逃离局部极小向$\tilde \gamma $_i运动，并在第46步到达$\tilde \gamma $_i. 图 4(c)中，机械臂从$\tilde \gamma $_i开始重新向γ_i运动，并在第66步运动到达γ_i附近，避障结束.仿真实验证实，本文算法可引导机械臂规划出一条从起始位姿到目标位姿的无碰撞路径，并能引导机械臂逃离局部极小，证明了算法的有效性.

Fig. 5
点击查看原图

图 5 机械臂各关节角度变化 Fig. 5 Change of angle of each joint

3.2 实验验证文献[17]中，建立机械臂引导势场时采用的算法为传统人工势场法，本文利用加拿大Kinova公司的Jaco2七自由度冗余机械臂，对提出的改进人工势场法与以文献[17]建立势场时采用的算法为例进行对比实验.改进人工势场法的实验参数设定：K_a=20 000，S_c=100，λ₀=3 °，λ₁=1 °，$\tilde \gamma $_iB=25 °，R_h=0.06 m，r=0.05 m，L₀=0.14 m.传统人工势场法的实验参数设定：K_a=20 000，λ₀=3 °，λ₁=1 °，R_h=0.06 m，r=0.05 m，L₀=0.14 m.实验通过示教获得机械臂起始、目标位姿，如图 6(a)所示，将长方体纸箱作为障碍物，末端可到达纸箱的F₁、F₂、F₃、F₄四个面，通过将末端的起始、终点位置设置在两个不同面附近，共进行了12(A₄²)组对比实验，实验目标是规划一条从起始位姿到目标位姿的无碰撞路径.下面以末端起始、终点位置分别位于面F₁、F₄的实验为例进行分析.

采用本文算法的机械臂避障过程如图 6所示.实验中，各关节起始运动角度、目标角度γ_i、采用改进算法运动终点角度及其与γ_i的误差、采用传统算法运动终点角度及其与γ_i的误差如表 3所示.实验中两种算法采用相同的起始位姿与目标位姿.由表 3可知，针对串联型冗余机械臂，采用改进算法时运动终点角度与γ_i的误差较小，其中6号关节角度误差最大，仅为0.8 °，在机械臂运动误差范围之内，表明本文提出的改进算法在引导末端到达目标位姿的同时，可以保证各关节角度到达γ_i；而采用传统算法时，各关节运动终点角度与γ_i之间误差普遍较大，最大高达24.6 °，表明采用传统算法虽然能够引导机械臂末端到达目标位姿，但不能对各个关节运动进行有效约束.

Fig. 6
点击查看原图

图 6 改进人工势场法避障过程 Fig. 6 Obstacle avoidance process by improved artificial potential field method

表 3

表 3 不同状态下各关节角度 Tab. 3 Angle of each joint in different states 关节i 起始/(°) γ_i/(°) 改进算法传统算法

终点/(°) 误差/(°) 终点/(°) 误差/(°)

1 101.9 103.3 103.3 0 96.6 6.7

2 157.4 152.2 151.6 0.6 161.0 8.8

3 178.7 180.0 180.3 0.3 183.2 3.2

4 54.7 139.1 138.4 0.7 114.5 24.6

5 266.9 265.2 265.6 0.4 246.4 18.8

6 257.8 248.3 249.1 0.8 236.6 11.7

7 0 0 0 0 20.7 20.7

表 3 不同状态下各关节角度 Tab. 3 Angle of each joint in different states

终点位姿下，末端的位姿误差包括位置误差与姿态误差.定义末端的位置误差为

${e_{\rm{s}}} = \left\| {\mathit{\boldsymbol{P}}_{\rm{e}}^0 - \mathit{\boldsymbol{P}}_{\rm{g}}^0} \right\|_2^2.$

式中P_e⁰为终点位姿下，末端坐标系原点在基坐标系x₀－z₀下的坐标向量.定义末端姿态误差为

${e_{\rm{ \mathsf{ φ} }}} = \left\| {{\mathit{\boldsymbol{\varphi }}_{\rm{e}}} - {\mathit{\boldsymbol{\varphi }}_{\rm{g}}}} \right\|_2^2.$

式中$\mathit{\boldsymbol{\varphi }}$_e为终点位姿下，末端坐标系x₇－z₇的x、y、z轴与基坐标系x₀－z₀对应轴间的夹角组成的向量.机械臂末端的目标位姿和终点位姿、采用两种算法避障完成时的位置误差e_s和姿态误差e_{${\rm{ \mathit{ φ} }}$}如表 4所示.

表 4

表 4 基坐标系下末端位姿 Tab. 4 Pose of the end in basic coordinate system 变量目标位姿改进算法传统算法

终点位姿位姿误差平均位姿误差终点位姿位姿误差平均位姿误差

位置P_g⁰/m $\left[{\begin{array}{*{20}{r}}{0.119}\\{0.606}\\{ - 0.802}\end{array}} \right]$ $\left[{\begin{array}{*{20}{r}}{0.116}\\{0.613}\\{ - 0.793}\end{array}} \right]$ 0.012 0.010 $\left[{\begin{array}{*{20}{r}}{0.121}\\{0.609}\\{ - 0.787}\end{array}} \right]$ 0.016 0.019

姿态$\mathit{\boldsymbol{\varphi }}$_g/(°) $\left[{\begin{array}{*{20}{c}}{90.57}\\{40.69}\\{101.54}\end{array}} \right]$ $\left[{\begin{array}{*{20}{c}}{90.00}\\{38.87}\\{100.98}\end{array}} \right]$ 1.99 2.40 $\left[{\begin{array}{*{20}{c}}{83.70}\\{40.06}\\{111.77}\end{array}} \right]$ 12.34 11.71

表 4 基坐标系下末端位姿 Tab. 4 Pose of the end in basic coordinate system

由表 4可知：本组实验中，采用改进算法避障完成时位置误差与姿态误差分别为0.012 m和1.99 °，均低于传统算法；对12组实验结果求取平均误差，采用改进算法时的平均位置误差和平均姿态误差分别为0.010 m和2.40 °，低于传统算法的0.019 m和11.71 °.对比表明，相较于传统算法，采用改进算法避障完成时末端位姿与目标位姿更为接近，末端位姿定位精度更高.针对串联型冗余机械臂，改进算法在避障结束时关节角度误差、末端位姿误差均小于传统算法，原因是改进算法可在关节空间中建立引力势场，引导与约束各独立关节运动，当各关节到达目标角度后，机械臂整体位姿与目标位姿相近，可进一步提高末端定位精度，而传统算法仅在末端建立势场，当末端位姿确定时有无数组关节角度与之对应，无法引导与约束各关节运动，则整体位姿可能与目标位姿产生较大差别，同时受障碍物斥力势场影响，末端位姿误差较大.

采用两种算法进行对比实验时，各关节角度随运动过程的变化分别如图 7、图 8所示. 图 7中机械臂运动步数为1、10、20、36时的位姿分别如图 6(a)、图 6(b)、图 6(c)、图 6(d)所示.在30组对比实验中，采用改进算法均避障成功，证明了算法有效性.对比图 7、图 8可知，当采用两种算法时，各关节角度值在运动过程中由于避障出现了波动，当采用改进算法时，关节1、关节2、关节5、关节6、关节7波动幅度均比传统算法小，采用两种算法时关节3的波动幅度基本相同，关节4整体呈现出趋向于γ₄的趋势.对比表明，在避障过程中，改进算法相较于传统算法各关节角度值整体波动幅度较小，则关节运动幅度较小，原因是改进算法可在关节空间中建立引力势场，在避障过程中对各关节运动进行引导与约束，完成避障的前提下引力势场值向减小方向变化，即引导关节向目标角度方向运动，而传统算法仅针对末端建立势场，无法对关节进行有效引导与约束，仅可通过碰撞检测算法保证其不与障碍物发生碰撞，而不能保证关节向目标角度运动.

Fig. 7
点击查看原图

图 7 改进人工势场法各关节角度变化 Fig. 7 Change of angle of each joint by improved APFM

Fig. 8
点击查看原图

图 8 传统人工势场法各关节角度变化 Fig. 8 Change of angle of each joint by traditional APFM

4 结论1) 提出在笛卡尔空间内建立目标点引力势场和障碍物斥力势场、在关节空间中建立目标角度引力势场相结合的算法，算法可有效引导机械臂从起始位姿避开障碍物到达目标位姿.

2) 提出在关节空间内求解虚拟目标角度，并采用高斯函数建立虚拟引力势场处理局部极小问题.仿真结果表明，算法可顺利引导机械臂逃离局部极小，最终完成避障到达目标位姿.

3) 改进算法提高了避障结束时末端位姿的定位精度，减小了避障过程中关节运动幅度.实验结果表明：改进算法避障完成时各关节角度误差、末端平均位置与姿态误差分别为0.8°、0.010 m和2.40°，均优于传统算法；避障过程中关节角度整体波动幅度小于传统算法.

4) 本文算法对冗余机械臂避障研究及应用具有一定的指导意义，受凹型障碍物影响的局部极小问题与虚拟引力势场建立算法是后续研究的重点.

参考文献
[1] GONCALVES V M, FRAISSE P, CROSNIER A, et al. Parsimonious kinematic control of highly redundant robots[J]. IEEE Robotics & Automation Letters, 2017, 1(1): 65. DOI:10.1109/LRA.2015.2506259

[2] YOO Y J, OH K J, CHOI Y J, et al.Obstacle avoidance for redundant manipulator without information of the joint angles[C]//Control Conference.Piscattaway, NJ: IEEE, 2013: 1

[3] KO N Y. Extended RMRC and its application to the motion of a mobile manipulator[J]. International Journal of Humanoid Robotics, 2015, 12(2): 1550016. DOI:10.1142/S0219843615500164

[4] CHEN Yujin, JU Mingyi, HWANG Kaoshing. A virtual torque-based approach to kinematic control of redundant manipulators[J]. IEEE Transactions on Industrial Electronics, 2017, 64(2): 1728. DOI:10.1109/TIE.2016.2548439

[5] KARPINSKA J, TCHON K. Performance-oriented design of inverse kinematics algorithms: extended jacobian approximation of the jacobian pseudo inverse[J]. Journal of Mechanisms & Robotics, 2012, 4(2): 437. DOI:10.1115/1.4006192

[6] HUANG Shuihua, PENG Yonggang, WEI Wei, et al. Clamping weighted least-norm method for the manipulator kinematic control with constraints[J]. International Journal of Control, 2016, 89(11): 2240. DOI:10.1080/00207179.2016.1153151

[7] DONG Hui, DU Zhijiang. Obstacle avoidance path planning of planar redundant manipulators using workspace density[J]. International Journal of Advanced Robotic Systems, 2015, 12(9): 1. DOI:10.5772/59973

[8] ABU-DAKKA F J, VALERO F J, SUNER J L, et al. A direct approach to solving trajectory planning problems using genetic algorithms with dynamics considerations in complex environments[J]. Robotica, 2015, 33(3): 669. DOI:10.1017/S0263574714000393

[9] HART P E, NILSSON N J, RAPHAEL B. A formal basis for the heuristic determination of minimum cost paths[J]. IEEE Transactions on Systems Science and Cybernetics, 1968, 4(2): 100. DOI:10.1109/TSSC.1968.300136

[10] KHATIB O. Real-time obstacle avoidance for manipulators and mobile robots[J]. International Journal of Robotics Research, 1986, 5(1): 90. DOI:10.1109/ROBOT.1985.1087247

[11] 于振中, 闫继宏, 赵杰, 等. 改进人工势场法的移动机器人路径规划[J]. 哈尔滨工业大学学报, 2011, 43(1): 50.
YU Zhenzhong, YAN Jihong, ZHAO Jie, et al. Mobile robot path planning based on improved artificial field method[J]. Journal of Harbin Institute of Technology, 2011, 43(1): 50. DOI:10.11918/j.issn.0367-6234.2011.01.011issn.0367-6234.2011.01.011

[12] HARGAS Y, MOKRANE A, HENTOUT A, et al.Mobile manipulator path planning based on artificial potential field: Application on RobuTER/ULM[C]//International Conference on Electrical Engineering. Piscattaway, NJ: IEEE, 2016: 1

[13] OH J, BAR H, OH J H.Analytic inverse kinematics considering the joint constraints and self-collision for redundant 7DOF manipu-lator[C]//IEEE International Conference on Robotic Computing.Piscattaway, NJ: IEEE, 2017: 123

[14] MOTAHARI A, ZOHOOR H, KORAYEM M H. A new obstacle avoidance method for discretely actuated hyper-redundant manipula-tors[J]. Scientia Iranica, 2012, 19(4): 1081. DOI:10.1016/j.sci-ent.2012.06.017

[15] 王俊龙, 张国良, 羊帆, 等. 改进人工势场法的机械臂避障路径规划[J]. 计算机工程与应用, 2013, 49(21): 266.
WANG Junlong, ZHANG Guoliang, YANG Fan, et al. Improved artificial field method on obstacle avoidance path planning for manipulator[J]. Computer Engineering and Applications, 2013, 49(21): 266. DOI:10.3778/j.issn.1002-8331.1201-0360

[16] LIU S, ZHANG Q, ZHOU D. Obstacle avoidance path planning of space manipulator based on improved artificial potential field method[J]. Journal of the Institution of Engineers, 2014, 95(1): 31. DOI:10.1007/s40032-014-0099-z

[17] CHENG Fengyi, JI Wei, ZHAO Dean, et al.Apple picking robot obstacle avoidance based on the improved artificial potential field method[C]//IEEE Fifth International Conference on Advanced Computa-tional Intelligence. Piscattaway, NJ: IEEE, 2012: 909.DOI: 10.1109/ICACI.2012.6463303