六轴机器人建模方法、正逆解、轨迹规划与实现
2019-09-09张兴华肖仕驰
张兴华 肖仕驰
摘要：本文用Matlab实现六轴机器人的建模和实现轨迹规划。讲解如何写正逆解以及轨迹插补的程序。程序是基于Matlab2 01 6a，工具箱版本为Robotic Toolbox9.10。
关键词：六轴机器人建模Matlab
1 D-H建模
三个两两相互垂直的XYZ轴构成欧几里得空间，存在六个自由度：沿XYZ平移的三个自由度，绕XYZ旋转的三个自由度。在欧几里得空间中任意线性变换都可以通过这六个自由度完成。
Denavit-Hartenberg提出的D-H参数模型能满足机器人学中的最小线性表示约定，用4个参数就能描述坐标变换：绕X轴平移距离a;绕X轴旋转角度alpha;绕Z轴平移距离d;绕Z轴旋转角度theta。
2标准D-H模型和改进D-H模型
对比来看参数并没有改变，标准的D-H模型是将连杆的坐标系固定在该连杆的输出端（下一关节），也即坐标系i 1与关节l对齐;改进的D-H模型则是将坐标系固定在该连杆的输入端（上一关节），也即坐标系i-l与关节对齐i-l。
3利用Matlab Robotic Toolbox建立机器人模型
alpha：连杆扭角;a：连杆长度;theta：关节转角;d：关节距离offset：偏移
clear;clc;%建立机器人模型
%theta d
a
alpha
offset
Ll=Link（[0
0.
25 p1/2
0
】）;%定义连杆的D-H参数
L2=Link（[p1/2
0
0.56
0
0
】）;
L3=Link（[0
0
0.035
p1/2
0
]）;
L4=Link（[0
0.515
0 p1/2
0
]）;
L5=Link（[pi
0
0
p1/2
0
]）;
L6=Link（[0
0.08
0
0
0
]）;
robot=SeriaILink（[Ll L2 L3 L4 L5 L61，1namel，manman）;%连接连杆，机器人取名manman
robot.plot（[0，p1/2，0，O，pi.O]）;%输出机器人模型，后面的六个角为输出时的theta姿态
robot.display（）;
其中表格为D-H参数，grav为重力加速度矢量base为基坐标系的齐次矩阵，tool為工具坐标系和末端连杆的坐标系之间的变换矩阵。在代码段l的后面加入teach指令，则可调整各个关节角度。
teach（robot）;
4运动学正逆解
运动学正解：根据6个关节角结算出末端位姿。
运动学逆解：根据末端位姿结算出关节角，这里会存在8组逆解，本文中用的反解函数会智能输出最优的一组解。
正解程序：
clear;clc;%建立机器人模型
%theta d
a
alpha
offset
Ll=Link（[0
0.
25 p1/2
0
]）;%定义连杆的D-H参数
L2=Link（[p1/2
0
0.56
0
0
】）;
L3=Link（[0
0
0.035 p1/2
0
]）;
L4=Link（[0
0.515
0
p1/2
0
】）;
L5=Link（[pi
O
pl/2
0
]）;
L6=Link（[0
0.08
0
0
]）;
robot=SerialLink（[Ll L2 L3 L4 L5 L6]，namel，manman）;%连接连杆，机器人取名manman
theta=LO，O，O，0，O，0];%指定的关节角
p=robot.fkine（theta）%fkine正解函数，根据给定的关节角theta，求解出末端位姿p
q=roloot.ikine（p）%ikine逆解函数，根据给定的末端位姿p，求解出关节角q
可以查看p和q，对比theta和q，发现是一致的
5轨迹规划
在实际应用中，我们一般都是知道末端的轨迹，然后使机器人动作。本文的例子是根据给定两个点的值，得到末端位姿，根据末端位姿再来规划轨迹。
clear;clc;%建立机器人模型
%theta
d
a
alpha
offset
Ll=Link（[0
0.4
0.025 p1/2
0
]）;%定义连杆的D-H参数
L2=Link（[p1/2
0
0.56
0
0
]）;
L3=Link（[0
0
0.035 p1/2
0
]）;
L4=Link（[0
0.515
0
p1/2
0
]）;
L5=Link（[pi
0
0
p1/2
0
]）;
L6=Link（[0
0.08
0
0
]）;
robot=SeriaILink（[Ll L2 L3 L4 L5 L61，1namel，manman）;%连接连杆，机器人取名manman
Tl=transl（0.5，O，0）;%根据给定起始点，得到起始点位姿
T2=transl（0，0.5，0）;%根据给定终止点，得到终止点位姿
ql=roloot.ikine（Tl）;%根据起始点位姿，得到起始点关节角
q2=roloot.ikine（T2）;%根据终止点位姿，得到终止点关节角
【q，qd，qddl=jtraj（ql，q2，50）;%五次多项式轨迹，得到关节角度，角速度，角加速度，50为采样点个数
grid on
T=robot.fkine（q）;%根据插值，得到末端执行器位姿
plot3（squeeze（T（1，4，：》，squeeze（T（2，4，i》，squeeze（T（3.4，：）））;%输出末端轨迹
hold on
robot.plot（q）;%动画演示
参考文献
[1]王永志.基于LinuxCNC的六軸机器人软件控制系统及运动算法研究[D].华南理工大学，2018.
[2]郭小羽，六轴机器人控制器底层软件设计及其算法仿真研究[D].深圳大学，2017.
