中正平和的机器人学笔记——3.几个MATLAB实例

0. 轨迹

学机器人学前最想知道的，就是如何把机器人末端执行器平滑地从A点移动到B点，也许不太准确，应当说从位姿A移动到位姿B。

0.1 关节空间运动

考虑末端执行器在以下两个笛卡尔位姿之间移动
下面两种语法都可以表示（x,y,z,θ）的位姿

mdl_puma560  %载入提供的puma560机械臂模型
T1 = transl(0.4, 0.2, 0) * trotx(pi);
%T1 = SE3(0.4, 0.2, 0) * SE3.Rx(pi);

T2 = transl(0.4, -0.2, 0) * trotx(pi/2);
%T2 = SE3(0.4, -0.2 0) * SE3.Rx(pi/2);

那么与之对应的关节坐标就是

q1 = p560.ikine6s(T1);
q2 = p560.ikine6s(T2);

我们再给出时间向量

t = [0:0.05:2]';

通过对q1和q2两个位姿之间进行平滑插值就可以得到一个关节空间轨迹，我们用如下函数

q = jtraj(q1, q2, qt); 
%jtraj相当于具有tpoly插值的mtraj，
%但优化了多轴情况，且允许使用额外参数设置v0和vt
%以后就不用下面两个插值函数
%q = mtraj(@tpoly, q1, q2, t);
%q = mtraj(@lspb, q1, q2, t);
%[q,qd,qdd] = jtraj(q1, q2, t);
%以下是SerialLink类提供的jtraj更简便的用法
q = p560.jtraj(T1, T2, t);

最终得到的轨迹可以绘制出来观察，包括关节角的变化都可以看到

p560.plot(q) %绘制关节角的变化如图a
plot(t, q) %绘制关节角随时间的变化，如图a

T = p560.fkine(q); %得到三位笛卡尔轨迹
p = transl(T);%轨迹的位移部分
% plot(p(:,1),p(:,2)) %绘制xy平面内的轨迹，如图c
% plot(t, tr2rpy(T)) %画出另一种位姿表达，如图d

从图c可以看到，显然我们做的这条轨迹不是一条直线。这个很好理解，我们只给机器人初始点和结束点两个坐标，那么机器人在运动时，转动其腰部关节，显然会是一个弧线。那么我们如何保证机器人末端在笛卡尔空间中也划出一条直线轨迹呢？
其实很简单，将原来的jtraj函数换成ctraj就可以，当然这里的简单只得是用MATLAB仿真起来很简单，并不是其他的。

Ts = ctraj(T1, T2, length(t));
%plot(t, transl(Ts) )
%plot(t, tr2rpy(Ts) )
qc = p560.ikine6s(Ts);
plot(t,q)
p560.plot(qc)