Artificial Intelligence and Robotics Research 人工智能与机器人研究, 2013, 2, 54-59 http://dx.doi.org/10.12677/airr.2013.21009 Published Online February 2013 (http://www.hanspub.org/journal/airr.html) Arc Welding Robot Kinematics Analysis and Trajectory Planning in Cartesian Space Tao Song School of Mechanical Engineering and Automation, Beijing University of Aeronautics and Astronautics, Beijing Email: songtao828@gmail.com Received: Jan. 14th, 2013; revised: Jan. 29th, 2013; accepted: Feb. 5th, 2013 Abstract: Arc welding robots, as typical 6-DOF serial robots, are gradually playing an important role in th e automotive and other industries. In this paper, taking an independently designed arc welding robot as an example, demonstrated the establishment of D-H coordinate system, the robot kinematic analysis, and run a robot Cartesian space of linear and circular trajectory planning based on previous work. Employing a MFC application program, the derivatively generated data points are plotted in the Matlab to verify the correctness of the theoretical calculatio ns. Keywords: Arc Welding Robot; Kinematics Analysis; Cartesian Space; Trajectory Planning 弧焊机器人运动学分析及笛卡尔空间轨迹规划 宋 涛 北京航空航天大学机械工程及自动化学院,北京 Email: songtao828@gmail.com 收稿日期:2013 年1月14 日;修回日期:2013 年1月29 日;录用日期:2013 年2月5日 摘 要:弧焊机器人作为一种典型的 6自由度串联机器人,在汽车等行业中发挥着越来越重要的作用。本文以 一款自主研发的弧焊机器人为实例,建立 D-H 坐标系,进行机器人的运动学分析,并在此基础上完成机器人笛 卡尔空间的直线和圆弧轨迹规划。编写 MFC 应用程序,将生成的数据点在Matlab 平台上进行绘图,验证理论 计算的正确性。 关键词:弧焊机器人;运动学分析;笛卡尔空间;轨迹规划 1. 引言 6自由度串联机器人是工业机器人的重要组成部 分,而弧焊机器人作为串联机器人的一种典型应用, 在汽车等行业发挥着十分重要的作用[1]。由于弧焊机 器人的作业特点,即要求焊枪在移动过程中,不仅经 过起始点和终止点,而且要经过规划出的若干中间点 (continuous-path motion),因而对机器人笛卡尔空间的 轨迹规划就十分必要[2]。 本文以课题组和企业共同研制的一款 6自由度机 器人为实例,建立 D-H 坐标系,进行机器人的运动学 分析[3]。然后在笛卡尔空间进行直线和圆弧的位置规 划[4-6],并利用四元数插值完成其姿态规划[7]。最后, 利用编写的 MFC 应用程序,将生成的数据点在 Matlab 平台上进行绘图[8],验证理论计算的正确性,为机器 人的逆解求出关节位置奠定了基础[9]。 2. 运动学分析 2.1. 建立 D-H 坐标系 该款自主研发的弧焊机器人如图1所示,其中2、 3、4关节轴线相互平行。 Copyright © 2013 Hanspub 54 弧焊机器人运动学分析及笛卡尔空间轨迹规划 正 正 正 正 正 正 正 大 臂 小臂 腰部 Figure 1. Structure diagram of arc welding robot 图1. 弧焊机器人结构图 对该机器人进行运动学建模,按照 D-H方法建立 其连杆坐标系,如图 2所示。各连杆的结构参数和运 动参数,如表 1所示。 其中 , 1165 mma22003 mma , , ,, ,。 32040a 466.5d mm430a mm552d 0 mm11d 5 mm 000 mm Figure 2. The establishment of D-H coordinate system 图2. D-H坐标系的建立 Table 1. The D-H parameter 表1. D-H参数表 i 1i 1i i i d 1 0 0 1 1 d 2 90 1 a 2 0 3 0 2 a 3 0 4 0 3 a 4 4 d 5 90 4 a 5 5 d 6 90 0 6 0 2.2. 运动学正解 根据连杆坐标系和 D-H 参数表,机器人运动学方 程可描述为, 6 123456 0012345 ΤTTTTTT (1) 式中表示第 i个连杆坐标系相对于第 1 i iT1i 个连杆 坐标系的齐次变换矩阵,即 1 111 1111 0 0001 ii i ii iiiii i iii iiiii cs a sc ccsds ss cscdc 1 1 T (2) 式中 cos ,sin,cos iiiii csc i , sin0,1, ,6 ii si 。 将各个连杆变换矩阵相乘,便可得到该机器人的 “手臂变换矩阵”6 0,它是关节变量T12 6 ,,, 的函 数。 6 0 0001 x xx x y yy y z zz z noap noap noap T (3) 式中, 1234 5 6234 615 6 T 1234 5 6234 615 6 234 5 6234 6 xyz cccc ssssc nnnnscccss csc scccs 1234 5 62346156 T 1234 56234 6156 234 56234 6 xyz cccssc sss oooosccssc css scs cc 1 5 2341 5 T 1 5 23415 234 5 xyz csc sc aaaassc cc ss T 1423452343234 12 121 1 1423452343234 12 121 1 4234 5234 323 22 1 xyz pppp cacdsacds accac s acdsacdcascas asdcasas d 2.3. 运动学逆解 由于设计的弧焊机器人 2、3、4关节轴线相互平 行,满足工业机器人存在封闭解的条件(Pieper 原则)。 Copyright © 2013 Hanspub 55 弧焊机器人运动学分析及笛卡尔空间轨迹规划 因此本文采用封闭解法,求解该逆运动学问题。 116 11 00 0 0001 x xx x y yy y z zz z noap noap noap TT T (4) 1) 求解 1 观察等式(4),建立矩阵第二行第四列的等式关系 11xy4 s pcp d ,可得, 1 222 44 tan 2, tan 2, yx xy App A dppd (5) 正、负号对应于 1 的两个可能解。 2) 求解 5 联立矩阵(4)两端第 2行前3列等式,可求得, 22 5111 11 tan 2, xy xy xy Asncn soco sa xa 1 (6) 3) 求解 6 同样方式,联立矩阵(4)两端第2行第 1列及第2 行第 2列等式,则有 11 5 11 56 xy xy 6 s ncn sc s ocoss (7) 如果 ,将上下两等式相除得, 50s 6111 tan 2,1 x yxy A so cosn cn (8) 4) 求解 2 联立矩阵(4)两端第 1行第3列和第 3行第 3列等 式,则有 115 234 5 234 xy z ca sasc ass (9) 易得 2341 1 tan 2, z xy A acasa (10) 结合式(3),有下列关系式, 1423452343234 12 1211 42345234 323 221 y z psacdsacdcascas pasdc asasd (11) 若 ,则令 50s 412345 123441111 4 23452341 2222 23 2 2 y z A pasc dss dcass Bpas dc d CABaa a (12) 通过上式可解得, 222 2tan 2,tan 2, A ABACA B C (13) 5) 求解 3 结合(8)、(9)两式有, 32322 323 22 A ac ac Bas as (14) 进而可解得, 232 22 2 tan 2, A BasAac (15) 3232 (16) 6) 求解 4 423423 (17) 该机器人的运动反解可能存在8组。在求解 12 , 以及 5 式中的正负号组合可能得到八种解。在机器人 的实际规划中,需根据已知的起点和终点进行取舍。 3. 笛卡尔空间规划 在笛卡尔空间轨迹规划系统中,作业是用机器人 末端抓手位姿(位置和姿态)的笛卡尔坐标结点序列规 定的。因此结点指的是表示抓手位姿的齐次变换矩 阵。 3.1. 直线位置规划 已知两点坐标 1111 ,,pxyz 2 v 2 3 v 和 以及 起点速度 ,过程速度、终点速度 。为了简单易 解并且同时保证整条轨迹上的位移和速度都连续,设 定从到 和从到 两个过程加速度均恒定,分别 为 和。 2222 ,,pxyz, 3 v 1 v 2 v 2 1 v 1 v a a 根据两点间距离方程以及匀变速位移方程, 22 2121 21 22 21 11 22 32 32 2 2 dxx yy zz vv la vv la 2 (18) Copyright © 2013 Hanspub 56 弧焊机器人运动学分析及笛卡尔空间轨迹规划 可得, ,继而求得匀速运动时间 21 ldll 3 32 21 22213 12 ,, vv vv tlvt t aa 。 因此,我们可以得到任意时刻的位移计算公式, 2 11 1 12 113 2 122 122 123 2, 0 , 2, t vt attt ddvtt ttt ddvttt atttttT (19) 同时,由 和可得单位向量为, 1 p2 p 21 2121 21 xx yyzz pp dd ij nk l (20) 则任意时刻的坐标位置可由下述公式确定为, 1t ppn (21) 3.2. 圆弧位置规划 已知不共线的三点坐标分别为 、 和,以及起点线速度 、 过程线速度 、终点线速度。为了简单易解并且同 时保证整条轨迹上的位移和速度都连续,设定从 到 和从 到两个过程角加速度均恒定,分别为 1111 ,,pxyz 1 v 1 v 2222 ,,pxyz 2 v 2 v2 v3 v 3333 ,,pxyz 3 v 1 和2 。与直线位置规划类似,根据给定的速度和时间, 可以确定每一段运动的插值点个数。 根据不共线的空间三点能够确定唯一的平面以 及圆弧上三点到圆心的距离相等两个条件联立方程, 111 222 333 22 2111 22 2222 22 2333 1 10 1 1 abc xyz xyz xyz rxaybzc rxayb zc rxayb zc 2 2 2 (22) 易得圆心坐标 。 ,,o abc 设三点确定平面的单位法向量为 ,根据其与 分别垂直联立方 程, 123 ,,ppp uvwN12 23 ,pp pp 2121 21 3232 32 0 0 xxuyyvzzw xxuyyvzzw (23) 求出单位法向量。 类似于直线位置规划,可以得到某一时间转过的 弧度 。已知 1111 ,,pxyz,圆心,切向量 ,,o abc wN,,uv ,则可确定转过 后圆弧上点的坐标为, 1 ep p (24) 其中, ˆ 称之为运动旋量,其表达式为, ˆˆ ˆee e01 NN I r (25) 3.3. 姿态规划 四元数可以描述三维空间刚体的姿态和旋转变 换。并且利用四元数表示转动将会使运动更均匀和有 效。因此在此应用四元数表示机器人手部的姿态,进 行规划。四元数的表示形式为, ,,,,qssxyz sxyz vi j k (26) 根据机器人的末端姿态矩阵, x xx y yy z zz noa noa noa R (27) 可以建立两者之间的关系, 12 4 4 4 xyz zy xz yx snoa aoas ban s cno s (28) 已知两点的姿态和,通过公式可以得到四 元数 和,利用下述公式进行姿态的插值。 1 R2 R 1 q2 q 12 12 sin 1sin Slerp, ,sin qtq qqt t (29) 其中, 0,1 ,t 为两四元数之间的夹角。 4. 实验验证 该部分利用上述算法编写MFC应用程序,根据 输入的具体数值,通过程序运算,将得到的结果在 Matlab 上进行绘图,以验证位置规划和姿态规划的正 确性。 Copyright © 2013 Hanspub 57 弧焊机器人运动学分析及笛卡尔空间轨迹规划 4.1. 位置规划实验 分别在 MFC 中输入直线的P1 和P2 点,以及圆 弧的 P1、P2 和P3 点的位置坐标,并且输入起始点、 中间点和终止点的速度以及加速度和扫描周期。根据 已有信息以及上述算法进行计算(图3)。 将直线位置计算结果写入 Matlab 程序, x=[2 2.41 2.88 3.39 3.92 4.44 4.96 5.54 5.98 6.37 7]; y=[1 1.24 1.53 1.83 2.15 2.46 2.77 3.12 3.39 3.62 4]; z=[3 3.41 3.88 4.39 4.92 5.44 5.96 6.54 6.98 7.37 8]; xx=linspace(2,7); yy=spline(x,y,xx); zz=spline(x,z,xx); plot3(xx,yy,zz,'r',x,y,z,'o'); grid on,可得图 4所示结果。 同理,将圆弧位置计算结果写入 Matlab 程序可得 图5所示结果。 Figure 3. Line and circular position programming interface 图3. 直线圆弧位置规划界面 234567 1 2 3 4 3 4 5 6 7 8 Figure 4. Linear position interpolation 图4. 直线位置插值 4.2. 姿态规划实验 根据机器人正运动学可以求得始末两点的末端 姿态矩阵,并将其输入进MFC应用程序,由插值公 式得到数据点,然后再 Matlab 上进行验证(图6)。 将姿态规划计算结果写入 Matlab 程序, X=[px px+nx*d]; Y=[py py+ny*d]; Z=[p z pz+nz*d]; plot3(X,Y,Z) ; grid on; 其中,px,py,pz为位置规划出来的空间点坐标,nx, ny,nz为对应点插值出来的姿态向量,可得图7所示 结果。 246810 0 5 10 3 4 5 6 7 8 Figure 5. Arc position interpolation 图5. 圆弧位置插值 Figure 6. Orientation planning interface 图6. 姿态规划界面 Copyright © 2013 Hanspub 58 弧焊机器人运动学分析及笛卡尔空间轨迹规划 Copyright © 2013 Hanspub 59 -400 -200 0200 400 -300 -200 -100 0 -500 0 500 1000 同理,将圆弧姿态计算结果写入 Matlab 程序可得 图8所示结果。 5. 结束语 本文首先对 6自由度弧焊机器人进行运动学建 模,并在笛卡尔空间进行轨迹规划,将算法编写程序 进行验证,通过生成的轨迹曲线可以直观的观察出理 论计算的正确性,同时也为机器人的逆解求出关节位 置奠定了基础。 Figure 7. Linear orientation interpolation 图7. 直线姿态插值 参考文献 (References) 2 4 6 8 0 2 4 6 3 4 5 6 7 8 [1] 孙英飞, 罗爱华. 我国工业机器人发展研 究[J]. 科学技术 与 工程, 2012, 4(12): 2912-2918. [2] 兰虎, 陶祖伟, 段宏伟. 弧焊机器人示教编程技术[J]. 实验 室研究与探索, 2011, 9(30): 46-49. [3] 熊有伦. 机器人学[M]. 北京: 机械工业出版社, 1993. [4] 沐俊福, 刘正华. 基于 OpenGL 的三维圆弧的绘制[J]. 电脑 编程技巧与维护, 2012, 13: 62-63. [5] 陈国梁, 黄心汉, 王敏. 机器人 圆周运动的轨迹规划与实 现 [J]. 华中科技大学学报, 2005, 33(11): 63-66. [6] 陈伟华, 张铁. 六自由度喷涂机 器人插补算法的研究[J]. 微 计算机信息, 2009, 25(3-2): 251-252. [7] 刘松国, 朱世强. 基于四元数和 B样条的机械手平滑姿态规 划期[J]. 浙江大学学报(工学版), 2009, 7(43): 1192-1196. [8] 罗家佳, 胡国清. 基于 MATLAB 的机器人运动仿真研究[J]. 厦门大学学报(自然科学版), 2005, 9(44): 640-644. [9] 赵献丹, 赵虎. 六自由度串联机 械手运动学逆解研究[J]. 现 代制造技术与装备, 2010, 3: 11-13. Figure 8. Arc orientation interpolation 图8. 圆弧姿态插值 |