机械臂运动路径设计问题

1、精选优质文档-倾情为你奉上研究生试卷2016年 2017 年度第 一 学期 课 程 名 称：机器人学导论 评分： 专 业： 机械工程 年级：2016级 研 究 生 姓 名: 学号： 任课教师姓名： 注意事项 1答题必须写清题号； 2字迹要清楚，保持卷面清洁； 3试题随试卷交回； 4考试课按百分制评分，考查课按5级分制评分； 5阅完卷后，一周内将试卷、试题、成绩单由任课教师签名后， 送有关部门。机械臂运动路径设计问题自1959年美国的英格伯格和德沃尔制造出世界上第一台工业机器人“尤尼梅特”开始，近半个世纪以来，机器人的研制和应用以惊人的速度发展并取得长足的进步。当今世界，机器人的应用领域已十分广

2、泛，包括工业生产、海空探索、医疗康复和军事活动等，此外，机器人已逐渐在医院、家庭和一些服务行业获得应用。从生产车间中的焊接机械手，到水下自治式机器人，从娱乐性的拳击机器人，到伊拉克战场上的无人驾驶机，机器人已经与我们的日常生活息息相关。 机器人通常分为关节式机器人（或称机械臂、机械手、机器人操作臂、工业机器人等）和移动式机器人。一般来说，前者具有更多的自由度，而后者的作业范围则更大一些。以某型号机器人为例，其示意图见图1：第一个自由度± 180度第二个自由度± 125度第三个自由度± 138度第四个自由度± 270度第五个自由度-133.5 120度第六

3、个自由度± 270度图1 机器人结构图这种机器人一共有6个自由度，分别由六个旋转轴（关节）实现，使机器人的末端可以灵活地在三维空间中运动。为了便于分析和计算，我们对机器人结构进行简化，简化后的数据见图2和参数表1。这里用七条直线段表示机器人的七个连杆，连杆之间用所谓的旋转关节连接，已知AB140mm，BC255mm，CD255mm，DE65mm。根据旋转的方向分成两类关节，旋转轴分平行连杆的（如图1的自由度一、四、六，对应于图2中的F，G，H）和垂直连杆的（如图1的自由度二、三、五，对应于图2中的B，C，D）两种，前者如笔帽的转动方向，后者如摇柄的转动方向。每一个关节对应一个角度，这

4、个角度表示前一个连杆方向到后一个连杆方向转角（对于B，C，D），连杆方向为AB、BC、CD、DE，或者相对于初始位置的转角（对于F，G，H），假设机器人的初始位置是在一个平面上的（y-z 平面）。为了使机器人运动得更加灵活允许关节的转角超过360°的。 图2：机器人的尺寸图 机器人关于六个自由度的每一个组合，表示机械臂的一个姿态，显然每个姿态确定顶端指尖的空间位置X：。假定机器人控制系统只能够接收改变各个关节的姿态的关于连杆角度的增量指令（机器指令），使得指尖（指尖图2中的E点，具有夹工具、焊接、拧螺丝等多种功能，不过在这里不要求考虑这方面的控制细节）移动到空间点X，其中各个增量只能

5、取到-2, -1.9, -1.8, ，1.8, 1.9, 2这41个离散值（即精度为0.1°，绝对值不超过2°）。通过一系列的指令序列可以将指尖依次到达位置X0，X1，Xn，则称X0，X1，Xn为从指尖初始位置X0到达目标位置Xn的一条路径（运动轨迹）。根据具体的目标和约束条件计算出合理、便捷、有效的指令序列是机器人控制中的一个重要问题。我们约定直角坐标系的原点设在图2的A点，z轴取为AB方向，x轴垂直纸面而y轴则在基座所固定的水平台面上。1根据市场需求，机械臂制造厂打算为他们的产品研发一个软件系统，能够直接将用户的运动命令自动转换成机器指令序列。即为这类机器人设计一个通用

6、的算法，用来计算执行下面指定动作所要求的指令序列，并要求对你们算法的适用范围、计算效率以及你们的近似算法所造成的误差和增量离散取值所造成的误差大小进行讨论（不考虑其他原因造成的误差）：已知初始姿态0和一个可达目标点的空间位置（Ox, Oy, Oz），计算指尖到达目标点的指令序列。要求指尖沿着预先指定的一条空间曲线x = x(s), y = y(s), z = z(s), a s b 移动，计算满足要求的指令序列。在第个问题中，假设在初始位置与目标位置之间的区域中有若干个已知大小、形状、方向和位置的障碍物，要求机械臂在运动中始终不能与障碍物相碰，否则会损坏机器。这个问题称机械臂避碰问题，要求机械

7、臂末端在误差范围内到达目标点并且整个机械臂不碰到障碍物（机械臂连杆的粗细自己设定）。2. 应用你的算法就下面具体的数据给出计算结果，并将计算结果以三组六维的指令序列（每行6个数据）形式存放在Excel文件里，文件名定为answer1.xls，answer2.xls和answer3.xls。假设在机械臂的旁边有一个待加工的中空圆台形工件，上部开口。工件高180mm，下底外半径168mm，上底外半径96mm，壁厚8mm。竖立地固定在xy平面的操作台上，底部的中心在 (210, 0, 0)。要求机械臂（指尖）从初始位置移动到工具箱所在位置的 (20,200, 120) 处，以夹取要用的工具。如果圆台

8、形工件外表面与平面 x = 2 z 的交线是一条裂纹需要焊接，请你给出机械臂指尖绕这条曲线一周的指令序列。有一项任务是在工件内壁点焊四个小零件，它们在内表面上的位置到xy平面的投影为（320,-104）、（120,106）、（190,-125）和（255,88）。要求机械臂从圆台的上部开口处伸进去到达这些点进行加工，为简捷起见，不妨不计焊条等的长度，只考虑指尖的轨迹。3制造厂家希望通过修改各条连杆的相对长度以及各关节最大旋转角度等设计参数提高机械臂的灵活性和适用范围。请根据你们的计算模型给他们提供合理的建议。关节变量符号初始位置变量范围10± 1802-90± 12530&

9、#177; 13840± 2705-90-120 +133.560± 270参数表1 初始姿态0和动作范围机器人学导论-大作业答案12. 机械臂运动路径设计问题12.1初始姿态0的确定关节转角按照逆时针旋转选择为正，按顺时针旋转为负。约定机器人的初始位置是在一个平面上的（y-z 平面），直角坐标系的原点设在A点，z轴取为AB方向，x轴垂直纸面而y轴则在基座所固定的水平台面上。初始位置是指的初始值,其中关节1,4,6的初始值就是使机器人的各个连杆在y-z平面,关节1的初始值就是使连杆BC等在y-z平面的角度,关节4初始值就是使机器人的连杆DE在连杆AB、BC、CD所生成的平面

10、内，从而可确定机械臂的初始姿态，如图12.1所示。图12.1 机械臂的初始姿态图12.2指尖在无障碍的情形下移动到指定点,我们建立如下模型12.2.1模型一设初始点的位置为其姿态各个角度是目标点的位置为, 设为由初始姿态,经过一些列指令变换后的各旋转角的总增量,其对应的位置为.由于连杆角度的增量指令是,而且只能取到-2,-1.9,-1.8,1.8,1.9,2 这 41个离散值(即精度为0.1, 绝对值不超过2),因此为了使机器指尖与目标点能够尽可能地接近,我们有如下的目标函数: 12.2.2问题1的算法:考虑到有效的控制角度有 5个而要完成从当前点移动到目标点,在精度要求满足的情况下仅仅需要改

11、变 3个角度就可达到目标点,所以本文提出的算法是首先固定和,令它们同时为0给出控制序列即可达到目标点.由于不考虑障碍物所以机械臂的转动很自由为了使得两点之间的运动时间最短操作指令最少应该使得所有关节同时转动。这样指令的长度就仅仅是中转动角度最大的关节的转动所对应得操作序列长度。设的转动角度对应得指令序列长度是 L1 L2 L3 L4 L5 总的操作序列长度 L=max(L1、 L2、 L3、 L4、 L5)。设每个关节的目标角度与当前角度差为（i =1,2,3,4,5 ）因为角度的增量是(2, 1.9, 1.8, ,0,-0.1,-0.2 -1.8,-1.9,-2.0),所以如果, 首先应该考

12、虑取大的转角,然后依次取较小的角度增量. 直至分解完毕.如果,则角度增量(负值)从小到大分解。详细算法可通过MatLab编程实现。12.2.3问题的求解下图为从初始点运动到给定点(20, -200,120)的各臂的运动轨迹:图12.2上图中加星粗线表示的是初始状态粗线表示的是目标状态，细线是控制指令下完成全过程的运动轨迹。12.3 指尖沿指定曲线运动12.3.1问题2的算法算法一要求指尖沿着预先指定的一条空间曲线迅速x= x(s ), y= y( s), z= z( s), asb移动。设曲线的端点为 P0和Pn，不妨规定P0为起始点，Pn为运动终点，从起始点的位姿出发，以=0 或 0.1 的

13、微小变化逐步搜寻相邻位姿，并从相邻位姿所对应的指尖实际位置中选取与给定曲线最接近的作为第 1 个近似插点记为 p1 ,同时记录对应指令为p1，然后再从p1出发搜寻合适的p2和P2,依次类推直至指尖距离曲线终点位置在一定的精确度范围内.这样通过一系列的指令 p1、p2、p3、p n,，将指尖依次到达曲线上或邻近位置，从而最终实现指尖沿着预先指定的空间曲线移动。算法二首先对曲线抽样离散化这样指尖沿曲线运动就可以转化成点到点的运动通过分析和计算机模拟结果可知，由于控制角度的离散化，指尖在三维空间的运动间隔为 1mm 数量级可以说本系统指尖的分辨率是 1mm 数量级，所以曲线的抽样间隔点空间距离应该同

14、样为1mm 的数量级。本文的算法是首先用上文提出的点到点算法到达线段的端点，然后固定改变，指尖可以在一定的空间范围内沿着空间曲线运动，由于固定指尖的运动范围比较有限，超出这个范围后要计时改变，每次在超出运动范围之前对做一次大的调整反复的进行以上的步骤即可达到相当好的精确度沿着空间曲线运动12.3.2问题2的求解根据算法二，通过Matlab编程仿真得到给定曲线以及指尖实际运动轨迹的图像可以看出精确度是很高的也就是说本算法得出的仿真结果误差非常小。在计算出的一系列控制指令的作用下机械臂指尖实际运动的轨迹与给定曲线几乎完全重合以致较难分辨，故同时给出局部放大图图12.3是三维视图，图12.4是x z

15、面的放大图。 图12.3图12.4给定曲线以及指尖运动轨迹放大图经过计算，给定曲线与指尖实际运动轨迹的误差仅为 0.1223mm。12.4避障运动问题12.4.1避障问题的运动规划对于机械臂来说，运动避障问题可以描述为机械臂在障碍物环境中计算一条安全高效的路径。首先说明一下，本文研究的方法是基于机械臂的位姿空间，即机械臂的关节空间的机械臂运动规划问题的输入为机械臂和障碍物的几何要素比如机械臂的尺寸关节数量等，以及障碍物的形状、大小等，机械臂运动规划问题的输出为连接初始和目标位姿的无碰的位姿序列。下面将就运动规划问题中的几个关键要素进行具体的阐述。对于静态运动规划障碍物的所有信息均已知运动规划在

16、此基础上进行有限制的运动规划如机器人机械臂的关节角度有一定的限制范围。本文所研究的机械臂是有一定的自身限制的根据完备性来分类运动规划算法有精确的和启发式的两种前者可以看作是用穷尽的方法找出符合要求的路径或者得出无法找到路径的结论。通常是很耗费计算量的后者则着眼于怎样快速的找到一条路经，通常加了一些启发性的因素，有可能会失败，也有可能找到的路径并不是最优的工程上一般采用基于启发式的算法而精确算法则决定了问题本身算法的复杂度。12.4.2位姿空间的计算方法对于机械臂来说，一般只能在位姿空间中来考虑规划问题。所以，怎么有效地将物理空间转化为位姿空间就很重要了。这里考虑用雅可比矩阵法计算位姿空间，该方

17、法通过求得给定位姿点 q 的 Jacobi 行列式绝对值的一个上界，来得出 q 点的某个邻域为自由空间区域或者为障碍物区域。模型的建立在有障碍物的情况下，我们假设组成障碍物的所有点的集合为A。设指尖所在位置为( x1, y1, z1),其姿态各个角度是,在旋转了任意小的角度之后，机械臂的位置以及姿态发生了变化，不妨设位置为( x2, y2, z2)，其相应的姿态为。对此我们写出各个机械臂的关节点以及机械臂各段的点集。如果在旋转的任意小的角度内，机械臂和关节都与集合A不相交，则我们避开了障碍物。12.5模型的分析与讨论：本文设计的算法和模型具有以下特点1 在不影响精度的基础上将多自由度运动问题合

18、理分解成若干低自由度运动的合成，从而减少了变量控制，简化了运动方程的求解。2 抛开传统方法，提出一种新的实时运动规划方法，大大缩小了搜索空间，从而有效地提高了系统效率。3 采用先整体定位后局部精调的交替控制方法，使机械臂达到更加合理、便捷、高效的运动。附录1Answer1: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=nxnynz; o=ox oy

19、oz; a=ax ay az; p=pxpypz;theta=zeros(6,100);fori=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;elsetheta(i,1) =theta11;end%theta3theta31=asin(1)+atan(1/6);theta32=pi-asin(1)+atan(1/6);if theta3

20、1-theta(i-1,3)>=theta32-theta(i-1,3)theta(i,3) = theta32;elsetheta(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-t

21、heta(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(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) = the

22、ta52;elsetheta(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;elsetheta(i,4) = theta41;end%theta6 theta61=asin(sin(c1(i,j)*ox+s1(i,j)*oy)-c231(i,j)*oz)/s

23、511(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;elsetheta(i,6) = theta61;endendAnswer2: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,

24、3);ay=T06(2,3);az=T06(3,3); px=T06(1,4);py=T06(2,4);pz=T06(3,4); n=nxnynz; o=ox oyoz; a=ax ay az; p=pxpypz;theta=zeros(7,6);fori=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;elsetheta(i,1) =

25、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;elsetheta(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(

26、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

27、) = theta22;elsetheta(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;elsetheta(i,5) = theta51;endc5(i)=cos(theta(i,5);s5(i)=sin(theta(i,5);%theta4

28、 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;elsetheta(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);

29、if theta61-theta(i-1,6)>=theta62-theta(i-1,6)theta(i,6) = theta62;elsetheta(i,6) = theta61;endendAnswer3: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(

30、-pi/2 0 0 0 0, 'standard');L6 = link(0 0 0 65 0, 'standard');r=robot(L1 L2 L3 L4 L5 L6);='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); holdonT=fkine(r,q); sizeT=size(T); fori=1:sizeT(3) x=T(1,4,i);

31、y=T(2,4,i); z=T(3,4,i);holdon plot3(x,y,z)endplot(r,q)附录：指令序列附录1 answer1的部分结果指令序列关节1关节2 关节3关节4关节5关节61-2.0 2.0 -2.0 0.0 2.0 0.0 2-2.0 2.0 -2.0 0.0 2.0 0.0 30-2.0 2.0 -2.0 0.0 2.0 0.0 31-2.0 1.0 -2.0 0.0 2.0 0.0 36-2.0 0.0 -2.0 0.0 2.0 0.0 37-2.0 0.0 -2.0 0.0 0.4 0.0 66-2.0 0.0 -2.0 0.0 0.0 0.0 67-2.0 0.0 -1.4 0.0 0.0 0.0 87-2.0 0.0 0.0 0.0 0.0 0.0 88-0.3 0.0 0.0 0.0 0.0 0.0 备注：由于关节6的转角对机械臂指尖的位置没有影响，本文不予考虑，其转角值认定为0，省略号表示中间指令序列与上一个指令相同，具体指令序列参见answer1.xls。附录2 answer2的部分结果指尖从最初