机械臂运动路径设计问题 中科院光电技术研究所 魏凌 郑翰清 郑清文

题号 B 全国第四届研究生数学建模竞赛 题 目 机械臂运动路径设计问题 摘 要：本文讨论了在给定环境的障碍物，以及起始和目标的位置的条件下，选择一条从起始点到目标点的路径，使机器人能够安全、无碰撞地通过所有的障碍的问题，即：避碰规划问题。文中采用空间解析几何的变换基本原理及坐标变换的矩阵解析方法，来建立机器人的运动系统的多级变换方程。根据每次移动时，不同连杆对指尖位置变化影响不同，根据优先权调节各个角度的值，采用这种搜索算法来寻求最优路径。并为提高搜索速率，降低搜索次数，采取通过改变角度变化步长对搜索算法进行优化，使针尖最初大幅度靠近目标点，后通过微调逐步逼近。在讨论沿曲线移动的问题时，加入对两点间最短路径的微分求最极值的处理方法。建立Z空间，使用0-1矩阵来表示障碍空间，采用基于直线生成的方法避碰。在计算中采用提前计算的方法预知下一步到达的目标点来处理避碰。本文最大的特点在于（1）通过改变角度变化步长对搜索算法进行优化；（2）基于Z空间的自由空间法处理避碰问题。关键词:避碰规划 坐标变换 最优路径 Z空间参赛密码 （由组委会填写）参赛队号 8015101 参赛学校 中科院光电技术研究所 参赛队员姓名 魏凌，郑翰清，郑清文 一、问题重述当今世界，机器人的应用领域十分广泛。机器人通常分为关节式机器人和移动式机器人。我们这里讨论有6个自由度的关节式机器人，6个自由度分别由六个旋转轴实现，使机器人的末端可以灵活地在三维空间中运动。为方便分析和计算，对机器人结构简化，用七条直线段表示机器人的七个连杆，连杆之间用所谓的旋转关节连接，给定每根连杆的长度。根据旋转的方向分成两类关节，旋转轴分平行连杆和垂直连杆。每个关节对应一个角度，这个角度用来表示前一个连杆方向到后一个连杆方向转角或者相对于初始位置的转角。给定机器人的初始位置。机器人关于六个自由度的每一个组合表示机械臂的一个姿态，每个姿态确定顶端指尖的空间位置。假定机器人控制系统只能够接收改变各个关节的姿态的关于连杆角度的增量指令，使得指尖移动到空间中指定，限定各个增量的取值范围和精度。通过一系列的指令序列便可将指尖依次到达相应位置。根据具体的目标和约束条件计算出合理、便捷、有效的指令序列是机器人控制中的一个重要问题。同时，题目中约定直角坐标系的原点设定位置。1为这类机器人设计一个通用的算法，用来计算执行下面指定动作所要求的指令序列，并对算法的适用范围、计算效率以及近似算法所造成的误差和增量离散取值所造成的误差大小进行讨论：已知初始姿态0和一个可达目标点的空间位置（Ox, Oy, Oz），计算指尖到达目标点的指令序列。要求指尖沿着预先指定的一条空间曲线x = x(s), y = y(s), z = z(s), a s b 移动，计算满足要求的指令序列。在第个问题中，假设在初始位置与目标位置之间的区域中有若干个已知大小、形状、方向和位置的障碍物，要求机械臂在运动中始终不能与障碍物相碰，否则会损坏机器。这个问题称机械臂避碰问题，要求机械臂末端在误差范围内到达目标点并且整个机械臂不碰到障碍物（机械臂连杆的粗细自己设定）。2. 应用所得的算法就下面具体的数据给出计算结果。假设在机械臂的旁边有一个待加工的中空圆台形工件，上部开口。工件高180mm，下底外半径168mm，上底外半径96mm，壁厚8mm。竖立地固定在xy平面的操作台上，底部的中心在 (210, 0, 0)。要求机械臂（指尖）从初始位置移动到工具箱所在位置的 (20,200, 120) 处，以夹取要用的工具。如果圆台形工件外表面与平面 x = 2 z 的交线是一条裂纹需要焊接，请你给出机械臂指尖绕这条曲线一周的指令序列。有一项任务是在工件内壁点焊四个小零件，它们在内表面上的位置到xy平面的投影为（320,-104）、（120,106）、（190,-125）和（255,88）。要求机械臂从圆台的上部开口处伸进去到达这些点进行加工，为简捷起见，不妨不计焊条等的长度，只考虑指尖的轨迹。3制造厂家希望通过修改各条连杆的相对长度以及各关节最大旋转角度等设计参数提高机械臂的灵活性和适用范围。请根据计算模型给他们提供合理的建议。二、基本假设1. 连杆横截面积设为2mm*2mm。2关节视为质点，它们所占的面积可忽略不计。3. 假设连杆为刚体，不考虑形变。三、问题分析机械臂运动路径设计问题主要涉及到机器人运动学分析、机器人运动路径的算法确定以及机器人避碰问题。1运动系统的建立从机构学观点来看，机器人属于空间机构范畴，因此，采用空间解析几何的变换基本原理及坐标变换的矩阵解析方法，来建立机器人的运动系统的多级变换方程。由于旋转轴涉及到平行连杆和垂直连杆两类，因此对于各类旋转变换，所使用的变换矩阵也不相同。同时，此题中涉及的机器人有6个自由度，则从工件的坐标位置到固定坐标系的变换要经过多级坐标变换。采用多级坐标变换的方法。由上述三点，便可建立机器人运动系统的坐标变换关系式。2搜索算法的确定题中给定机器人的七个连杆的长度，关节类别以及可旋转的角度和控制系统每次可改变的增量。采用递归搜索的算法。在递归中，根据每次移动时，对E点位置变化影响最大的连杆取最大权重，优先调节该角度的值，这种由主到次调节支杆的方法能使得针尖由初始点尽快地接近目标点。尽管如此，如果仅仅以此方法进行搜索，由于移动时各个角度增量的精度为0.1，计算量仍非常庞大。为提高搜索速率，降低搜索次数，通过改变角度变化步长以加速算法，使针尖E最初大幅度移动，后通过微调逐步逼近目标点。3避碰问题对于避碰问题，采用基于Z空间的自由空间法。Lazona-Perze提出基于Z空间的自由空间法。以机械臂的关节轴为坐标系建立Z空间(Configuration Space)，将障碍物映射到Z空间，形成Z空间障碍，那么在Z空间内Z空间障碍的补集。就对应自由空间， 自由空间内的点代表不与障碍干涉的机器人构形。而Z空间障碍内的点代表与障碍干涉的机器人构形。应用搜索算法，避碰规划就转化为在自由空间内寻找连接初始点和目标点的路径问题。避碰路径规划问题不仅要考虑末端是否与障碍碰撞，还要考虑机械臂的各个连杆是否与障碍碰撞，因此，仅考虑直角坐标空间中末端与障碍的空间关系是不够的。如果用Z空间描述机器人的运动，此时，机器人的状态与Z空间形成一一对应的关系。如果将障碍也用Z空间来描述，并将所有的障碍都映射到Z空间，那么，无碰路径规划问题就转化为在Z空间中寻找一条连接始点和目标点的自由区域，当机器人沿该路径运动时，能保证不与障碍发生碰撞。基于Z空间避碰规划的自由空间法的一般步骤是：首先，要建立Z空间；其次，确定单元的连通性，建立搜索树；然后，确定搜索方法；最后，进行路径优化。机器人的七个连杆的长度和关节移动范围已知，根据这个条件可确定出机器人的最大活动范围。为判断机器人是否与障碍物相碰，将最大活动范围空间离散化，分为由确定大小的单位单元格的集合。在使用计算机搜索时，使用矩阵存贮单元格的状态。将障碍物存在处定义为1，不存在处定义为0。即将自由空间与障碍物所处分离开来。要求机械臂在运动中始终不能与障碍物相碰，因此，必须考虑避碰问题。在确定搜索算法时，使用权重影响最大的递归算法。为解决这一问题，对上述权重影响最大的递归算法进行改进，当每准备进行一次移动时，提前判断机械臂下一次将会到达的位置，如果下次到达的位置会与障碍相碰，则改变机械臂搜索的权重，取第二影响最大因素作为移动权重，重新搜索下一运动位置。4.自由度分析：由题中指出的：指尖E点，具有夹工具、焊接、拧螺丝等多种功能，不过在这里不要求考虑这方面的控制细节。则将情况理想化，不考虑第6个自由度对运动的影响。四、符号的说明-从固定坐标系移动到目标坐标系的距离-固定坐标系的原点-第i个自由度的转角(i=1,2,3,4,5,6)-第i个自由度转角的增量(i=1,2,3,4,5,6)-第j次变换的变换矩阵（j=1,2,3,4,5,6,7,8,9）-目标坐标系的原点-连杆AB的长度-连杆BC的长度-连杆CD的长度-连杆DE的长度五、模型建立St. -22(i=1,2,3,4,5,6)=140mm，255mm，255mm，65mm-180180，-125125，-138138，-270270，-120133.5，-270270六、模型求解1机器人运动方程的建立（图一：坐标系变换）如图示，从机座的固定坐标系的原点O，经过绕Z轴回转变换，沿Z轴的平移变换,绕轴回转变换,沿轴平移,绕轴回转变换，绕轴回转变换,沿轴平移变换,绕轴回转变换,沿轴平移变换最终变换到目标位置。由题设已知目标坐标系为，设定固定坐标系为。由题设可以求出相应的变换矩阵分别为：， ， ，,，,建立运动方程：2确定固定坐标系中E点位置由题设中，已知固定坐标系原点，根据给定的连杆长度和角度，易计算得出，在固定坐标系中E点位置： -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)+255*sin(s1)*cos(s2)*sin(s3)+255*sin(s1)*sin(s2)*cos(s3)+255*sin(s1)*sin(s2) -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)-255*cos(s1)*sin(s2)*cos(s3)-255*cos(s1)*sin(s2) 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)-255*sin(s2)*sin(s3)+255*cos(s2)*cos(s3)+255*cos(s2)3问题1的和问题2的（1）问题1的在解决问题1的时，不考虑避碰问题。题中给定机器人的七个连杆的长度，关节类别以及可旋转的角度和控制系统每次可改变的增量。采用递归搜索的算法。在递归中，根据每次移动时，对E点位置变化影响最大的连杆取最大权重，优先调节该角度的值，这种由主到次调节支杆的方法能使得针尖由初始点尽快地接近目标点。尽管如此，如果仅仅以此方法进行搜索，由于移动时各个角度增量的精度为0.1，计算量仍非常庞大。为提高搜索速率，降低搜索次数，通过改变角度变化步长以加速算法，使针尖E最初大幅度移动，后通过微调逐步逼近目标点。已知各个增量只能取到-2, -1.9, -1.8, ，1.8, 1.9, 2这41个离散值（即精度为0.1，绝对值不超过2），那么的步长最小为0.1，最大为2。当连杆水平拉伸到极点，我们用的步长最大为2作粗略范围估算：。为保证其有效性，我们取。对加速度ratio进行解析，相应的伪代码：If ratio20 ratio=20Else If 1ratio20 return Else If ratio1 ratio=1搜索算法流程图：初始化域值判断距离是否大于阈值是：则取大步长搜索否：取小步长搜索确定各个角度变化对距离变化的影响权重。按权重由大到小，改变相应角依次搜索判断结果是否小于误差精度要求是：结束否(图二：问题1的算法流程图)（2）问题2的根据（1）中的算法，将问题2的的数据代入该算法中，得出结果，见附件一answer1.xls.用MATLAB对针尖运动路径作图分析如下：(图三：问题2的的针尖运动路径图)模型结果的分析：从answer1的数据可以看出，对针尖E的影响由大到小可排序为：这是与目标点的初始位置E与目标位置E的相对位置相符合的。4.问题1的、和问题2的、（1）问题1的、已给定一条空间曲线x = x(s), y = y(s), z = z(s), a s b 。由于代入计算机运算。我们将空间曲线离散化，取为若干个离散点，对离散点进行计算。即：将目标坐标由中的一固定点变化为若干个满足空间曲线分布的离散点的集合。若根据问题1的的算法即逐步逼近的方法来计算，则会存在两点之间逼近过程中出现迂回的现象。现对此算法进行优化。目标坐标系中存在的关系。为求得两点的最短路径，作如下计算： ，是，的参数。则对，求偏导，有即： 即： 算法的流程图：将曲线上的点离散化为n个（）计算出当前点的坐标和目标点坐标计算坐标差向量计算A矩阵计算A的广义逆阵计算调整值使指尖E从点处移动到判断k是否等n是：结束 否：K=k+1(图四：问题1的算法流程图)同时，在中，考虑避碰问题。将机械臂进行拉伸，考虑机械臂的最大活动范围。下面分析矩阵大小的选择：因为连杆在x、y方向的极限长度为，设定一个区间为：400mm*400mm*400mm.设定单位小区间为：2mm*2mm*2mm.考虑与设定单位小区间为：1mm*1mm*1mm相比，用这个数据有利于节省计算机内存。这也是我们将连杆横截面积取为2mm*2mm*2mm的原因。将此空间用矩阵来描述。在障碍物存在的地方，设定取值为1，否则，设定取值为0。这样建立一个Z空间来处理避碰问题。（2）问题2的根据题设给定圆台以及平面x = 2 z ,确定圆台形工件外表面与平面 x = 2 z 的交线。圆台形工件外表面公式为：，与平面的交线为。化成参数方程形式：将交线上的点离散化，根据问题1的的算法，代入该交线的数据，得出结果，见附件二answer2.xls.对结果进行分析，并用MATLAB对针尖运动路径作图如下：（由于是立体图形，所以用两个图显示两个不同的方位示意）(图五：问题2的的针尖运动路径图视图1)(图六：问题2的的针尖运动路径图视图2)（3）问题2的根据上述两点的分析结果和算法，可得内表面的曲面方程为： （）四个焊接点坐标：（320，-104，21.5492）（120，-106，52.3651）（190，-125，83.5253）（255，88，152.9044）建立三维矩阵空间建立障碍物的Z空间（即将障碍物所在处赋值为1，其余赋为0）按问题1的中优化搜索算法进行搜索并在此加以改进。每次移动E点前计算在该坐标处，A,B,C,D的坐标，判断其连线AB,BC,CD,DE上是否有点处值为1（即与障碍物相碰）否：移动E点到目标点排除该优先级继续搜索(图七：问题2的算法流程图) 将数据代入计算，可得结果见answer3.xls对结果进行分析，并用MATLAB对针尖运动路径作图如下：（由于是立体图形，所以用两个图显示两个不同的方位示意）(图八：问题2的的针尖运动路径图视图1)(图九：问题2的的针尖运动路径图视图2)5问题3考虑修改各条连杆的相对长度以及各关节最大旋转角度等设计参数提高机械臂的灵活性和适用范围。（1）从灵活性角度考虑，连杆每作一次移动，产生的变化，则相对的角度也会出现变动。要提高灵活性即是要提高角度变化向量的变化范围。这里我们考虑能量最省的优化原则。则用角度变化向量的二范数来判定。（2）适用范围：在所有杆的长度之和为固定值的情况下，指尖点E所能扫过的最大空间体积。灵活性和适用范围存在的一些相互制约的关系，根据本文的分析结果，我们建议越靠近基座的值精度高一些，使得E点所能扫过的空间点数更多。七、模型推广与评价1模型的优点(1) 通过求解整个Z空间，使得自由空间、障碍空间一目了然，因而，可按任何性能指标搜索路径且具有完备解。(2)通过权重来判断影响因素最大的方法来选择搜索最优路径，有利于节省能量。(3)用矩阵来刻划Z空间，可提高精度和计算速度。(4)通过曲线的参数方程直接得到值的增量，精度比较高，连续性比较好。2模型的缺点(1)在焊接裂纹的时候没有考虑焊接角度即杆DE与曲线法线方向所成的角度，实际上此夹角越大越容易造成对圆台壁的损伤，相应的焊接效果越差。(2)障碍的Z空间只能通过离散化求得，而难获得Z空间障碍的解析表达式，因而，为了提高精度要占用大量的内存。(3)对于关节敬较多的机械臂，Z空间自由路径的搜索也将花费较多的时间。(4)在搜索时，由于自由度的牵连关系，导致在搜索时可能会生成锯齿形路径。八、参考文献1黄献龙, 梁斌, 吴宏鑫. 机器人避碰规划综述. 航天控制. 2002.1: 34-462张建民. 工业机器人. 北京理工大学出版社. 19883姜启源. 数学模型. 高等教育出版社. 19954张贤达. 矩阵分析与应用. 清华大学出版社. 20055王晓东. 计算机算法设计与分析. 电子工业出版. 20066Donald Hearn, M.Pauline Baker. 计算机图形学. 电子工业出版社. 2002九、附录程序说明：pasrch.h及pasrch.cpp为本程序的核心，我们在这两个文件中整合了搜索、曲线运动、避障等算法。以下为函数的说明：1-4 为合并排序的函数1. int partition(double s, int index, int p, int r);这是在合并排序的分划函数2. void swap(double &si, double &sj);这是合并排序的对双精度型的交换函数3. void swap(int &si, int &sj); 这是合并排序的对整型的交换函数4. void sort(double s, int index, int p, int r);这是合并排序算法的主程序5-6 为得到C、D、E点座标的函数5. inline cord3D getptc(double s5);此为通过值得到C点座标的函数。6. inline cord3D getptd(double s5);此为通过值得到D点座标的函数。7. inline cord3D getpte(doub