机械臂运动路径设计分析

BatchDoc Word文档批量处理工具机械臂运动路径设计分析摘要本文探讨了六自由度机械臂从一点到另一点沿任意轨迹移动路径、一点到另一点沿着给定轨迹移动路径、以及无碰撞路径规划问题，并讨论了设计参数对机械臂灵活性和使用范围的影响，同时给出了建议。问题一：（1）首先确定初始坐标均为零时机械臂姿态，建立多级坐标系，利用空间解析几何的变换基本原理及相对坐标系的齐次坐标变换的矩阵解析方法，来建立机器人的运动系统的多级变换方程。通过逆运动学解法和构建规划，来求优化指令（2）假定机械臂初始姿态为0，曲线离散化，每个离散点作为末端位置，通过得到的相邻两点的姿态，利用（1）中算法计算所有相邻两点间的增量指令，将满足精度要求的指令序列记录下来。（3）通过将障碍物理想化为球体，将躲避问题就转化成保证机械手臂上的点与障碍球球心距离始终大于r的问题。进而通过迭代法和指令检验法，剔除不符合要求的指令，从而实现避障的目的 问题二：将问题二中的实例应用到问题一中的相对应的算法中，部分结果见附录 问题三：灵活性与适用范围相互制约，只能根据权重求得较优连杆长度。关键词：多级坐标变换 逆运动学解法 优化 指令检验 一、问题重述1.1 问题背景 某型号机器人（图示和简化图略）一共有6个自由度，分别由六个旋转轴（关节）实现，使机器人的末端可以灵活地在三维空间中运动。机器人关于六个自由度的每一个组合，表示机械臂的一个姿态，显然每个姿态确定顶端指尖的空间位置X：。假定机器人控制系统只能够接收改变各个关节的姿态的关于连杆角度的增量指令（机器指令），使得指尖（指尖具有夹工具、焊接、拧螺丝等多种功能，不过在这里不要求考虑这方面的控制细节）移动到空间点X，其中各个增量只能取到-2, -1.9, -1.8, ，1.8, 1.9, 2这41个离散值（即精度为0.1，绝对值不超过2）。通过一系列的指令序列可以将指尖依次到达位置X0，X1，Xn，则称X0，X1，Xn为从指尖初始位置X0到达目标位置Xn的一条路径（运动轨迹）。根据具体的目标和约束条件计算出合理、便捷、有效的指令序列是机器人控制中的一个重要问题。假设机器人的初始位置在y-z平面上，约定直角坐标系的原点设在图示的A点，z轴取为AB方向，x轴垂直纸面而y轴则在基座所固定的水平台面上 1.2 目标任务 问题一：设计一个通用的算法，用来计算执行下面指定动作所要求的指令序列，并要求对算法的适用范围、计算效率以及近似算法所造成的误差和增量离散取值所造成的误差大小进行讨论（不考虑其他原因造成的误差）：（1）已知初始姿态0和一个可达目标点的空间位置（Ox, Oy, Oz），计算指尖到达目标点的指令序列。（2）要求指尖沿着预先指定的一条空间曲线x = x(s), y = y(s), z = z(s), a s b 移动，计算满足要求的指令序列。（3）在第个问题中，假设在初始位置与目标位置之间的区域中有若干个已知大小、形状、方向和位置的障碍物，要求机械臂在运动中始终不能与障碍物相碰，否则会损坏机器。这个问题称机械臂避碰问题，要求机械臂末端在误差范围内到达目标点并且整个机械臂不碰到障碍物（机械臂连杆的粗细自己设定）。问题二：应用你的算法就下面具体的数据给出计算结果，并将计算结果以三组六维的指令序列（每行6个数据）形式存放在Excel文件里，文件名定为answer1.xls，answer2.xls和answer3.xls。假设在机械臂的旁边有一个待加工的中空圆台形工件，上部开口。工件高180mm，下底外半径168mm，上底外半径96mm，壁厚8mm。竖立地固定在xy平面的操作台上，底部的中心在 (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.各关节连杆在输入指令后同时开始转动，速度为低速，各杆件之间无摩擦，臂各旋转轴最大运动速度相同2.在轨迹规划中不考虑机械臂关节转角的限制。3.不考虑机械臂结构和装配产生的误差。4.连杆为刚体，不会发生形变5.设机器人的初始位置是在一个平面上的（y-z 平面）。6.关节视为质点，它们所占的面积可忽略不计。四、符号说明：坐标系，为固定坐标系，其余为固定在杆上的相对坐标系：到的坐标系变换矩阵:尖端移动终点在固定坐标系中的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五、模型建立与求解5.1运动学模型建立与求解5.1.1机器人运动方程的建立 为了产生点到点的运动序列, 首先我们先作出初始角度均为0时的姿态，以每个节点为原点建立坐标系到如图5.1.1。图-5.1.1从图-5.1.2可以看出F点相当于将绕Z轴旋转得到，同理其他节点分别相当于将前一坐标系绕X,X,Z,X,Z轴转动。可得到各坐标系相对于前一个的坐标系的坐标变换公式。从而得到机械臂各个关节的变换矩阵旋转矩阵：位置矢量：图-5.1.2：机器人连杆参数（见表格1）： 其中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)这样便得到了运动学方程。5.1.2 利用逆运动学方法求解（1）求可用逆变换左乘运动方程式两边得：根据对性元素相等可解的解得:（2）求由：解得:（3）求 （1） （2）解得：其中 （4）求 或（5）求由，可以解得：或（6）求；或5.2问题11的模型在已有六自由度手臂运动方程和逆运动学解法的基础上，若已知机械臂末端转动终点坐标和转动起点坐标已知，就可以得到各关节的坐标，连杆的相对于初始状态的角度变化及机械臂的姿态。5.2.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）后，只需要设计一种方式使机械臂从转到。这里根据点位机械臂运动特点选用加速匀速减速三段式控制方式。如图52中的方式二。规定一个加速时间T(T=theta12-theta(i-1,1) theta(i,1) =theta12; else theta(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; 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 %theta5theta51=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-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 %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 %theta5theta51=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问题2-1L1 = 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 onT=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)endplot(r,q)BatchDoc Word文档批量处理工具