机械臂运动路径设计问题

1、第38卷第14期2008年7月数学的实践与认识M AT HEM A TICS IN PRACTICE AND T HEORY V o l.38No.14July ,2008机械臂运动路径设计问题胡淼,雷思磊,商晓燕(解放军理工大学通信工程学院,南京210007摘要:针对机械臂运动的逆问题,提出了无障碍空间的牛顿迭代算法.并对牛顿迭代法进行了改进,建立了有障碍空间中避碰问题的一般模型,并对问题进行了求解.对于求出的机械臂指尖的姿态,先采用最大步长调整,再对剩余部分采用一次微调的方法生成指令序列,使生成的序列尽可能的短.针对具体问题¹,给出了达到目标点的一个指令序列,步数为89步,精度为

2、0.179m m;对于问题º,离散化裂纹,应用大步长调整的牛顿迭代法,给出了一个运动序列,实现了避碰焊接,各点的平均精度达到0.188mm ,并对最低点附近在1mm 误差范围内不可达到的区域进行了讨论,用解析和仿真的方法证明和验证了最低点不可达;对于问题»,改进了避碰问题的一般模型和算法,用试探法实现了四个焊接点的无碰焊接,并对圆台内表面在1mm 误差范围内不可焊接的区域进行了分析.关键词:牛顿迭代法;盲点;试探法1问题的简述收稿日期:2008-04-01机器人最主要的操作机构是各种形态的机械臂,机械臂的动作形态是由三种不同的单位动作旋转、回转、伸缩组合而成的.旋转或回转是

3、指机械臂产生相对转动,两者的不同仅在于转动部件的轴线与转动轴线是否同轴1.旋转如笔帽的转动,回转如摇柄的转动.根据具体的目标和约束条件计算出合理、便捷、有效的指令序列是机器人控制中的一个重要问题.本文就是对这个问题进行讨论.2问题的分析3自由空间中机械臂指尖运动逆问题的求解模型3.1机械臂指尖坐标公式求解对机械臂在空间运动的描述离不开各关节的坐标,因此须首先求出各关节的坐标.首先,建立直角坐标系.原点设在图1中的A 点.z 轴与AB 重合,正向与AB 方向一致.x 轴垂直纸面,正向向外.y 轴在基座所在的水平面上,正向向右.如图1所示 :图1假设机械臂连杆的粗细忽略不计,对角度方向作如下约定:

4、机械臂的关节旋转时,约定顺时针方向为负.坐标系变换时,约定顺时针方向为正.由于E 点与上述A 、B 、C 、D 点异面,所以直接在此坐标系下求E 的坐标比较困难.因此,我们在D 点建立新坐标系,求得E 点在新坐标系下的坐标后,再变换到原坐标系,得到E 点在原坐标系下的坐标公式.坐标转换公式如下:x E y E z E =x E y E z E T 1T 2+x D y D z D (1其中:T 2=co s 0cos P 2cos P 2co s P 2cos P 2+H 2+H 3cos (H 2+H 3co s P co s(P +H 2+H 3cos P 2+H 2+H 3T 1=H 1

5、cos P 2cos P 2cos 0由式(1可得E 点在原坐标系下的坐标为9514期胡淼,等:机械臂运动路径设计问题x E =sin H 1-255sin H 2-255sin(H 2+H 3+65cos H 1sin H 4sin H 5+sin H 1-65cos H 5sin (H 2+H 3-65co s(H 2+H 3cos H 4sin H 5y E =co s H 1-255sin H 2-255sin(H 2+H 3-65sin H 1sin H 4sin H 5+co s H 1-65cos H 5sin(H 2+H 3-65cos(H 2+H 3cos H 4sin H

6、5z E =140+255cos H 2+255cos(H 2+H 3+65cos(H 2+H 3co s H 5-65co s H 4sin(H 2+H 3sin H 53.2牛顿迭代算法由于机械臂指尖轨迹是离散的,而要求指尖沿着预先指定的一条空间曲线x =x (s ,y =y (s ,z =z (s ,a ¸s ¸b 移动,需先将曲线抽样成离散点,再运用牛顿迭代法2求可行解,使指尖逼近抽样点.假设机械臂指尖姿态为f (,(=(H 1,H 2,H 3,H 4,H 5.对于空间任一目标点Q (x Q ,y Q ,z Q ,建立运动学方程组:X (=x QY (=y QZ (

7、=z Q利用牛顿迭代法求解此方程,迭代初始点为(0=(H 01,H 02,H 03,H 04,H 05.具体步骤如下:从T =0开始,1对每一个方程的左边进行一阶泰勒展开,展开点为(T =(H T 1,H T 2,H T 3,H T 4,H T 5,例如:对X (进行一阶泰勒展开:X (=X (T +(H1-H T 15X (5H 1(=(T +(H 2-H T 25X (5H 2(=(T +(H 5-H T 55X (5H5(=(T 2对1中的泰勒展开式,以H i (i =1,2,3,4,5为变量,建立方程组:A H =B (2对方程组(2中的系数矩阵A =A 1A 2A 3A 3A 5进行

8、遍历查找,查找所有3×3大小的且行列式不为0的矩阵集#(+的列向量为A 的列向量的一个排列,找出#中行列式最大的矩阵,记为+.假设+=A i A j A k ,i ,j ,k =1,2,3,4,5,且i ,j ,k 互不相等,则集合1,2,3,4,5中未被i ,j ,k 取中的值记为p ,q .3将(=(H 1,H 2,H 3,H 4,H 5中的H i ,H j ,H k 作为变量,对H p ,H q 赋值,H p =H T p ,H q =H T q ,建立自由度为3的方程组:+H i H j H k =B (3由方程组(3解出H i ,H j ,H k ,其解记为H *i ,H

9、*j ,H *k ,集合(T +1=H T +1i =H *i ,H T +1j =H *j ,H T +1k=H *k ,H T +1p =H T p ,H T +1q=H T q .4判断(T +1和(T 误差是否在误差容限内,若是,则停止迭代,如果(T +1的参数H T +1i 超出其限制范围,则重新选择迭代初始点(0,令T =0,转向步骤1,若H T +1i 在其范围内,输出最终的(T +1,停止算法.若(T +1和(T 不相等,T =T +1,转向步骤1.根据迭代停止的(T +1,计算出此时机械臂指尖的位置(x (E ,y (E ,z (E .按步进精度0.1ü,调整(x

10、(E ,y (E E ,z (E .机械臂指尖最终到达位置与目标点之间的距离为:96数学的实践与认识38卷d =(x Q -x (E 2+(y Q -y (E 2+(z Q -z (E 24机械臂避碰问题的一般模型和算法在实际情况中,机械臂的运动空间可能存在障碍物,所以在从初始位置到达目标位置的过程中,需考虑避开障碍物的问题,不仅要考虑机械臂指尖避碰,也要考虑机械臂连杆避碰.并且,障碍物的外表形状对于机械臂避碰运动的影响很大,未知的或者不规则的外表形状增加了问题的复杂度,通过对障碍物的封装,可以降低问题求解的难度:对于不规则外表的障碍物,沿与高垂直的方向做切平面,作一个使得不规则的切平面全部位

11、于圆内的半径最小的圆,如果每个切平面上的圆的圆心都在一条垂直于底面的直线上,那么这些位于不同高度切平面上的圆就形成了一个外表规则的圆台,这样考虑简化了问题,并且由于所作的圆台是包含障碍物的最小圆台,所以在大多数情况下,由此规划的路径误差还是在一个可以接受的范围内.从前述运动学逆问题模型中可知,可以将运动轨迹分解为一系列的点进行求解,所以在机械臂避碰问题中还是先考虑单点精确定位过程中的避碰问题.自由空间机械臂指尖定位问题的目标函数为:min d 2=(x Q -x (E 2+(y Q -y (E 2+(z Q -z (E 2对于有障碍的问题需要再增加约束条件.设机械臂在(=(H 1,H 2,H

12、3,H 4,H 5姿态时对应的空间折线段为l (Q ,第i 个障碍物外表面为S i ,若有n 个障碍物,则约束条件可以表示为:对于运动轨迹避碰问题,一般首先构造一条连接初始点与目标点,与障碍物不相交的折线,然后在每个分段点上进行带约束条件的求解,理论上说是可以求解避碰的运动轨迹问题.定义1如果对点P 存在一个姿态,机械臂与障碍物无碰撞,则称P 有一个良好的姿态.假设初始点为Src,目的点Dst.1构造一条连接Src 和Dst 的折线L (P 0P 1P n ,P 0=Src,P n =Dst,折线与障碍物不交,且P 0,P 1,P n 都有一个良好的状态.L (P 0P 1P n 构造方法a.

13、连接Src 与Dst,如果连线与障碍物相交,则求出这两点垂直平分面上的一点P 1,P 1不在障碍物上,且具有一个良好的状态,P 1与Sr c 和Dst 之间的距离和最小.如果连线与障碍物不相交,则中间不加入任何途经点.b .对Src 与P 1,P 1与Dst 采取步骤a 同样的方法可以得到P 2,P 3.继续下去,最终得到P 0,P 1,P n ,按顺序重新编序后,不妨仍然记作P 0,P 1,P n ,则得到所求的折线.2无碰撞轨迹求解过程顺序的求解P i P i +1的无碰撞运动轨迹.如果对i =0,n -1都能完成,则联合起来从Src 到Dst 无碰撞运动轨迹.如果P i P i +1对某

14、i 不能得到无碰撞运动轨迹,则就以当前点P i 代替Src ,重复前面的工作.根据实际情况,一般情况下,可以断言可行解是存在的.9714期胡淼,等:机械臂运动路径设计问题5具体问题的求解5.1问题¹的求解求机械手从初始位置(0,510,75移动到工具位置(20,-200,120处的指令序列.1计算机械手最接近目标的位置定位后姿态为:H 2=90ü-(DB G +CBD 11.2008üH 3=2CB D 132.359üH 5=180ü-CB D -(90ü-DBG 36.4398üH 1=arctg20200+90

15、2;=174.289ü按步进精度0.1.,得到实际的最终姿态为:H 1=174.3ü,H 2=11.2ü,H 3=132.4ü,H 4=0ü,H 5=36.4ü对于此姿态,机械臂指尖位置为:x E =A H sin(H 119.985;y E =A H cos(H1-199.855;z E =119.986;工作误差为:d =(20-19.9852+(-200+199.8552+(120-119.986 2=0.179408mm 2工作过程:¹H 1,H 3,H 5不变,从90.开始按精度2.,1.9.,步进减小H 2,达到

16、11.2.;ºH 2,H 3,H 5不变,从0.开始按精度2.,1.9.,步进增加H 1,达到174.3.;»H 1,H 2,H 5不变,从0.开始按精度2.,1.9.,步进增加H 3,达到132.4.;¼H 1,H 2,H 3不变,从90.开始按精度2.,1.9.,步进减小H 5,达到36.4.;共需要89步指令.5.2问题º的求解当考虑平台对机械臂的阻碍以及机械臂各关节转角限制时,直接利用牛顿迭代法是不行的,因为其在迭代运算过程中未考虑是否发生碰撞,也未考虑机械臂各关节转角的限制,需对牛顿迭代法进行改进.算法的基本思想是,在固定其中两个关节转角时,通

17、过调整其他三个关节转角可以到达的目标点可能是多个,这样可以连续找到多个目标点,在最后到达的目标点调整那两个固定的关节转角,再利用这一对固定值,搜索此时通过调整其他三个关节转角可以达到的目标点,如此重复执行,由于在判断目标点是否可达时加入了转角和碰撞判断因素,因此可以起到考虑机械臂的阻碍以及机械臂各关节转角限制时的空间曲线轨迹控制.假设机械臂达到姿态(90ü,H 2,H 3,H 4,H 5,可以调整(90ü,H 2,H 3,H 4,H 5,使其处于一种既不发生碰撞,又满足机械臂各关节转角限制要求,在具体实现时,可以微量调整,记此状态为14 期 胡淼, 等: 机械臂运动路径设计

18、问题 99 算法步骤: 0 0 1 从初始姿态出发, 因为针对本题中特定的圆台 H , H 对问题的求解最为重要, 影响最 1 3 0 0 大, 所以这里固定 H , H , 对关于另外三个参数的 H , H , H 位置非线性方程组采用牛顿迭代法 1 3 2 4 5 0 0 0 求出焊接第一个目标点的机械臂姿态参数 ( 1 牛顿迭代法中的迭代初始值为( H , H , H , 并 2 4 5 判断此时没有发生碰壁, ( 1 的各关节转角是在限制范围内, 如果两种条件中有一个不满足, 则需要调整 ( 0, 否则继续. 1 1 3 3 2 4 5 2 在第 i 个目标点, 仍然在 H = H ,

19、 H = H 的前提下, 对关于参数 H , H , H 的位 置非线性方程组继续用牛顿迭代法确定下一个目标点 ( i 的姿态, 并判断此时没有发生碰 i- 1 i- 2 i- 1 i- 2 壁, ( 2 的各关节转角是在限制范围内, 直到在确定目标点 ( m 的姿态时, 上述的两个条件有一 个不满足, 则转向步骤 3 . mm m0 0 0 mm 3 在目标点调整 H 1 , H - 1, H 1 分别等于 H , H , H , 同时调整 H 1 , H - 1 的大小, 可以分 2 4 5 2 4 5 1 3 m m别在 H - 1, H 1 附近遍历搜索, 搜索目标是机械臂在此姿态下可

20、以向前连续到达的目标点数 1 3 最多, 如果遍历搜索得到的新姿态全部无法到达下一个目标点, 则转向 4 , 反之, 回到步骤 2 , 直到到达最终目的点, 或者发现下一个无法到达的目标点 ( j , 回到步骤 3 . 4 报告发现不可达点, 等待下一步的指令. 在利用上述算法解决问题 º 时, 采用的裂纹离散化方法是: P i , i 依次从 1 到 720 递增, 这样得到 720 个离散的目标焊点, 经过计算验证, 取 360 ü ( 0 = 85 , - 45 ü, - 110 ü, 0, - 18ü , 下表是 i 取 1 到 360

21、 时部分中间结果, 如表 1: 令t= 表1 第 i 次调整 1 2 3 4 5 6 i i 调整后的固定角( H , H ( 单位: 度 1 3 此时可以连续到达的目标点数( 单位: 个 88 62 88 89 23 8 ( 85, - 110 ( 75, - 120 ( 65, - 130 ( 55, - 135 ( 50, - 138 ( 59, - 138 在计算曲线最低点( 52. 5, 0, 26. 25 时发生了找不到满足条件的解的情况, 此时需要人 工调整继续计算的初始值, 在后文中将会证明此点为盲点. 焊接效果如图 2. 由图 2 可以看出, 焊接点很好的逼近了裂缝, 经过误

22、差分析, 发现焊接点的平均误差为 0. 188mm . 5. 2. 2 对最低焊点的讨论 定义 2 对于机械臂指尖在 1mm 误差范围内不可达到的点称为机械臂的盲点. 所有盲 点组成的区域称为盲区. 得出最低焊点的坐标, 由于机械臂的参数角有一定范围控制, 因此讨论最低焊点是否为 盲点问题, 如图 3 所示, A BDE 分别为机械臂的关节点, 假设 E 点已经处在焊点位置上, 这样 E 为定点, 连接 BD 做辅助线, 可以得出 D 点在以 B 为球心, BD 长为半径的球, 与 E 点为球 心, ED 长为半径的球的交线上, 其坐标方程满足为: 100 数学 的实践与认识 38 卷 图2

23、图3 2 ( x D - 52. 5 2 + y D + ( z D - 26. 25 2 = E D 2 x D + y D + ( z D - 140 = B D 2 2 2 2 并且假设 O 为 DE 与圆台的交点, 则肯定在 DE 上, 因此 O 点坐标满足如下方程: x O = K× 52. 5 + ( 1 - K × x D y O = ( 1 - K × y D ( 0 < KF 1 z O = ( 1 - K × z D + 26. 25 × K 由上述方程给定 BD 和 z D 的值可以求得 K以及交点的坐标. 判定 E

24、 点可以在不碰到外壁的情况下接触到焊点的条件是: 1 K大于 1 或小于 0 2 D 点必须在圆台的外部 3 z D 必须大于 0 因此, 由于 BCD 的夹角最小为 42 度, 可以求的 B D 的最小值为 182. 768 毫米, 利用计算 机仿真, 不停控制 BD 和 z D 的迭代值, 在所有的解空间中找出满足条件时的 BD 值最大为 156. 8 毫米, 这样可以得证, 在焊接过程中外壁碰撞问题时, 最低的焊点为盲点, 即机械臂指 尖不可到达点. 5. 3 问题 » 的求解 5. 3. 1 试探法 按照本文第 4 部分提出的避碰的一般算法, 由于找到状态良好的点比较困难,

25、对一般算 法作了一些改进, 采用试探的方法: 定义 如果两个点之间可用一条直线连接, 且这条直线不被障碍物所遮断, 则称这两 点是视距可达. 定义 H 集合包含已经到过的点的位置信息, S 集合包含从当前点出发可以到达的下 一个点的位置信息. 试探法的详细步骤: 步骤 1 选定合适的初始点, 选择初始点的原则是所选的点与目标点视距可达, 如: 对 14 期 胡淼, 等: 机械臂运动路径设计问题 101 于本题中的圆台, 目标点在圆台内部, 那么初始点可以选在圆台的顶部中心, 这一位置与圆 台内每一点都视距可达. i i 步骤 2 每一步指令的 H( i = 0, 1, 2, 3, 5 从 -

26、0. 1, 0, 0. 1 三个值中选取, 去掉 H = 0( i = 0, 1, 2, 3, 5 的情况, 共有 242 种情况, 即 S 中有 242 个元素. 从其中选择一种指令, 使到达 的下一点与目标点的距离小于选择其它指令时所到达的下一点与目标点的距离, 并且满足 1 到达的下一点是不在 H 中的, 2 到达的下一点机械臂与障碍物不会发生碰撞, 3 到达的 下一点时的机械臂各个弯角没有超过限制. 直到到达目标点, 或与目标点的距离小于某个设 定值, 停止试探. 如果在某个位置找不到满足上述三点要求的指令, 则转向步骤 3. 步骤 3 如果当前点 Pi 就是选择的初始点, 那么跳到步

27、骤 4, 否则, 将当前点 P i 的位置 信息加入集合 H , 从 H 中选择到达当前点经过的上一点 P i- 1, 返回到 P i- 1 , 在 P i- 1 处重复步 骤 2. 步骤 4 结束算法, 目标点不可达. 按照以上算法求出机械臂指尖的最终姿态和误差如表 2: 表2 需焊接位置 A B C D ( 255, 88, 151. 4044 ( 190, - 125, 82. 0253 ( 120, 106, 50. 8651 ( 320, - 104, 20. 0492 选择的临近点 ( 210, 0, 175 ( 210, 0, 81 ( 210, 0, 55 ( 210, 0,

28、35 实际焊接位置 ( 254. 84, 88. 25, 152. 91 ( 190. 08, - 124. 86, 83. 82 ( 120. 16, 106. 31, 52. 40 ( 320. 25, - 104. 06, 21. 67 误差( 单位: mm 0. 2969 0. 3383 0. 3504 0. 2841 5. 3. 2圆台内表面盲区的分析 由于机械臂的连杆长度固定, 每个关节的旋转范围受限制, 圆台内表面必有机械臂指尖 在一定的误差范围内不能达到的地方. 在这些因素中, 对盲区影响比较大的是连杆的长度与 关节 C 的转角 H 的活动范围. 而连杆的长度是固定的, 所以盲

29、区主要受 H 的活动范围限制. 5 5 ü ü H 的活动范围为 - 138 138 , 则盲区点( x n, y n, z n 满足 42 F x n F 210, 关键是求 z n 的范 5 围. 其算法如下: 以平面族 y = t( - 168 F t F 168 去切圆台, 对于圆台的每一个剖切面, 作如下分析: 将机械臂平面 A BCD 移到剖切面上, 调整 H 使连杆 BC 恰与圆台相交, 再调整 H 到最大 2 3 限制范围, 以关节点 D 为圆心, 连杆 DE 长度为半径画圆, 求出与剖切面内表面线段的上下 交点 S 、 , 若下交点 R 存在, 则此时 R

30、 为机械臂指尖在此平面上所能达到的最低点, R 点以 R 下的部分为不可达点, 若下交点 R 不存在, 则此内表面线段上的点都可达, 如图 4. ( 上交点 S 以上的区域可以通过调整连杆 BC 达到 . 以 y = 0 平面切圆台为例计算该剖切面上的不可达点: 将连杆 BC 调整到与圆台表面相切, 此时 H = 70ü . 调整 H= - 138ü , 此时 D 的位置为 2 ( 118, 0, 2 , 以 D 为圆心, 连杆 DE 的长度为半径画圆, 与内表面线段的交点为 R( 0. 61, 0, 0. 26 和 S( 0. 68, 0, 0. 44 , 则此剖切面上

31、R 点以下的内表面线段为不可达点. 通过求出平面族 y = t ( - 168 F t F 168 的剖切面上所有不可达点即可得到机械臂指 尖的盲区. 102 数学 的实践与认识 38 卷 图4 6模型的分析 牛顿迭代法中初始展开点的选择对于结果的影响很大, 初始迭代值选择得不好会导致参数 值超出限制范围. 因此, 当参数超出范围时需重新选择初始迭代点. 在自由空间中, 牛顿迭代法的 精确性较高, 最终的定位点与目标点之间的误差范围很小, 在0. 1mm 数量级范围内. 参考文献: 1 周远清等. 智能机器人系统 M . 北京: 清华大学出版社, 1989. 2 J. M . 奥特加等. 多元

32、非线性方程组得迭代解法 M . 北京: 科学出版社, 1983. A Design on the Moving Trcuk of Mechanism-arm HU M iao, LEI Si- SHA NG Xiao-yan lei, ( ICE , PL A U ST , N anjing 210007, China Abstract: T his passage put f or war d N ew ton- tera tiv e- rithmetic to solve the inver se pr oblem I A of the mechanism - m kinematics in the fr ee space. Aft er impro ving t he N ewt on - erativear It A rithmetic, the gener al model is founded to keep awa y fr om the barr ier in