六关节机械臂运动路径设计

1、第38卷第14期2008年7月数学的实践与认识V o l. 38No. 14July , 2008六关节机械臂运动路径设计陈明生, 沙威, 谢莹(安徽大学计算智能与信号处理教育部重点实验室, 安徽合肥230039摘要:就关节式机械臂指尖在任意两点间移动、沿固定曲线移动、机械臂绕开障碍物执行任务以及参数优化等问题展开研究. 首先确定了自由度组合到指尖空间位置的映射, 建立了求解上述问题的最小二乘模型、泛函条件极值模型, 并给出了数值解法. 最后, 结合图像处理等技术, 对各参数的优化设计提出了改进措施.关键词:机械臂; 非线性最小二乘; 泛函条件极值1自由度到指尖位置映射的构造六自由度的关节式机

2、械臂平面示意图如图1所示, A B =140mm , BC =255m m, CD =255mm , DE =65mm. 自由度向量 =( 1, 2, 3, 4, 5, 6 与指尖E 的空间坐标X =(x , y , z 之间存在多对一的映射f , 使得f ( =X . 该映射的确定应当为本文首要解决的问题.图1机器人结构示意图图2空间坐标系建立如图2所示, 以A 为原点, AB 为+z 方向, 以垂直纸面向外为+x 方向, 建立右手系, 并给出相应的球坐标系(r , , . 由于F 与G 为水平连杆, 所以A 、B 、C 、D 应该在同一个 = 0平面上. A 、B 坐标业已确定, 其余坐标

3、可通过引入旋转矩阵计算:首先引入向量a (x a , y a , z a , 使其在单位法向量为b (x b , y b , z b 的平面内逆时针旋转角度! 后, 所得向量c (x c , y c , z c 可按旋转矩阵T (! , b 计算x c z c(1-cos(! x 2 b +cos(!, b =T (!(1-co s(! x b y b +z b sin(! x a(1z a(1-co s(! y 2 b +cos(!(1-cos(! y b z b -x b sin(! (1-cos(! z 2 b +cos(!y c =T (! , b y a(1-cos(! x b y

4、b -z b sin(! (1-cos(! x b z b +y b sin(! (2(1-co s(! x b z b -y b sin(! (1-cos(! y b z b +x b sin(! 收稿日期:2008-04-01 :104数学的实践与认识38卷首先在y -z 面内计算好各点初值, 平移到原点后对按T ( 1, e Z 旋转, 对先按T ( 1, e Z 旋转, 再按T ( 4, /CD 旋转, 最后将原平移向量加上可得各点坐标.2任意两点间平移算法2. 1非线性最小二乘模型的建立在映射f ( =X 确定之后, 考虑如何使机器臂指尖E 从初始点(Ox (0 , Oy (0 ,

5、Oz (0 移动到空间任意一点(Ox , Oy , Oz .为了便于表述, 且考虑到第六个自由度对指尖位置没有影响, 我们将f ( =X 分解为标量映射:x =f x ( 1, 2, 3, 4, 5 , y =f y ( 1, 2, 3, 4, 5 , z =f z ( 1, 2, 3, 4, 5 . 从而有:(0 (0 (0 (0 (0 (0 (0 (0 (0 (0 (0 (0 (0 (0 Ox (0 =f x ( 1, 2, 3, 4, 5 , Oy =f y ( 1, 2, 3, 4, 5 , Oz =f z ( 1, 2, 3, 4, 5. (0(0(0为使指尖从(Ox (0 , Oy

6、 (0 , Oz (0 到达(Ox , Oy , Oz , 各自由度必须在允许的范围内产生角度增量( 1, 2, 3, 4, 5 , 使得Ox =f x ( 1, 2, 3, 4, 5 Oy =f y ( 1, 2, 3, 4, 51, 2, 3, 4, 5 Oz =f z (0i = i + i , i =1, 2, 3, 4, 5(3由于方程数少于未知量数, 所以(3 理论上有无穷多解, 即使限定各自由度范围后仍可能有多解. 为了确定一组最佳的角度增量, 并根据实际物理意义, 我们将问题的求解转化为以下非线性最小二乘问题52m in G =( i i(4i =1Ox =f x ( 1, 2

7、, 3, 4, 5 Oy =f y ( 1, 2, 3, 4, 5 s. t.Oz =f z ( 1, 2, 3, 4, 5 i = i + i , i =1, 2, 3, 4, 5 1-180, 180, 2-125, 1253-138, 138, 4-270, 270, 5-133. 5, 120即在解空间中寻找总增量二范数最小解, 也就是方程(3 的最小范数最小二乘解, 此解唯一且能够使得机械臂以最少的动作达到目的. 2. 2数值解法方程(4 的解可在M atlab 中调用以下格式命令获得:sx fval=fminim ax(ObjectFunction1, x 0, , , , ,le

8、ft, right, Constraint -EqualObjectFunction1为函数脚本文件, 定义为52i =(1(0(5其中sx 为返回解, fval 为解对应的最小范数; ObjectFunction1为一个匿名函数形式,(14期陈明生, 等:六关节机械臂运动路径设计105x 0为初始值, left =(-180, -125, -138, -270, -133. 5 , right =(180, 125, 138, 270, 120 ; Co nstr aint -Equal 为限制条件, 定义为22(Ox -f x ( 1, 2, 3, 4, 5 +(Oy -f y ( 1,

9、2, 3, 4, 5 2+(Oz -f z ( 1, 2, 3, 4, 5 =0(7在不考虑障碍物的情况下, 为了节约时间, 求解出 i 的最小范数最小二乘解后, 可令前五个自由度(第六个不动 同时以2 为步长改变. 直至某个自由度增量快达到 i 后以小步长完成该自由度的最后一次增量, 随后该自由度增量为零, 继续执行其他自由度完成目标.2. 3实例验证问题:要求机械臂(指尖 从初始位置移动到工具箱所在位置的(20, -200, 120 处, 以夹取要用的工具.采用上述解法, 并考虑到各增量的最小步长为0. 1 , 求解出最佳增量组合由表1给出.表1各自由度最佳增量(单位:度 15. 75i

10、=1 298. 1 3138 40. 3 5103. 3 602总增量:( i =39371. 08, 指尖终点位置:(20. 0436, -200. 0230, 119. 9591 , 终点位置的相对二范数误差:2. 7353e -4.3沿固定曲线平移算法3. 1泛函条件极值模型不失一般性, 设空间曲线的参数方程为:x =x (s , y =y (s , z =z (s , s s 0, s n . 使机械臂指尖沿固定曲线移动问题转化为寻求如下一组函数簇: i = i (s , (i =1, 2, 3, 4, 5.为了沿目标曲线移动时, 使机械臂各关节变化幅度尽量小, 应该使得下式尽可能小5

11、sn d id s (8 s d s i =10s sn 0为了便于计算, 对上式进行修正得i =15id s d ss s5n2(9至此问题转化为如下泛函条件极值问题min J ( 1, 2, 3, 4, 5 =F (s , d s =n 0s s0i =12id s d s(10x (s -f x ( 1(s , 2(s , 3(s , 4(s , 5(s =0s. t. y (s -f y ( 1(s , 2(s , 3(s , 4(s , 5(s =0z (s -f z ( 1(s , 2(s , 3(s , 4(s , 5(s =0 (s =( 1(s , 2(s , 3(s , 4

12、(s , 5(s i (s (i =1, 2, , 5 , 使J 达到最值, 并满足附加条件即求函数簇#(s , =(x (s -f x ( 2+(y (s -f y ( 2+(z (s -f z ( 2=0(11n106数学的实践与认识38卷件极值的拉格朗日数乘法的直接推广, 即可通过作辅助函数的方式获得其解析解. 3. 2泛函条件极值模型的近似解法考虑到自由度到指尖空间位置映射的复杂性, 以及时间有限, 我们并不能获得上述模型的解析解. 为此, 接下来考虑该泛函条件极值问题的近似解法.在上述泛函条件极值问题中, 问题的关键是为了求解函数簇 i (s (i =1, 2, , 5 , 可按以下

13、步骤进行近似求解:对于给定曲线, 首先计算其上离指尖初始位置(Ox , Oy , Oz y , z(0(0(0(0(0距离最小点(x(0,;(0(0(0以(x , y , z 为初始点, 对s 在s 0, s n 中以很小步长离散化, 获得曲线空间位置的离散值(x (s j , y (s j , z (s j ;使机械臂指尖在(x (0 , y (0 , z (0 , , (x (s j , y (s j , z (s j , 空间点列中依次移动, 并按2. 1中的非线性最小二乘模型, 计算出对应于s j (j =1, 2, 3, , N 的 i (s j 值, i =1, 2, , 5, 对

14、该值在s 0, s n 进行插值或逼近可得到 i (s , s s 0, s n .! 根据不同需求选取s , 离散后计算出 i (s j (i =1, 2, , 5 , 可得各自由度的增量. 3. 3实例验证问题:假设在机械臂的旁边有一个待加工的中空圆台形工件, 上部开口. 工件高180mm , 下底外半径168m m, 上底外半径96mm, 壁厚8m m. 竖立地固定在x -y 平面的操作台上, 底部的中心在(210, 0, 0. 如果圆台形工件外表面与平面x =2z 的交线是一条裂纹需要焊接, 请给出机械臂指尖绕这条曲线一周的指令序列.首先求得交线的参数方程为x =210+y =z =1

15、68-168- co s(s 10+2ss 0, 2(12sin(s 10+2s2+5s图3圆台与平面交线的三维视图图4最大移动步长s 随采样点变化计算结果按3. 2节中所给算法, 我们按如下步骤求解该问题:114期陈明生, 等:六关节机械臂运动路径设计107图3中*为机械臂指尖初始位置(Ox (0 , Oy (0 , Oz (0 =(3. 1228e -014, 510, 75. 0000 , 而“空心方块”则表示曲线上离指尖初始位置最近的点(x (0 , y (0 , z (0 =(139. 41567, 121. 03962, 69. 707837 , 其对应的参数s 0=120. 24

16、86 ;步骤2离散化获得曲线空间位置的离散值(x (s j , y (s j , z (s j , 本题中取s =0. 05; 步骤3从初始点(x , y , z 开始, 让指尖在离散点列(x , y , z , , (x (s j , y (s j , z (s j , 上依次移动, 并按(4 式求解出s 0, s 1, s 2, s j , 处对应的各关节处自由度i (s j (i =1, 2, , 5 ;(0(0(0(0(0(0步骤4以尽量使相邻两点间s 最大为目标, 即在s 由0增大的过程中, 当 i (s j (i =1, 2, , 5 中第一次出现越界(超出-2, 2 时的s ,

17、为s j 点处最大可移动步长.在此种情况下求得的总移动次数恰为N =100次, 每次最大s 值如图6所示. s j (j =1, 2, , N 处, 各关节处的自由度值 i (增量为0. 1的整数倍, 并不超越-2, 2 的具体结果如图7中阶梯线近似所示.以下为在每次s 按图6尽量取大的情况下, 前五个自由度随s 的变化图, 图中s 为相对其始点s 0逆时针方向的旋转角 .图5最大移动步长时 1随参量s 变化图图6最大移动步长时指尖位置与曲线在x oy 面内的投影为了检验指尖空间位置与曲线的逼近效果, 由于三维视图不容易分辨, 当s 按最大步长计时, 我们给出了指尖移动曲线图(阶梯线 与曲线在

18、x oy 面内投影图. 同时在获得函数簇 i = i (s 后, 可以较小s 均匀采样获得指令的另一组解, 受篇幅限制不再敷述.4绕障碍物移动算法4. 1通用算法的描述当障碍物出现时应根据其空间位置、大小和形状, 首先设计好指尖的前进路线, 并且保证指尖在按该路线移动时, 时刻保持机械臂到障碍物表面最小距离最大化. 设指尖移动过程中, 机械臂构成的曲线为%(s , 障碍物表面为S , 障碍物表面到机械臂最小距离为d (s . 那么本问题可用如下模型描述max d (s , s. t. #(s , =(x (s -f x ( 2+(y (s -f y ( 2+(z (s -f z ( 2=0 0

19、 (13108 4. 2具体问题的求解 数学 的实践与认识 38 卷 问题: 在工件内壁点焊四个小零件, 它们在内表面上的位置到 x y 平面的投影为( 320, - 104 、 120, 106 、190, - 125 和( 255, 88 . 要求机械臂从圆台的上部开口处伸进去到达 ( ( 这些点进行加工, 为简捷起见, 不妨不计焊条等的长度, 只考虑指尖的轨迹. 首先根据 x y 平面的投影 计算出四点的真 实坐标为: ( 320, 104, 20. 0086 、 120, 106, ( 50. 8254 、( 190, - 125, 81. 98406 和 ( 255, 88, 151

20、. 3637 . 计 算 得 - 71. 9958, - 41. 4552, - 146. 6593, - 19. 0395. 1 增 量 分 别 为: 我们的思路是在 1 固定的情况下应使得指尖尽可能抬高, 然后再增加 1. 为此, 首先在 y z 面内改变 2 与 3, 使得机械臂 D 点 z 坐标尽量大, 可通过改变( 4 式目标函数为: max ( z d 即在 y z 面内使得 D 点运动到( 0, 0, A B + B C + CD . 随后按计算出的 1 旋转到目标点所在 面. 固定 1 后, 调整 2, 3 使 D 点下移, 过程中保 持 CD 与圆台中轴线夹角最小使得 D 点

21、运动到圆台上底面中心( 210, 0, 180 , 中轴线方向为 ( 0, 0, 1 , CD 为( x d - x c , 0, z d - z c , 二者做内积可得目标函数为 min 210( x d - x c D 运动到圆台上底面中心后, 保持 1 ( 14 ( 15 , 2 和 3 不变, 调整 4 、5 , 使得DE 指向为( Ox - 210, Oy , Oz - 180 , 由于上底面外半径为 96m m, 减去厚度 8mm 得内半径为 88mm , 而 DE = 65m m, 因此可以断定在该过程中 DE 不会碰上圆台上任何一点. DE 调整后指向目标点( Ox , Oy

22、, Oz . 以( 4 描述的目标函数, 即最小总变化幅度为目标 函数, 沿直线( ( 210, 0, 180 , ( Ox , Oy , Oz 连线 运动即可达到目标. 5 关于设计参数的合理建议 对于 本文所研究关节式机器人而言, 其有 效作业范围明 显受到两方面因 素的影响: 自 由度变化范围; 关节长度. 在给出描述这两个因素的设计参数建议前, 我们首先对题目给定参数下机器人的作业 范围进行了分析. 在 yz 面内, 给出 4 = 0 情况下给出了 E 点变化范围如图 7 所示. 可以看出 如果 1 稍稍旋转后, 若 4 仍然固定为 0 , E 点在空间中将会在每个 面内出现如图 7

23、所示的 圆形盲区, 该圆形区域绕 z 轴旋转后, 在整个空间中出现一个“ 轮胎形” 盲区. 通过图像处理技术分别计算出 D、 两点盲区的半径分别为 R D = 182. 6119mm , R E = E 117. 5235mm , 而 R D - R E = 65. 0884mm 接近 DE 长度, 验证了算法的正确性. 为了弥补上述“ 轮胎形” 盲区, 当 1 稍稍旋转后, D E 应该指回上一个 面内的圆形区域. 然而由于 E 点盲区半径 R E = 117. 5235 大于 DE 长度, 必然会形成如图 8 所示的绝对盲区 ( 实际盲区应该还要大 . 要减小 E 点的盲区面积, 最简单的

24、就是直接减小 D 点的盲区, 当 D 点盲区半径小于 DE 时, E 的盲区就减小为 0; 为此, 我们尝试着改变影响 D 点空间位置的参数: A B 、 CD 、2 BC、 和 3 . 当将 3 变化范围由 ± 138 扩大到 ± 180 时, D 的盲区半径减小为 0. 但是, 由于 DE 杆的存在, D 点的盲区没有必要减小为 0; 只要其半径小于等于 DE 长度 即可, 为此我们用二分法在 138, 180 间寻找到了 166 使得 D 点盲区的半径刚好等于 DE 长 度 65cm, 所以 3 变化范围应由 ± 138 扩大到 ± 166 即可使

25、指尖 E 的盲区完全消失. 14 期 陈明生, 等: 六关节机械臂运动路 径设计 109 图 7 题目给参数下 E 点的轨 迹 图 8 改变 1 , 4 后 E 点绝对盲区估计范围 接下来考虑各连杆的长度, DE 由于处于末端, 处于灵活性考虑不宜太长. 对于平地作 业, A B 杆应该抬高 100m m 左右, 以将地面以下范围充分利用. 我们尝试着分别对 BC 和 CD 进行改变, 数值仿真表明, 随着 BC 和 CD 的增加, 有效作业范围有所扩大( 扩大范围与增加 BC 还是增加 CD 关系不大 , 并且 D 的盲区半径扩大, 因而要进一步扩大 区. 也便于指令的编写: 自由度只有 旋

26、转过程中, 当增加 ( 再继续改变 杆, 将 2 1 3 的范围, 减小盲 同时由于在臂碰问题中, 我们发现如果机械臂只考虑到 D 点, 算法会变得简单而易行, 1 , 2 和 3 , 1 控制 D 的 角, 2 和 3 控制 D 的 角, 在机器 1 时, 若机械臂离障碍物越来越近, 则停止改变 2 , 通过改变 2 和 1 3 后再增加 1 , 即可绕开障碍物. 同理若 和 3 改变过程中离障碍物越来越近, 则先改变 后, 和 3, 也可绕开障碍物. 为了使 E 能够自由收回到 D, 并在D 点靠近目标点后, E 伸出的过程中可以按指定直线到达目标点, 我们给出以下两点意见: 将 DE 调

27、整为伸缩 5 的变化范围设置为 ± 180. 变化范围由 ± 138 扩大到 5 综合以上讨论我们给出以下几点意见: 为减小 D 点盲区范围( 使盲区半径小于 DE , 应将 ± 166; 为便于绕障碍物算法的求解以及指令的编写, 建议将 DE 调整为伸缩杆, 将 化范围设置为 ± 180. 如是平地作业, A B 长度应适当增加约 100mm 以将地面以下部分范围提上地面. ! 若要扩大绝对作业范围, CD 和 B C 增加的影响几乎一样, 但是 CD 和 BC 增加后, D 点盲区的范围扩大, 应进一步扩大 3 3 的变 变化范围以减小 D 点盲区.

28、 最后, 关于图像处理技术计算半径. 首先, 将 D 点或者 E 点产生的 y 、 轨迹散点图转化 z 为 2 值图像. 首先将坐标平移, 使任意 y 0, z 0; 再将坐标取整 x = x ,y = y ; 最后将坐标落在图像 G 的位置的灰度值设为 1, 其它位置设为 0. 由于离散化的影响, 生成的 图像不具有几何的连续性, 而呈现出离散化特点, 如图 9 所示. 其次, 采用“ 摩尔” 运算, 对图像进行“ 操作. 首先选择以 10 为半径的圆盘作为“ 开” 膨胀 元”对 Z“ , 膨胀” 得到 Z ; 其次仍选择以 10为半径的圆盘作为“ 腐蚀元”对 Z 进行“ , 腐蚀” 操作

29、. 保持了原图像 Z 的边缘信息, 又填补了图像中不连续的空洞. 得到 Z Z 110 数学 的实践与认识 38 卷 最后分割包含中间黑色圆的子图像 C , 计算其中黑色像素( 灰度值为 0 的总个数, 其值 Z 也就是这个圆的面积. 经过简单计算, 也就得到了该圆的半径. 图 9D 轨迹散点图转化为 2 值图像 参考文献: 图 10“ 摩尔”运算处理后 D 轨迹图像 1 Donald Hearn, M Paul ine Baker . Compu ter Graphics w ith O penG L ( 3rd Edit ion M . Bei jing: Pu blish ing Hou

30、se of Elect ronics Indust ry, 2004. 2 马振华. 现代应用数学手册 M . 北京: 清华大学出版社, 1998. 3 Wil liam K Pratt . Digi tal Imag e Proces sing ( 3rd Edit ion M . Wil ey Int ers cience, 2003. 4 Sh eng Ping-Liu , Li C hen g-Wu, Zhen Lu . Impact dynamics and con tr ol of a fl exib le dual -arm space robot capt urin g an object J . A pplied M ath emat ics and Comput ation , 2007, 185( 2 : 1149-1159. 5 张志涌. 精通 M A T LA TB 6. 5 版 M .