﻿
 哈尔滨工业大学学报  2019, Vol. 51 Issue (7): 184-191  DOI: 10.11918/j.issn.0367-6234.201806121 0

### 引用本文

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.

### 文章历史

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 机械臂模型 1.1 运动学模型

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

 $\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}$

1.2 碰撞检测模型

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

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

1) 若LmhRh+r，则发生碰撞.

2) 若LmhRh+r，则没有发生碰撞.

2 改进人工势场法 2.1 势场函数建立

 ${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).$

 ${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.$

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

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

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

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

 ${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 局部极小处理

 ${\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}}}}}.$

 $\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.$

 ${\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)} .$

 ${\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)关节中心点为Ti，目标位姿i关节中心点为${\tilde T}$i，障碍物球体包络盒模型为Sm，构建三角形区域T2${\tilde T}$4T4T4${\tilde T}$4${\tilde T}$6T4${\tilde T}$6T6T6${\tilde T}$6${\tilde T}$7T6${\tilde T}$7T7.若球体包络盒模型与多个三角形区域相交，在求解$\tilde \gamma$i时将其归属于相交面积最大的三角形区域.三角形区域所在平面将包络盒模型切分为两部分，取体积较小的部分所在空间为$\tilde \gamma$i求解方向.以三角形区域的一个顶点为活动顶点，以该顶点所在关节的上一个$\tilde \gamma$i求解关节(较小标号方向)为转动关节，关节转动带动活动顶点运动，使得三角形区域所在平面与球体包络盒相切，此时关节角度为$\tilde \gamma$iA，为减弱障碍物斥力势场的影响，需给定角度安全余量$\tilde \gamma$iB，使得机械臂向虚拟目标角度方向的运动幅度更大，则

 ${\tilde \gamma _i} = {\tilde \gamma _{iA}} + {\tilde \gamma _{iB}}.$
 图 3 虚拟目标角度求解示意图 Fig. 3 Schematic diagram for solving a virtual target angle

2.3 算法实现

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

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

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

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

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

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

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

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

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

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

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

3 仿真与实验 3.1 仿真验证

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

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

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

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

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

 图 7 改进人工势场法各关节角度变化 Fig. 7 Change of angle of each joint by improved APFM
 图 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