????這是《機(jī)器人技術(shù)基礎(chǔ)》個(gè)人課程實(shí)驗(yàn)之一，按照學(xué)號(hào)尾數(shù)不同分配給每人的取放軌跡規(guī)劃方式也不同，包括3-4-3、4-3-4等三段軌跡規(guī)劃方法，而我抽中的是七次多項(xiàng)式實(shí)現(xiàn)，不存在優(yōu)劣之分，特地說(shuō)明一下。下面是詳細(xì)內(nèi)容：
一、開發(fā)平臺(tái)及工具
開發(fā)平臺(tái)：Matlab
開發(fā)工具：Robotics ToolBox
二、過(guò)程記錄
2.1 選定建模對(duì)象
????選擇PUMA560作為建模對(duì)象：
2.2 選定轉(zhuǎn)移過(guò)程點(diǎn)
????主要是確定起始位置（取點(diǎn)）、轉(zhuǎn)移位置（抬起或離開點(diǎn)）、卸下位置（下降點(diǎn)）和目標(biāo)位置（放置點(diǎn)）；選定四個(gè)點(diǎn)位置坐標(biāo)如下：
起始位置：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 整理邊界條件
利用七次多項(xiàng)式進(jìn)行軌跡規(guī)劃：（1）已知本地時(shí)間t = 0時(shí)，初始位置：（2）已知本地時(shí)間t = 0時(shí)，初始速度：（3）已知本地時(shí)間t = 0時(shí)，初始加速度：（4）已知第一中間點(diǎn)位置：（5）已知第二中間點(diǎn)位置：（6）已知本地時(shí)間t = tm時(shí)，目標(biāo)位置：（7）已知本地時(shí)間t = tm時(shí)，目標(biāo)速度：（8）已知本地時(shí)間t = tm時(shí)，目標(biāo)加速度：
????由(1)~（8）可以寫成如下矩陣形式：
2.4 求解轉(zhuǎn)移過(guò)程點(diǎn)關(guān)節(jié)角
????利用Matlab Robotics Box 對(duì)四個(gè)位置點(diǎn)求逆解得到關(guān)節(jié)角度：
theta0 = p560.ikine6s(T0,'rdf');%左臂、手肘朝下、手腕翻轉(zhuǎn)(旋轉(zhuǎn)180度)
theta1 = p560.ikine6s(T1,'rdf');
theta2 = p560.ikine6s(T2,'rdf');
thetax = p560.ikine6s(Tx,'rdf');
2.4 求解軌跡未知系數(shù)
%初始條件
theta0_ = [0 0 0 0 0 0];%初始位置速度
theta0__ = [0 0 0 0 0 0];%初始位置加速度
thetax_ = [0 0 0 0 0 0];%目標(biāo)位置速度
thetax__ = [0 0 0 0 0 0];%目標(biāo)位置加速度
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列對(duì)應(yīng)第i個(gè)關(guān)節(jié)的其次多項(xiàng)式系數(shù)
2.5 求解各關(guān)節(jié)相關(guān)運(yùn)動(dòng)指標(biāo)
????計(jì)算關(guān)節(jié)的位置、速度及加速度軌跡函數(shù)：
%計(jì)算關(guān)節(jié)各函數(shù)
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 繪制運(yùn)動(dòng)軌跡
%正運(yùn)動(dòng)學(xué)分析
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;
%畫出四個(gè)過(guò)程點(diǎn)
[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) %畫起始點(diǎn)
surf(x1,y1,z1) %畫提升點(diǎn)
surf(x2,y2,z2) %畫下降點(diǎn)
surf(xx,yx,zx) %畫目標(biāo)點(diǎn)
hold on;
????運(yùn)動(dòng)軌跡如藍(lán)線曲線所示，完整視頻B站傳送門：https://www.bilibili.com/video/av88855623 。
三、思考
????由上圖可以明顯發(fā)現(xiàn)7次多項(xiàng)式進(jìn)行擬合規(guī)劃的軌跡為一空間不規(guī)則曲線，原因在于我們是對(duì)機(jī)械臂6個(gè)關(guān)節(jié)進(jìn)行獨(dú)立的7次多項(xiàng)式軌跡規(guī)劃，已知條件只有初末位置的關(guān)節(jié)位置、速度及加速度以及兩個(gè)中間點(diǎn)的關(guān)節(jié)位置共8個(gè)參數(shù)，同時(shí)由于4個(gè)位置點(diǎn)的各關(guān)節(jié)角度值是由ikine6s進(jìn)行運(yùn)動(dòng)學(xué)逆解求解得到，雖然在計(jì)算時(shí)限制了手臂、手肘以及手腕的狀態(tài)，但是這樣得到逆解并一定能滿足我們的實(shí)際需求，因此在僅存在這些已知條件的情況下，我們沒有辦法對(duì)整個(gè)長(zhǎng)距離的機(jī)器人的運(yùn)動(dòng)狀態(tài)進(jìn)行精確控制。因此，要想機(jī)器人的運(yùn)動(dòng)狀態(tài)足夠精確，我必須引入更多的條件，比如增加中間點(diǎn)的數(shù)量或者各時(shí)刻的關(guān)節(jié)速度等等。
四、附源碼
close all;
clc;
mdl_puma560
t0 = 0;%開始時(shí)刻
t1 = 2;%提升結(jié)束時(shí)刻
t2 = t1 + 4;%平移結(jié)束時(shí)刻
tm = t2 + 3;%下降結(jié)束時(shí)刻
t0_1 = 0:0.2:2;%上升時(shí)間
t1_2 = 0:0.5:4;%平移時(shí)間
t2_x = 0:0.3:3;%下降時(shí)間
aim0 = [0,-0.5,-0.5];%取貨點(diǎn)
aim1 = [0,-0.5,0.2];%提升點(diǎn)
aim2 = [-0.5,0.5,0.2];%下落點(diǎn)
aimx = [-0.5,0.5,-0.5];%存貨點(diǎn)
T0 = transl(aim0);
T1 = transl(aim1);
T2 = transl(aim2);
Tx = transl(aimx);
theta0 = p560.ikine6s(T0,'rdf');%左臂、手肘朝下、手腕翻轉(zhuǎn)(旋轉(zhuǎn)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];%目標(biāo)位置速度
thetax__ = [0 0 0 0 0 0];%目標(biāo)位置加速度
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列對(duì)應(yīng)第i個(gè)關(guān)節(jié)的其次多項(xiàng)式系數(shù)
%計(jì)算關(guān)節(jié)各函數(shù)
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;
%正運(yùn)動(dòng)學(xué)分析
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;
%畫出四個(gè)過(guò)程點(diǎn)
[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) %畫起始點(diǎn)
surf(x1,y1,z1) %畫提升點(diǎn)
surf(x2,y2,z2) %畫下降點(diǎn)
surf(xx,yx,zx) %畫目標(biāo)點(diǎn)
hold on;
%畫軌跡圖
p560.plot(Q);
%畫關(guān)節(jié)位置、速度、加速度曲線
figure
subplot(3,1,1);
% plot(T,Q(:,1));
plot(T,Q);
title('關(guān)節(jié)位移');
xlabel('時(shí)間t/s');
ylabel('位移s/rad');
legend('關(guān)節(jié)1','關(guān)節(jié)2','關(guān)節(jié)3','關(guān)節(jié)4','關(guān)節(jié)5','關(guān)節(jié)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('關(guān)節(jié)速度');
xlabel('時(shí)間t/s');
ylabel('速度v/(rad/s)');
legend('關(guān)節(jié)1','關(guān)節(jié)2','關(guān)節(jié)3','關(guān)節(jié)4','關(guān)節(jié)5','關(guān)節(jié)6','location','northeastoutside' );
grid on;
subplot(3,1,3);
plot(T,Qa);
title('關(guān)節(jié)加速度');
xlabel('時(shí)間t/s');
ylabel('加速度a/(rad/s^2)');
legend('關(guān)節(jié)1','關(guān)節(jié)2','關(guān)節(jié)3','關(guān)節(jié)4','關(guān)節(jié)5','關(guān)節(jié)6','location','northeastoutside' );
grid on;
