給定位置：

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);

% 會(huì)發(fā)現(xiàn)末端朝上，因?yàn)槟┒俗鴺?biāo)系默認(rèn)和世界坐標(biāo)系重合，世界坐標(biāo)系是朝上的
% 因此變換矩陣需要乘trotx(180),將坐標(biāo)系調(diào)整至向下

遍歷方式求軌跡：

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'); % 畫(huà)軌跡
ur3.plot(Qtraj,'trail','b','movie','tpolytrail.gif'); % 保存為動(dòng)畫(huà)

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');

直接使用插值函數(shù)求軌跡：

位姿插值：trinterp()

trinterp(T0, T1, M)

? T0：初始變換矩陣

? T1：結(jié)束變換矩陣

? 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');

% 五次多項(xiàng)式插值
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('五次多項(xiàng)式插值')
legend('x','y','z');
xlabel('time');
ylabel('position');
figure(4)
ur3.plot(ur3.ikunc(T_liner),'trail','r'); % 畫(huà)軌跡
ur3.plot(ur3.ikunc(T_liner),'trail','r','movie','linertrail.gif'); % 保存為動(dòng)畫(huà)
figure(5)
ur3.plot(ur3.ikunc(T_tpoly),'trail','g'); % 畫(huà)軌跡
ur3.plot(ur3.ikunc(T_tpoly),'trail','g','movie','tpolytrail.gif'); % 保存為動(dòng)畫(huà)

線性插值軌跡動(dòng)畫(huà)：(軌跡如上圖左所示)

五次多項(xiàng)式插值軌跡動(dòng)畫(huà)：（軌跡如上圖右所示，和上面用mtraj遍歷方式的軌跡相同）

笛卡爾軌跡ctraj():

TC = ctraj(T0, T1, N)

? T0：初始變換矩陣

? T1：結(jié)束變換矩陣

? N：插值次數(shù)（默認(rèn)梯形速度圖像），改成tpoly()可作為五次多項(xiàng)式插值方式使用。

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'); % 畫(huà)軌跡
ur3.plot(ur3.ikunc(T_liner),'trail','r','movie','linertrail.gif'); % 保存為動(dòng)畫(huà)
figure(5)
ur3.plot(ur3.ikunc(T_tpoly),'trail','g'); % 畫(huà)軌跡
ur3.plot(ur3.ikunc(T_tpoly),'trail','g','movie','tpolytrail.gif'); % 保存為動(dòng)畫(huà)

文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-420964.html

到了這里，關(guān)于【RTB機(jī)器人工具箱學(xué)習(xí)記錄】軌跡規(guī)劃實(shí)例的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！