机械臂运动路径设计分析

1、机械臂运动路径设计分析摘要本文探讨了六自由度机械臂从一点到另一点沿任意轨迹移动路径、 - 点到另一点沿着给定轨迹移动路径、 以及无碰撞路径规划问题，并讨论了 设计参数对机械臂灵活性和使用范围的影响，同时给出了建议。问题一：(1)首先确定初始坐标均为零时机械臂姿态， 建立多级坐标 系，利用空间解析几何的变换基本原理及相对坐标系的齐次坐标变换的矩 阵解析方法，来建立机器人的运动系统的多级变换方程。 通过逆运动学解 法和构建规划，来求优化指令(2)假定机械臂初始姿态为 0,曲线离散化，每个离散点作为末端 位置，通过得到的相邻两点的姿态，利用(1)中算法计算所有相邻两点 间的增量指令，将满足精度要求的

2、指令序列记录下来。(3)通过将障碍物理想化为球体，将躲避问题就转化成保证机械手臂 上的点与障碍球球心距离始终大于 r的问题。进而通过迭代法和指令检验 法，剔除不符合要求的指令，从而实现避障的目的问题二：将问题二中的实例应用到问题一中的相对应的算法中，部 分结果见附录问题三：灵活性与适用范围相互制约，只能根据权重求得较优连杆 长度。关键词：多级坐标变换 逆运动学解法 优化 指令检验一、问题重述1.1问题背景某型号机器人（图示和简化图略）一共有6个自由度，分别由六个旋转轴（关节） 实现，使机器人的末端可以灵活地在三维空间中运动。机器人关于六个自由度的每一 个组合 （1, 2, 3, 4, 5, 6

3、）,表示机械臂的一个姿态，显然每个姿态确定顶端指尖 的空间位置X： f（ ） X。假定机器人控制系统只能够接收改变各个关节的姿态的 关于连杆角度的增量指令（机器指令）P （ 1, 2, 3, 4, 5, 6）,使得指尖（指尖具有夹工具、焊接、拧螺丝等多种功能，不过在这里不要求考虑这方面的 控制细节）移动到空间点X',其中各个增量i只能取到-2, -1.9, -1.8, 一,1.8,1.9, 2这41个离散值（即精度为0.1 0 ,绝对值不超过2° ）通过一系列的指令序列 P1,P2,P3, ,Pn可以将指尖依次到达位置X。，X,Xn,则称X, X, 一一, Xn为从指尖初始位

4、置 宠到达目标位置Xn的一条路径（运动轨迹）。根据具体的目标和约束条件 计算出合理、便捷、有效的指令序列是机器人控制中的一个重要问题。假设机器人的 初始位置在y-z平面上，约定直角坐标系的原点设在图示的A点，z轴取为AB方向，x轴垂直纸面而y轴则在基座所固定的水平台面上1.2目标任务问题一：设计一个通用的算法，用来计算执行下面指定动作所要求的指令序列， 并要求对算法的适用范围、计算效率以及近似算法所造成的误差和增量i离散取值所造成的误差大小进行讨论（不考虑其他原因造成的误差）：（1）已知初始姿态 。和一个可达目标点的空间位置（Q,。，O）,计算指尖到 达目标点的指令序列。(2)要求指尖沿着预先

5、指定的一条空间曲线 x = x(s), y = y(s), z = z(s), a 三s Wb移动，计算满足要求的指令序列。(3)在第个问题中，假设在初始位置与目标位置之间的区域中有若干个已知大小、形状、方向和位置的障碍物，要求机械臂在运动中始终不能与障碍物相碰，否则 会损坏机器。这个问题称机械臂避碰问题，要求机械臂末端在误差范围内到达目标点 并且整个机械臂不碰到障碍物(机械臂连杆的粗细自己设定) 。问题二：应用你的算法就下面具体的数据给出计算结果，并将计算结果以三组六维的 指令序列(每行6个数据)形式存放在 Excel文件里，文件名定为 answer1.xls , answer2.xls 和

6、 answer3.xls 。假设在机械臂的旁边有一个待加工的中空圆台形工件，上部开口。工件高180mm下底外半径168mm上底外半径96mm壁厚8mm竖立地固定在x y平面的操作台上， 底部的中心在(210, 0, 0)。.要求机械臂(指尖)从初始位置移动到工具箱所在位置的(20, 200,120)处，以夹取要用的工具。 .如果圆台形工件外表面与平面 x =2z的交线是一条裂纹需要焊接，请你给出 机械臂指尖绕这条曲线一周的指令序列。 .有一项任务是在工件内壁点焊四个小零件，它们在内表面上的位置到x-y平面的投影为(320,-104 )、(120,106)、(190,-125 )和(255,88

7、)。要求机械臂从圆台的上部开口处伸进去到达这些点进行加工，为简捷起见，不妨不计焊条等的长度，只考虑指尖的轨迹问题三：制造厂家希望通过修改各条连杆的相对长度以及各关节最大旋转角度等设计参数提高机械臂的灵活性和适用范围。请根据你们的计算模型给他们提供合理的 建议。二、问题分析机械臂运动路径设计问题主要涉及到相对坐标系坐标变换、机器人正运动学分析、逆运动学求解、优化以及机器人避碰问题。1 .运动方程的建立从机构学观点来看，机器人属于空间机构范畴。杆件每次转动因此，采用空间解析几何的变换基本原理及坐标变换的矩阵解析方法，来建立机器人的运动系统的多级变换方程。由于旋转轴涉及到平行连杆和垂直连杆两类，因此

8、对于各类旋转变换，所使用的 变换矩阵也不相同。同时，此题中涉及的机器人有 6个自由度，则从工件的坐标位置到固定坐标系的 变换要经过多级坐标变换。采用多级坐标变换的方法。由上述三点，便可建立机器人运动系统的坐标变换关系式。2 .尖端轨迹曲线模型的建立对于已给定一条空间曲线x=(s),y=(y),z=(z),可将其看成一个点的集合。因此机械臂实现一个空间轨迹的过程是实现轨迹离散的过程。如果这些离散点间距很大，机械臂轨迹与要求的轨迹就有较大误差。只有这些离散点彼此很接近，才有可能使机械臂的轨迹以满足要求精确度逼近要求的轨迹。连续轨迹的控制实际上就是在多次执行离散点间的点位控制，离散点点越密集越能逼近

9、要求的运动曲线3 .避碰问题假设空间障碍物为半径为r的球体，则这些球体空间便形成了机械手臂的约束。而躲避问题就转化成保证机械手臂上的点与障碍球球心距离始终大于r的问题。根据运动规律，不难知道手臂相对于初始位置的姿态决定于之前执行的所有指令的和。已 '知连杆上的点L在其所在的相对坐标系中的坐标 x, y,z及转动的指令，根据齐次坐 标变换矩阵就可得到L在固定坐标系中的坐标 x,y,z ,然后可计算距离。所以 可以 用问题1-1算法产生点到点的指令，可以利用迭代法从初始位置开始提前检验每个指 令，不满足要求的无法执行。4 .自由度分析：由题中指出的：指尖E点，具有夹工具、焊接、拧螺丝等多种

10、功能，不过在这里不要求考虑这方面的控制细节。则将情况理想化，不考虑第6个自由度对运动的影响。三、模型的假设1 .各关节连杆在输入指令后同时开始转动，速度为低速，各杆件之间无摩擦,臂各旋转轴最大运动速度相同2 .在轨迹规划中不考虑机械臂关节转角的限制。3 .不考虑机械臂结构和装配产生的误差。4 .连杆为刚体，不会发生形变5 .设机器人的初始位置是在一个平面上的（y-z平面）。6 .关节视为质点，它们所占的面积可忽略不计。四、符号说明U i :坐标系,U o为固定坐标系,其余为固定在杆上的相对坐标系i 1T : U i到Ui 1的坐标系变换矩阵'x :尖端移动终点在固定坐标系中的x方向坐标

11、 'y :尖端移动终点在固定坐标系中的y方向坐标 'z :尖端移动终点在固定坐标系中的z方向坐标x 0 :尖端转动起点在固定坐标系中初始x方向坐标V0 :尖端转动起点在固定坐标系中初始y方向坐标zo ：尖端转动起点在固定坐标系中初始z方向坐标i-第i个自由度的相对于初始姿态转角(i=1,2,3,4,5,6)i -第i个自由度转角的增量(i=1,2,3,4,5,6)sisin ici cos i五、模型建立与求解5.1 运动学模型建立与求解5.1.1 机器人运动方程的建立为了产生点到点的运动序列，首先我们先作出初始角度均为0时的姿态，以每个节点为原点建立坐标系U0到U6如图5.1

12、.1 oEDCBFA教育资料图-5.1.1从图-5.1.2可以看出F点相当于将Ua绕Z轴旋转得到0T,同理其他节点分别相当于 将前一坐标系绕X,X,Z,X,Z轴转动。可得到各坐标系相对于前一个的坐标系的坐标变 换公式。从而得到机械臂各个关节的变换矩阵r11r12r1333旋转矩阵：AR21222331位置矢量:PxPyPz1tAr apo 1图-5.1.2:机器人连杆参数（见表格1）cos( 1)sin( 1)0010000Tsin( 1)cos( 1)002t1400sin( 2)cos( 2)0110010cos( 2)sin( 2)0000100011000cos(4) sin(4)00

13、0cos( 3)sin(3)003Tsin(4) 0cos(4)000sin( 3)0cos(3)12554001255000100011000cos( 6 )sin( 6)000sin( 5)cos(5)0 4T0 50sin( 6)0cos( 6)000cos( 5)sin(5)001000010001nxOxaxPx0T6Al（ 1）A2（ 2）A3（ 3）A4（ 4）A5（ 5）A6（ 6）nyOyayPynzozazPz0001其中nxC1C23C4c5c6C1C23S4s6S1S4c5c6SC4s6C1S23S5s6nyS1C23C4c5c6S1C23S4s6C1S4c5c6C1C

14、4s6S1S23S5s6nz$23C4c5c6$23$4 s3C238s6oyCQ23c4c5s6CG3s4c6*4 c50SC4c6 C&s5s6S|C23c4c5s6S|C23 s4 s6CS4c5s66s1 s23 s5 s6OzS23c4c5s6s23s4c6C23s5s6C1C23c4 s5s1s4 s5C1s23c5aySC23C4 s5C1s4s56 s23c5azS23c4 s5C23 c5Px65cle23C4s5 65§s4 s5 65cls23 c5255C1 s23255C1C2Pz65s23C4 8565C23C5255C23 255s2Py 65&

15、amp;C23C4s5 65C1s4 s5 65S&3C5 255ss23 255&C22.确定固定坐标系中E点位置由题设中，已知固定坐标系原点，根据给定的连杆长度和i角度，易计算得出,在固定坐标系中E位置：&=-65(-cos(s1)sin(s4)+(-sin(s1)cos(s2)cos(s3)+sin(s1)sin(s2)sin(s3)cos(s 4)sin(s5)+65(sin(s1)cos(s2)sin(s3)+sin(s1)sin(s2)cos(s3)cos(s5)+255s in(s1)cos(s2)sin(s3)+255sin(s1)sin(s2)cos(

16、s3)+255sin(s1)sin(s2)R=-65(-sin(s1)sin(s4)+(cos(s1)cos(s2)cos(s3)-cos(s1)sin(s2)sin(s3)cos(s4 )sin(s5)+65(-cos(s1)cos(s2)sin(s3)-cos(s1)sin(s2)cos(s3)cos(s5)-255 cos(s1)cos(s2)*sin(s3)-255cos(s1)sin(s2)cos(s3)-255cos(s1)sin(s2)Ez=140-65(sin(s2)cos(s3)+cos(s2)sin(s3)cos(s4)sin(s5)+65(-sin(s2)sin( s3)

17、+cos(s2)cos(s3)cos(s5)-255sin(s2)sin(s3)+255cos(s2)cos(s3)+255cos (s2)这样便得到了运动学方程。5.1.2利用逆运动学方法求解(1)求 i可用逆变换A 1( i)左乘运动方程式两边得：A (1)k6 A2( 2) A3 ( 3)A4( 4)As( 5) A6 ( 6)根据对性元素相等可解的 解得：Py 65ay fPy 65dy1 arctan或 1 arctanPx 65axPx 65ax(2)求 3由:nxoxaxpx解得:解得:其中A3 1( 3)A 1( 2)A 1(25(2c23(GPxsin(4)求 51)nynz

18、00y 0z0ay az 0PyPz 1A( 4)A( 5)A6( 6)3 arcsina arctan16s1ay)s23azslPy)223 Pz23 W) Barcsin aarctan16C4s5(255 C3) 65c4s523 arcsin B w;或 23tanw c1Px 6py 65(Gax Oay)pz 65azB (255c3) ，(qpx 科Py 65c,ax2233 arcsin B w 3；或 2arcsin B(1)(2)J22655ay)(Pz paz)arcsin B w 3s23(ciax say)c23azc55arccoss23(c1ax s1ay) c

19、23az或 52arccoss23(c,ax s1ay) 3az(5)求 4由21ax GayS4S,可以解得：s4落 GayS5.Sax Gay4 arcsinSs；或4arcsins1ax 网s5(6)s23(G0xG0y)c230zs5s3sin 6S23(GOxS0y) c230z sin 56 arcsinS23(c10x60y) c230zsin 5.s23(c10x 60y) 30zarcsinsin 55.2 问题1 1的模型在已有六自由度手臂运动方程和逆运动学解法的基础上，若已知机械臂末端转 动终点坐标和转动起点坐标已知，就可以得到各关节的坐标，连杆的相对于初始状态 的角度变

20、化 (1, 2, 3, 4, 5, 6)及机械臂的姿态。对于一般六自由度机械臂，带入末端坐标(x,y,z )会得到16组(1, 2, 3, 4, 5, 6),考虑用机械臂指尖实际到达的空间位置(X , 丫，Z )到理想位置(x,y,z )的偏差距离与移动距离的比值最少来衡量机械臂是否是最佳姿态，所以我们通过定义一个参量-偏移系数来寻找最佳指令, 其数学模型可表示为:Min M,'(Xx')2(yy')2(zz')2.(xXo)2(yyo)2(zzo)2X :尖端移动终点在固定坐标系中的 x方向坐标 'y :尖端移动终点在固定坐标系中的y方向坐标 '

21、;z :尖端移动终点在固定坐标系中的z方向坐标x。：尖端转动起点在固定坐标系中初始x方向坐标丫0 ：尖端转动起点在固定坐标系中初始 y方向坐标zo：尖端转动起点在固定坐标系中初始z方向坐标当尖端移动终点坐标(x, y, z)已知情况下，利用 MATLAB Robotics Tool可得到T矩阵，利用逆运动学解法，可得到(1, 2, 3, 4, 5, 6),因指令中各个增量i只能取到-2,-1.9,-1.8,1.8, 1.9, 2 这41个离散值(即精度为0.1 ° ,绝对值不超过2。)，所以最终转动的1, 2, 3, 4, 5, 6均只保留小数点后一位。所以对得到的(1, 2, 3,

22、 4, 5, 6)取位处理，然后回代入正方向运动学方程，解得的(x , y , z )才为 ,.、 *实际位置。通过对M的比较得到最佳(1, 2, 3, 4, 5, 6)。5.2.2 指令生成在已知i (i=1,2,3,4,5 , 6)后，只需要设计一种方式使机械臂从 1转到2 o 这里根据点位机械臂运动特点选用加速一一匀速一一减速三段式控制方式。如图5-2中的方式二。5.2.1机械臂最佳姿态的确定教育资料规定一个加速时间T(T<L/2),当1=机器人以vt匀速运动，最后在目标点前T时刻开始减速。这种实时控制计算量小，易于控制。在我们的程序中不防假设前10个指令加速，后10个指令减速，其

23、他匀速运动。程序见附录 2。(假设变速过程极快)5.3 问题12 模型机械臂连续轨迹的控制问题曲线对于已给定一条空间曲线x=(s),y=(y),z=(z),可将其看成一个点的集合。因此机械臂实现一个空间轨迹的过程是实现轨迹离散的过程。如果这些离散点间距很大，机 械臂轨迹与要求的轨迹就有较大误差。只有这些离散点彼此很接近，才有可能使机械 臂的轨迹以满足要求精确度逼近要求的轨迹。连续轨迹的控制实际上就是在多次执行离散点间的点位控制，离散点点越密集越能逼近要求的运动曲线。对于问题1 2预先指定的曲线我们先要把它离散化， 把得到点ai的坐标Xi,yi,zi带入逆运动方程中，然后根据一定的路径约束和实际

24、情况的约束找出通过点ai时机械臂的姿态，然后ai 1的姿态1,L , 6作为初始位置问题11的算法中得到a 1移动到 *ai 的取佳指令(1, 2, 3, 4, 5, 6)i由于问题11算法中假定各关节在一次转动中产生等量指令，且各关节同时转动所以机械臂尖端在a，ai之间运动路径基本相同，再加之取点较多，所以此处对于a，ai之间，尖端的路径曲线不予以考虑 因此曲线运动的最佳指令为n,、*,、*c( 1, 2, 3, 4, 5, 6) =( 1, 2, 3, 4, 5, 6) ii 15.4 问题13模型一一机械手臂避障问题(指令检验法)假设空间障碍物为半径为r的球体，则这些球体空间便形成了机械

25、手臂的约束。而躲避问题就转化成保证机械手臂上的点与障碍球球心距离始终大于r的问题。根据运动规律，不难知道手臂相对于初始位置的姿态决定于之前执行的所有指令的和。已 、4 4 ,一 一 ,' . .X, y,z ,然后可计算距离。所以 可以知连杆上的点L在其所在的相对坐标系中的坐标 X, y,z及转动的指令，根据齐次坐标变换矩阵就可得到L在固定坐标系中的坐标用问题1-1算法产生点到点的指令，可以利用迭代法从初始位置开始提前检验每个指令，不满足要求的无法执行。根据这些要求建立执行检验模型如下:min obj0180011800125021250138031380270042700st 133

26、.505 1200270062700222Obj0 (lx Ox)2 (ly Oy)2 (lz Oz)2 2220(lx Ox) (ly Ox)(lz Ox)robj0222100(lxOx)2(lyOx)2(lzOx)2r(615)此模型的目标函数是一个符合的函数，目标函数obj 0表征机械臂上的点与目标点的距离，若与内壁的距离小于r, ob"就会突然变大，从而不满足目标函数。六、模型的验证6.1问题2-1的求解根据问题1 1的模型算法，计算得出以连杆DE从空间起始点0指向工具箱位置(20,-200 , 120)的姿态，通过机械臂逆运动学计算得出它到这点的角度为(-84.3 0,6

27、1.0 0,-43.3 0,0,-17.6 0)。把得到的角度代入指令生成算法中得到：(0, 0, -1.6 , 0, -1,6 , 0); (-0.1 , -0.1 , -1.6 , 0, -1,6 , 0); (-0.2 , 0.1 , -1.6 , 0, -1,6 , 0)等，详见 answer1.xls 。MATLAB Robotic仿真如下(程序见附件2)。教育资料600表6- 1关节角度误差6.2.2对模型二的仿真对图中曲线采用1-2模型解得指令见answer2,（程序见附件3）七、模型的评价1、基于齐次坐标变换法的点位控制模型评价：该模型可以适用于所有已知目标位置和姿态的点位控制

28、问题。算法的计算效率非常的高，并且实时控制性好。算法误 差产生于多次三角变换求解所带来的舍入误差2、连续轨迹控制模型评价：该模型适用于所有参数方程的空间曲线生成机器指令。适用于机器人在低速运动的情况。但离散点数量较少时，会产生误差。3、指令检验法模型评价：该模型精度高，易于理解。但设计算法上较为复杂参考文献参考文献按正文中的引用次序列出，其中：1机械运动学教程2机器人手臂下棋运动的逆运动学分析与仿真赵艳云机械臂参数大干变量符号i初始位置变量范围110士 18022-90士 125330士 138440±27055-90-120+ 133.5660±270附件：问题1-1nx

29、=T06(1,1);ny=T06(2,1);nz=T06(3,1);ox=T06(1,2);oy=T06(2,2);oz=T06(3,2);ax=T06(1,3);ay=T06(2,3);az=T06(3,3);px=T06(1,4);py=T06(2,4);pz=T06(3,4);n=nx ny nz;o=ox oy oz;a=ax ay az;P=px py pz；theta=zeros(6,100);for i=2:100%theta1theta11=atan(py-65*ay)/(px-65*ax);theta12=pi+atan(py-65*ay)/(px-65*ax);if the

30、ta11-theta(i-1,1)>=theta12-theta(i-1,1) theta(i,1) =theta12;elsetheta(i,1) =theta11;end%theta3theta31=asin(1)+atan(1/6);theta32=pi-asin(1)+atan(1/6);if theta31-theta(i-1,3)>=theta32-theta(i-1,3)theta(i,3) = theta32;elsetheta(i,3) = theta31;end%theta2w=atan(c1(i,j)*px+s1(i,j)*py-65*(c1(i,j)*ax+

31、s1(i,j)*ay)/(pz-65*az);B=(255*c3(i,j)/sqrt(c1(i,j)*px+s1(i,j)*py-65*c1(i,j)*ax-65*s1(i,j)*ay)A2+(pz-p*az)A2);theta21=asin(B-w-theta(i,3);theta22=pi-asin(B)-w-theta(i,3);if theta21-theta(i-1,2)>=theta22-theta(i-1,2)theta(i,2) = theta22;elsetheta(i,2) = theta21;end%theta5theta51=acos(s231(i,j)*(c1(

32、i,j)*ax+s1(i,j)*ay)-c231(i,j)*az);theta52=2*pi-acos(s232(i,j)*(c1(i,j)*ax+s1(i,j)*ay)-c232(i,j)*az);if theta51-theta(i-1,5)>=b5-theta(i-1,5)theta(i,5) = theta52;elsetheta(i,5) = theta51;end%theta4theta41=asin(s511(i,j)*ax-c1(i,j)*ay)/s511(i,j);theta42=pi-asin(s1(i,j)*ax-c1(i,j)*ay)/s511(i,j);if a

33、theta41-theta(i-1,4)>=theta42-theta(i-1,4)theta(i,4) = theta42;elsetheta(i,4) = theta41;end%theta6theta61=asin(sin(c1(i,j)*ox+s1(i,j)*oy)-c231(i,j)*oz)/s511(i,j);theta62=pi-asin(s231(i,j)*(c1(i,j)*ox+s1(i,j)*oy)-c231(i,j)*oz)/s511(i,j);if theta61-theta(i-1,6)>=theta62-theta(i-1,6) theta(i,6) =

34、 theta62;elsetheta(i,6) = theta61;end end问题2-1T06=transl(20,-200,120);nx=T06(1,1);ny=T06(2,1);nz=T06(3,1);ox=T06(1,2);oy=T06(2,2);oz=T06(3,2);ax=T06(1,3);ay=T06(2,3);az=T06(3,3);px=T06(1,4);py=T06(2,4);pz=T06(3,4);n=nx ny nz;o=ox oy oz;a=ax ay az;p=px py pz;theta=zeros(7,6);for i=2:100%theta1theta11

35、=atan(py-65*ay)/(px-65*ax);theta12=pi+atan(py-65*ay)/(px-65*ax);if theta11-theta(i-1,1)>=theta12-theta(i-1,1)theta(i,1) =theta12;elsetheta(i,1) =theta11;end%theta3theta31=asin(1)+atan(1/6);theta32=pi-asin(1)+atan(1/6);if theta31-theta(i-1,3)>=theta32-theta(i-1,3)theta(i,3) = theta32;elsetheta(

36、i,3) = theta31;endc1(i)=cos(theta(i,1);s1(i)=sin(theta(i,1);c3(i)=cos(theta(i,3);s3(i)=sin(theta(i,3);c23(i)=cos(theta(i,1)+theta(i,3);s23(i)=sin(theta(i,1)+theta(i,3);%theta2w(i)=atan(c1(i)*px+s1(i)*py-65*(c1(i)*ax+s1(i)*ay)/(pz-65*az);B(i)=(255*c3(i)/sqrt(c1(i)*px+s1(i)*py-65*c1(i)*ax-65*s1(i)*ayF2+(pz-1*az)A2);theta21=asin(B(i)-w(i)-theta(i,3);theta22=pi-asin(B(i)-w(i)-theta(i,3);if theta21-theta(i-1,2)>=theta22-theta(i-1,2)theta(i,2) = theta22;elseth