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

全国第四届研究生数学建模竞赛题号 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≦谢谢阅读≦b移动，计算满足要求的指令序列。③．在第①个问题中，假设在初始位置与目标位置之间的区域中有若干个已知大小、形状、方向和位置的障碍物，要求机械臂在运动中始终不能与障碍物相碰，否则会损坏机器。这个问题称机械臂避碰问题，要求机械臂末端在误差范围内到达目标点并且整个机械臂不碰到障碍物（机械臂连杆的粗细自己设定）。感谢阅读应用所得的算法就下面具体的数据给出计算结果。假设在机械臂的旁边有一个待加工的中空圆台形工件，上部开口。工件高精品文档放心下载180mm，下底外半径168mm，上底外半径96mm，壁厚8mm。竖立地固定谢谢阅读在xy－平面的操作台上，底部的中心在(210,0,0)。感谢阅读①．要求机械臂（指尖）从初始位置移动到工具箱所在位置的(20,－200,精品文档放心下载120)处，以夹取要用的工具。②．如果圆台形工件外表面与平面x=2z的交线是一条裂纹需要焊接，请你给出机械臂指尖绕这条曲线一周的指令序列。谢谢阅读③．有一项任务是在工件内壁点焊四个小零件，它们在内表面上的位置到谢谢阅读,.xy 平面的投影为（320,-104）、（120,106）、（190,-125）和（255,88）。要求感谢阅读机械臂从圆台的上部开口处伸进去到达这些点进行加工，为简捷起见，不妨不计精品文档放心下载焊条等的长度，只考虑指尖的轨迹。3．制造厂家希望通过修改各条连杆的相对长度以及各关节最大旋转角度等感谢阅读设计参数提高机械臂的灵活性和适用范围。请根据计算模型给他们提供合理的建谢谢阅读议。二、基本假设连杆横截面积设为2mm*2mm。2．关节视为质点，它们所占的面积可忽略不计。假设连杆为刚体，不考虑形变。三、问题分析机械臂运动路径设计问题主要涉及到机器人运动学分析、机器人运动路径的精品文档放心下载算法确定以及机器人避碰问题。1．运动系统的建立从机构学观点来看，机器人属于空间机构范畴，因此，采用空间解析几何的谢谢阅读变换基本原理及坐标变换的矩阵解析方法，来建立机器人的运动系统的多级变换感谢阅读方程。由于旋转轴涉及到平行连杆和垂直连杆两类，因此对于各类旋转变换，所使感谢阅读用的变换矩阵也不相同。同时，此题中涉及的机器人有6个自由度，则从工件的坐标位置到固定坐标精品文档放心下载,.系的变换要经过多级坐标变换。采用多级坐标变换的方法。感谢阅读由上述三点，便可建立机器人运动系统的坐标变换关系式。谢谢阅读2．搜索算法的确定题中给定机器人的七个连杆的长度，关节类别以及可旋转的角度和控制系统每次可改变的增量。采用递归搜索的算法。在递归中，根据每次移动时，对E点位置变化影响最大的连杆取最大权重，优先调节该角度的值，这种由主到次调节支杆的方法能使得针尖由初始点尽快地接近目标点。尽管如此，如果仅仅以此方法进行搜索，由于移动时各个角度增量的精度为0.1°，计算量仍非常庞大。为提高搜索速率，降低搜索次数，通过改变角度变化步长以加速算法，使针尖E最初大幅度移动，后通过微调逐步逼近目标点。谢谢阅读3．避碰问题对于避碰问题，采用基于Z空间的自由空间法。Lazona-Perze提出基于Z空间的自由空间法。以机械臂的关节轴为坐标系建立Z空间(ConfigurationSpace)，将障碍物映射到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个自由度转角的增量(i=1,2,3,4,5,6)精品文档放心下载ij------第j次变换的变换矩阵（j=1,2,3,4,5,6,7,8,9）谢谢阅读-----目标坐标系的原点0-----连杆AB的长度AB-----连杆BC的长度BC-----连杆CD的长度CD-----连杆DE的长度DE五、模型建立min:LSt.XTTTTTTTTX谢谢阅读1 2 3 4 5 6 7 8 0-2°2°(i=1,2,3,4,5,6)感谢阅读i=140mm，l＝255mm，l＝255mm，l＝65mm精品文档放心下载AB BC CD DE-180°180°，-125°125°，-138°138°，123-270°270°，-120°133.5°，-270°270°4 5 6,.六、模型求解1．机器人运动方程的建立ZnZOYn1ZnnZOXnn2n1YXZn1n13ZOYZn2n22X1On23YX3O3Y22XOY1X12OXY1（图一：坐标系变换）如图示，从机座的固定坐标系的原点O，经过绕Z轴回转变换T()，沿Z精品文档放心下载1 1轴的平移变换T(),绕X轴回转变换T(),沿Z轴平移T(l),绕X轴回转2AB13224BC2变换T()),沿Z轴平移变换T(l),绕X轴回转变换5336447CD5T(),沿Z轴平移变换T(l)最终变换到目标位置。8559DE由题设已知目标坐标系为X(x,y,z,1)T，设定固定坐标系为0000(x,y,z,1)T。l l l由题设可以求出相应的变换矩阵分别为：cs001000100011csTsc00T0100T0011，，22，10010200130s2c20lAB000100010001,.10001001000cTT001l，0s45BC00010010001001000cTT001l，0s78CD000100

00cs00s044Tsc003c33，44,0600103010001001000s00100,T5c55090015lDE010001建立运动方程：XTTTTTTTTTX感谢阅读1 2 3 4 5 6 7 8 9 02．确定固定坐标系中E点位置由题设中，已知固定坐标系原点，根据给定的连杆长度和角度，易计算得精品文档放心下载i出 ， 在 固 定 坐 标 系 中 E 点 位 置 ：精品文档放心下载-65*(-cos(s1)*sin(s4)+(-sin(s1)*cos(s2)*cos(s3)+sin(s1)*sin(s2)*sin(s3)感谢阅读x)*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)谢谢阅读y-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)谢谢阅读Ez140-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个离散值（即谢谢阅读i精度为0.1°，绝对值不超过2°），那么的步长最小为0.1°，最大为2°。当连感谢阅读i杆水平拉伸到极点，我们用的步长最大为2°作粗略范围估算：感谢阅读il(LLL)220mm。为保证其有效性，我们取l25mm。0BCCDDE1800对加速度ratio进行解析，相应的伪代码：ratio25l20Ifratio20ratio=20Else,.If1<ratio20returnElseIfratio1ratio=1搜索算法流程图：,.初始化域值判断距离是否大于阈值否：取小步长搜索是：则取大步长搜索否确定各个角度变化对距离变化的影响权重。按权重由大到小，改变相应角依次搜索判断结果是否小于误差精度要求是：结束(图二：问题1①的算法流程图)（2）问题2的①根据（1）中的算法，将问题2的①的数据代入该算法中，得出结果，见附件一answer1.xls.感谢阅读用MATLAB对针尖运动路径作图分析如下：,.(图三：问题2的①的针尖运动路径图)模型结果的分析：从answer1的数据可以看出，对针尖E的影响谢谢阅读1 2 3 4 5由大到小可排序为：这是与目标点的初始位置E与目标位置E’的相对感谢阅读5 3 2 1 4位置相符合的。4.问题1的②、③和问题2的②、③（1）问题1的②、③已给定一条空间曲线x=x(s),y=y(s),z=z(s),a≦s≦b。由于代入计算机运算。我们将空间曲线离散化，取为若干个离散点，对离散点进行计算。即：将目标坐标由①中的一固定点变化为若干个满足空间曲线分布的离散点的集合。若根据问题1的①的算法即逐步逼近的方法来计算，则会存在两点之间逼近过程精品文档放心下载,.中出现迂回的现象。现对此算法进行优化。E1目标坐标系中存在x2EA的关系。为求得两点的最短路径，作如下计y3Ez45算：1，2，3，4，5是x，y，z的参数。则对1，2，3，4，5EEE求偏导，有EEEEExxxxxE1E2E3E4E5Ayyyyy12345EEEEEzzzzz123451E即：A2xE3yE5z即：AEAT(AAT)1E4感谢阅读,.算法的流程图：,.将曲线上的点离散化为n个（E，E，12n计算出当前点的坐标E 和目标点坐标1计算坐标差向量E计算A矩阵计算A的广义逆阵A计算A•E调整值使指尖E从E 点处移动到Ek1 k否：K=k+1判断k是否等n是：结束(图四：问题1②的算法流程图)同时，在②中，考虑避碰问题。将机械臂进行拉伸，考虑机械臂的最大活动谢谢阅读范围。下面分析矩阵大小的选择：因为连杆在x、y方向的极限长度为l l l 25525565575mm，精品文档放心下载BC CD DE,.设定一个区间为：400mm*400mm*400mm.设定单位小区间为：2mm*2mm*2mm.考虑与设定单位小区间为：1mm*1mm*1mm相比，用这个数据有利于节省计算机内存。这也是我们将连杆横截面积取为2mm*2mm*2mm的原因。将此空间用矩阵来描述。在障碍物存在的地方，设定取值为1，否则，设定取值为0。感谢阅读这样建立一个Z空间来处理避碰问题。（2）问题2的②根据题设给定圆台以及平面x=2z,确定圆台形工件外表面与平面x=2z的交线。谢谢阅读圆台形工件外表面公式为：(1680.4z)2(x210)2y2，感谢阅读与平面x2z的交线为(1680.4z)2(x210)2y2。感谢阅读x2z131.25cosx183.75化成参数方程形式：y128.6sin183.75131.25cosz2将交线上的点离散化，根据问题1的②的算法，代入该交线的数据，得出结感谢阅读果，见附件二answer2.xls.对结果进行分析，并用MATLAB对针尖运动路径作图如下：（由于是立体图形，谢谢阅读所以用两个图显示两个不同的方位示意）,.(图五：问题2的②的针尖运动路径图视图1),.(图六：问题2的②的针尖运动路径图视图2)（3）问题2的③根据上述两点的分析结果和算法，可得内表面的曲面方程为：(1600.4z)2(x210)2y2（0z180）四个焊接点坐标：（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）从灵活性角度考虑，连杆每作一次移动，产生l的变化，则相对的角感谢阅读也会出现变动。要提高灵活性即是要提高角度变化向量的变化范围。这里我们考虑能量最省的优化原则。则用角度变化向量的二范数来判定。谢谢阅读 22222精品文档放心下载1 2 3 4 5（2）适用范围：在所有杆的长度之和为固定值的情况下，指尖点E所能扫精品文档放心下载过的最大空间体积。灵活性和适用范围存在的一些相互制约的关系，根据本文的分析结果，我们谢谢阅读建议越靠近基座的θ值精度高一些，使得E点所能扫过的空间点数更多。精品文档放心下载七、模型推广与评价1．模型的优点通过求解整个Z空间，使得自由空间、障碍空间一目了然，因而，可按任何性能指标搜索路径．且具有完备解。精品文档放心下载(2)通过权重来判断影响因素最大的方法来选择搜索最优路径，有利于节省能量。精品文档放心下载(3)用矩阵来刻划Z空间，可提高精度和计算速度。(4)通过曲线的参数方程直接得到值的增量，精度比较高，连续性比较好。谢谢阅读,.2．模型的缺点(1)在焊接裂纹的时候没有考虑焊接角度——即杆DE与曲线法线方向所成的角精品文档放心下载度，实际上此夹角越大越容易造成对圆台壁的损伤，相应的焊接效果越差。感谢阅读(2)障碍的Z空间只能通过离散化求得，而难获得Z空间障碍的解析表达式，因精品文档放心下载而，为了提高精度．要占用大量的内存。(3)对于关节敬较多的机械臂，Z空间自由路径的搜索也将花费较多的时间。精品文档放心下载(4)在搜索时，由于自由度的牵连关系，导致在搜索时可能会生成锯齿形路径。精品文档放心下载八、参考文献[1]黄献龙,梁斌,吴宏鑫.机器人避碰规划综述.航天控制.2002.1:34-46谢谢阅读[2]张建民.工业机器人.北京理工大学出版社.1988精品文档放心下载[3]姜启源.数学模型.高等教育出版社.1995精品文档放心下载[4]张贤达.矩阵分析与应用.清华大学出版社.2005谢谢阅读[5]王晓东.计算机算法设计与分析.电子工业出版.2006精品文档放心下载[6]DonaldHearn,M.PaulineBaker.计算机图形学.电子工业出版社.2002感谢阅读九、附录程序说明：pasrch.h及pasrch.cpp为本程序的核心，我们在这两个文件中整合了搜索、精品文档放心下载曲线运动、避障等算法。以下为函数的说明：,.1-4为合并排序的函数intpartition(doubles[],intindex[],intp,intr);精品文档放心下载这是在合并排序的分划函数voidswap(double&si,double&sj);谢谢阅读这是合并排序的对双精度型的交换函数voidswap(int&si,int&sj);精品文档放心下载这是合并排序的对整型的交换函数voidsort(doubles[],intindex[],intp,intr);精品文档放心下载这是合并排序算法的主程序5-6为得到C、D、