【RTB机器人工具箱学习记录】轨迹规划实例

给定位置：

T1 = transl(0.3,-0.5,0)*trotx(180);
T2 = transl(0.3,0.5,0.5)*trotx(180);
q1 = ur3.ikunc(T1);
q2 = ur3.ikunc(T2);

ur3.plot(q1);
pause;
ur3.plot(q2);

% 会发现末端朝上，因为末端坐标系默认和世界坐标系重合，世界坐标系是朝上的
% 因此变换矩阵需要乘trotx(180),将坐标系调整至向下

遍历方式求轨迹：

P1=[0.3,-0.5,0];
P2=[0.3,0.5,0.5];
t=linspace(0,2,51);
Traj=mtraj(@tpoly,P1,P2,t);

n=size(Traj,1);
T=zeros(4,4,n);
for i=1:n
    T(:,:,i)=transl(Traj(i,:))*trotx(180);
end

Qtraj=ur3.ikunc(T);
figure(1)
% ur3.plot(Qtraj,'trail','b'); % 画轨迹
ur3.plot(Qtraj,'trail','b','movie','tpolytrail.gif'); % 保存为动画

figure(2)
hold on
plot(t,Traj(:,1),'.-','linewidth',1);
plot(t,Traj(:,2),'.-','linewidth',1);
plot(t,Traj(:,3),'.-','linewidth',1);
grid on
legend('x','y','z');
xlabel('time');
ylabel('position');

---

直接使用插值函数求轨迹：

位姿插值：trinterp()

trinterp(T0, T1, M)

​ T0：初始变换矩阵

​ T1：结束变换矩阵

​ M：

T1=transl(P1);
T2=transl(P2);
% 线性插值
T_liner = trinterp(T1,T2,51);
P_liner = transl(T_liner);
t1 = linspace(0,2,51);
figure(3)
subplot(1,2,1);
hold on 
plot(t1,P_liner(:,1),'.-','linewidth',1);
plot(t1,P_liner(:,2),'.-','linewidth',1);
plot(t1,P_liner(:,3),'.-','linewidth',1);
grid on
title('线性插值')
legend('x','y','z');
xlabel('time');
ylabel('position');

% 五次多项式插值
T_tpoly = trinterp(T1,T2,tpoly(0,2,50)/2);
P_tpoly = transl(T_tpoly);
t2 = linspace(0,2,50);
subplot(1,2,2);
hold on 
plot(t2,P_tpoly(:,1),'.-','linewidth',1);
plot(t2,P_tpoly(:,2),'.-','linewidth',1);
plot(t2,P_tpoly(:,3),'.-','linewidth',1);
grid on
title('五次多项式插值')
legend('x','y','z');
xlabel('time');
ylabel('position');
figure(4)
ur3.plot(ur3.ikunc(T_liner),'trail','r'); % 画轨迹
ur3.plot(ur3.ikunc(T_liner),'trail','r','movie','linertrail.gif'); % 保存为动画
figure(5)
ur3.plot(ur3.ikunc(T_tpoly),'trail','g'); % 画轨迹
ur3.plot(ur3.ikunc(T_tpoly),'trail','g','movie','tpolytrail.gif'); % 保存为动画

线性插值轨迹动画：(轨迹如上图左所示)

五次多项式插值轨迹动画：（轨迹如上图右所示，和上面用mtraj遍历方式的轨迹相同）

---

笛卡尔轨迹ctraj():

TC = ctraj(T0, T1, N)

​ T0：初始变换矩阵

​ T1：结束变换矩阵

​ N：插值次数（默认梯形速度图像），改成tpoly()可作为五次多项式插值方式使用。

T1=transl(P1);
T2=transl(P2);
T_clspb = ctraj(T1,T2,51);
T_ctpoly = ctraj(T1,T2,tpoly(0,2,50)/2);
Pcls = transl(T_clspb);
Pctp = transl(T_ctpoly);
t1 = linspace(0,2,51);
t2 = linspace(0,2,50);
figure(6)
hold on 
plot(t1,Pcls(:,1),'.-','linewidth',1);
plot(t1,Pcls(:,2),'.-','linewidth',1);
plot(t1,Pcls(:,3),'.-','linewidth',1);
plot(t2,Pctp(:,1),'.-','linewidth',1);
plot(t2,Pctp(:,2),'.-','linewidth',1);
plot(t2,Pctp(:,3),'.-','linewidth',1);
grid on
xlabel('time');
ylabel('position');
legend('x\_lspb','y\_lspb','z\_lspb','x\_tpoly','y\_tpoly','z\_tpoly');
figure(4)
ur3.plot(ur3.ikunc(T_liner),'trail','r'); % 画轨迹
ur3.plot(ur3.ikunc(T_liner),'trail','r','movie','linertrail.gif'); % 保存为动画
figure(5)
ur3.plot(ur3.ikunc(T_tpoly),'trail','g'); % 画轨迹
ur3.plot(ur3.ikunc(T_tpoly),'trail','g','movie','tpolytrail.gif'); % 保存为动画