Thiết kế và mô phỏng robot 3d trên MATLAB (Kèm code MATLAB)

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