(完整word版)机械臂运动路径设计分析(word文档良心出品)

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

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

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

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

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

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

7、壁点焊四个小零件，它们在内表面上的位置到 x-y 平 面的投影为(320,-104 )、(120,106 )、( 190,-125 )和(255,88 )。要求机械臂从圆台 的上部开口处伸进去到达这些点进行加工，为简捷起见，不妨不计焊条等的长度，只 考虑指尖的轨迹 问题三：制造厂家希望通过修改各条连杆的相对长度以及各关节最大旋转角度等 设计参数提高机械臂的灵活性和适用范围。请根据你们的计算模型给他们提供合理的 建议。 二、问题分析 机械臂运动路径设计问题主要涉及到相对坐标系坐标变换、机器人正运动学分 析、逆运动学求解、优化以及机器人避碰问题。 1. 运动方程的建立 从机构学观点来看，机器人属于

8、空间机构范畴。杆件每次转动因此，采用空间解 析几何的变换基本原理及坐标变换的矩阵解析方法， 来建立机器人的运动系统的多级 变换方程。 由于旋转轴涉及到平行连杆和垂直连杆两类，因此对于各类旋转变换，所使用的 变换矩阵也不相同。 同时，此题中涉及的机器人有 6 个自由度，则从工件的坐标位置到固定坐标系的 变换要经过多级坐标变换。采用多级坐标变换的方法。 由上述三点，便可建立机器人运动系统的坐标变换关系式。 2. 尖端轨迹曲线模型的建立 对于已给定一条空间曲线 x=(s),y=(y),z=(z), 可将其看成一个点的集合。因此机 械臂实现一个空间轨迹的过程是实现轨迹离散的过程。如果这些离散点间距很大

9、，机 械臂轨迹与要求的轨迹就有较大误差。只有这些离散点彼此很接近，才有可能使机械 臂的轨迹以满足要求精确度逼近要求的轨迹。 连续轨迹的控制实际上就是在多次执行 离散点间的点位控制，离散点点越密集越能逼近要求的运动曲线 3. 避碰问题 假设空间障碍物为半径为 r 的球体，则这些球体空间便形成了机械手臂的约束。 而躲避问题就转化成保证机械手臂上的点与障碍球球心距离始终大于 r 的问题。根据 运动规律，不难知道手臂相对于初始位置的姿态决定于之前执行的所有指令的和。已 知连-4- 杆上的点 L 在其所在的相对坐标系中的坐标(x,y,zj 及转动的指令，根据齐次坐-5- 标变换矩阵就可得到 L 在固定坐

10、标系中的坐标 x,y,z ,然后可计算距离。所以 可以 用问题1-1 算法产生点到点的指令，可以利用迭代法从初始位置开始提前检验每个指 令，不满足要求的无法执行。 4自由度分析： 由题中指出的：指尖 - E 点，具有夹工具、焊接、拧螺丝等多种功能，不过 在这里不要求考虑这方面的控制细节。则将情况理想化，不考虑第 6 个自由度对运动 的影响。 三、模型的假设 1. 各关节连杆在输入指令后同时开始转动，速度为低速，各杆件之间无摩擦, 臂各旋转轴最大运动速度相同 2. 在轨迹规划中不考虑机械臂关节转角的限制。 3. 不考虑机械臂结构和装配产生的误差。 4. 连杆为刚体，不会发生形变 5. 设机器人的

11、初始位置是在一个平面上的（y-z 平面）。 6. 关节视为质点，它们所占的面积可忽略不计。 四、符号说明 U i :坐标系，u o为固定坐标系，其余为固定在杆上的相对坐标系 i : Ui到Ujj的坐标系变换矩阵 I x :尖端移动终点在固定坐标系中的 x 方向坐标 I y :尖端移动终点在固定坐标系中的 y 方向坐标 -6- x 0 :尖端转动起点在固定坐标系中初始 x 方向坐标 yo :尖端转动起点在固定坐标系中初始 y 方向坐标 Zo :尖端转动起点在固定坐标系中初始 z 方向坐标 -第 i 个自由度的相对于初始姿态转角(i=1,2,3,4,5,6) 宀-第 i 个自由度转角的增量(i=1

12、,2,3,4,5,6) si -sin r ci - cos R 五、模型建立与求解 5.1 运动学模型建立与求解 5.1.1 机器人运动方程的建立 为了产生点到点的运动序列，首先我们先作出初始角度均为 0 时的姿态，以 每个节点为原点建立坐标系Uo到U6如图 5.1.1 oz :尖端移动终点在固定坐标系中的 z 方向坐标 -7- 图-5.1.1 从图-5.1.2 可以看出 F 点相当于将UA绕 Z 轴旋转得到0T ,同理其他节点分别相当于 将前一坐标系绕 X,X,Z,X,Z 轴转动。可得到各坐标系相对于前一个的坐标系的坐标变 换公式。从而得到机械臂各个关节的变换矩阵 ri1 ri2 ri3

13、I 旋转矩阵：AR= % % 丿31 r32 r33 _-8- AR i _ 0 图-5.1.2 :机器人连杆参数（见表格 1） Px! Py 位置矢量: -Pz cos（ ） -si n（q） 0 0 j sin（q） cos（q） 0 0 ；T 0 0 0 1 14-0 0 0 1 _ 卫 0 sin（） _C0S（V2） 0 0 0 COS但 2） 0 sin 但 2） 0 0 1 -9- .0 0 0 px 65C1C23C4S5 65S| S4 S5 65C1S23C5 255C1S23 255C1C2 p 65S23C4S -65C23C255C23 255S2 2确定固定坐标系中

14、 E 点位置1 0 0 0 1 _cos4； -s in （日 ）0 0 1 0 cos（日3） -s in （日 3）0 0 sin （04） 0cos（ e4） 0 0 0 sin （夷）。 COS（83） 1 255 0 0 1 255 卫 0 0 1 0 0 0 1 1 1 0 0 0 -cos但 6 ） -si nd） 0 0 0 sin （85） cos（日5） 0 4 0si n（B6） 0cos（日6） 0 0 0 -cos） sin （日0 0 0 1 0 0 0 1 0 0 0 1 nx Ox ax ny nz Oy Oz ay az Px Py Pz Py =65S1C2

15、3C4S5 _65C1S4S5 65S|S23C5 2550S23 255$C2 其中 nx =C1C23C4C5C - C1C23S4S6 S1S4C5C6 S1C4S _ C1S23S5S6 ny -SIC23C4C5C _ S1C23S4S6 _ C1S4C5C _ C1C4S6 _ S1S23S5S6 nz -S23C4C5C _ S23S4S3 C23S5S6 Ox =_C1C23C4C5S6 _ C1C23S4C _ S1S4C5 S3 S1C4C6 C1S23S5S3 Oy = -S1C23C4C5S6 -$1。23&56 C1S4C5S6 - C1C4C6 S1S23S

16、5S3 Oz = _S23C4C5S6 _ S23S4C6 _C23S5S3 ax =C1C23C4S5 S1S4S5 C1 SCay -S1C23C4 S5 1C1S4S5 * S1S23C5 az -S23C4S5 _C23C5 -10- 由题设中，已知固定坐标系原点，根据给定的连杆长度和 M 角度，易计算得出, 在固定坐标系中 E 位置： &=_65(-cos(s1)si n( s4)+(-si n(s1)cos(s2)cos(s3)+si n( s1)si n( s2)si n(s3)cos(s 4)si n( s5)+65(si n(s1)cos(s2)si n(s3)+si

17、 n( s1)si n(s2)cos(s3)cos(s5)+255s in (s1)cos(s2)si n( s3)+255si n( s1)si n(s2)cos(s3)+255si n( s1)si n( s2) E/=-65(-si n( s1)si n(s4)+(cos(s1)cos(s2)cos(s3)-cos(s1)si n( s2)si n( s3)cos(s4 )si n(s5)+65(-cos(s1)cos(s2)si n(s3)-cos(s1)si n(s2)cos(s3)cos(s5)-255 cos(s1)cos(s2)*si n(s3)-255cos(s1)si n(

18、 s2)cos(s3)-255cos(s1)si n(s2) Ez=140-65(si n( s2)cos(s3)+cos(s2)si n(s3)cos(s4)s in (s5)+65(-si n( s2)s in( s3)+cos(s2)cos(s3)cos(s5)-255si n( s2)si n( s3)+255cos(s2)cos(s3)+255cos (s2) 这样便得到了运动学方程。 5.1.2 利用逆运动学方法求解 （1）求 韦 可用逆变换A（K）左乘运动方程式两边得: Al佝）6 =A2（日 2）Aa（日 3）几（日4於5（%爪6但6） 根据对性元素相等可解的 解得: K =a

19、rctan卩厂鴉或*arctan Py吨 Px _6 5Px - 65ax (2)求 6 由： - nx Ox ax Px A/（ej A2 （82）A， （q） ny Oy ay Py =A4但4）人但5）人（日6） nz Oz az Pz 0 0 0 1 解得: 1 =一 arcs in a arcta n 6 二3 = arcsin a arcta n1 户 3 6 (3) 求二 2 c23(c1ax S1ay) s23az = c4s5 C23(C1Px $Py) sPz -(255 03)=65c4s 解得: si n23 w) = B = 咕 二 arcs in B - w; 或

20、23 二二 - arcsin B - w ci +$Py -65(鬼 +阿) tanw =- 其中 Pz -65az B =(255Q)十 J(q Px sy 65qax 65sy)2 +(Pz paz)2 (1) (2) -11- R - % -乜 二 arcs in B - w - J3； 或- 2 =二-arcs in B - w - J3 (4) 求屯 S23(Gax say) C23az = C5 = k =arccoss23(ciax say)C23az 或 k =2二-arccosS23(qax sy)-辭乙 (5) 求二 4 由sax -ciay =S4S，可以解得： Sax

21、-Gay a . SaxGRy 卡 r . sia - ciay s4 4 =arcs in ;或二 4 - - arcs in s5 s5 S5 (6) 求二 6 / 丄 . 口 s23(qOx +0y)C23z S23(GOx SOy) -Xz 二 sin 出： sin 日 5 a . s23(qx +$Oy)qOz 十 a . (qOx + $Oy) 一 Oz r6 二 arcs in ；或 二6 -二-arcs in sin屯 sin也 5.2 问题 1 1 的模型 在已有六自由度手臂运动方程和逆运动学解法的基础上， 若已知机械臂末端转 动终点坐标和转动起点坐标已知，就可以得到各关节的

22、坐标，连杆的相对于初始状态 的角度变化门.广行,3,忑九忑对及机械臂的姿态 5.2.1 机械臂最佳姿态的确定 对于一般六自由度机械臂，带入末端坐标（x,y,z）会得到 16 组 总忌斥），考虑用机械臂指尖实际到达的空间位置（ x , y , z ）到理想位 置（x,y,z ）的偏差距离与移动距离的比值最少来衡量机械臂是否是最佳姿态，所以 我们通过定义一个参量-偏移系数来寻找最佳指令， 其数学模型可表示为： (x-x)2 (y-y)2 (z-z)2 ,(x-沧)2 (y- y。)2 (z- %)2 x :尖端移动终点在固定坐标系中的 x 方向坐标 I y :尖端移动终点在固定坐标系中的 y 方向

23、坐标 I z :尖端移动终点在固定坐标系中的 z 方向坐标 X。：尖端转动起点在固定坐标系中初始 x 方向坐标 yo :尖端转动起点在固定坐标系中初始 y 方向坐标 Min M -12- z。：尖端转动起点在固定坐标系中初始 z 方向坐标 当尖端移动终点坐标(x, y, z)已知情况下，利用 MATLAB Robotics Tool 可 得到 T矩阵，利用逆运动学解法，可得到因指令中各个增量心i只能 取到-2, -1.9, -1.8, , 1.8, 1.9, 2 这 41 个离散值(即精度为 0.1 ，绝对值不 超过 2 )，所以最终转动的 an总九話氏均只保留小数点后一位。所以对得到的 (q

24、屯息,Q,Q)取位处理，然后回代入正方向运动学方程，解得的(x , y , z)才为 实际位置。通过对M的比较得到最佳 龙卫卫宀卫)*。 5.2.2 指令生成 在已知 K (i=1,2,3,4,5 ，6)后，只需要设计一种方式使机械臂从 于转到于。 这里根据点位机械臂运动特点选用加速一一匀速一一减速三段式控制方式。 如图 5-2 中的方式二。 萨(速度) 方式1 规定一个加速时间 T(T=theta12-theta(i-1,1) theta(i,1) =theta12; else theta(i,1) =theta11; end %theta3 theta31=asin(1)+atan(1/6

25、); theta32=pi-asin(1)+atan(1/6); if theta31-theta(i-1,3)=theta32-theta(i-1,3) theta(i,3) = theta32; else theta(i,3) = theta31; end %theta2 w=atan(c1(i,j)*px+s1(i,j)*py-65*(c1(i,j)*ax+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)A 2+(pz-p*azF2); theta2

26、1=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; else theta(i,2) = theta21; end %theta5 theta51=acos(s231(i,j)*(c1(i,j)*ax+s1(i,j)*ay)-c231(i,j)*az); -19- theta52=2*pi-acos(s232(i,j)*(c1(i,j)*ax+s1(i,j)*ay)-c232(i,j)*az); if t

27、heta51-theta(i-1,5)=b5-theta(i-1,5) theta(i,5) = theta52; else theta(i,5) = theta51; end %theta4 theta41=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 atheta41-theta(i-1,4)=theta42-theta(i-1,4) theta(i,4) = theta42; else theta(i,4) = theta41; end %thet

28、a6 theta61=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) = theta62; else theta(i,6) = theta61; end end 问题 2-1 T06=transl(20,-200,120); nx=T06(1,1);ny=T06(2,1

29、);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 %theta1 theta11=atan(py-65*ay)/(px-65*ax); theta12=pi+atan(py-65*ay)/(px-65*ax); if theta11-thet

30、a(i-1,1)=theta12-theta(i-1,1) theta(i,1) =theta12; else theta(i,1) =theta11; end %theta3 theta31=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; else theta(i,3) = theta31; end c1(i)=cos(theta(i,1); s1(i)=sin(theta(i,1); c3(i)=cos(th

31、eta(i,3); -20- s3(i)=sin(theta(i,3); c23(i)=cos(theta(i,1)+theta(i,3); s23(i)=sin(theta(i,1)+theta(i,3); %theta2 w(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; else theta(i,2) = theta21; end-21- %theta5 theta51=acos(s23(i)*(c1(i)*ax+s1(i)*ay)-c23(i)*az); theta52=2*pi-acos(s23(i)*(c1(i)*ax+s1(i)*ay)-c23(i)*az); if theta5