MATLAB环境下六自由度焊接机器人运动学逆解及优化_王战中

机械设计与制造
第7期
182
Machinery Design ＆ Manufacture
2013 年 7 月
MATLAB 环境下六自由度焊接机器人运动学逆解及优化
王战中，杨长建，刘超颖，赵 赛，杜启鑫，熊 蒙
（石家庄铁道大学，河北 石家庄 050043）
摘 要：通用工业机器人运动学方程有多组逆解，而机器人系统最终只能选择其中的一组解来驱动机器人进行工作， 因此需要对多组逆解进行选择优化后作为机器人的最优解。由于逆解的求解过程非常复杂，因此通过分析焊接机器 人的正、逆运动学求解过程，提出用 MATLAB GUI 编程来自动求解机器人的多组逆解，并采用最短行程的原则自动 寻找机器人的最优解。与其他方法相比，该方法简单明了，计算速度快。最后给出两组计算实例来验证计算机求解结 果的正确性。 关键词：焊接机器人；逆运动学；MATLAB；最优解 中图分类号：TH16 文献标识码：A 文章编号：1001-3997（2013）07-0182-03
图 1 焊接机器人三维实体模型
Fig.1 3D Model of Welding Robot
a3
x4
y3
y4 z4
z3 x3
a2
z1 z0
y2
x0 y1 x1 y0
z2 x2
d4
xx65 z5
y6
z6 y5
d2
d
图 2 焊接机器人各连杆坐标系 Fig.2 Coordinate System of Welding Robot Joints
2 机器人运动学方程的建立
根据焊接机器人的三维实体模型，如图 1 所示。在上关节处 建立各连杆的连杆坐标系，如图 2 所示。关节 1 的轴线为铅直方 向，关节 1 和 2 的轴线垂直相交。关节 2 和 3 的轴线水平，距离为 a2。关节 3 和 4 的轴线垂直相交，距离为 a3。连杆的 D-H 参数，如 表 1 所示。
0 姨 姨 θ1 =Atan2 0py ，px 0-Atan2 d2 ，±
222
px +py -d2
（2）
其中，正、负号对应 θ1 的两个不同的解。
姨 θ3 =Atan2 5 a3，d4 姨 -Atan2 0k，±
姨 2 2 2
a3 +d4 -k
（3）
2 2 222 2 2
其中，k=
px
+py
+pz
-a2 -a3 2a2
-d2
-d4
5 5 θ23 =θ2 +θ3 =Atan2
0-a3 -a2 c3 0-d4 +a2 s3
姨pz + 姨pz +
0c1 px 0c1 px
+s1 +s1
py py
姨0a2 s3 -d4 姨0a2 c3 +a3
姨， 姨 （4）
对应 θ1 、θ3 的四种可能的组合，可以得到相应的四种 θ23 的
在求解 θ1 、θ3 中，分别对应的正、负号，可以得到四种不同的
解的组合，另外，腕部的翻转也可能得到两组解。其关系如下：
5′
θi =θi +180°（i=4、6）
′
θi =-θi （i=5）
（8）
在求解逆解的时候在三角方程求解过程中可能会出现方程
两端相差一个符号出现伪解，这时，如果若方程一端的每一个单
ay az
p0 y0 0
p0 z0
0
0
０0
0
０
０
１
0 0
3 机器人运动学逆解及最优解
（1）
θ5 =Atan2 5s5 ，c5 姨
（6）
其中，s5 =-ax 5c1 c23 c4 +s1 s4 姨-ay 5s1 c23 c4 -c1 s4 姨+az 5s23 c4 姨；
c5 =ax 5-c1 s23 姨+ay 5-s1 s23 姨+az 5-c23 姨
来稿日期：2012-09-16 基金项目：河北省科技支撑项目（122121113D）；河北省自然基金项目（E2013210107） 作者简介：王战中，（1969-），男，河北鹿泉人，博士，副教授，主要研究方向：机械制造及自动化；
杨长建，（1988-），男，河北武邑人，硕士，主要研究方向：机械制造及自动化
θ1
-160°～160°
θ2
-225°~45°
θ3
-45°~225°
θ6
-110°~170°
θ5
-100°~100°
θ6
-266°~266°
各连杆变换矩阵为：
cθ 0
0 0
1
-sθ1
0 1
T=
sθ 0
0 0
1
0 0
0
cθ1 0
0 00
0
0
0
0
0 0
0
cθ 0
0 0
2
-sθ1
0 1
0 0
0
sθ 0
1
，T= 0
Kinematics Inverse Solution and Optimization of 6DOF Welding Robot with MATLAB
WANG Zhan-zhong，YANG Chang-jian，LIU Chao-ying，ZHAO Sai，DU Qi-xin，XIONG Meng
解的选择并没有统一的标准，应根据具体的实际情况而定，一般情
况下满足行程最短、功率最省、回避障碍和受力最好等原则要求。由
于不考虑避免碰撞的情况，所以本焊接机器人采用“最短行程”的
标准来选择最优解，即使每个关节的移动量最小。由于机器人的
前三个转动关节比较大，后三个转动关节相对比较小，所以应该
加权处理，遵循“少移动大关节，多移动小关节”的原则[6]。
0
0
0 0
0
1
00 0
0
00 0
0
0
1 00 0
其中，Si =sinθi ；ci =cosθi 。
00
1
2
3
5
运动学方程可以表示为：6 T=1 T×2 T×3 T×4 T×6 T
方程的左端为末端位姿，可以表达为：
n o a p 0
0
0 0
x
x
x
x
0 0
0
6 T=［n
o
a
p］=
n0
0y 0
n0
0z
oy oz
θ6 的封闭解为：θ6 =Atan2 5s5 ，c5 姨
（7）
其中，-nx 5c1 c23 s4 -s1 c4 姨-ny 5s1 c23 s4 +c1 c4 姨+nz 5s23 s4 姨=s6
nx 55c1 c23 c4 +s1 s4 姨c5 -c1 s23 s5 5+
ny 55s1 c23 c4 -c1 s4 姨c5 -s1 s23 s5 5-nz 5s23 c4 c5 +c23 s5 姨=c6
第7期
王战中等：MATLAB 环境下六自由度焊接机器人运动学逆解及优化
183
表 １ 焊接机器人的 Ｄ－Ｈ 参数表 Ｔａｂ．１ Ｄ－Ｈ Ｐａｒａｍｅｔｅｒｓ ｏｆ Ｗｅｌｄｉｎｇ Ｒｏｂｏｔ
i
ai-1
αi-1
di
1
0
0°
0
2
0
-90°
d2
3
a2
0°
0
4
a3
-90°
d4
5
0
90°
0
6
0
-90°
0
θi
关节变量范围
4 MATLAB 编程自动求解
MATLAB GUI 是一款高性能的数值计算和可视化软件，其 特点为：功能强大，可实现数组、向量和矩阵的计算，编程效率高， 提供丰富的库函数，界面友好，函数命令非常多等特点 [7-8]。由于 在求解该机器人的逆解过程中其末端的位姿矩阵只用到了 n、a、 p 三个向量所以在创建见交互界面时不用创建 o 的动态文本框。 设定各个动态文本框的 string 和 tag 属性，作为输入窗口。用其中 的 get 函数得到输入的数据并转化为数值型以便计算；设定各个 静态文本框的 string 和 tag 属性，作为输出（显示）窗口。用 set 函 数将计算后得到的数据输出到对应的静态文本框；用 set 函数来 编写“清除”按钮的回调函数，当点击“清除”按钮时，将空字符赋 予各个静态和动态文本框。如图 3 所示。
解，所以 θ2 的四种可能的解为：
θ2 =θ23 -θ3
θ4 =Atan2 5-ax s1 +ay c1 ，-ax c1 c23 -ay s1 c23 +az s23 姨
（5）
当 θ5 =0 时，机器人处于畸形位，即机器人的第 4 和第 6 关
节轴重合，若方程（5）的两个变量都接近 0，那么机器人就处于畸
1 引言
六自由度机器人逆解的个数不是唯一的，一般情况下，连杆 的非零参数越多，到达某一特定位置的途径也就越多。在不考虑 外部因素的情况下，一个全旋转关节的六自由度机器人的逆解可 能多达 16 组，对于解耦六自由度机器人 （具有球型手腕的机器 人），最多有 8 组逆解。但在实际工作中，机器人系统最终只能选 择一种最优解来执行任务[1-2]。针对工业中常用的六自由度焊接机 器人，按“最短行程”的标准求解最优逆解并用 MATLAB 软件编 写程序，同时用 MATLAB 中 GUI 模块编程建立交互界面，从而实 现了只需输入机器人末端的位置和姿态，就可以自动求解各关节 变量，并通过判断选取最优解。
形位，否则不是。若是畸形位就可以任意选取 θ4 的值，然后计算
0 0
0
-sθ4 0