1.實驗目的

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

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

2.實驗內容

2.1標準D-H參數法

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

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

本實驗給定的參數表：

2.2實驗中使用的Matlab函數

Link函數

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

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

可采用DH法建立模型,其中包含參數：關節轉角，關節距離，連桿長度，連桿轉角，關節類型（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];  %指定的關節角
p=robot.fkine(theta)    %fkine正解函數，根據關節角theta，求解出末端位姿p

ikine逆解函數

用于求解出關節角q。

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

軌跡規劃

（1）jtraj

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

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

（2）ctraj

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

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的關節型六軸機械臂仿真


%% 參數定義
%機械臂為六自由度機械臂
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法建立模型,關節轉角，關節距離，連桿長度，連桿轉角，關節類型（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')


% 定義關節范圍
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];    %初始關節角度
figure(f)
robot0.plot(theta);
title('六軸機械臂模型');
%% 加入teach指令，則可調整各個關節角度
robot1 = SerialLink(L,'name','sixsix');
f = 2
figure(f)
robot1.plot(theta);
robot1.teach
title('六軸機械臂模型可調節');
%% 實驗二 基于MATLAB的六軸機械臂軌跡規劃仿真


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


%% 2.3 jtraj 已知初始和終止的關節角度，利用五次多項式來規劃軌跡
% 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);    %根據起始點位姿，得到起始點關節角
targ_ang=robot2.ikine(T2);    %根據終止點位姿，得到終止點關節角
step = 20;
f = 3


%軌跡規劃方法
figure(f)
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多項式軌跡，得到關節角度，角速度，角加速度，50為采樣點個數
grid on
T=robot2.fkine(q);      %根據插值，得到末端執行器位姿
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 對畫面分區 三行兩列 占用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規劃軌跡 考慮末端執行器在兩個笛卡爾位姿之間移動 
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('直線軌跡');

審核編輯：湯梓紅