六自由度机器人逐加步长路径规划算法研究

章有，翟 华，2

（1.合肥工业大学 机械工程学院，安徽 合肥 230009；2.合肥工业大学 工业与装备技术研究院，安徽 合肥 230009）

六自由度机器人逐加步长路径规划算法研究

（1.合肥工业大学 机械工程学院，安徽 合肥 230009；2.合肥工业大学 工业与装备技术研究院，安徽 合肥 230009）

针对六自由度机器人路径规划问题，采用D－H法建立机器人的运动学方程，提出逐加步长算法，搜索有效路径，仿真结果证明了该算法的有效性。

六自由度机器人；运动学方程；逐加步长搜索算法；路径规划

在汽车大型覆盖件冲压生产线中，搬运机器人 合理的路径规划能使机器人运动避免自身结构缺陷，让机器人安全稳定地在复杂环境下工作[1]。路径是指空间的曲线，对于六自由度关节式机器人来说，它是指机器人运动位姿特定的一个序列[2]。机器人路径规划是指在给定的初始、目标位姿，通过采用恰当的算法搜索出一条从初始位姿到目标位姿的有效路径[3－4]。

1 六自由度机器人D-H坐标系的运动学模型

机器人运动学是研究机器人性能的基础，描述了关节空间与任务空间的关系，是路径规划的基础[5]。建立六自由度机器人连杆坐标系如图1所示，α1、α2、α3为连杆1、2、3的长度，d2、d4为连杆1与2、连杆3与4之间的偏距，θi为各连杆的转角。

图1 六自由度机器人连杆坐标系

令si=sin（θi），ci=cos（θi），sij=sin（θi+θj），cij=cos（θi+θj），分别为相邻连杆的变换矩阵，用来描述末端连杆坐标系{6}在基坐标系{0}的位姿，用来描述坐标系{6}的原点在基坐标系{0}中的位置矢量。机器人末端连杆坐标系相对于基坐标系的齐次变换矩阵[6]，即为：

某型六自由度机器人相应的连杆参数如表1所示。

表1 机器人连杆参数

将表1中的数据带入，则可得到以下运动方程[6]：

2 路径规划算法

通过选择恰当的算法使机器人实现路径搜索，能反映机器人的自主能力和高度智能化的自主行为[7-8]。本文提出逐加步长搜索算法来对机器人路径进行规划。该算法的基本思想是：对于六自由度关节式机器人，它的前三个连杆的尺寸较大，后面三个杆件相对较小，机器人的腕部位置主要由前三个关节变量决定，它的腕部姿态由后三个关节所决定。因此，在对六自由度关节式机器人进行路径规划时，可以选择优先转动前三个关节，使机器人达到目标位置，之后再转动后三个关节，使机器人运动至目标姿态。机器人的路径规划分成两步进行，先达位置，后达姿态。

由于机器人末端执行器固接在连杆坐标系{6}上，因此可以用坐标系{6}原点的路径代替末端执行器的路径。根据式（1），可得{6}原点在系{0}的位置描述为：

若已知六自由度关节式机器人初始位姿（初始节点）的关节变量值q0为，目标位姿（目标节点）的关节变量值qt为，算法的搜索步长为δ。在搜索路径节点的过程中，相应关节增加步长的正负与机器人初始位姿和目标位姿处关节的大小有关，正负规定如下：①若时，步长为正；②若时，步长为负；即：

将q0、qt代入式（3），可得机器人处于初始位姿和目标位姿时系{6}的原点在系{0}的坐标分别为：

从节点n-1搜索后继节点n的基本过程如下：

将节点n-1处的前三个关节的关节变量值分别增加一个步长δ，得到候选节点n1、n2、n3，如图2所示，

图2 搜索节点

候选节点n1、n2、n3的关节变量值为：

将式（7）代入式（3），可得机器人分别处于候选节点n1、n2、n3所对应的位姿时，系{6}的原点相对于系{0}位置分别描述为：

此时，引入评价函数：

式中：f（n）表示节点n-1经过节点n到达目标节点的总距离代价，g（n）表示节点n距上一节点n-1的距离代价，h（n）表示节点n距目标节点的距离代价。利

用评价函数对三个候选节点进行评价，比较评价函数值（fn1）、（fn2）、（fn3）的大小，取其中最小值，

fmin（n）所对应的候选节点就是所要搜索的节点n。

其他的路径节点均可按上述过程进行搜索，当搜索到的路径节点的前三个关节已到达目标节点相应值时，由于关节轴4和6均垂直于轴5，因此优先选择将关节轴5由转至接着依次将关节轴4由关节轴6由 。此时，机器人已经到达了目标节点，路径搜索过程结束。

3 机器人路径规划仿真

六自由度机器人初始位姿处的关节角为q0=（-55°，-65°，45°，0°，-50°，0°，），目标位姿处的关节角为qt=（-30°，-30°，25°，15°，-45°，80°，），步长设定为1°。在MATLAB环境下，结合机器人工具箱对机器人路径进行仿真，得到的路径规划仿真如图3所示。

从图3可以看出，根据本文提出的算法，可以规划出一条平滑且有效的路径，同时也能有效发现机器人在实际工作过程中的安全问题。

4 结论

针对六自由度机器人路径规划问题，提出了逐加步长搜索算法实现了路径节点搜索，将路径节点依次连接得到机器人完整路径。仿真结果验证了该方法的有效性和可行性。该算法的优点是：求解过程中计算量小，提高了路径规划的效率。

[1]H.C.Fang，S.K.Ong，A.Y.C.Nee.Interactive robot trajectory planning and simulation using Augmented Reality [J].Robotics and Computer-Integrated Manufacturing，2012，28（2）：227-237.

[2]Cesare Rossi，Sergio Savino.Robottrajectory planning by assigning positions and tangential velocities[J].Robotics and Computer-Integrated Manufacturing，2013，29（1）：139-156.

[3]J.W.S.Chong， S.K.Ong. Robot programming using augmented reality：An interactive method for planning collision-free paths[J].Roboticsand Computer Integrated Manufacturing，2009，25（3）：689-701.

[4]贾庆轩，陈 钢.基于A*算法的空间机械臂避障路径规划[J].机械工程学报，2010，46（13）：109-115.

[5]熊有伦.机器人技术基础[M].武汉：华中科技大学出版社，1996：47-49.

[6]蔡自兴.机器人学[M].北京：机械工业出版社，2009：32-35.

[7]Garg D P，Kumar M.Optimization techniques applied to multiple mannipulators for path planning and torque minimization[J]. Engineering Applications of Artificial Intelligence，2002，15（3-4）：241-252.

[8]张 雷.基于启发式搜索的最优规划算法研究[D].南京：南京大学，2010

Study on step by step searchpath planning algorithm of six-degree freedom robot

ZHANG Youqi1,ZHAI Hua1,2
（1.School of Mechanical Engineering,Hefei University of Technology,Hefei 230009,Anhui China; 2.Institute of Industry and Equipment Technology,Hefei University of Technology,Hefei 230009,Anhui China）

Aiming at the path planning problem of six-degree freedom robot,the kinematic equation has been built to the robot with D-H method.The step by step search algorithm has been proposed to search an effective path.The validity of algorithm has been demonstrated by simulation result.

Six-degree freedom robot;Kinematic equation;Step by step search algorithm;Path planning

图3 机器人路径仿真图

TP272；TP278

A

10.16316/j.issn.1672－0121.2016.04.015

1672－0121（2016）04－0046－04

2016－04－07；

2016－05－16

2013年度安徽省合芜蚌自主创新试验区专项资助（20130205）

翟 华（1973－），男，博士，教授，硕导，从事机械设计、精密校直技术、机器人技术研究。E－mail：jxzhaihuajx＠sina.com