這是《機器人技術基礎》個人課程實驗之一，按照學號尾數不同分配給每人的取放軌跡規划方式也不同，包括3-4-3、4-3-4等三段軌跡規划方法，而我抽中的是七次多項式實現，不存在優劣之分，特地說明一下。下面是詳細內容：
一、開發平台及工具
開發平台：Matlab
開發工具：Robotics ToolBox
二、過程記錄
2.1 選定建模對象
選擇PUMA560作為建模對象：
2.2 選定轉移過程點
主要是確定起始位置（取點）、轉移位置（抬起或離開點）、卸下位置（下降點）和目標位置（放置點）；選定四個點位置坐標如下：
起始位置：aim0 = [0,-0.5,-0.5];
離開位置：aim1 = [0,-0.5,0.2];
下降位置：aim2 = [-0.5,0.5,0.2];
放置位置：aimx = [-0.5,0.5,-0.5];
2.3 整理邊界條件
利用七次多項式進行軌跡規划：
（1）已知本地時間t = 0時，初始位置：
（2）已知本地時間t = 0時，初始速度：
（3）已知本地時間t = 0時，初始加速度：
（4）已知第一中間點位置：
（5）已知第二中間點位置：
（6）已知本地時間t = tm時，目標位置：
（7）已知本地時間t = tm時，目標速度：
（8）已知本地時間t = tm時，目標加速度：
由(1)~（8）可以寫成如下矩陣形式：
或表示為：
和
通過計算 [M]-1 即可求出所有的未知系數，於是就求得了目標運動軌跡的運動方程，從而可以控制機器人使其經過給定的位置，同樣的方法可用於其他關節求解。
2.4 求解轉移過程點關節角
利用Matlab Robotics Box 對四個位置點求逆解得到關節角度：
theta0 = p560.ikine6s(T0,'rdf');%左臂、手肘朝下、手腕翻轉(旋轉180度) theta1 = p560.ikine6s(T1,'rdf'); theta2 = p560.ikine6s(T2,'rdf'); thetax = p560.ikine6s(Tx,'rdf');
計算結果：
2.4 求解軌跡未知系數
%初始條件 theta0_ = [0 0 0 0 0 0];%初始位置速度 theta0__ = [0 0 0 0 0 0];%初始位置加速度 thetax_ = [0 0 0 0 0 0];%目標位置速度 thetax__ = [0 0 0 0 0 0];%目標位置加速度 Theta = [theta0' theta0_' theta0__' theta1' theta2' thetax' thetax_' thetax__']'; M = [1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 2 0 0 0 0 0 1 t1 t1^2 t1^3 t1^4 t1^5 t1^6 t1^7 1 t2 t2^2 t2^3 t2^4 t2^5 t2^6 t2^7 1 tm tm^2 tm^3 tm^4 tm^5 tm^6 tm^7 0 1 2*tm 3*tm^2 4*tm^3 5*tm^4 6*tm^5 7*tm^6 0 0 2 6*tm 12*tm^2 20*tm^3 30*tm^4 42*tm^5]; C = M^-1 * Theta;%第i列對應第i個關節的其次多項式系數
各關節利用七次多項式進行軌跡規划的系數列表如下：
2.5 求解各關節相關運動指標
計算關節的位置、速度及加速度軌跡函數：
%計算關節各函數 tmietick = 0.1; T = 0: tmietick:9; %角度 Q = [ones(int16(9/tmietick)+1,1) T' (T.^2)' (T.^3)' (T.^4)' (T.^5)' (T.^6)' (T.^7)']*C; %速度 Qv =[zeros(int16(9/tmietick)+1,1) ones(int16(9/tmietick)+1,1) 2* T' 3*(T.^2)' 4*(T.^3)' 5*(T.^4)' 6*(T.^5)' 7*(T.^6)']*C; %加速度 Qa =[zeros(int16(9/tmietick)+1,1) zeros(int16(9/tmietick)+1,1) 2*ones(int16(9/tmietick)+1,1) 6*T' 12*(T.^2)' 20*(T.^3)' 30*(T.^4)' 42*(T.^5)']*C;
各關節運動位置、速度及加速度軌跡曲線繪制如下：
2.6 繪制運動軌跡
%正運動學分析 Txy=p560.fkine(Q); %畫軌跡 Tjtraj1=transl(Txy); x = Tjtraj1(:,1); y = Tjtraj1(:,2); z = Tjtraj1(:,3); figure waitforbuttonpress; plot3(x,y,z,'b');%軌跡圖像 hold on; %畫出四個過程點 [x0,y0,z0] = ellipsoid(aim0(1),aim0(2),aim0(3),0.05,0.05,0.05); [x1,y1,z1] = ellipsoid(aim1(1),aim1(2),aim1(3),0.05,0.05,0.05); [x2,y2,z2] = ellipsoid(aim2(1),aim2(2),aim2(3),0.05,0.05,0.05); [xx,yx,zx] = ellipsoid(aimx(1),aimx(2),aimx(3),0.05,0.05,0.05); surf(x0,y0,z0) %畫起始點 surf(x1,y1,z1) %畫提升點 surf(x2,y2,z2) %畫下降點 surf(xx,yx,zx) %畫目標點 hold on;
運動軌跡如藍線曲線所示：
三、思考
由上圖可以明顯發現7次多項式進行插值得到的軌跡為一空間不規則曲線，原因在於我們是對機械臂6個關節進行獨立的7次多項式軌跡規划，已知條件只有初末位置的關節位置、速度及加速度以及兩個中間點的關節位置共8個參數，同時由於4個位置點的各關節角度值是由ikine6s進行運動學逆解求解得到，雖然在計算時限制了手臂、手肘以及手腕的狀態，但是這樣得到逆解並一定能滿足我們的實際需求，因此在僅存在這些已知條件的情況下，我們沒有辦法對整個長距離的機器人的運動狀態進行精確控制。因此，要想機器人的運動狀態足夠精確，我必須引入更多的條件，比如增加中間點的數量或者各時刻的關節速度等等。
完整視頻B站傳送門：Matlab Robotics ToolBox 實戰 -- 七次多項式取放軌跡規划
四、附源程序
close all;
clc;
mdl_puma560
t0 = 0;%開始時刻
t1 = 2;%提升結束時刻
t2 = t1 + 4;%平移結束時刻
tm = t2 + 3;%下降結束時刻
t0_1 = 0:0.2:2;%上升時間
t1_2 = 0:0.5:4;%平移時間
t2_x = 0:0.3:3;%下降時間
aim0 = [0,-0.5,-0.5];%取貨點
aim1 = [0,-0.5,0.2];%提升點
aim2 = [-0.5,0.5,0.2];%下落點
aimx = [-0.5,0.5,-0.5];%存貨點
T0 = transl(aim0);
T1 = transl(aim1);
T2 = transl(aim2);
Tx = transl(aimx);
theta0 = p560.ikine6s(T0,'rdf');%左臂、手肘朝下、手腕翻轉(旋轉180度)
theta1 = p560.ikine6s(T1,'rdf');
theta2 = p560.ikine6s(T2,'rdf');
thetax = p560.ikine6s(Tx,'rdf');
%初始條件
theta0_ = [0 0 0 0 0 0];%初始位置速度
theta0__ = [0 0 0 0 0 0];%初始位置加速度
thetax_ = [0 0 0 0 0 0];%目標位置速度
thetax__ = [0 0 0 0 0 0];%目標位置加速度
Theta = [theta0' theta0_' theta0__' theta1' theta2' thetax' thetax_' thetax__']';
M = [1 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0
0 0 2 0 0 0 0 0
1 t1 t1^2 t1^3 t1^4 t1^5 t1^6 t1^7
1 t2 t2^2 t2^3 t2^4 t2^5 t2^6 t2^7
1 tm tm^2 tm^3 tm^4 tm^5 tm^6 tm^7
0 1 2*tm 3*tm^2 4*tm^3 5*tm^4 6*tm^5 7*tm^6
0 0 2 6*tm 12*tm^2 20*tm^3 30*tm^4 42*tm^5];
C = M^-1 * Theta;%第i列對應第i個關節的其次多項式系數
%計算關節各函數
tmietick = 0.1;
T = 0: tmietick:9;
%角度
Q = [ones(int16(9/tmietick)+1,1) T' (T.^2)' (T.^3)' (T.^4)' (T.^5)' (T.^6)' (T.^7)']*C;
%速度
Qv =[zeros(int16(9/tmietick)+1,1) ones(int16(9/tmietick)+1,1) 2* T' 3*(T.^2)' 4*(T.^3)' 5*(T.^4)' 6*(T.^5)' 7*(T.^6)']*C;
%加速度
Qa =[zeros(int16(9/tmietick)+1,1) zeros(int16(9/tmietick)+1,1) 2*ones(int16(9/tmietick)+1,1) 6*T' 12*(T.^2)' 20*(T.^3)' 30*(T.^4)' 42*(T.^5)']*C;
%正運動學分析
Txy=p560.fkine(Q);
%畫軌跡
Tjtraj1=transl(Txy);
x = Tjtraj1(:,1);
y = Tjtraj1(:,2);
z = Tjtraj1(:,3);
figure
waitforbuttonpress;
plot3(x,y,z,'b');%軌跡圖像
hold on;
%畫出四個過程點
[x0,y0,z0] = ellipsoid(aim0(1),aim0(2),aim0(3),0.05,0.05,0.05);
[x1,y1,z1] = ellipsoid(aim1(1),aim1(2),aim1(3),0.05,0.05,0.05);
[x2,y2,z2] = ellipsoid(aim2(1),aim2(2),aim2(3),0.05,0.05,0.05);
[xx,yx,zx] = ellipsoid(aimx(1),aimx(2),aimx(3),0.05,0.05,0.05);
surf(x0,y0,z0) %畫起始點
surf(x1,y1,z1) %畫提升點
surf(x2,y2,z2) %畫下降點
surf(xx,yx,zx) %畫目標點
hold on;
%畫軌跡圖
p560.plot(Q);
%畫關節位置、速度、加速度曲線
figure
subplot(3,1,1);
% plot(T,Q(:,1));
plot(T,Q);
title('關節位移');
xlabel('時間t/s');
ylabel('位移s/rad');
legend('關節1','關節2','關節3','關節4','關節5','關節6','location','northeastoutside' );
str=[ '\leftarrow' '(' num2str(t1) ',' num2str(theta1(1)) ')'];
text(t1,theta1(1),cellstr(str));
str=[ '\leftarrow' '(' num2str(t2) ',' num2str(theta2(1)) ')'];
text(t2,theta2(1),cellstr(str));
grid on;
subplot(3,1,2);
plot(T,Qv);
title('關節速度');
xlabel('時間t/s');
ylabel('速度v/(rad/s)');
legend('關節1','關節2','關節3','關節4','關節5','關節6','location','northeastoutside' );
grid on;
subplot(3,1,3);
plot(T,Qa);
title('關節加速度');
xlabel('時間t/s');
ylabel('加速度a/(rad/s^2)');
legend('關節1','關節2','關節3','關節4','關節5','關節6','location','northeastoutside' );
grid on;