1 創建機器人

clc;clear;close all;
deg = pi/180;L1= Revolute('d', 0, 'a', 0, 'alpha', 0,'modified', ...'I', [0.1183 -0.0001 0.0001;-0.0001 0.1182 0.0001;0.0001 0.0001 0.0140], ...'r', [0.0002 0.0002 0.1264], ...'m', 5.6431, ...'Jm', 2.2e-4, ...'G', 81, ...'B', 1.48e-3, ...'Tc', [0.395 -0.435], ...'qlim', [-180 180]*deg );L2 = Revolute('d', 0.06, 'a', 0, 'alpha', -pi/2,'modified', ...'I', [0.0723,0.0000,-0.0051;0.0000,0.0784,0.0000;-0.0051,0.0000,0.0169;], ...'r', [-0.0062,0.0001,0.1080], ...'m', 5.0478, ...'Jm', 2.2e-4, ...'G', 121, ...'B', .817e-3, ...'Tc', [0.126 -0.071], ...'qlim', [-105 105]*deg );
L3 = Revolute('d', -0.004, 'a', 0.332, 'alpha', 0, 'modified', ...'I', [0.4263,0.0000,-0.0072;0.0000,0.4334,0.0001;-0.0072,0.0001,0.0191], ...'r', [-0.0131,0.0001,0.2402], ...'m', 5.7542, ...'Jm', 2.2e-4, ...'G', 81, ...'B', 1.38e-3, ...'Tc', [0.132, -0.105], ...'qlim', [-225 45]*deg );L4 = Revolute('d', -0.056, 'a', 0, 'alpha', pi/2, 'modified', ...'I', [0.0821,0.0000,-0.0314;0.0000,0.1257,0.0001;-0.0314,0.0001,0.0451], ...'r', [-0.0850,0.0003,0.1540], ...'m', 3.0870, ...'Jm', 2.2e-4, ...'G', 81, ...'B', 71.2e-6, ...'Tc', [11.2e-3, -16.9e-3], ...'qlim', [-110 110]*deg);
L5 = Revolute('d', 0.050, 'a', 0, 'alpha', -pi/2, 'modified', ...'I', [0.0235,0.0000,-0.0002;0.0000,0.0253,0.0000;-0.0002,0.0000,0.0045], ...'r', [0.0001,0.0002,0.0982], ...'m', 2.0459, ...'Jm', 2.2e-4, ...'G', 81, ...'B', 82.6e-6, ...'Tc', [9.26e-3, -14.5e-3], ...'qlim', [-115 115]*deg );
L6 = Revolute('d', -0.050, 'a', 0, 'alpha', pi/2, 'modified', ...'I', [0.0684,0.0000,0.0001;0.0000,0.0696,-0.0001;0.0001,-0.0001,0.0047], ...'r', [-0.0111,-0.0003,0.1366], ...'m', 2.6317, ...'Jm', 2.2e-4, ...'G', 51, ...'B', 36.7e-6, ...'Tc', [3.96e-3, -10.5e-3], ...'qlim', [-180 180]*deg );
% SerialLink類函數
robot=SerialLink([L1,L2,L3,L4,L5,L6],'name','VIPER7','comment','LL');
% Link類函數，顯示建立機器人DH參數 
robot.display();

輸出：

robot = 
VIPER7:: 6 axis, RRRRRR, modDH, slowRNE                          - LL;                                                           
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|          0|          0|          0|          0|
|  2|         q2|       0.06|          0|    -1.5708|          0|
|  3|         q3|     -0.004|      0.332|          0|          0|
|  4|         q4|     -0.056|          0|     1.5708|          0|
|  5|         q5|       0.05|          0|    -1.5708|          0|
|  6|         q6|      -0.05|          0|     1.5708|          0|
+---+-----------+-----------+-----------+-----------+-----------+

2 機器人顯示

%通過手動輸入各個連桿轉角，模型會自動運動到相應位置
theta1=[0 -pi/2 -pi/2 0 0 0];
% SerialLink類函數,顯示機器人圖像
robot.plot(theta1);
title('機器人plot顯示')

3 機器人示教

robot.teach();
title('機器人teach示教')

4 機器人路徑規劃：給定關節角路徑

% 初始角
theta1=[0 -pi/2 -pi/2 0 0 0];
% 目標關節角
theta2=[-pi/2 0 -pi/4 -pi/3 pi/4 pi/2];
% 時間
t=[0:0.01:2];
% 關節空間路徑規劃
% 相當于具有tpoly插值的mtraj，但是對多軸情況進行了優化，還允許使用額外參數設置初始和最終速度
g=jtraj(theta1,theta2,t);
% 通過可選的輸出參數，獲得隨時間變化的關節速度加速度向量
[q,qd,qdd]=jtraj(theta1,theta2,t);
figure
i=1:6;
%繪制動畫
subplot(2,3,1);
robot.plot(g)
%繪制每個關節位置
subplot(2,3,2);
qplot(q(:,i));
grid on;title('位置');
%繪制每個關節速度
subplot(2,3,3);
qplot(qd(:,i));
grid on;title('速度');
%繪制每個關節加速度
subplot(2,3,4);
qplot(qdd(:,i));
grid on;title('加速度');
% 獲得每個時間點所需要的關節力矩
Q = robot.rne(q,qd,qdd);
%繪制每個關節的力矩
subplot(2,3,5)
qplot(t,Q);
grid on;title('關節力矩');

5 機器人路徑規劃：給定末端位姿，求關節角路徑

%% 已知機械臂初始和目標的關節角度，利用五次多項式進行軌跡規劃
T1=transl(0.3,0.1,0);						%根據給定起始點，得到起始點位姿
T2=transl(0,0.3,0.1);						%根據給定終止點，得到終止點位姿
init_ang=robot.ikine(T1);				%根據起始點位姿，得到起始點關節角
targ_ang=robot.ikine(T2);				%根據終止點位姿，得到終止點關節角
step = 20;%軌跡規劃方法
figure
%關節空間軌跡規劃
%五次多項式軌跡，得到關節角度，角速度，角加速度，20為采樣點個數
[q, qd, qdd]=jtraj(init_ang,targ_ang,step); 
grid on
%根據插值，得到末端執行器位姿
T=robot.fkine(q);		
% 
nT=T.T;
%輸出末端軌跡
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));
title('根據運動學求正解得到目標軌跡');
%動畫演示 
robot.plot(q);	
%robot.plot(q,'trail','b');  %運行后在命令行窗口再復制運行一次，trail軌跡，b藍色

%% 求解上述運行過程中的位置、速度、加速度的變化曲線
figure
subplot(3,2,[1,3]); 					%subplot 對畫面分區 三行兩列 占用1到3的位置
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%輸出末端軌跡
robot.plot(q);							%動畫演示subplot(3, 2, 2);
i = 1:6;
plot(q(:,i));
title('位置');
grid on;subplot(3, 2, 4);
i = 1:6;
plot(qd(:,i));
title('速度');
grid on;subplot(3, 2, 6);
i = 1:6;
plot(qdd(:,i));
title('加速度');
grid on;

6 工作空間可視化

figure
num = 30000;
p = zeros(num,3);%先聲明0矩陣可加快運行速度
for i=1:numq1 = L1.qlim(1) + rand * (L1.qlim(2) - L1.qlim(1));q2 = L2.qlim(1) + rand * (L2.qlim(2) - L2.qlim(1));q3 = L3.qlim(1) + rand * (L3.qlim(2) - L3.qlim(1));q4 = L4.qlim(1) + rand * (L4.qlim(2) - L4.qlim(1));q5 = L5.qlim(1) + rand * (L5.qlim(2) - L5.qlim(1));q6 = L6.qlim(1) + rand * (L6.qlim(2) - L6.qlim(1));q = [q1 q2 q3 q4 q5 q6];Ts = robot.fkine(q);  % SerialLink.fkine正向運動學, 給一個關節變量，可以求出變換矩陣P(i,:) = transl(Ts);
end
plot3( P(:,1), P(:,2), P(:,3),'b.','markersize',1); %在三維空間內繪制30000個點hold on; %添加新繪圖的時候保留當前繪圖
grid on; %在畫圖的時候添加網格線view([45 45]);
robot.plot([0 0 0 0 0 0]);

參考鏈接

Matlab機械臂建模：機器人工具箱的使用&&導入自己的機械臂模型
Matlab機器人工具箱——動力學
MATLAB機器人工具箱學習（一）