TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
KHOA ĐIỆN
BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG
====o0o====
BÁO CÁO
ROBOT CÔNG NGHIỆP
Giáo viên hướng dẫn
: PGS.TS. Phan Bùi Khôi
Sinh viên thực hiện
: Trịnh Văn Vương
MSSV
: 20073539
Lớp
: KSTN - ĐKTĐ
Khóa
: 52
Đề số
: 17
Hà nội, 12-2011
MỤC LỤC
GIỚI THIỆU....................................................................................................................1
Chương 1........................................................................................................................... 2
THIẾT LẬP HỆ PHƯƠNG TRÌNH ĐỘNG HỌC ROBOT.........................................2
1.1. Phân tích động học Denavit–Hartenberg...................................................................2
1.2. Thiết lập hệ phương trình động học...........................................................................3
Ma trận trạng thái khâu thao tác theo cấu trúc động học..............................................3
Dựa trên bảng tham số động học 1.1, xây dựng các ma trận biến đổi tọa độ thuần
nhất Denavit-Hartenberg dựa trên công thức tổng quát sau:........................................3
Chương 2........................................................................................................................... 5
BÀI TOÁN ĐỘNG HỌC ROBOT..................................................................................5
Giải bài toán động học Robot là vấn đề kỹ thuật kinh điển. Trong chương này sẽ trình
bày lý thuyết thuật toán giải các bài toán thuận nghịch và thực hiện thuật toán ấy trên
công cụ tính toán mô phỏng Matlab. Do mã nguồn chương trình quá dài nên không
tiện cho việc trình bày nên ở đây chủ yếu mô tả cách thực hiện thuật toán trên phần
mềm. Mã chương trình có thể tham khảo ở phụ lục hoặc file mềm đi kèm....................5
2.1. Bài toán động học thuận............................................................................................5
2.2. Bài toán động học ngược...........................................................................................6
Chương 3........................................................................................................................... 9
THIẾT KẾ VÀ MÔ PHỎNG ROBOT...........................................................................9
3.1. Thiết kế mô hình Robot 3D bằng Solidworks...........................................................9
3.2. Mô phỏng hoạt động Robot trên Matlab.................................................................10
TÀI LIỆU THAM KHẢO.............................................................................................13
PHỤ LỤC........................................................................................................................ 14
Giới thiệu
GIỚI THIỆU
Số thứ tự đề bài: 17
Ký hiệu tên robot: ‘Robot No.17’
Nội dung báo cáo: Bao gồm 3 bài tập trong đề, chia làm 3 chương. Đó là:
1. Thiết lập hệ phương trình động học của robot.(bắt buộc)
2. Giải bài toán động học robot.(bắt buộc)
3. Thiết kế và mô phỏng.(tự chọn)
Công cụ tính toán và mô phỏng: Matlab.
Công cụ thiết kế cơ khí: Solidworks.
Kết quả thu được: Tính toán, giải quyết và mô phỏng thành công động học robot.
Hướng phát triển: Có thể ứng dụng trong thực tiễn công nghiệp, như hàn, ghép phôi,
hay vận chuyển hàng thông minh.
Giáo viên hướng dẫn
Sinh viên thực hiện
PGS.TS. Phan Bùi Khôi
Trịnh Văn Vương
1
Chương 1. Thiết lập hệ phương trình động học Robot
Chương 1
THIẾT LẬP HỆ PHƯƠNG TRÌNH ĐỘNG HỌC ROBOT
1.1. Phân tích động học Denavit–Hartenberg
Thiết lập hệ tọa độ Denavit-Hartenberg
Hình vẽ dưới mô tả cấu trúc của Robot No.17 cùng các hệ tọa độ Descartes gắn
vào mỗi khâu theo quy ước Denavit-Hartenberg:
Z3
Z2
Zo
X2
Z1
X1
Xo
Hình 1.1. Robot No.17 và các hệ tọa độ Descartes.
Mô hình robot trên được cụ thể hóa từ mô hình thuần túy mang tính cấu trúc ở đề
bài ra. Robot có bốn khâu (tính cả đế) với ba khớp động quay - tịnh tiến – quay.
Các tham số động học Denavit-Hartenberg
2
X3
Chương 1. Thiết lập hệ phương trình động học Robot
Dựa trên các hệ tọa độ đã thiết lập ở trên, tiến hành tính toán các thông số
Denavit-Hartenberg thu được bảng sau:
D-H Parameters of Robot No. 17
Joint
di
θi
ai
αi
1
d1
θ1*
a1
90o
2
d 2*
0
0
0
3
0
θ3*
a3
90o
Hình 1.2. Tham số động học Denavit – Hartenberg.
Ở đây, các biến khớp được đánh dấu *. Cụ thể, vector biến khớp ở đây là:
q =θ
1
d
2
θ 3
T
1.2. Thiết lập hệ phương trình động học
Ma trận trạng thái khâu thao tác theo cấu trúc động học
Dựa trên bảng tham số động học 1.1, xây dựng các ma trận biến đổi tọa độ thuần
nhất Denavit-Hartenberg dựa trên công thức tổng quát sau:
i-1
Ai =
Cụ thể, ta có các ma trận biến đổi tọa độ thuần nhất như sau:
0
A1 =
1
2
A2 =
A3 =
Từ đó, thiết lập ma trận trạng thái khâu thao tác theo cấu trúc động học ( các biến
khớp) dựa trên công thức: 0A3(q) = 0A1.1A2.2A3
3
Chương 1. Thiết lập hệ phương trình động học Robot
Kết quả tính toán cho:
0
A3(q) =
Ma trận trạng thái khâu thao tác theo tọa độ thao tác
Dựa theo lý thuyết, trạng thái khâu thao tác End-Effector có thể biểu diễn qua
vector định vị (mô tả bới các tọa độ và góc định hướng như các góc Euler, Cardan, Roll0
Pitch-Yaw), cụ thể là:
A3(t) =
Hệ phương trình động học Robot
Từ các kết quả thu được ở trên, thiết lập phương trình động học Robot dưới dạng
hệ phương trình động học. Đây là hệ phương trình đại số phi tuyến gồm các phương trình
độc lập tuyến tính, như trình bày phía dưới đây:
f1 = cos(θ1 )a 3cos(θ3 )+sin(θ1 )d 2 +a1cos(θ1 )-x E = 0
f 2 = sin(θ1 )a 3cos(θ3 )-cos(θ1 )d 2 +a1sin(θ1 )-y E = 0
f 3 = a 3sin(θ3 )+d1 -z E = 0
f 4 = cos(θ1 )cos(θ3 )-cos(β)cos(η) = 0
f 5 = -cos(θ1 )+sin(α)sin(β)sin(η)-cos(α)cos(η) = 0
f 6 = -cos(θ3 )-cos(α)cos(β) = 0
Ghi chú Công việc tính toán ở đây sử dụng sự trợ giúp của phần mềm Matlab.
4
Chương 2. Bài toán động học Robot
Chương 2
BÀI TOÁN ĐỘNG HỌC ROBOT
Giải bài toán động học Robot là vấn đề kỹ thuật kinh điển. Trong chương này sẽ
trình bày lý thuyết thuật toán giải các bài toán thuận nghịch và thực hiện thuật toán
ấy trên công cụ tính toán mô phỏng Matlab. Do mã nguồn chương trình quá dài nên
không tiện cho việc trình bày nên ở đây chủ yếu mô tả cách thực hiện thuật toán
trên phần mềm. Mã chương trình có thể tham khảo ở phụ lục hoặc file mềm đi kèm.
2.1. Bài toán động học thuận
2.1.1. Bài toán và giải thuật
Bài toán Cho trước giá trị các biến khớp (các góc quay, các di chuyển tịnh tiến). Yêu cầu
xác vận tốc điểm tác động cuối và vận tốc góc khâu thao tác.
Giải thuật Theo giải thuật trong bài giảng Robot công nghiệp ( Tài liệu tham khảo [1])
2.1.2. Chương trình lập trình
Giải thuật thực hiện trên Matlab:
‘FwdKinematics.m’
‘RuleGen.m’
Gọi hàm ‘RuleGen.m’ để tạo
file dữ liệu số biến khớp
‘RuleDat.mat’
Khai báo quy luật biến khớp
‘MatrixDH.m’
Gọi hàm ‘MatrixDH.m’sử dụng
vòng lặp for tính ma trận A
Kết quả
Position, Velocity, Omega
Hình 2.1. Trình tự giải bài toán thuận trên Matlab
5
Chương 2. Bài toán động học Robot
5
Position of End-Efector
4000
1
xE
yE
zE
0
-2000
-4000
Velocity of End-Efector
vX
vY
vZ
0.5
Velocity
Position
2000
x 10
0
-0.5
0
1
2
3
4
-1
5
0
1
Time
2
3
4
5
Time
Rotate Angle Velocity of End-Efector
Orbit of End-Efector
20
wX
wY
wZ
15
Position
10
4000
2000
5
0
0
-2000
5000
-5
5000
-10
0
0
1
2
3
4
5
0
-5000
Time
-5000
Hình 2.2. Nghiệm của ví dụ về bài toán động học thuận
2.2. Bài toán động học ngược
2.2.1. Bài toán và giải thuật
Bài toán Cho trước vị trí điểm tác động cuối (và có thể cả vị rí các khâu thao tác). Yêu
cầu tìm giá trị các biến khớp ứng với vị trí cho trước đó.
Giải thuật
Ta sẽ sử dụng phương pháp số nhờ khai triển Taylor và ma trận Jacobi.
Gọi ma trận x là ma trận tọa độ điểm E tại các thời điểm.
Từ bài toán động học thuận ta có hệ thức: x = f ( q ) đạo hàm 2 vế ta có:
x&=
∂f
q&= J ( q ) q&
∂q
6
Chương 2. Bài toán động học Robot
∂f1
∂q
1
∂f 2
∂f
= ∂q1
Trong đó J ( q ) =
∂q
...
∂f n
∂q1
∂f1
∂q2
∂f 2
∂q2
...
∂f n
∂q2
∂f1
∂qn
∂f 2
...
∂qn được gọi là ma trân Jacobi
... ...
∂f n
...
∂qn
−1
Từ đó ta tính được q&( t ) = J ( q ( t ) ) x&( t )
Áp dụng khai triển Taylor sử dụng sai phân lùi để tính các nghiệm tiếp theo :
q k +1 = q ( tk + ∆t ) ≈ q k + q&k ∆t
2.2.2. Chương trình lập trình
Giải thuật thực hiện trên Matlab: Với quỹ đạo chuyển động là đường xoắn ốc.
‘InvKinematics.m’
‘PosGen.m’
‘PosDat.mat’
Gọi hàm ‘PosGen.m’ để tạo file
dữ liệu số vị trí khâu thao tác
Khai báo quy luật thao tác và
tìm nghiệm đầu.
Tính ma trận Jacobi và sử dụng
vòng lặp for tính nghiệm theo
xấp xỉ sai phân lùi.
Kết quả
Giá trị biến khớp
Hình 2.3. Trình tự giải bài toán ngược trên Matlab
7
Chương 2. Bài toán động học Robot
Inverse Kinematics: Θ
1
Inverse Kinematics: Distance 2
1500
0.05
Distance 2
Θ1
0
-0.05
1000
500
-0.1
-0.15
0
2
4
0
6
0
1
Time
2
3
4
5
Time
Inverse Kinematics: Θ
3
Orbit Position
0.04
100
Θ3
0.02
0
0
-0.02
-100
-0.04
-200
1200
-0.06
1180
0
2
4
6
0
1160
Time
-100
Hình 2.4. Nghiệm của ví dụ về bài toán ngược
8
-50
50
Chương 3. Thiết kế và mô phỏng Robot
Chương 3
THIẾT KẾ VÀ MÔ PHỎNG ROBOT
3.1. Thiết kế mô hình Robot 3D bằng Solidworks
Robot No.17 có ba bậc tự do thuộc loại robot có các khâu nối tiếp hở, có hai bậc
xoay và một bậc tịnh tiến. Robot No.17 được thiết kế nhờ Solidworks, một công cụ rất
phổ biến trong các trương đại học kỹ thuật:
Hình 3.1. Mô hình 3D Robot No.17 trên nền Solidworks.
Do sự hạn chế về kiến thức thuộc lĩnh vực cơ khí (không đúng với chuyên ngành
đào tạo) nên mô hình robot ở đây được thiết kế thông qua tham khảo từ hình mẫu các
robot như Stanford, Scara, Puma, Kuka… Kích thước robot cũng được tham khảo từ
nhiều nguồn, chủ yếu từ các bản vẽ robot thông dụng, phổ biến ở các phòng thí nghiệm,
các trường đại học trên thế giới.
9
Chương 3. Thiết kế và mô phỏng Robot
Hình dưới trình bày bản vẽ CAD hình chiếu 2D của Robot :
Hình 3.2. Bản vẽ 2D Robot No.17.
3.2. Mô phỏng hoạt động Robot trên Matlab
10
Chương 3. Thiết kế và mô phỏng Robot
Hình 3.3. Giao diện mô phỏng hoạt động Robot No.17
Tiến hành xây dựng giao diện mô phỏng hoạt động của Robot No.17 trên nền
Matlab như hình vẽ trên, một công cụ kỹ thuật cực mạnh. Giao diện mô phỏng này kế
thừa các giải thuật của bài toán động học để tính toán và hiển thị hoạt động của robot.
Lưu đồ dưới mô tả các bước thực hiện :
Từ các bản vẽ 3D xuất ra file định dạng nhị phân
‘*.stl’ mang thông tin vê cấu trúc cơ khí của robot.
Lập trình file hàm ‘stlread.m’ để xử lý các file ‘*.stl’, trả về các
vector đỉnh và mặt (Vertices & Faces) của vật thể. Lưu các thông
số này vào file dữ liệu ‘linksdata.mat’ mang thông tin về các khâu.
Lập trình file ‘RobotNo17.m’ tạo giao diện GUI (Graphical User Interface) để tiến hành mô
phỏng, trong đó sử dụng lệnh ‘patch’ để tái tạo lại mô hình 3D của robot và hiển thị trên
Matlab. Giao diện này hiển thị hình ảnh thao tác của robot và các công cụ chức năng phong
phú cho phép tùy biến các trạng thái không gian của robot, cụ thể là:
Bốn nút bấm chức năng
Các hộp thoại và thanh trượt
Demo Giải bài toán động học ngược để
robot thao tác theo quỹ đạo xoắn ốc.
Các hộp thoại và thanh trượt này để cái
đặt và quan sát thông số biến khớp:
Random Move Đặt giá trị ngẫu nhiên cho
các biến khớp.
Clr Trail Xóa vết quỹ đạo của khâu thao
tác cuối.
Home Đưa robot về vị trí ban đầu.
Hình 3.4. Lưu đồ các bước xây dựng giao diện mô phỏng Robot No.17
11
Chương 3. Thiết kế và mô phỏng Robot
Giao diện mô phỏng này kế thừa các thuật toán giải bài toán động học đã xây
dựng ở chương 2 để giải bài toán động học trong quá trình mô phỏng.
Một số hình ảnh mô phỏng:
Hình 3.5. Hình ảnh mô phỏng hoạt động của Robot No. 17.
Giao diện mô phỏng này còn đang được phát triển them nhiều tính năng khác,
chẳng hạn mô phỏng phối hợp thao tác của hai robot, hay bổ sung đối tượng thao tác lẫn
môi trường làm việc ( băng chuyền, khớp hàn …).
12
Tài liệu tham khảo
TÀI LIỆU THAM KHẢO
[1] PGS.TS. Phan Bùi Khôi, Bài giảng Robot công nghiệp.
[2] Internet
13
Phụ lục
PHỤ LỤC
Phụ lục 1. Mã chương trình thiết lập phương trình động học Gồm hai mã
chương trình:
‘MatrixDH.m’
function
DH11
DH12
DH13
DH14
DH21
DH22
DH23
DH24
DH31
DH32
DH33
DH34
DH41
DH42
DH43
DH44
DH =
end
DH = MatrixDH(alpha, a, d, theta)
= cos(theta);
= -sin(theta)*cos(alpha);
= sin(theta)*sin(alpha);
= a*cos(theta);
= sin(theta);
= cos(theta)*cos(alpha);
= -cos(theta)*sin(alpha);
= a*sin(theta);
= 0;
= sin(alpha);
= cos(alpha);
= d;
= 0;
= 0;
= 0;
= 1;
[ DH11 DH12 DH13 DH14;
DH21 DH22 DH23 DH24;
DH31 DH32 DH33 DH34;
DH41 DH42 DH43 DH44];
‘KinematicsEquation.m’
%%%%%%%%%%%%%%%%%%%%%%%% Equation Kinematics Generate %%%%%%%%%%%%%%%%%%%
%
Written by: Vuong V. Trinh - Automatics Control
%
%
Hanoi University of Science and Technology
%
%
20/12/2011
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Equation Kinematics Generate
% Preparation
clear all
clc
% Define parameters and variables as symbols
syms q1 q2 q3 a1 a3 d1 xE yE zE alpha beta eta real
% Define DH parameters
alpha = [pi/2 0 pi/2]';
a = [a1 0 a3]';
d = [d1 q2 0]';
theta = [q1 0 q3]';
% Initial and calculate DH matrix A
A = eye(4);
for j = 1:3
DH = MatrixDH(alpha(j),a(j),d(j),theta(j));
A = A*DH;
end
14
Phụ lục
% Define Cardan matrix
CD11 = cos(beta)*cos(eta);
CD14 = xE;
CD22 = -sin(alpha)*sin(beta)*sin(eta)+cos(alpha)*cos(eta);
CD24 = yE;
CD33 = -sin(alpha)*cos(beta);
CD34 = zE;
% Define and display kinematics equations systems
Eqt1 = A(1,4) - CD14
Eqt2 = A(2,4) - CD24
Eqt3 = A(3,4) - CD34
Eqt4 = A(1,1) - CD11
Eqt5 = A(2,2) - CD22
Eqt6 = A(3,3) - CD33
Phụ lục 2. Mã chương trình bài toán động học thuận Gồm ba mã chương trình:
‘MatrixDH.m’
% View on the previous.
‘RuleGen.m’
% This function generates rule of joint variables for
% Forward kinematics problems
close all
clear all
clc
t = 0:.01:5;
q1 = 2*t.^2; q2 = 2.*exp(-t); q3 = 2*t.*cos(t);
save('RuleDat.mat');
clear all
clc
‘FwdKinematics.m’
%%%%%%%%%%%%%%%%%%%%%%%% Forward Kinematics Solution %%%%%%%%%%%%%%%%%%%%
%
Written by: Vuong V. Trinh - Automatics Control
%
%
Hanoi University of Science and Technology
%
%
20/12/2011
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Forward Kinematics Solution
close all
clear all
clc
% Load rule of joint variables
RuleGen
load('RuleDat.mat');
% Initial
i = 1;
alpha = [pi/2 0 pi/2]';
a = [800 0 2600]';
d = [1000 q2(i) 0]';
theta = [q1(i) 0 q3(i)]';
A = eye(4);
for j = 1:3
DH = MatrixDH(alpha(j),a(j),d(j),theta(j));
A = A*DH;
end
15
Phụ lục
r = A(1:3,4);
xE = r(1); yE = r(2); zE = r(3);
x_E(i) = xE;y_E(i) = yE;z_E(i) = zE;
% Calculate
for i = 2:length(t)
% Assign DH parameters
alpha = [pi/2 0 pi/2]';
a = [800 0 2600]';
d = [1000 q2(i) 100]';
theta = [q1(i) 0 q3(i)]';
% Compute state matrix through kinematics variables
Aold = A;
A = eye(4);
for j = 1:3
DH = MatrixDH(alpha(j),a(j),d(j),theta(j));
A = A*DH;
end
% Forward kinematics of position, velocity and rotate angle
% Calculate essential matrices from DH matrix
Adot = (A - Aold)/(t(i)-t(i-1));
% A dot
R = A(1:3,1:3);
% R
Rdot = Adot(1:3,1:3);
% R dot
r = A(1:3,4);
% r
rdot = Adot(1:3,4);
% rdot
w_wave = Rdot*R';
% w wave
w = [w_wave(3,2) w_wave(1,3) w_wave(2,1)]';
% w
% Show roots of forward kinematics problem
% Position
xE = r(1); yE = r(2); zE = r(3);
% Velocity
vX = rdot(1); vY = rdot(2); vZ = rdot(3);
% Rotate angle velocity
wX = w(1); wY = w(2); wZ = w(3);
% Save data to plot
x_E(i) = xE;y_E(i) = yE;z_E(i) = zE;
v_X(i) = vX;v_Y(i) = vY;v_Z(i) = vZ;
w_X(i) = wX;w_Y(i) = wY;w_Z(i) = wZ;
end
% Visualize
figure1 = figure;
plot(t,x_E,'.',t,y_E,'b',t,z_E,'--');
grid on
hold on
xlabel({'Time'});
ylabel({'Position'});
title({'Position of End-Efector'});
legend('xE','yE','zE');
figure2 = figure;
plot(t,v_X,'.',t,v_Y,'b',t,v_Z,'--');
grid on
hold on
xlabel({'Time'});
title({'Velocity of End-Efector'});
ylabel({'Velocity'});
legend('vX','vY','vZ');
figure3 = figure;
plot(t,w_X,'.',t,w_Y,'b',t,w_Z,'--');
grid on
hold on
16
Phụ lục
xlabel({'Time'});
title({'Rotate Angle Velocity of End-Efector'});
ylabel({'Position'});
legend('wX','wY','wZ');
figure4 = figure;
plot3(x_E,y_E,z_E);
grid on
hold on
title({'Orbit of End-Efector'});
% End
Phụ lục 3. Mã chương trình bài toán động học ngược Gồm hai mã chương trinh:
‘PosGen.m’
% This function generates position rule for inverse kinematics problems
close all
clear all
clc
t = 0:.01:5;
xE = 30*t.*cos(t);
yE = 1200-300*t.*(t)/(50*pi);
zE = 30*t.*sin(t);
save('PosDat.mat');
clear all
clc
‘InvKinematics.m’
%%%%%%%%%%%%%%%%%%%%%%%% Inverse Kinematics Solution %%%%%%%%%%%%%%%%%%%%
%
Written by: Vuong V. Trinh - Automatics Control
%
%
Hanoi University of Science and Technology
%
%
20/12/2011
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Inverse Kinematics Solution
% Preparation
close all
clear all
clc
PosGen
load('PosDat.mat')
tstep = t(2)-t(1); % Step time
% Choose initial
q1old = 0; q2old = 1000; q3old = 0;
q = [q1old q2old q3old]';
for i=2:length(t)
q1 = q1old; q2 = q2old; q3 = q3old;
% Jacobian matrix calculate
J11 = q2*cos(q1)-(1614521220006991075*cos(q1)*sin(q3))...
/10141204801825835211973625643008-2600*cos(q3)*sin(q1)-800*sin(q1);
J12 = sin(q1);
J13 = - 2600*cos(q1)*sin(q3)-(1614521220006991075*cos(q3)*...
sin(q1))/10141204801825835211973625643008;
J21 = 800*cos(q1)+2600*cos(q1)*cos(q3)-(1614521220006991075*...
sin(q1)*sin(q3))/10141204801825835211973625643008+q2*sin(q1);
J22 = -cos(q1);
17
Phụ lục
J23 = (1614521220006991075*cos(q1)*cos(q3))/...
10141204801825835211973625643008-2600*sin(q1)*sin(q3);
J31 = 0;
J32 = 4967757600021511/81129638414606681695789005144064;
J33 = 2600*cos(q3);
J = [J11 J12 J13; J21 J22 J23; J31 J32 J33];
J_inv = inv(J);
% Inverse Jacobian matrix
% Position deratives matrix calculate
xdot = (xE(i)-xE(i-1))/tstep;
ydot = (yE(i)-yE(i-1))/tstep;
zdot = (zE(i)-zE(i-1))/tstep;
Posdot = [xdot ydot zdot]';
% Approximate use backward algorithm
q = q+J_inv*Posdot*tstep;
q1old = q(1); q2old = q(2); q3old = q(3);
q1_(i) = q(1); q2_(i) = q(2); q3_(i) = q(3);
end
figure1 = figure;
plot(t,q1_,'*');
xlabel('Time');
ylabel('\Theta_1');
title('Inverse Kinematics: \Theta_1');
grid on
figure2 = figure;
plot(t,q2_,'o');
xlabel('Time');
ylabel('Distance 2');
title('Inverse Kinematics: Distance 2');
grid on
figure3 = figure;
plot(t,q3_,'d');
xlabel('Time');
ylabel('\Theta_3');
title('Inverse Kinematics: \Theta_3');
grid on
figure4 = figure;
plot3(xE, yE, zE,'*');
title('Orbit Position');
grid on
Phụ lục 4. Mã chương trình tạo giao diện mô phỏng Do đoạn mã quá dài, xấp xỉ
600 dòng lệnh và 200 dòng chú thích nên không tiện đưa vào đây. Có thể tham khảo chi
tiết ở file mềm kèm theo.
18