机器人的三种规则曲线插补算法

《装备制造技术》2009年第 11期 机器人的轨迹规划，在机器人控制中具有重要的作用，直 接影响着控制的准确性和快速性。例如机器人在进行电弧焊、 电切割、喷涂等动作时，不仅要求机器人在运动终点准确定 位，而且必须沿着所希望的路径在一定的精度范围内移动，即 要求对手臂进行精确的连续轨迹控制 (Continuous Path Con- trol)。对于工业机器人运动的位置控制，通常是采用示教再现 的方法，即首先示教机器人运动到哪些位置点，机器人记住每 个位置点，再重复这些位置点[1]。显然，示教点的数量，决定了 运动轨迹的精度和机器人的工作效率。示教点越多，精度越 高，但效率越低。因此，我们希望只示教机器人运动路径上的 某些关键点，然后根据轨迹特征算出这些示教点之间必须到 达的中间位置点，通过插补进行控制，从而实现高效高精的运 动控制。运动路径一般由一些基本曲线组成，本文研究的是空 间直线、平面圆弧、空间圆弧等三种规则曲线的插补算法。 1 轨迹规划方法 传统的轨迹规划分为两种，一种是关节空间的规划，另一 种是笛卡尔空间的规划。这两种方式各有其优缺点：关节空间 的轨迹规划相对简单，计算量小，且不会出现奇异位形，但是 其末端的运动轨迹不直观；笛卡尔空间的轨迹规划则比较直 观，但在规划过程中容易进入机器人的奇异位形[2]。本文采用 笛卡尔空间轨迹规划方法，但由于大部分机器人是关节机器 人，系统控制的是关节坐标轴，该轨迹规划方法生成的值是机 器人末端执行器的位姿，必须反复求解逆运动学方程来计算 关节角，具体规划步骤如下： （1）由示教得到几个关键路径点（对六自由度机器人而 言，得到的是 6个关节角度值），通过机器人正运动学正解得 出其在笛卡尔空间中所 表 关于同志近三年现实表现材料材料类招标技术评分表图表与交易pdf视力表打印pdf用图表说话 pdf 示的位置和姿态值； （2）根据关键点的位置和姿态值以及轨迹要求，按照一 定算法进行插补，从而形成一系列笛卡尔空间里的路径点； （3）将规划好的一系列路径点，通过机器人逆运动学反 解得出机器人能识别的关节空间表示量。 插补算法在以上步骤中，占据着举足轻重的地位，是整个 机器人轨迹规划控制过程（图 1）的精华所在，因此研究它具有 十分重要的意义。 2 规则曲线插补算法 机器人终端执行器在笛卡尔空间中的描述，包括位置描 述与姿态描述，因此其插补算法中也包括位置插补与姿态插 补。其中，姿态插补一般采取线性方式，即把终端执行器在曲 线上的终点和起点的方位差均匀地分配到插补的每一步，算 法简单，在此不作讨论。位置插补方式，包括直线插补，圆弧插 补，抛物线插补，样条线插补等。本文只研究最基础的直线和 圆弧插补算法。 2.1 空间直线插补 已知空间直线的起点坐标 A(xA，yA，zA)、终点坐标 C(xC，yC， zC)和插补次数 N，则 △x=(xC -xA )/(N+1) △y=(yC -yA )/(N+1) △z=(zC -zA )/(N+1 ! # # ## " # # ## $ ) ， 对于该直线上的任一点 i (1≤ i≤N)有 xi = xA+△ x·i yi = yA+△ y·i zi = zA+ △ z· ! # # ## " # # ## $ i 。 2.2 平面圆弧插补 已知标准平面（如 X- Y平面、Y-Z平面或 Z- X平面）上 的圆心坐标 O(xO，yO，zO)、半径 R、圆弧方向（顺时针或逆时针）、 起始角 α、圆弧圆心角 θ，以及插补次数 N，求此平面圆弧上的 机器人的三种规则曲线插补算法 卓扬娃，白晓灿，陈永明 （厦门大学机电工程系数控技术研发中心，福建厦门 361005） 摘要：针对机器人末端执行器在笛卡尔空间中的轨迹规划方法，研究了空间直线、平面圆弧、空间圆弧等三种基本规则曲线的插补算 法。此算法在理论上可使所有插补点均落在所要求的曲线上，算法精简且无累积误差。研究成果已在实际机器人中得到实现。 关键词：机器人；轨迹规划；插补算法 中图分类号：TP242 文献标识码：A 文章编号：1672- 545X（2009）11- 0027- 03 收稿日期：2009- 08- 27 作者简介：卓扬娃（1984—），女，福建厦门人，硕士研究生，攻读机械电子工程专业；陈永明（1962—），男，福建三明，副教授，主要从事数控技术及 其应用、工业自动化、智能仪器技术等方面的研究工作。 轨迹上几个 示教点的位姿 插补 算法 轨迹中间 点位姿 逆向 运动学 各关节 给定值 关节角 控制系统 要求 的位姿 图 1 机器人轨迹规划控制过程 设计与计算 %%%%&%&%%%%&% & 27 Equipment Manufactring Technology No.11，2009 点坐标。以 X- Y平面上的圆弧为例，如图 2所示，圆弧 AC为 顺时针方向，起始角 α 是指圆弧起始点 A(xA，yA，zA)与 Y轴之 间的夹角，α与 θ均用弧度制表示，则 xA= xO+ R·sinα yA= yO+ R·cosα zA= zO � � � �� � � � �� � ， xC= xO+ R·sin(α + θ) yC= yO+ R·cos(α + θ) zC= zO � � � �� � � � �� � ； △θ = θ / (N+1) 对于圆弧上任一点 Pi (xi，yi，zi) (1≤ i≤N)有 xi= xO+ R·sin(α + i·θ) yi= yO+ R·cos(α + i·θ) zi= zO � � � �� � � � �� � 2.3 空间圆弧插补 已知空间任意三点为圆弧起点 A(xA，yA，zA)、中间点 B(xB， yB，zB)和终点 C(xC，yC，zC)，A、B、C三点不在同一直线上，以及插 补次数 N（不包括 A、C点），求此三点所在空间圆弧上的系列 点坐标。步骤如下： （1）求圆弧圆心 O(xO，yO，zO)和半径R。由 AO = BO 得 (xA- xO ) 2 +(yA- yO ) 2 +(zA- zO ) 2姨 = (xB- xO ) 2 +(yB- yO ) 2 +(zB- zO ) 2姨 （1） 由 BO = CO 得 (xB- xO ) 2 +(yB - yO ) 2 +(zB - zO ) 2姨 = (xC- xO ) 2 +(yC- yO ) 2 +(zC- zO ) 2姨 （2） 由不共线的三点确定的平面方程，有 xO yO zO 1 xA yA zA 1 xB yB zB 1 xC yC zC 1 = 0。 联立式（1）式（2）式（3），即可求出圆心 O(xO，yO，zO)； 半径 R= AO = (xA - xO ) 2 +(yA - yO ) 2 +(zA - zO ) 2姨 。 （2）求圆弧所在平面的法矢量 n。在数控机床中，平行于 坐标平面的圆弧编程，一般只给出起点、终点和圆心，因而需 要用 G02和 G03区分为顺时针圆弧和逆时针圆弧。给定了起 点、终点和一个中间点的空间三点圆弧的走向是确定的，用 ABBB×BBBC表示空间三点圆弧所在平面的法矢量 n，则从 n的正 方向看,从 A到 B到 C的圆弧始终是逆时针圆弧[3~4]。 设 n = ABBB×BBBC = ui + vj + wk， 则 u=(yB - yA )(zC - zB )- (zB - zA )(yC - yB ) v=(zB - zA )(xC - xB )- (xB - xA )(zC - zB ) w=(xB - xA )(yC - yB )- (yB - yA )(xC - xB � � � �� � � � �� � ) 。 （3）求圆心角 θ。如图 3所示，有两种情况： 当 θ≤π（圆弧 ABC）时， θ = 2arcsin (xC - xA ) 2 +(yC - yA ) 2 +(zC - zA ) 2B B 1/2 / (2RB B) ； 当 θ > π（圆弧 ABC'）时， θ = 2π- 2arcsin (xC - xA ) 2 +(yC - yA ) 2 +(zC - zA ) 2B B 1/2 / (2RB B) 。 而如何判断 θ与 π的关系呢？我们设矢量OBBA×ABBC在各 坐标轴方向上的分量为 u1 = (yA- yO) (zC - zA )- (zA - zO ) (yC - yA) v1 = (zA - zO ) (xC - xA)- (xA - xO) (zC - zA ) w1 = (xA - xO) (yC - yA )- (yA - yO ) (xC - xA � � � �� � � � �� � ) ， 并设 H= uu1 + vv1 + ww1，则当 H≥0时，矢量OBBA×ABBC 与 圆弧所在平面的法矢量ABBB×BBBC方向相同，此时 θ ≤π；当 H < 0时，矢量OBBA×ABBC与圆弧所在平面的法矢量ABBB×BBBC方向 相反，此时 θ > π。 （4）求步距角 δ。每次插补走过的步距角 δ是不变的，有 δ = θN+1 。 （5）求插补递推公式[5]。如图 4所示，圆弧上任一点 Pi (xi , yi , zi)处沿前进方向的切矢量 mi i + ni j+ li k= n×OPiBB= i j k u v w xi-xo yi- yo zi- zo ， Y X A Pi Pi+1 CR O α △θ 图 2 平面圆弧插补原理 A B C R R 0 C ' θ θ ' 图 3 圆心角的计算 X Y Z 0 R C R Pi+1P 'i+1 Pi △s n A δ 图 4 空间圆弧插补原理 28 《装备制造技术》2009年第 11期 可得 mi = v(zi - zO ) - w(yi - yO) ni = w(xi - xO ) - u(zi - zO ) li = u(yi - yO ) - v(xi - xO � � � �� � � � �� � ) 设经过一个插补周期后，机器人终端执行器从点 Pi (xi , yi , zi)沿圆弧切向移动距离 △s(△s≈δ·R)后，到达点 P 'i+1(x'i+1， y'i+1，z'i+1)，则有 x'i+1=xi +△x'i = xi +Emi y'i+1=yi +△y'i = yi +Eni z'i+1=zi +△z'i = zi +Eli � � � �� � � � �� � （4） 其中 E = △s / (m 2 i +n 2 i +l 2 i） 1/2。又因为 m 2 i +n 2 i +l 2 i = (u2 + v2 + w2)· (xi - xo ) 2 +(yi - yo ) 2 +(zi - zo ) 2� � + (xi - xo )u+(yi - yo )v+(zi - zo )w� � = (u2 + v2 + w2)R 2 故 E = △s / R(u2 + v2 + w2) 1/2� �，为一常量。 从图 4可看出，点Pi+1 ' 并不在圆弧上，为使所有插补点都 落在圆弧上，需对式（4）进行修正。连接 OP i+1 ' 交圆弧于点 Pi+1 ' ，以 Pi+1 ' 代替 Pi+1 ' 作为实际插补点，则可保证插补点始终落 在所求圆弧上。 在直角三角形 △OPiPi+1 ' 中，有 OPi+1 ' 2 = OPi 2 + Pi Pi+1 ' 2， 即(R+△R)2 = R2+△s2，则有 xi+1= xo + R(x'i+1-xo ) / (R2 + △s2)1/2 yi+1= yo+ R(y'i+1-yo ) / (R2 + △s2)1/2 zi+1= zo + R(z'i+1-zo ) / (R2 + △s2)1/ � � � �� � � � �� � 2 （5） 令 G = R / (R2+△s2)1/2（为一常量），同时把式（4）代入式 （5），得插补递推公式 xi+1= xo +G(xi + Emi-xo ) yi+1= yo+G(yi + Eni-yo ) zi+1= zo +G(zi + Eli-zo � � � �� � � � �� � ) ，(0≤i≤N- 1) （6） 另设 i = 0时，插补起点 P0为点 A，即 x0 = xA y0 = yA z0 = zA � � � �� � � � �� � 。 综上由式（6）可递推得到空间圆弧上的一系列插补点的 坐标值，并能保证插补点总在圆弧上，此算法没有累积误差。 3 结束语 插补算法独立于机器人结构[6]，直线和圆弧插补是机器人 系统中不可缺少的插补算法，对于非直线、非圆弧的轨迹，都 可以采用直线、圆弧来逼近，以实现这些轨迹。本文研究了机 器人轨迹规划中的空间直线、平面圆弧、空间圆弧等三种规则 曲线的插补算法，理论上可使所有插补点均落在要求曲线上， 在空间圆弧插补中还采用了矢量算法，避免了插补方向和过 象限的判断，算法精简且没有累积误差。这些算法已经编写成 计算机语言，结合机器人正逆运动学算法，在本 实验室 17025实验室iso17025实验室认可实验室检查项目微生物实验室标识重点实验室计划 中的一 台六自由度机器人的控制中得到实现。本研究成果也可适用 于高性能要求的机床数控系统。 参考文献： [1]谢存禧,张 铁. 机器人技术及其应用[M]. 北京:机械工业出版社, 2005. [2] [美]Saeed B.Niku. 机器人学导论——— 分析 定性数据统计分析pdf销售业绩分析模板建筑结构震害分析销售进度分析表京东商城竞争战略分析 、系统及应用[M]. 北京: 电子工业出版社,2004. [3]别卫春，朱志红，叶伯生. HNC- IR机器人语言解释系统的研究与 实践[J].机电一体化, 2000，(3):27- 30. [4]叶伯生，朱志红，刘恩沧.基于 PC的教学机器人控制系统[J].机械 与电子, 2000，(6):43- 45. [5]金建新.机床 CNC系统中任意空间曲线的可控步长插补方法[J]. 机械工程学报, 2002，(4):95- 97. [6]贤海华，杨培君.机械手轨迹插补算法的研究与仿真[J].装备制造 技术，2008，(9)：57- 59. Interpolation of Three Regular Curves for Robot ZHUOYang- wa, BAI Xiao- can, CHEN Yong- ming (R & D Center for Numerical Control Technology, Department of Mechanical and Electrical Engineering， Xiamen University, Xiamen Fujian 361005, China) Abstract: Precise trajectory planning is needed during modern industry robot work. Based on the method of trajectory planning in Carte- sian space for robot’s end-effector, in this paper, the interpolation of three regular curves, including the line and arc in plane or in space, is proposed. This method can make all interpolated points fall on the curve accurately through the algorithm theoretically with little error. The result has been applied to a practical robot. Key words: robot ; trajectory planning ; interpolation 29