機器人軌迹規劃
- 關節空間軌迹
[q,qd,qdd] = jtraij(q0,qf,m)
該函數表示關節坐标從初始關節角度q0（1xN）到終止關節角度qf（1xN）變化。預設利用五次多項式與預設零邊界條件計算軌迹。假定時間以M步從0到1變化。關節速度和加速度可以分别以qd（MxN）qdd（MxN）傳回。軌迹q，qd和qdd是MxN矩陣，每個時間步長一行，每個關節一列。
- 笛卡爾空間軌迹
Tc=ctraj（T0，T1，n）
該函數表示從姿态T0到T1的笛卡爾軌迹（4x4xN），沿路徑具有梯形速度分布的N個點。Tc是齊次變換序列，最後一個下标是點索引，即T（：，：，i）是第i個點路徑。如果T0或T1為[]，則視其為機關矩陣。
知道了上述軌迹規劃函數之後，我們可以通過工具箱自帶的PUMA560機器人模型進行試驗。在matlab中輸入 mdl_pima560 ,我們将獲得機器人模型以及幾個模型的初始狀态參數。
mdl_puma560 %打開模型
qz %零角度狀态
qr %就緒狀态，機械臂伸直且垂直
qs %伸展狀态，機械臂伸直且水準
qn %标準狀态，機械臂靈巧工作狀态
p560.plot(qn) %畫出标準狀态的圖形
接下來我将利用上述機器人模型來進行軌迹運動規劃的仿真。
mdl_puma560%調出puma560
t=[0:0.05:2]; %兩秒完成軌迹，步長0.05
%産生位姿矩陣法1：直接給出關節角度
T1 = p560.fkine([0 0 0 0 0 0]);%生成一個位姿
T2 = p560.fkine([pi/2 pi/3 pi/6 0 0 0]);%生成一個位姿
q=p560.jtraj(T1,T2,t);%輸入SE3 or 4*4變換矩陣，生成軌迹
%生成模型運動軌迹圖
filename = 'demo.gif';
for i = 1:length(t)
pause(0.01)
p560.plot(q(i,:));
f = getframe(gcf);
imind = frame2im(f);
[imind,cm] = rgb2ind(imind,256);
if i == 1
imwrite(imind,cm,filename,'gif', 'Loopcount',inf,'DelayTime',0.1);
else
imwrite(imind,cm,filename,'gif','WriteMode','append','DelayTime',0.1);
end
end
- 關節空間和笛卡爾空間的軌迹規劃
關節空間規劃是根據首位位姿，求出關節角度然後再進行規劃，而笛卡爾空間規劃是直接根據位姿進行規劃。為了友善對比，接下來我将給出首位位姿矩陣，然後分别運用上述兩種不同的方法進行規劃，最後都轉換為關節空間進行比較。
mdl_puma560%調出puma560
t=[0:0.05:2];%兩秒完成軌迹，步長0.05
T1 = [0 0 1 0.5963
0 1 0 -0.1501
-1 0 0 -0.01435
0 0 0 1 ];
T2 = [0 0 1 0.8636
0 1 0 -0.1501
-1 0 0 -0.0203
0 0 0 1];
%關節空間軌迹規劃
q1 = p560.ikine6s(T1);%根據末端位姿，求各關節
q2 = p560.ikine6s(T2);
qq=jtraj(q1,q2,t);%根據各關節，生成軌迹
Tqq=p560.fkine(qq);
%笛卡爾運動
%笛卡爾空間中直線運動,生成從SE3空間兩點間直線的一系列中間位置，結果表達為4*4齊次換矩陣
Ts=ctraj(T1,T2,length(t));
qs=p560.ikine6s(Ts);
figure(1)%繪制各關節角度
for i=1:6
subplot(2,3,i)
plot(t, qq(:,i))
hold on;
plot(t, qs(:,i))
legend(‘關節空間’,‘笛卡爾空間’)
hold off;
end
figure(2) %繪制空間運動軌
pq=transl(Tqq);%提取旋轉矩陣中的位移部分
ps=transl(Ts);
%依次是 ‘關節空間’,‘笛卡爾空間’
subplot(2,1,1)
plot3(pq(:,1),pq(:,2),pq(:,3))
subplot(2,1,2)
plot3(ps(:,1),ps(:,2),ps(:,3))
各關節的運動規劃圖
兩種方法的運動軌迹圖
觀察可得：盡管機器臂最後的始末位置相同，但是在不同的軌迹規劃下，機器臂的關節運動和末端執行器的運動軌迹确相差甚遠。不同的方法各有自己的優缺點，在實際運用中，我們應該根據自己的實際需求來選擇。