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

4 MATLAB 编程自动求解
MATLAB GUI 是一款高性能的数值计算和可视化软件，其 特点为：功能强大，可实现数组、向量和矩阵的计算，编程效率高， 提供丰富的库函数，界面友好，函数命令非常多等特点 [7-8]。由于 在求解该机器人的逆解过程中其末端的位姿矩阵只用到了 n、a、 p 三个向量所以在创建见交互界面时不用创建 o 的动态文本框。 设定各个动态文本框的 string 和 tag 属性，作为输入窗口。用其中 的 get 函数得到输入的数据并转化为数值型以便计算；设定各个 静态文本框的 string 和 tag 属性，作为输出（显示）窗口。用 set 函 数将计算后得到的数据输出到对应的静态文本框；用 set 函数来 编写“清除”按钮的回调函数，当点击“清除”按钮时，将空字符赋 予各个静态和动态文本框。如图 3 所示。
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 姨
-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
Fra Baidu bibliotek
+s1 +s1
py py
姨0a2 s3 -d4 姨0a2 c3 +a3
姨， 姨 （4）
对应 θ1 、θ3 的四种可能的组合，可以得到相应的四种 θ23 的
机械设计与制造
第7期
182
Machinery Design ＆ Manufacture
2013 年 7 月
MATLAB 环境下六自由度焊接机器人运动学逆解及优化
王战中，杨长建，刘超颖，赵 赛，杜启鑫，熊 蒙
（石家庄铁道大学，河北 石家庄 050043）
摘 要：通用工业机器人运动学方程有多组逆解，而机器人系统最终只能选择其中的一组解来驱动机器人进行工作， 因此需要对多组逆解进行选择优化后作为机器人的最优解。由于逆解的求解过程非常复杂，因此通过分析焊接机器 人的正、逆运动学求解过程，提出用 MATLAB GUI 编程来自动求解机器人的多组逆解，并采用最短行程的原则自动 寻找机器人的最优解。与其他方法相比，该方法简单明了，计算速度快。最后给出两组计算实例来验证计算机求解结 果的正确性。 关键词：焊接机器人；逆运动学；MATLAB；最优解 中图分类号：TH16 文献标识码：A 文章编号：1001-3997（2013）07-0182-03
θ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
在求解 θ1 、θ3 中，分别对应的正、负号，可以得到四种不同的
解的组合，另外，腕部的翻转也可能得到两组解。其关系如下：
5′
θi =θi +180°（i=4、6）
′
θi =-θi （i=5）
（8）
在求解逆解的时候在三角方程求解过程中可能会出现方程
两端相差一个符号出现伪解，这时，如果若方程一端的每一个单
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
来稿日期：2012-09-16 基金项目：河北省科技支撑项目（122121113D）；河北省自然基金项目（E2013210107） 作者简介：王战中，（1969-），男，河北鹿泉人，博士，副教授，主要研究方向：机械制造及自动化；
杨长建，（1988-），男，河北武邑人，硕士，主要研究方向：机械制造及自动化
1 引言
六自由度机器人逆解的个数不是唯一的，一般情况下，连杆 的非零参数越多，到达某一特定位置的途径也就越多。在不考虑 外部因素的情况下，一个全旋转关节的六自由度机器人的逆解可 能多达 16 组，对于解耦六自由度机器人 （具有球型手腕的机器 人），最多有 8 组逆解。但在实际工作中，机器人系统最终只能选 择一种最优解来执行任务[1-2]。针对工业中常用的六自由度焊接机 器人，按“最短行程”的标准求解最优逆解并用 MATLAB 软件编 写程序，同时用 MATLAB 中 GUI 模块编程建立交互界面，从而实 现了只需输入机器人末端的位置和姿态，就可以自动求解各关节 变量，并通过判断选取最优解。
0 0
0
-sθ4 0
-cθ4 0
0a 0
3
0 0
1
0
d ， 4
0 0
00
0 0
0
01
0 0
cθ 0
0 0
5
4 5
T=
0 0
0
sθ 0
0 05
0 00
0
-sθ5
0
0
0 0
0
cθ 0
0 0
6
0 cθ5
-1 0
0 0
0 0 0 0 0
5
，6 T=
0 0
0
000-sθ6
0
0
1
00 0
0 00
0
-sθ6 0 -cθ6 0
θ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
独项都含有所求关节转角的正弦值和余弦值，将所求角度进行
1，3 或 2，4 象限互换[5]。
3.2 最优解的选取
六自由度的机器人虽然有 8 组逆解，但是由于机器人结构
的限制，有的关节变量不能在全部 3600 内运动，有的解甚至是全
部解都无法解出。机器人在多组解的情况下必须选取其中的一组作
为最优解。如何从多组解中选择其中的一种作为最优解？运动学逆
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
3.1 机器人运动学逆解
机器人运动学逆解的求解有多种方法，主要有几何法、解析
法、迭代法以及三种方法的组合。这三种方法中，解析法简单明
了，计算速度快，对机器人运动学分析很有效[3-4]。如果连杆末端的
i-1 -1
位姿已知，即 n、o、a、p 都是已知的，则可以以i T （i=1，2，3，4，5）
依次左乘方程（1）的两边，把关节变量分离出来，进而求解。
图 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
解的选择并没有统一的标准，应根据具体的实际情况而定，一般情
况下满足行程最短、功率最省、回避障碍和受力最好等原则要求。由
于不考虑避免碰撞的情况，所以本焊接机器人采用“最短行程”的
标准来选择最优解，即使每个关节的移动量最小。由于机器人的
前三个转动关节比较大，后三个转动关节相对比较小，所以应该
加权处理，遵循“少移动大关节，多移动小关节”的原则[6]。
（Department of Mechanical Engineering，Shijiazhuang Tiedao University，Hebei Shijiazhuang 050043，China）
Abstract：Usually there are more than one inverse solutions to kinematics equation of general industrial robot，while the robot control -system will only use one optimized solution as the optimal solution to drive the robot，so the kinematics inverse solutions should be optimized and chosen. Due to the complexity of the inverse solution and based on the analysis of the welding robot's positive and inverse kinematics solution，a method that use the MATLAB GUI programming to solve the inverse solution and the shortest stroke principle to automatic search for the optimal solution of the robot is proposed. Compared with other methods，this method is simple and clear，and the computing speed is fast. Finally two group of calculating examples are given to verify the correctness of the result calculated with this method. Key Words：Welding Robot；Inverse Kinematics；MATLAB；Optimal Solution
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
形位，否则不是。若是畸形位就可以任意选取 θ4 的值，然后计算
0
2
0 0
0
0 0
1
0
0
cθ1 0
0
1 00 0
0 00
0
0
0
0
0 0
0
0
0
0
， 0
0
1
00 0
0
1 00 0
cθ 0
0 0
3
-sθ3
2 3
T=
sθ 0
0 0
3
0 0
0
cθ3 0
0 00
0
0
0
0
0 0
0
cθ 0
0 0
4
0 1
0 0
0 0 0 0 0
3
，4 T=
0 0
0 0
00-sθ4
0
1 00 0
0
第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
关节变量范围
解，所以 θ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，那么机器人就处于畸
2 机器人运动学方程的建立
根据焊接机器人的三维实体模型，如图 1 所示。在上关节处 建立各连杆的连杆坐标系，如图 2 所示。关节 1 的轴线为铅直方 向，关节 1 和 2 的轴线垂直相交。关节 2 和 3 的轴线水平，距离为 a2。关节 3 和 4 的轴线垂直相交，距离为 a3。连杆的 D-H 参数，如 表 1 所示。