基于MATLAB的關(guān)節(jié)型六軸機(jī)械臂軌跡規(guī)劃仿真

基于機(jī)器人學(xué)理論知識(shí)，利用標(biāo)準(zhǔn)D-H參數(shù)法建立關(guān)節(jié)型機(jī)器人的數(shù)學(xué)模型，使用Matlab的Robotics Toolbox工具包搭建模型。

tip：實(shí)驗(yàn)工具:Matlab R2021a （有很多玄學(xué)問題是因?yàn)檐浖姹荆?br /> 以及注意先安裝Robotics Toolbox工具包！！

2.實(shí)驗(yàn)內(nèi)容

2.1標(biāo)準(zhǔn)D-H參數(shù)法

標(biāo)準(zhǔn)D-H參數(shù)法常用于建立關(guān)節(jié)型機(jī)器人的數(shù)學(xué)模型，D-H參數(shù)法是一種對(duì)連桿的坐標(biāo)描述，而關(guān)節(jié)機(jī)器人本質(zhì)上就是一系列連桿通過關(guān)節(jié)連接起來而組成的空間開式運(yùn)動(dòng)鏈。

對(duì)于連桿本身，其功能在于保持其兩端的關(guān)節(jié)軸線具有固定的幾何關(guān)系，連桿的特性由軸線決定，通常用四個(gè)連桿參數(shù)來描述，連桿長(zhǎng)度，連桿扭轉(zhuǎn)角，連桿偏移量和關(guān)節(jié)角。

本實(shí)驗(yàn)給定的參數(shù)表：

2.2實(shí)驗(yàn)中使用的Matlab函數(shù)

Link函數(shù)

用于定義六軸機(jī)器人的一個(gè)軸。

包含了機(jī)器人的運(yùn)動(dòng)學(xué)參數(shù)、動(dòng)力學(xué)參數(shù)、剛體慣性矩參數(shù)、電機(jī)和傳動(dòng)參數(shù)；

可采用DH法建立模型,其中包含參數(shù)：關(guān)節(jié)轉(zhuǎn)角，關(guān)節(jié)距離，連桿長(zhǎng)度，連桿轉(zhuǎn)角，關(guān)節(jié)類型（0轉(zhuǎn)動(dòng)，1移動(dòng)）。

% 定義六軸機(jī)器人的一個(gè)軸
L(1) = Link([theta1, D1, A1, alpha1, offset1], 'standard')

SerialLink函數(shù)

用于構(gòu)建機(jī)械臂。

它的類函數(shù)比較多，包括顯示機(jī)器人、動(dòng)力學(xué)、逆動(dòng)力學(xué)、雅可比等；

% 'six'為機(jī)械臂名稱
robot = SerialLink(L,'name','six');

fkine正解函數(shù)

用于求解出末端位姿p。

theta = [0.1,0,0,0,0,0];  %指定的關(guān)節(jié)角
p=robot.fkine(theta)    %fkine正解函數(shù)，根據(jù)關(guān)節(jié)角theta，求解出末端位姿p

ikine逆解函數(shù)

用于求解出關(guān)節(jié)角q。

q=ikine(robot,p)      %ikine逆解函數(shù)，根據(jù)末端位姿p，求解出關(guān)節(jié)角q

軌跡規(guī)劃

（1）jtraj

已知初始和終止的關(guān)節(jié)角度，利用五次多項(xiàng)式來規(guī)劃軌跡；

T1=transl(0.5,0,0);   %根據(jù)給定起始點(diǎn)，得到起始點(diǎn)位姿
T2=transl(0,0.5,0);   %根據(jù)給定終止點(diǎn)，得到終止點(diǎn)位姿
init_ang=robot2.ikine(T1);%根據(jù)起始點(diǎn)位姿，得到起始點(diǎn)關(guān)節(jié)角
targ_ang=robot2.ikine(T2);%根據(jù)終止點(diǎn)位姿，得到終止點(diǎn)關(guān)節(jié)角
step = 20;
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多項(xiàng)式軌跡，得到關(guān)節(jié)角度，角速度，角加速度，50為采樣點(diǎn)個(gè)數(shù)

（2）ctraj

已知初始和終止的末端關(guān)節(jié)位姿，利用勻加速、勻減速運(yùn)動(dòng)來規(guī)劃軌跡。

T0 = robot2.fkine(init_ang);%運(yùn)動(dòng)學(xué)正解
T1 = robot2.fkine(targ_ang);%運(yùn)動(dòng)學(xué)正解
Tc = ctraj(T0,T1,step);   %得到每一步的T陣
tt = transl(Tc);

3.實(shí)驗(yàn)結(jié)果

4.全部代碼

%% MATLAB素質(zhì)三連
clear;
close all;
clc;
%% 實(shí)驗(yàn)一 基于MATLAB的關(guān)節(jié)型六軸機(jī)械臂仿真


%% 參數(shù)定義
%機(jī)械臂為六自由度機(jī)械臂
clear L;
 
%角度轉(zhuǎn)換
angle=pi/180; %度
 
%D-H參數(shù)表
theta1 = 0;  D1 = 0.4;  A1 = 0.025; alpha1 = pi/2; offset1 = 0;
theta2 = pi/2;D2 = 0;   A2 = 0.56; alpha2 = 0;  offset2 = 0;
theta3 = 0;  D3 = 0;   A3 = 0.035; alpha3 = pi/2; offset3 = 0;
theta4 = 0;  D4 = 0.515; A4 = 0;   alpha4 = pi/2; offset4 = 0;
theta5 = pi; D5 = 0;   A5 = 0;   alpha5 = pi/2; offset5 = 0;
theta6 = 0;  D6 = 0.08; A6 = 0;   alpha6 = 0;  offset6 = 0;


%% DH法建立模型,關(guān)節(jié)轉(zhuǎn)角，關(guān)節(jié)距離，連桿長(zhǎng)度，連桿轉(zhuǎn)角，關(guān)節(jié)類型（0轉(zhuǎn)動(dòng)，1移動(dòng)）


L(1) = Link([theta1, D1, A1, alpha1, offset1], 'standard')
L(2) = Link([theta2, D2, A2, alpha2, offset2], 'standard')
L(3) = Link([theta3, D3, A3, alpha3, offset3], 'standard')
L(4) = Link([theta4, D4, A4, alpha4, offset4], 'standard')
L(5) = Link([theta5, D5, A5, alpha5, offset5], 'standard')
L(6) = Link([theta6, D6, A6, alpha6, offset6], 'standard')


% 定義關(guān)節(jié)范圍
L(1).qlim =[-180*angle, 180*angle];
L(2).qlim =[-180*angle, 180*angle];
L(3).qlim =[-180*angle, 180*angle];
L(4).qlim =[-180*angle, 180*angle];
L(5).qlim =[-180*angle, 180*angle];
L(6).qlim =[-180*angle, 180*angle];


%% 顯示機(jī)械臂
robot0 = SerialLink(L,'name','six');
f = 1          %畫在第1張圖上
theta = [0 pi/2 0 0 pi 0];    %初始關(guān)節(jié)角度
figure(f)
robot0.plot(theta);
title('六軸機(jī)械臂模型');
%% 加入teach指令，則可調(diào)整各個(gè)關(guān)節(jié)角度
robot1 = SerialLink(L,'name','sixsix');
f = 2
figure(f)
robot1.plot(theta);
robot1.teach
title('六軸機(jī)械臂模型可調(diào)節(jié)');
%% 實(shí)驗(yàn)二 基于MATLAB的六軸機(jī)械臂軌跡規(guī)劃仿真


%% 2.2求解運(yùn)動(dòng)學(xué)正解
robot2 = SerialLink(L,'name','sixsixsix');
theta2 = [0.1,0,0,0,0,0];     %實(shí)驗(yàn)二指定的關(guān)節(jié)角
p=robot2.fkine(theta2)       %fkine正解函數(shù)，根據(jù)關(guān)節(jié)角theta，求解出末端位姿p
q=ikine(robot2,p)         %ikine逆解函數(shù)，根據(jù)末端位姿p，求解出關(guān)節(jié)角q


%% 2.3 jtraj 已知初始和終止的關(guān)節(jié)角度，利用五次多項(xiàng)式來規(guī)劃軌跡
% T1=transl(0.5,0,0);     %根據(jù)給定起始點(diǎn)，得到起始點(diǎn)位姿
% T2=transl(0,0.5,0);     %根據(jù)給定終止點(diǎn)，得到終止點(diǎn)位姿
T1=transl(0.5,0,0);      %根據(jù)給定起始點(diǎn)，得到起始點(diǎn)位姿
T2=transl(0,0.5,0);      %根據(jù)給定終止點(diǎn)，得到終止點(diǎn)位姿
init_ang=robot2.ikine(T1);    %根據(jù)起始點(diǎn)位姿，得到起始點(diǎn)關(guān)節(jié)角
targ_ang=robot2.ikine(T2);    %根據(jù)終止點(diǎn)位姿，得到終止點(diǎn)關(guān)節(jié)角
step = 20;
f = 3


%軌跡規(guī)劃方法
figure(f)
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多項(xiàng)式軌跡，得到關(guān)節(jié)角度，角速度，角加速度，50為采樣點(diǎn)個(gè)數(shù)
grid on
T=robot2.fkine(q);      %根據(jù)插值，得到末端執(zhí)行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%輸出末端軌跡
title('輸出末端軌跡');
robot2.plot(q);       %動(dòng)畫演示 


%% 求解位置、速度、加速度變化曲線
f = 4
figure(f)
subplot(3,2,[1,3]);      %subplot 對(duì)畫面分區(qū) 三行兩列 占用1到3的位置
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%輸出末端軌跡
robot2.plot(q);       %動(dòng)畫演示


figure(f)
subplot(3, 2, 2);
i = 1:6;
plot(q(:,1));
title('位置');
grid on;


figure(f)
subplot(3, 2, 4);
i = 1:6;
plot(qd(:,1));
title('速度');
grid on;


figure(f)
subplot(3, 2, 6);
i = 1:6;
plot(qdd(:,1));
title('加速度');
grid on;


t = robot2.fkine(q);     %運(yùn)動(dòng)學(xué)正解
rpy=tr2rpy(t);        %t中提取位置（xyz）
figure(f)
subplot(3,2,5);
plot2(rpy);


%% ctraj規(guī)劃軌跡 考慮末端執(zhí)行器在兩個(gè)笛卡爾位姿之間移動(dòng) 
f = 5
T0 = robot2.fkine(init_ang);   %運(yùn)動(dòng)學(xué)正解
T1 = robot2.fkine(targ_ang);   %運(yùn)動(dòng)學(xué)正解


Tc = ctraj(T0,T1,step);     %得到每一步的T陣


tt = transl(Tc);
figure(f)
plot2(tt,'r');
title('直線軌跡');

審核編輯：湯梓紅