21题机械臂运动

机械臂运动路径设计问题 数理学院            1019131125            刘修远 一、问题重述 1. 问题背景 当今世界，机器人的应用领域十分广泛。机器人通常分为关节式机器人和移动式机器人。我们这里讨论有6个自由度的关节式机器人，6个自由度分别由六个旋转轴实现，使机器人的末端可以灵活地在三维空间中运动。 为方便分析和计算，对机器人结构简化，用七条直线段表示机器人的七个连杆，连杆之间用所谓的旋转关节连接，给定每根连杆的长度。根据旋转的方向分成两类关节，旋转轴分平行连杆和垂直连杆。每个关节对应一个角度，这个角度用来表示前一个连杆方向到后一个连杆方向转角或者相对于初始位置的转角。给定机器人的初始位置。 机器人关于六个自由度的每一个组合表示机械臂的一个姿态，每个姿态确定顶端指尖的空间位置。假定机器人控制系统只能够接收改变各个关节的姿态的关于连杆角度的增量指令，使得指尖移动到空间中指定，限定各个增量的取值范围和精度。通过一系列的指令序列便可将指尖依次到达相应位置。根据具体的目标和约束条件计算出合理、便捷、有效的指令序列是机器人控制中的一个重要问题。同时，题目中约定直角坐标系的原点设定位置。 2． 所求目标 问题一：设计一个通用的算法，用来计算执行下面指定动作所要求的指令序列，并要求对算法的适用范围、计算效率以及近似算法所造成的误差和增量 离散取值所造成的误差大小进行讨论（不考虑其他原因造成的误差）： （1）已知初始姿态Φ0和一个可达目标点的空间位置（Ox, Oy, Oz），计算指尖到达目标点的指令序列。 （2）要求指尖沿着预先指定的一条空间曲线x = x(s), y = y(s), z = z(s), a ≦ s ≦b 移动，计算满足要求的指令序列。 （3）在第①个问题中，假设在初始位置与目标位置之间的区域中有若干个已知大小、形状、方向和位置的障碍物，要求机械臂在运动中始终不能与障碍物相碰，否则会损坏机器。这个问题称机械臂避碰问题，要求机械臂末端在误差范围内到达目标点并且整个机械臂不碰到障碍物（机械臂连杆的粗细自己设定）。 问题二：应用你的算法就下面具体的数据给出计算结果 假设在机械臂的旁边有一个待加工的中空圆台形工件，上部开口。工件高180mm，下底外半径168mm，上底外半径96mm，壁厚8mm。竖立地固定在x－y平面的操作台上，底部的中心在 (210, 0, 0)。 ①．要求机械臂（指尖）从初始位置移动到工具箱所在位置的 (20,－200, 120) 处，以夹取要用的工具。 ②．如果圆台形工件外表面与平面x =2z的交线是一条裂纹需要焊接，请你给出机械臂指尖绕这条曲线一周的指令序列。 ③．有一项任务是在工件内壁点焊四个小零件，它们在内表面上的位置到x-y平面的投影为（320,-104）、（120,106）、（190,-125）和（255,88）。要求机械臂从圆台的上部开口处伸进去到达这些点进行加工，为简捷起见，不妨不计焊条等的长度，只考虑指尖的轨迹 问题三：制造厂家希望通过修改各条连杆的相对长度以及各关节最大旋转角度等设计参数提高机械臂的灵活性和适用范围。请根据你们的计算模型给他们提供合理的建议。 二、模型分析 机械臂运动路径设计问题主要涉及到相对坐标系坐标变换、运动学分析、逆运动学求解、优化以及机器人避碰问题。 1．运动方程的建立 从机构学观点来看，机器人属于空间机构范畴。杆件每次转动因此，采用空间解析几何的变换基本原理及坐标变换的矩阵解析方法，来建立机器人的运动系统的多级变换方程。 由于旋转轴涉及到平行连杆和垂直连杆两类，因此对于各类旋转变换，所使用的变换矩阵也不相同。 同时，此题中涉及的机器人有6个自由度，则从工件的坐标位置到固定坐标系的变换要经过多级坐标变换。采用多级坐标变换的方法。 由上述三点，便可建立机器人运动系统的坐标变换关系式。 2．尖端轨迹曲线模型的建立 对于已给定一条空间曲线x=(s),y=(y),z=(z),可将其看成一个点的集合。因此机械臂实现一个空间轨迹的过程是实现轨迹离散的过程。如果这些离散点间距很大，机械臂轨迹与要求的轨迹就有较大误差。只有这些离散点彼此很接近，才有可能使机械臂的轨迹以满足要求精确度逼近要求的轨迹。连续轨迹的控制实际上就是在多次执行离散点间的点位控制，离散点点越密集越能逼近要求的运动曲线。 3．避碰问题 假设空间障碍物为半径为r的球体，则这些球体空间便形成了机械手臂的约束。而躲避问题就转化成保证机械手臂上的点与障碍球球心距离始终大于r的问题。根据运动规律，不难知道手臂相对于初始位置的姿态决定于之前执行的所有指令的和。已知连杆上的点L在其所在的相对坐标系中的坐标 及转动的指令，根据齐次坐标变换矩阵就可得到L在固定坐标系中的坐标 ，然后可计算距离。所以 可以用问题1-1算法产生点到点的指令，可以利用迭代法从初始位置开始提前检验每个指令，不满足要求的无法执行。 4.自由度分析： 由题中指出的：指尖———E点，具有夹工具、焊接、拧螺丝等多种功能，不过在这里不要求考虑这方面的控制细节。则将情况理想化，不考虑第6个自由度对运动的影响。 三、模型的假设与符号说明 1．基本假设 （1）各关节连杆在输入指令后同时开始转动，速度为低速，各杆件之间无摩擦，臂各旋转轴最大运动速度相同 （2）在轨迹规划中不考虑机械臂关节转角的限制。 （3）不考虑机械臂结构和装配产生的误差。 （4）连杆为刚体，不会发生形变    （5）设机器人的初始位置是在一个平面上的（y-z 平面）。 （6）关节视为质点，它们所占的面积可忽略不计。 2.符号说明 ：坐标系， 为固定坐标系，其余为固定在杆上的相对坐标系 ： 到 的坐标系变换矩阵 :尖端移动终点在固定坐标系中的x方向坐标 ：尖端移动终点在固定坐标系中的y方向坐标 ：尖端移动终点在固定坐标系中的z方向坐标 ：尖端转动起点在固定坐标系中初始x方向坐标 ：尖端转动起点在固定坐标系中初始y方向坐标                                                            ：尖端转动起点在固定坐标系中初始z方向坐标 ------第i个自由度的相对于初始姿态转角(i=1,2,3,4,5,6) -----第i个自由度转角的增量(i=1,2,3,4,5,6) si------sin ci------cos 四．模型建立 1．机器人运动方程的建立                                                                                 为了产生点到点的运动序列, 首先我们先作出初始角度均为0时的姿态，以每个节点为原点建立坐标系 到 如图 从下图可以看出F点相当于将 绕Z轴旋转得到 ，同理其他节点分别相当于将前一坐标系绕X,X,Z,X,Z轴转动。可得到各坐标系相对于前一个的坐标系的坐标变换公式。从而得到机械臂各个关节的变换矩阵 旋转矩阵：           位置矢量：     机器人连杆参数： 机械臂参数 关节 变量符号 初始位置 变量范围 1 0 ± 180 2 -90 ± 125 3 0 ± 138 4 0 ± 270 5 -90 -120 +133.5 6 0 ± 270         其中 2．确定固定坐标系中E点位置 由题设中，已知固定坐标系原点，根据给定的连杆长度和 角度，易计算得出，在固定坐标系中E位置： Ex=-65(-cos(s1)sin(s4)+(-sin(s1)cos(s2)cos(s3)+sin(s1)sin(s2)sin(s3))cos(s4))sin(s5)+65(sin(s1)cos(s2)sin(s3)+sin(s1)sin(s2)cos(s3))cos(s5)+255sin(s1)cos(s2)sin(s3)+255sin(s1)sin(s2)cos(s3)+255sin(s1)sin(s2) Ey=-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)-255cos(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)+cos(s2)cos(s3))cos(s5)-255sin(s2)sin(s3)+255cos(s2)cos(s3)+255cos(s2) 这样便得到了运动学方程。 五．模型求解 1. 利用逆运动学方法求解 （1）求 可用逆变换 左乘运动方程式两边得： 根据对性元素相等可解的 解得: （2）求 由： 解得: （3）求 （1） （2） 解得： 其中        （4）求 或 （5）求 由 ，可以解得： 或 （6）求 ；或 2.问题1的三个分问的模型 问题1—1的模型 在已有六自由度手臂运动方程和逆运动学解法的基础上，若已知机械臂末端转动终点坐标和转动起点坐标已知，就可以得到各关节的坐标，连杆的相对于初始状态的角度变化 及机械臂的姿态。 机械臂最佳姿态的确定 对于一般六自由度机械臂，带入末端坐标（x,y,z）会得到16组 ，考虑用机械臂指尖实际到达的空间位置（ , , ）到理想位置（x,y,z）的偏差距离与移动距离的比值最少来衡量机械臂是否是最佳姿态，所以我们通过定义一个参量－偏移系数来寻找最佳指令， 其数学模型可表示为： Min :尖端移动终点在固定坐标系中的x方向坐标 ：尖端移动终点在固定坐标系中的y方向坐标 ：尖端移动终点在固定坐标系中的z方向坐标 ：尖端转动起点在固定坐标系中初始x方向坐标 ：尖端转动起点在固定坐标系中初始y方向坐标                                                            ：尖端转动起点在固定坐标系中初始z方向坐标 当尖端移动终点坐标（x，y，z）已知情况下，利用MATLAB Robotics Tool可得到T矩阵，利用逆运动学解法，可得到 ，因指令中各个增量 只能取到-2, -1.9, -1.8, ┅，1.8, 1.9, 2这41个离散值（即精度为0.1°，绝对值不超过2°），所以最终转动的 均只保留小数点后一位。所以对得到的 取位处理，然后回代入正方向运动学方程，解得的（ , , ）才为实际位置。通过对M的比较得到最佳 。 5.2.2  指令生成 在已知 （i=1,2,3,4,5，6）后，只需要设计一种方式使机械臂从 转到 。这里根据点位机械臂运动特点选用加速——匀速——减速三段式控制方式。如图5－2中的方式二。 规定一个加速时间T(T=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 %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)^2+(pz-p*az)^2); 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; else theta(i,2) = theta21; end %theta5 theta51=acos(s231(i,j)*(c1(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; 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 %theta6 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程序 T06=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          %theta1 theta11=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; 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(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)); %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)*ay)^2+(pz-1*az)^2); 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 %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 theta51-theta(i-1,5)>=theta52-theta(i-1,5) theta(i,5) = theta52; else theta(i,5) = theta51; end c5(i)=cos(theta(i,5)); s5(i)=sin(theta(i,5)); %theta4 theta41=asin((s5(i)*ax-c1(i)*ay)/s5(i)); theta42=pi-asin((s1(i)*ax-c1(i)*ay)/s5(i)); if theta41-theta(i-1,4)>=theta42-theta(i-1,4) theta(i,4) = theta42; else theta(i,4) = theta41; end %theta6 theta61=asin((sin(c1(i)*ox+s1(i)*oy)-c23(i)*oz)/s5(i)); theta62=pi-asin((s23(i)*(c1(i)*ox+s1(i)*oy)-c23(i)*oz)/s5(i)); if theta61-theta(i-1,6)>=theta62-theta(i-1,6) theta(i,6) = theta62; else theta(i,6) = theta61; end end 附件3程序 L1 = link([ pi/2 0  0  140 0], 'standard'); L2 = link([ 0  225 0  0  0], 'standard'); L3 = link([-pi/2 0  0  0  0], 'standard'); L4 = link([pi/2 0  0  225 0], 'standard'); L5 = link([-pi/2 0  0  0  0], 'standard'); L6 = link([0    0  0  65  0], 'standard'); r=robot({L1  L2  L3 L4 L5 L6}); r.name='robot'; drivebot(r); q=[0  0  -pi/2  0  -pi/2 0]; q1=[-1.4713 1.0647 -0.7557 0  -0.3072  0]; qi=q1+q; t=0:0.056:15;        q=jtraj(q,qi,t);        hold on T=fkine(r,q);          sizeT=size(T);          for  i=1:sizeT(3) x=T(1,4,i); y=T(2,4,i); z=T(3,4,i); hold on plot3(x,y,z) end plot(r,q)