基于齐次变换矩阵的机器人轨迹规划方法*

沈雅琼，叶伯生，熊 烁

(华中科技大学 机械科学与工程学院，武汉 430074)



基于齐次变换矩阵的机器人轨迹规划方法*

沈雅琼，叶伯生，熊 烁

(华中科技大学 机械科学与工程学院，武汉 430074)

基于齐次变换矩阵，提出一种笛卡尔空间内的机器人轨迹规划方法。该轨迹规划方法首先引入关节属性来预知轨迹示教点的位置是否合适，然后以机器人末端执行器的轨迹特征和齐次位姿矩阵的优势为基础，将机器人笛卡尔空间内的轨迹规划划分为平动和转动轨迹规划，建立平动和转动轨迹方程，进行位置和姿态的插补。该轨迹规划方法适用于机器人笛卡尔空间的直线和圆弧轨迹规划，不仅克服了工业机器人原有轨迹规划方法中由于机器人的奇异性和欧拉角算法所引起的缺点，而且具有概念直观、规划路径准确、可操作性强的特点。

齐次变换矩阵；轨迹规划；关节属性；

0 引言

机器人系统依据运动的轨迹来工作，其效率和工作质量的优劣与轨迹规划的好坏密不可分[1]。为此，研究机器人轨迹规划的方法是尤为重要的。所谓轨迹，是指操作臂在运动中的位移、速度和加速度。而机器人轨迹规划就是根据机器人需要完成的任务，来设计机器人的运动规律[2]。轨迹规划可以在笛卡尔空间和关节空间中进行，但都要求所规划的轨迹函数必须连续且平滑，以保证机器人的末端执行器的运动平稳[3]。

在关节空间的轨迹规划是以关节角度的函数来描述机器人轨迹的，在笛卡尔空间进行规划是将机器人手部位姿、速度和加速度表示为时间的函数，而相应的关节位移、速度和加速度由机器人手部信息导出[4]。关节空间的轨迹规划，不必在笛卡尔坐标系中描述两个路径点的路径形状，计算简单、容易，且不会发生机构的奇异性问题[5]。但如果要求机器人末端执行器按照预定的路径运动或要求绕过障碍物，关节空间法是无法实现的，而需要采用笛卡尔空间法。并且机器人的空间圆弧运动必须在笛卡儿空间中进行规划。故本文重点研究机器人笛卡尔空间的轨迹规划方法，并以莫托曼SK6型机器人为实验对象。

目前，有很多文献对机器人笛卡尔空间的轨迹规划方法进行了研究。文献[6-7]研究了笛卡尔空间内基本的轨迹规划，文献[8-9]对原有的轨迹规划方法进行改进，从而使机器人运行更平稳、路径更圆滑，文献[10]针对机器人轨迹规划的时间最短性能指标，提出一种轨迹规划方法。但是这些算法都没有提出采用欧拉角或RPY法描述机器人姿态时，给机器人笛卡尔空间轨迹规划所带来一系列问题的解决方法。

本文首先通过分析机器人机械结构特征，引入关节属性，然后结合机器人末端执行器在笛卡尔空间的运动特征和齐次变换矩阵的优势，并且对机器人传统轨迹规划中出现的问题进行分析，提出一种基于齐次变换矩阵的轨迹规划方法。该方法通过将机器人笛卡尔空间的轨迹规划细分为平动和转动轨迹规划，采用齐次变换矩阵的方式来设计机器人笛卡尔空间的轨迹方程，并进行路程和姿态的插补。

1 机器人的关节属性及姿态表示方法

一般，在机器人笛卡尔空间进行轨迹规划时，相应的关节位移是通过机器人运动学反解得到的，故机器人笛卡尔空间的轨迹规划存在由于运动学反解带来的问题，即笛卡尔路径上解的存在性、唯一性和奇异性问题。

机器人的轨迹是通过示教来实现的，示教过程中，若所选择的起止点位置不当，而又无法预知，使得所规划的轨迹经过机器人的奇异点，不仅增加工作量、浪费时间，而且造成机器人的速度激增。实际上，所容许的关节速度是有限的，因而将导致操作臂偏离预期轨迹。这是机器人运动学反解为机器人笛卡尔空间的轨迹规划带来的一种问题。同时，在描述机器人姿态时，大多情况会采用可以简便描述机器人方位的欧拉角或RPY方法，但这些方法的计算过程中，在某些特殊值处都不可避免的存在反解退化等问题。

1.1机器人的关节属性

本文所用莫托曼SK6机器人为是一款典型的平行四边形结构的6轴机器人，主要用于弧焊操作。该机器人属于关节型6轴机器人，具有3种奇异形位[11]，可以用3种奇异形位作为临界位置把机器人的状态划分为8种，由此可规定机器人的3种关节属性，即前/后、左/右和上/下，如图1所示。根据这3种关节属性，可以得出以下结论：当路径起止示教点的关节属性不同时，机器人在笛卡尔坐标系下的直线运动有可能经过机器人奇异形位。而机器人在做线性运动的同时变换关节属性是不可能的，故可以通过在轨迹规划中引入关节属性的判断，使得机器人运动是否经过奇异位置得以预知[12]，即在进行轨迹规划前，先根据上述3中关节属性的划分，对示教点的关节属性进行判断，如果关节属性不同，则重新示教。这样，就有效的避免了机器人在笛卡尔空间运行过程中经过奇异形位。

图1 SK6型工业机器人关节属性界定

1.2 机器人的姿态表示方法

采用3个参数的方法可以简便地描述机器人的方位，如欧拉角和RPY方法，但无论是欧拉角还是RPY描述位姿，在进行相应的反解时，都会出现反解退化的问题，并且边界位置的角度值是不连续的。为了分析采用三个参数来描述机器人姿态方法中存在的问题，以z-y-z欧拉角为例予以详细说明。采用z-y-z欧拉角描述机器人末端执行器方位的方法进行姿态规划插补，所谓z-y-z欧拉角描述法，即三次转动的顺序为先绕z轴旋转角度A，再绕y轴旋转角度B，最后绕z轴旋转角度C。

在欧拉角的反解表达式中，A角和C角在边界位置是不连续的，即A或C角超过360°时从0°开始计算。在所规划的轨迹中，当A角或C角需要跨越边界位置时，机器人末端执行器不能保证按最短路径通过规划的连续轨迹。而在机器人运动过程中，合理的选择应该是尽量保证机器人按最短路径行走到目标位置[13]。并且欧拉角算法本身存在奇异点，即当绕y轴旋转的角度B为0°或180°时，其他两个旋转角度是人为确定的，从而造成了机器人末端执行器不能按预期的路径行走或中间工作点超出工作空间范围。

本文中提出的轨迹规划方法采用齐次变换矩阵来表示机器人姿态，而非欧拉角或PRY方法来对直线和圆弧进行轨迹规划和插补，这种方式本身不存在算法上的奇异点，从而可以有效的避免欧拉角正反解中带来的问题。

2 机器人的直线轨迹规划

(1)

Li左乘，表示平动是相对于固定坐标系的平移；Ri右乘，表示绕某个定轴的转动是相对于运动坐标系的旋转。

图2 机器人直线轨迹规划

(2)

2.1 平动轨迹规划

对于平动轨迹规划，在建立直线运动的轨迹方程之后，仅需要对其路程参数进行插补即可。已知轨迹示教起点的位置坐标为WpS，终点位置坐标WpE。设终点WpE到起点WpS的轨迹长度为L，有：

(3)

中间点Wpi到起点WpS的轨迹长度为si，则以si为参数的直线运动轨迹方程为：

(4)

平动轨迹规划只需要对路程参数si进行插值，便能计算出各中间点位置坐标。故本文采用抛物线过渡的线性插值，也就是传统的梯形加减速控制策略计算路程参数si。已知设定线速度为v，加速度为a，并设si处线速度为vi，下一插值点si+1处线速度为vi+1，插补时间间隔为ΔT，有：

si+1=si+vi+1·ΔT

(5)

插值点处于加速匀速段，有：

(6)

插值点处于减速段，机器人必需以a的加速度减速直到完成本段插补。故有：

vi+1=vi-aΔT

(7)

将式(5)代入(4)得：

(8)

上式(8)即为平动轨迹规划的基本算法，联合式(6)、(7)便可计算出vi+1。

2.2 转动轨迹规划

(9)

将RS写成：

根据旋转变换通式，得：

(10)

(11)

(12)

θi+1=θi+ωi+1·ΔT

(13)

式(13)中的角速度ω一般要根据平动时间进行调整。这是因为示教定义的角速度ω可能会与线速度v相矛盾，即按ω执行的转动可能与按v执行的平动不能同步完成。

插值点处于加速匀速段。有：

(14)

插值点处于减速段。有：

ωi+1=ωi-αΔT

(15)

将式(13)代入(12)有：

(16)

式(16)便是绕某个定轴转动轨迹规划的基本算法，联合式(14)、(15)便可计算出ωi+1。

3 机器人圆弧轨迹规划

对于平动轨迹规划，已知圆弧起点的位置坐标为WpS，中间点位置坐标WpC，终点位置坐标WpE，设路径中间点Wpi到起点WpS的弧长为si，圆心角为φi，终点WpE到起点WpS的轨迹长度为L，圆心角为Φ。

|WpS-WpO| =|WpC-WpO|=|WpE-WpO|= R

(17)

(WpS-WpO)×(WpC-WpS)=λ(WpC-WpS)×(WpE-WpC)

(18)

图3 圆弧轨迹示意

由式(17)可建立两个方程：

(19)

由式(19)可建立另一个方程：

(20)

联立式(19)和(20)，并写成系数矩阵的形式有：

(21)

式中，A为3×3矩阵，

B为3×3矩阵，

B2=A20xS+A21yS+A22zS

由式(21)可求出圆弧圆心WpO，并由式(17)求出圆弧半径R。

则，过圆弧圆心的旋转轴k为：

(22)

圆弧圆心角需要根据劣弧或是优弧分别计算。设：

如果k与n方向相同，则轨迹圆弧为劣弧，则：

φ=arccos{[(xE-xS)2+(yE-yS)2+(zE-zS)2-2R2]/(2R2)}

(23)

如果k与n方向相反，则轨迹圆弧为优弧，则：

(24)

(25)

以si为参数的姿态旋转轨迹方程为：

(26)

与直线运动轨迹规划一样，通过对路程参数si的插值，生成圆弧运动路径中间点Wpi。且采用抛物线过渡的线性插值，即梯形加减速控制策略计算路程参数si，并分为加速匀速段和减速段两部分对路程参数si插值。参照3.1节的插值过程，计算出si后，带入式(26)，再与绕某个定轴转动的轨迹规划结果联合，便构成了完整的圆弧轨迹规划。

4 结束语

本文提出的基于齐次变换矩阵的机器人轨迹规划方法对机器人笛卡尔空间的直线和圆弧轨迹规划均适用。该方法具有以下优点：1，充分发挥齐次变换矩阵的优势，避免了采用欧拉角进行轨迹规划时存在的问题；2，在轨迹规划中引入关节属性的概念，使得示教路径点的合适与否得以预知，从而有效的避免了机器人在笛卡尔空间运行过程中经过奇异形位；3，将机器人末端执行器的在笛卡尔空间的运动分为平动和转动，使得轨迹规划方法概念清晰，步骤明确，且可操作性强。

本文研究的算法已经在华中科技大学国家数控系统工程技术研究中心实施项目中的莫托曼SK6型机器人上得以验证，证明该方法是可行的并且满足实用要求。

[1] 王晓丽，侯媛彬，王涛. 基于VC++的工业机器人轨迹规划研究[J]. 工况自动化，2009(5)：34-37.

[2] 秦锋, 徐向荣. 基于样条函数法的机器人运动轨迹规划[J].机械科学与技术，1996，15(4)：561-565.

[3] 王卫忠，赵杰，高永生，等. 机器人的平面曲线轨迹规划方法[J]. 哈尔滨工业大学学报，2008，40(3)：389-392.

[4] 熊有伦. 机器人技术基础[M] . 武汉：华中理工大学出版社，1996.

[5] 吴瑞祥. 机器人技术及应用[M]. 北京：北京航空航天大学出版社，1994.

[6] 邓勇.样条曲线的实时插补[J]. 机械与电子，2001(4)：23-24.

[7] 任敬轶，孙汉旭. 一种新颖的笛卡尔空间轨迹规划方法[J]. 机器人，2002，24(3):217-221.

[8] 周苑，基于改进性遗传算法的Bezier曲线笛卡尔空间轨迹规划[J]. 上海电机学院学报，2012，15(4)：237-240.

[9] 张小江，高秀华. 三次样条插值在机器人轨迹规划应用中的改进研究[J]. 机械设计与制造，2008(9)：170-171.

[10] 张红强. 工业机器人时间最优轨迹规划[D]. 长沙：湖南大学，2004：28-61.

[11] 毕诸明，蔡鹤皋. 六自由度操作手的逆运动学问题[J]. 机器人，1994，16(2)：68-71.

[12] 叶伯生，郭显金，熊烁. 计及关节属性的6轴工业机器人反解算法[J]. 华中科技大学学报(自然科学版)，2013，41(3)：68-72.

[13] 丁学恭. 机器人控制研究[M]. 杭州：浙江大学出版社，2006.

[14] 李天友，孟正大. 基于焊接机器人的实时轨迹插补算法[J]. 工业控制计算机，2009，22(6)：34-36.

[15] 叶伯生. 机器人空间三点圆弧功能的实现[J]. 华中科技大学学报(自然科学版)，2007，35(8)：5-8.

(编辑 李秀敏)

A Method of Robot’s Trajectory Planning Based on Homogeneous Transformation Matrix

SHEN Ya-qiong,YE Bo-sheng,XIONG Shuo

(School of Mechanical Science and Engineering,HuaZhong University of Science and Technology, Wuhan 430074,China)

Based on homogeneous transformation matrix, a method of trajectory planning for robot in cartesian space is put forward. In the method, joint attribute is first introduced to predict whether the position of demonstration points is appropriate. Then, based on the trajectory characteristics of the robot’s end effector and the advantage of homogeneous transformation matrix, the trajectory planning in the robot’s cartesian space is subdivided into translation and rotation trajectory planning, based on which the equation of translation and rotation trajectory can be founded, and the interpolation of position and posture can be proceeded. The method is available for both arc and line trajectory planning for robot in cartesian space. It not only overcomes the original shortcomings of trajectory planning method caused by the singularity of robot and Euler angular algorithm, but also has many characteristics, such as visualized concept, accurate path planning and strong maneuverability.

homogeneous transformation matrix; joint attribute; euler angle; trajectory planning;

1001-2265(2014)01-0005-05

10.13462/j.cnki.mmtamt.2014.01.002

2013-05-24

“高档数控机床与基础制造装备”科技重大专项“开放式高档数控系统、伺服装置和电机成套产品开发与综合验证”(2012ZX04001012)；“高档数控机床与基础制造装备”科技重大专项“高精度、高分辨力绝对式光栅旋转编码器研制”(2012ZX04001041)；高等学校博士学科点专项科研基金(新教师基金课题)：基于机床动力学得复杂曲面轨迹优化技术研究 (20090142120035)。

沈雅琼(1988—)，女，甘肃景泰人，华中科技大学硕士研究生，研究方向为工业机器人控制，(E-mail)yaqionghappy@126.com；通讯作者：叶伯生(1966—)，男，湖北黄冈人，华中科技大学副教授，博士，研究领域为数控技术，机器人控制等，(E-mail) yebosh@mail.com。

TH132;TG659

A