采用五次多項(xiàng)式插值法進(jìn)行機(jī)械臂軌跡規(guī)劃，基于Matlab Robotics Toolbox平臺(tái)進(jìn)行關(guān)節(jié)空間軌跡規(guī)劃，得到各關(guān)節(jié)角度、速度和加速度與時(shí)間關(guān)系曲線。

此外，每次重新啟動(dòng)MATLAB時(shí)都需要重新輸入“startup_rvc”回車(chē)來(lái)啟動(dòng)這個(gè)工具箱。

本文所控對(duì)象為串聯(lián)六R機(jī)械臂，其具體尺寸參數(shù)見(jiàn)于代碼中的D-H表。

MATLAB代碼

% Modified DH
% ABB robot
% lujingguihua
clear;
clc;
% %機(jī)器人建模
th(1) = 0; d(1) = 0; a(1) = 0; alp(1) = 0;
th(2) = 0; d(2) = 0; a(2) = 3.20; alp(2) = pi/2;   
th(3) = 0; d(3) = 0; a(3) = 9.75; alp(3) = 0;
th(4) = 0; d(4) = 8.87; a(4) = 2; alp(4) = pi/2;
th(5) = 0; d(5) = 0; a(5) = 0; alp(5) = -pi/2;
th(6) = 0; d(6) = 0; a(6) = 0; alp(6) = pi/2;
% DH parameters  th     d    a    alpha  sigma
L1 = Link([th(1), d(1), a(1), alp(1), 0], 'modified');
L2 = Link([th(2), d(2), a(2), alp(2), 0], 'modified');
L3 = Link([th(3), d(3), a(3), alp(3), 0], 'modified');
L4 = Link([th(4), d(4), a(4), alp(4), 0], 'modified');
L5 = Link([th(5), d(5), a(5), alp(5), 0], 'modified');
L6 = Link([th(6), d(6), a(6), alp(6), 0], 'modified');
robot = SerialLink([L1, L2, L3, L4, L5, L6]); %SerialLink 類(lèi)函數(shù)
robot.name='Robot-6-dof';
robot.display(); %顯示D-H表


%軌跡規(guī)劃參數(shù)設(shè)置
init_ang = [pi/6,0, 2*pi/3,pi/3, 0, 0];
targ_ang = [pi/2,pi/6,0,0, -pi/2, pi/6];
T =(0:0.1:5);
%關(guān)節(jié)空間軌跡規(guī)劃方法
[q,qd,qdd] = jtraj(init_ang,targ_ang,T); %直接得到角度、角速度、角加速度的的序列


%%顯示
figure(1);
%動(dòng)畫(huà)顯示
subplot(1,2,1); 
title('動(dòng)畫(huà)過(guò)程');
robot.plot(q);
% 軌跡顯示
t=robot.fkine(q);%運(yùn)動(dòng)學(xué)正解
rpy=tr2rpy(t);  %t中提取位置（xyz）
subplot(1,2,2);
plot2(rpy);
xlabel('X/mm'),ylabel('Y/mm'),zlabel('Z/mm');hold on
title('空間軌跡');
text(rpy(1,1),rpy(1,2),rpy(1,3),'A點(diǎn)');
text(rpy(51,1),rpy(51,2),rpy(51,3),'B點(diǎn)');
% 指定文件夾保存圖片
filepath=pwd;           %保存當(dāng)前工作目錄
cd('C:UsersAdministratorDesktoppic')                %把當(dāng)前工作目錄切換到圖片存儲(chǔ)文件夾
print(gcf,'-djpeg','C:UsersAdministratorDesktoppic1.jpeg'); %將圖片保存為jpg格式，
cd(filepath)            %切回原工作目錄


%單個(gè)關(guān)節(jié)的位置title('關(guān)節(jié)1位置');
figure(2);
subplot(3,2,1);
plot(T,q(:,1));
xlabel('t/s'),ylabel('θ1/rad');hold on
subplot(3,2,2);
plot(T,q(:,2));
xlabel('t/s'),ylabel('θ2/rad');hold on
subplot(3,2,3);
plot(T,q(:,3));
xlabel('t/s'),ylabel('θ3/rad');hold on
subplot(3,2,4);
plot(T,q(:,4));
xlabel('t/s'),ylabel('θ4/rad');hold on
subplot(3,2,5);
plot(T,q(:,5));
xlabel('t/s'),ylabel('θ5/rad');hold on
subplot(3,2,6);
plot(T,q(:,6));
xlabel('t/s'),ylabel('θ6/rad');hold on
% 指定文件夾保存圖片
filepath=pwd;           %保存當(dāng)前工作目錄
cd('C:UsersAdministratorDesktoppic')                %把當(dāng)前工作目錄切換到圖片存儲(chǔ)文件夾
print(gcf,'-djpeg','C:UsersAdministratorDesktoppic2.jpeg'); %將圖片保存為jpg格式，
cd(filepath)            %切回原工作目錄


%單個(gè)關(guān)節(jié)的速度
figure(3);
subplot(3,2,1);
plot(T,qd(:,1));
xlabel('t/s'),ylabel('Ω1/rad');hold on
subplot(3,2,2);
plot(T,qd(:,2));
xlabel('t/s'),ylabel('Ω2/rad');hold on
subplot(3,2,3);
plot(T,qd(:,3));
xlabel('t/s'),ylabel('Ω3/rad');hold on
subplot(3,2,4);
plot(T,qd(:,4));
xlabel('t/s'),ylabel('Ω4/rad');hold on
subplot(3,2,5);
plot(T,qd(:,5));
xlabel('t/s'),ylabel('Ω5/rad');hold on
subplot(3,2,6);
plot(T,qd(:,6));
xlabel('t/s'),ylabel('Ω6/rad');hold on
% 指定文件夾保存圖片
filepath=pwd;           %保存當(dāng)前工作目錄
cd('C:UsersAdministratorDesktoppic')                
%把當(dāng)前工作目錄切換到圖片存儲(chǔ)文件夾
print(gcf,'-djpeg','C:UsersAdministratorDesktoppic3.jpeg'); %將圖片保存為jpg格式，
cd(filepath)            %切回原工作目錄


%單個(gè)關(guān)節(jié)的加速度
figure(4);
subplot(3,2,1);
plot(T,qdd(:,1));
xlabel('t/s'),ylabel('α1/rad');hold on
subplot(3,2,2);
plot(T,qdd(:,2));
xlabel('t/s'),ylabel('α2/rad');hold on
subplot(3,2,3);
plot(T,qdd(:,3));
xlabel('t/s'),ylabel('α3/rad');hold on
subplot(3,2,4);
plot(T,qdd(:,4));
xlabel('t/s'),ylabel('α4/rad');hold on;
subplot(3,2,5);
plot(T,qdd(:,5));
xlabel('t/s'),ylabel('α5/rad');hold on
subplot(3,2,6);
plot(T,qdd(:,6));
xlabel('t/s'),ylabel('α6/rad');hold on
% 指定文件夾保存圖片
filepath=pwd;           %保存當(dāng)前工作目錄
cd('C:UsersAdministratorDesktoppic')                %把當(dāng)前工作目錄切換到圖片存儲(chǔ)文件夾
print(gcf,'-djpeg','C:UsersAdministratorDesktoppic4.jpeg'); %將圖片保存為jpg格式，
cd(filepath)            %切回原工作目錄

通過(guò)軌跡規(guī)劃，不僅得到了機(jī)械臂末端執(zhí)行器的空間軌跡，還可以得到其關(guān)節(jié)的角位移、角速度和角加速度。從上圖 可以看出機(jī)械臂到達(dá)預(yù)定的位置，證明了該機(jī)械臂設(shè)計(jì)的合理性。

隨著運(yùn)動(dòng)的進(jìn)行， 各個(gè)關(guān)節(jié)的角度與時(shí)間關(guān)系的曲線， 可以看到其運(yùn)動(dòng)過(guò)程連續(xù)平滑。而且在圖 中可以看出各關(guān)節(jié)的角速度和角加速度都是光滑變化的，沒(méi)有出現(xiàn)跳變點(diǎn)。進(jìn)一步地，在首末兩點(diǎn)的速度以及加速度都可以有效地約束為零。