Matlab机器人工具箱（3）——轨迹规划

前言

承接上一篇博客Matlab机器人工具箱（2）——预分析（可达空间与可操作性的可视化），本篇介绍轨迹规划的方法，包括通用的规划方法，关节空间的规划和笛卡尔空间的轨迹规划。
*说明：*之前用的是10.2版本的工具箱，发现了一些bug，现在更新成了10.4的，有需要的可以在Peter Corke老爷子的网站上下载

1、通用的规划方法

1.1、点到点的规划

五次多项式

代码：

%% 五次多项式插值，0-1，插值50个点
s=tpoly(0 ,1 ,50);
[s,sd,sdd]=tpoly(0 ,1 ,50);  %把速度，加速度也得出来
plot(s);hold on;
s=tpoly(0 ,1 ,50, 0.5, 0.5);  % 指定起始点的速度
plot(s)
legend('端点速度为0','端点速度为0.5')

轨迹曲线：

梯形轨迹（抛物线过渡的直线插值）

代码：

%% 梯形轨迹（抛物线过渡的直线插值）0-1，插值50个点
[s1,s1d,s1dd] = lspb( 0, 1, 50 );
[s2,s2d,s2dd] = lspb( 0, 1, 50, 0.025 ); % 指定直线段速度
figure
plot(s1d);hold on;
plot(s2d)
legend('默认','直线段速度为0.025')
figure
plot(s1);hold on;
plot(s2)
legend('默认','直线段速度为0.025')

轨迹曲线：

速度曲线：

多维的情况

代码：

%% 多维的情况
%          插值函数  初始点  终点  插值个数
x=mtraj( @tpoly , [-1,0] , [1,-1],   100 );  
figure
plot(x);
legend('x','y')

轨迹曲线：

1.2 via point 的情况

代码：

%% via point 的情况
via = [ 4,1 ; 4,4 ; 5,2 ; 2,5 ];
%              via points   最大速度    每段运行时间  起点各轴坐标  采样时间间隔   抛物线过渡时间
q = mstraj(     via ,            []  ,     [2,2,2,2]   ,  [3,0] ,       0.05 ,        1  );
figure
plot(q)

这里要注意，最大速度和每段运行时间是冲突的，只能指定一个，另一个为空。
轨迹图：

1.3姿态插值

鉴于其他方法的局限性，姿态插值一般用四元数进行：
代码：

%% 姿态插值 （四元数）
q0 = UnitQuaternion(eye(3));
q1 = UnitQuaternion(rpy2r(15,30,45));
q = interp(q0,q1,[0:49]'/49);
tranimate(q.R,'movie','quat.gif');

插值结果在三维空间的效果就是绕测地线单轴转动，这样插值是最快的结果，相当于欧式三维空间的直线插值。

提到这个gif，在Matlab机器人工具箱（1）——机器人的建立、绘制与正逆运动学中介绍了一个输出gif的方法，但是更新了RTB版本后发现了一个更方便的方法，plot也可以通用：

tranimate(q.R,'movie','quat.gif');

还可以输出mp4格式的视频，把.gif后缀改成.MP4就好啦。

位姿插值

RTB提供了直接对位姿规划的函数：
代码：

%% 位姿插值
T0 = rpy2tr(45,10,20);
T1 = transl(1,2,3)*rpy2tr(15,30,45);
Ts = trinterp(T0,T1,[0:49]'/49);
figure
tranimate(Ts);
figure
plot(transl(Ts))
figure
plot(tr2rpy(Ts));
% 指定插值轨迹
Ts_ = trinterp(T0,T1,lspb(0,1,50));
figure
plot(transl(Ts_))
figure
plot(tr2rpy(Ts_));

% 速记函数ctraj,相当于trinterp(T0,T1,lspb(0,1,50));
Ts_2 = ctraj(T0,T1,50);
figure
plot(transl(Ts_2))
figure
plot(tr2rpy(Ts_2));

这里就不放图了，太多了，可以自己去跑一跑。

2、机器人关节空间轨迹规划

这里以matlab自带的puma560机器人为例进行说明。值得说明的是，在 第1节 中介绍的通用方法，对关节空间的规划仍然有效，只不过RTB提供了更为简洁的函数，因此单独说明。
代码：

%% 机器人关节空间轨迹规划
mdl_puma560
T1 = transl(0.3,0,0.3)*rpy2tr(45,10,20);
T2 = transl(0.6,0,0.6)*rpy2tr(45,20,25);
q1 = p560.ikine6s(T1);
q2 = p560.ikine6s(T2);
t = 0:0.05:2;
% 速记代码，jtraj相当于 q = mtraj(@tpoly,q1,q2,t);
q = jtraj(q1,q2,t);
% 还可以输出关节角速度和角加速度
[q,qd,qdd] = jtraj(q1,q2,t);
p560.plot(q,'trail','b-','movie','jtraj.gif')

机器人笛卡尔空间轨迹规划

在 第1节 中介绍的通用方法，对笛卡尔空间轨迹规划仍然有效，只不过RTB提供了更为简洁的函数，因此单独说明。
代码：

%% 机器人笛卡尔空间轨迹规划
mdl_puma560
t = 0:0.05:2;
T1 = transl(0.3,0,0.3)*rpy2tr(45,10,20);
T2 = transl(0.6,0,0.6)*rpy2tr(45,20,25);
Tc = ctraj(T1,T2,length(t));
figure
plot(t',transl(Tc));
figure
plot(t',tr2rpy(Tc));
figure
qc = p560.ikine6s(Tc);
p560.plot(qc,'trail','b-','movie','ctraj.gif')

只放个动图吧。

总结

本文介绍了RTB工具箱中关于轨迹规划部分常用的函数，一定注意工具箱的版本，越新越好，因为越新bug越少。下面会继续介绍动力学相关的部分。