1.實驗目的

基于機器人學理論知識，利用標準D-H參數法建立關節(jié)型機器人的數學模型，使用Matlab的Robotics Toolbox工具包搭建模型。

tip：實驗工具:Matlab R2021a （有很多玄學問題是因為軟件版本）
以及注意先安裝Robotics Toolbox工具包！！

2.實驗內容

2.1標準D-H參數法

標準D-H參數法常用于建立關節(jié)型機器人的數學模型，D-H參數法是一種對連桿的坐標描述，而關節(jié)機器人本質上就是一系列連桿通過關節(jié)連接起來而組成的空間開式運動鏈。

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

本實驗給定的參數表：

2.2實驗中使用的Matlab函數

Link函數

用于定義六軸機器人的一個軸。

包含了機器人的運動學參數、動力學參數、剛體慣性矩參數、電機和傳動參數；

可采用DH法建立模型,其中包含參數：關節(jié)轉角，關節(jié)距離，連桿長度，連桿轉角，關節(jié)類型（0轉動，1移動）。

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

SerialLink函數

用于構建機械臂。

它的類函數比較多，包括顯示機器人、動力學、逆動力學、雅可比等；

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

fkine正解函數

用于求解出末端位姿p。

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

ikine逆解函數

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

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

軌跡規(guī)劃

（1）jtraj

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

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

（2）ctraj

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

T0 = robot2.fkine(init_ang);%運動學正解
T1 = robot2.fkine(targ_ang);%運動學正解
Tc = ctraj(T0,T1,step);   %得到每一步的T陣
tt = transl(Tc);

3.實驗結果

4.全部代碼

%% MATLAB素質三連
clear;
close all;
clc;
%% 實驗一 基于MATLAB的關節(jié)型六軸機械臂仿真


%% 參數定義
%機械臂為六自由度機械臂
clear L;
 
%角度轉換
angle=pi/180; %度
 
%D-H參數表
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法建立模型,關節(jié)轉角，關節(jié)距離，連桿長度，連桿轉角，關節(jié)類型（0轉動，1移動）


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')


% 定義關節(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];


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


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


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


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


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


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);     %運動學正解
rpy=tr2rpy(t);        %t中提取位置（xyz）
figure(f)
subplot(3,2,5);
plot2(rpy);


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


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


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

審核編輯：湯梓紅