Luận án Động lực học ngư của robot song song delta không gian ợc và điều khiển chuy ển động

BỘ GIÁO DỤC VÀ ĐÀO T
HỌ
ĐỘNG LỰC HỌC NGƯ
CỦA ROBOT SONG SONG DELTA KHÔNG GIAN
LUẬN ÁN TI
ẠO VIỆN HÀN LÂM KHOA H
VÀ CÔNG NGHỆ VI
C VIỆN KHOA HỌC VÀ CÔNG NGHỆ 
----------------------------- 
NGUYỄN ĐÌNH DŨNG 
ỢC VÀ ĐIỀU KHIỂN CHUY
ẾN SĨ KỸ THUẬT CƠ KHÍ VÀ CƠ KỸ
Hà Nội – 2018 
ỌC 
ỆT NAM 
ỂN ĐỘNG 
 THUẬT 
BỘ GIÁO DỤC VÀ ĐÀO TẠO 
VIỆN HÀN LÂM KHOA HỌC 
VÀ CÔNG NGHỆ VIỆT NAM 
HỌC VIỆN KHOA HỌC VÀ CÔNG NGHỆ 
----------------------------- 
NGUYỄN ĐÌNH DŨNG 
ĐỘNG LỰC HỌC NGƯỢC VÀ ĐIỀU KHIỂN CHUYỂN ĐỘNG 
CỦA ROBOT SONG SONG DELTA KHÔNG GIAN 
Chuyên ngành: Cơ kỹ thuật 
Mã số: 9 52 01 01 
LUẬN ÁN TIẾN SĨ KỸ THUẬT CƠ KHÍ VÀ CƠ KỸ THUẬT 
NGƯỜI HƯỚNG DẪN KHOA HỌC: 
 1. GS. TSKH. Nguyễn Văn Khang 
 2. PGS. TS. Nguyễn Quang Hoàng 
Hà Nội – 2018 
I 
LỜI CẢM ƠN 
Lời đầu tiên, tác giả xin gửi lời cảm ơn chân thành và sâu sắc tới GS. TSKH. 
Nguyễn Văn Khang cùng PGS. TS. Nguyễn Quang Hoàng. Các thầy đã tận tình 
hướng dẫn và truyền cho tôi những kinh nghiệm quý báu trong nghiên cứu khoa 
học. Tác giả xin gửi lời cảm ơn tới Viện Cơ học – Viện Hàn lâm Khoa học và Công 
nghệ Việt Nam đã tạo mọi điều kiện thuận lợi trong suốt quá trình làm luận án này. 
Tác giả xin cảm ơn sự ủng hộ của bạn bè, đồng nghiệp đã giúp đỡ tạo điều kiện 
thuận lợi trong quá trình làm luận án. 
Cuối cùng, tác giả xin chân thành cảm ơn đến gia đình đã động viên ủng hộ 
trong suốt quá trình làm luận án. 
II 
LỜI CAM ĐOAN 
Tôi xin cam đoan đây là công trình nghiên cứu của riêng tôi, các kết quả 
nghiên cứu được trình bày trong luận án là trung thực, khách quan và chưa từng 
được công bố trong bất kỳ công trình nào khác. 
Tôi xin cam đoan rằng mọi sự giúp đỡ cho việc thực hiện luận án đã được cám 
ơn, các thông tin trích dẫn trong luận án này đều được chỉ rõ nguồn gốc. 
 Hà Nội, ngày....... tháng....... năm 2018 
Tác giả luận án 
Nguyễn Đình Dũng 
III 
MỤC LỤC 
LỜI CẢM ƠN ........................................................................................................................ I 
LỜI CAM ĐOAN.................................................................................................................. II 
DANH MỤC CÁC BẢNG ................................................................................................ VIII 
DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ ............................................................................ VIII 
MỞ ĐẦU ............................................................................................................................... 1 
Chương 1 TỔNG QUAN VỀ BÀI TOÁN ĐỘNG LỰC HỌC VÀ ĐIỀU KHIỂN ROBOT 
SONG SONG ......................................................................................................................... 3 
1.1 Robot có cấu trúc song song ......................................................................................... 3 
1.2 So sánh robot nối tiếp và robot song song..................................................................... 3 
1.3 Giới thiệu về hai robot song song Delta không gian 3RUS và 3PUS đã chế tạo ............ 5 
1.4 Ứng dụng của robot song song ..................................................................................... 7 
1.4.1 Ứng dụng trong công nghiệp ................................................................................. 7 
1.4.2 Ứng dụng trong mô phỏng .................................................................................... 8 
1.4.3 Ứng dụng trong y học ......................................................................................... 10 
1.4.4 Các ứng dụng khác ............................................................................................. 11 
1.5 Một số nghiên cứu về động lực học và điều khiển robot song song ở ngoài nước ....... 12 
1.5.1 Động lực học robot song song ............................................................................. 12 
1.5.2 Điều khiển bám quỹ đạo robot song song ............................................................ 14 
1.6 Các nghiên cứu tại Việt Nam ...................................................................................... 15 
1.7 Xác định vấn đề cần nghiên cứu của luận án .............................................................. 17 
Kết luận chương 1 ................................................................................................................ 18 
Chương 2 XÂY DỰNG MÔ HÌNH CƠ HỌC VÀ MÔ HÌNH TOÁN HỌC CHO ROBOT 
SONG SONG DELTA KHÔNG GIAN ................................................................................ 19 
2.1 Mô hình động học robot song song Delta không gian ................................................. 19 
2.1.1 Mô hình động học robot song song Delta không gian 3RUS ............................... 20 
2.1.2 Mô hình động học robot song song Delta không gian 3PUS ................................ 22 
2.2 Mô hình động lực robot song song Delta không gian .................................................. 25 
2.2.1 Mô hình động lực robot song song Delta không gian 3RUS ................................ 25 
2.2.2 Mô hình động lực robot song song Delta không gian 3PUS ................................ 26 
2.3 Dạng ma trận mới phương trình Lagrange dạng nhân tử [51] ...................................... 26 
IV 
2.4 Thiết lập phương trình chuyển động của robot song song Delta không gian 3RUS ..... 28 
2.4.1 Thiết lập phương trình chuyển động cho mô hình 1 của robot 3RUS ................... 28 
2.4.2 Thiết lập phương trình chuyển động cho mô hình 2 của robot 3RUS ................... 38 
2.5 Thiết lập phương trình chuyển động robot song song Delta không gian 3PUS ............ 43 
2.5.1 Thiết lập phương trình chuyển động mô hình 1 của robot 3PUS .......................... 43 
2.5.2 Thiết lập phương trình chuyển động mô hình 2 của robot 3PUS .......................... 50 
2.6 So sánh phương trình chuyển động các mô hình của robot ......................................... 53 
Kết luận chương 2 ................................................................................................................ 53 
Chương 3 MÔ PHỎNG SỐ ĐỘNG HỌC NGƯỢC VÀ ĐỘNG LỰC HỌC NGƯỢC 
ROBOT SONG SONG DELTA KHÔNG GIAN .................................................................. 54 
3.1 Tính toán động học ngược robot song song bằng phương pháp Newton – Raphson cải tiến .... 54 
3.1.1 Thiết lập công thức tính vận tốc và gia tốc suy rộng ............................................ 54 
3.1.2 Các công thức xác định véc tơ tọa độ suy rộng tq ........................................... 55 
3.1.3 Thuật toán hiệu chỉnh độ chính xác véc tơ tọa độ suy rộng tq tại mỗi bước tính ........ 56 
3.1.4 Đánh giá sai số ................................................................................................... 57 
3.2 Phương pháp số giải bài toán động lực học ngược robot song song ............................ 58 
3.2.1 Bài toán động lực học ngược .............................................................................. 58 
3.2.2 Giải bài toán động lực học ngược bằng phương pháp khử các nhân tử Lagrange [4] ....... 59 
3.3 Mô phỏng số bài toán động học ngược robot song song Delta không gian .................. 61 
3.3.1 Mô phỏng số bài toán động học ngược robot 3RUS ............................................ 61 
3.3.2 Mô phỏng số bài toán động học ngược robot Delta 3PUS ................................... 66 
3.4 Mô phỏng số bài toán động lực học ngược robot song song Delta không gian ............ 68 
3.4.1 Mô phỏng số bài toán động lực học ngược robot Delta 3RUS ............................. 68 
3.4.2 Mô phỏng số bài toán động lực học ngược robot Delta không gian 3PUS ........... 72 
Kết luận chương 3 ................................................................................................................ 73 
Chương 4 ĐIỀU KHIỂN BÁM QUỸ ĐẠO CHUYỂN ĐỘNG ROBOT SONG SONG 
DELTA KHÔNG GIAN DỰA TRÊN CÁC MÔ HÌNH CƠ HỌC ........................................ 74 
4.1 Tổng quan về điều khiển bám quỹ đạo của khâu thao tác ............................................ 74 
4.1.1 Giới thiệu chung ................................................................................................. 74 
4.1.2 Bài toán điều khiển trong không gian khớp ......................................................... 74 
4.1.3 Bài toán điều khiển trong không gian thao tác ..................................................... 75 
V 
4.2 Điều khiển bám quỹ đạo robot song song trong không gian khớp dựa trên phương 
trình Lagrange dạng nhân tử. ............................................................................................ 75 
4.2.1 Cơ sở động lực học hệ nhiều vật có cấu trúc mạch vòng ..................................... 75 
4.2.2 Cơ sở lý thuyết xây dựng các thuật toán điều khiển ............................................. 78 
4.3 Mô phỏng số các luật điều khiển cho robot song song Delta không gian dựa trên các 
mô hình động lực ............................................................................................................. 89 
4.3.1 Sơ đồ mô phỏng số các phương pháp điều khiển ................................................. 89 
4.3.2 Mô phỏng số các phương pháp điều khiển robot song song Delta không gian 3RUS ...... 90 
4.3.3 Mô phỏng số các phương pháp điều khiển robot song song Delta không gian 3PUS ....... 98 
Kết luận chương 4 .............................................................................................................. 106 
KẾT LUẬN ........................................................................................................................ 107 
DANH MỤC CÔNG TRÌNH CỦA TÁC GIẢ .................................................................... 110 
TÀI LIỆU THAM KHẢO .................................................................................................. 111 
PHỤ LỤC .......................................................................................................................... 119 
VI 
DANH MỤC CÁC KÝ HIỆU 
M Ma trận khối lượng 
C Ma trận quán tính ly tâm và Coriolis 
sΦ Ma trận Jacobi 
g Véc tơ gia tốc trọng trường 
s Véc tơ tọa độ suy rộng dư 
s Véc tơ vận tốc suy rộng dư 
s Véc tơ gia tốc suy rộng dư 
q Véc tơ tọa độ khớp 
q Véc tơ vận tốc khớp 
q Véc tơ gia tốc khớp 
aq Véc tơ tọa độ suy rộng khớp chủ động (active joints) 
aq Véc tơ vận tốc suy rộng khớp chủ động 
aq Véc tơ gia tốc suy rộng khớp chủ động 
pq Véc tơ tọa độ suy rộng khớp bị động (passive joints) 
pq Véc tơ vận tốc suy rộng khớp bị động 
pq Véc tơ gia tốc suy rộng khớp bị động 
x Véc tơ tọa độ suy rộng khâu thao tác 
x Véc tơ vận tốc suy rộng khâu thao tác 
x Véc tơ gia tốc suy rộng khâu thao tác 
z Véc tơ tọa độ suy rộng phụ thuộc dư 
f Véc tơ các phương trình liên kết 
aτ Véc tơ mô men/ lực dẫn động 
λ Véc tơ các nhân tử Lagrange 
u Véc tơ lực điều khiển 
R Bán kính bàn máy cố định 
r Bán kính bàn máy di động 
L1 Chiều dài khâu chủ động 
L2 Chiều dài khâu bị động 
I Ma trận của ten xơ quán tính khối trong hệ quy chiếu gắn với khối tâm 
VII 
TJ Ma trận Jacobi tịnh tiến 
RJ Ma trận Jacobi quay 
ω Véc tơ vận tốc góc khâu 
T Động năng cơ hệ 
 Thế năng cơ hệ 
V Hàm Lyapunov 
ae Véc tơ sai số vị trí các khớp chủ động 
xe Sai số vị trí khâu thao tác theo phương x 
ye Sai số vị trí khâu thao tác theo phương y 
ze Sai số vị trí khâu thao tác theo phương z 
DANH MỤC CÁC CHỮ VIẾT TẮT 
RUS Revolute-Universal-Spherical 
PUS Presmatic - Universal-Spherical 
PD Proportional - Derivate 
PID Proportional - Integral – Derivate 
RBF Radial Basic Functions 
VIII 
DANH MỤC CÁC BẢNG 
Bảng 1.1: So sánh robot chuỗi và robot song song ............................................................. 4 
Bảng 1.2: Tham số robot Delta 3RUS ................................................................................ 6 
Bảng 1.3: Bảng các tham số robot 3PUS ............................................................................ 6 
Bảng 1.4: Thông số kỹ thuật động cơ bước của hai robot 3RUS và 3PUS .......................... 7 
Bảng 2.1: So sánh phương trình chuyển động của mô hình 1 và 2 .................................... 53 
Bảng 3.1: Các tham số robot theo tài liệu [61] ................................................................. 64 
Bảng 4.1: Bảng so sánh sai số bám quỹ đạo của robot ..................................................... 91 
Bảng 4.2: Bảng so sánh lực điều khiển robot ................................................................... 95 
Bảng 4.3: Bảng so sánh sai số bám quỹ đạo của robot ..................................................... 99 
Bảng 4.4: Bảng so sánh mô men điều khiển ................................................................... 103 
DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ 
Hình 1.1. Cấu trúc robot song song .................................................................................... 3 
Hình 1.2: Robot Puma [67] ................................................................................................ 4 
Hình 1.3: Robot thực 3RUS đã chế tạo .............................................................................. 5 
Hình 1.4: Robot Delta 3PUS đã chế tạo ............................................................................. 5 
Hình 1.5: Sơ đồ điều khiển robot ....................................................................................... 6 
Hình 1.6: Cơ cấu song song Gough [67] ............................................................................ 7 
Hình 1.7: Robot Delta ứng dụng trong công nghệ thực phẩm [29] ..................................... 8 
Hình 1.8: Cơ cấu song song Stewart [67] ........................................................................... 9 
Hình 1.9: Sản phẩm Persival của École National d'Elquitation (Pháp) [67] ........................ 9 
Hình 1.10: Bộ mô phỏng xe đạp của Viện KAIST và sản phẩm Caren của Motek [67] ...... 9 
Hình 1.11: Sản phẩm SuriScope đang vận hành, Đại học Humboldt (Berlin, Đức) [43] ... 10 
Hình 1.12: Robot CRIGOS dùng để phẫu thuật tái tạo xương .......................................... 10 
Hình 1.13: Robot song song kiểu Stewart [67] ................................................................. 11 
Hình 1.14: Robot Delta [29] ............................................................................................ 11 
Hình 1.15: Cấu trúc chấp hành song song 3RPS [96] ....................................................... 12 
Hình 2.1: Robot song song Delta không gian 3RUS ......................................................... 20 
Hình 2.2: Mô hình động học robot Delta 3RUS ............................................................... 21 
Hình 2.3: Tọa độ suy rộng định vị ................................................................................... 21 
Hình 2.4: Mô hình thiết kế robot Delta 3PUS .................................................................. 23 
H ... =0.3;L2=0.8; alpha1=0; alpha2=2*pi/3; 
alpha3=4*pi/3; m1=0.42; 
m2=2*0.20; m3=0.75; Ix=0; Iz=m1*L1*L1/12; Iy=Iz; I2y=m2*L2*L2/12; 
I2z=I2y; g=9.81; I2x=0; 
qd=[in(1),in(2),in(3)]'; % Vi tri khop chu dong 
mong muon; 
dqd=[in(13),in(14),in(15)]'; % Van toc khop chu dong 
mong muon; 
ddqd=[in(25),in(26),in(27)]'; 
s=[in(37),in(38),in(39),in(40),in(41),in(42),in(43),in(44),in(45),in
(46),in(47),in(48)]'; % Vi tri toa do khop thuc te 
ds=[in(49),in(50),in(51),in(52),in(53),in(54),in(55),in(56),in(57),i
n(58),in(59),in(60)]'; % Van toc toa do khop thuc te 
q=[in(37),in(38),in(39),in(40),in(41),in(42),in(43),in(44),in(45)]; 
dq=[in(49),in(50),in(51),in(52),in(53),in(54),in(55),in(56),in(57)]; 
qa=[in(37),in(38),in(39)]'; 
dqa=[in(49),in(50),in(51)]'; 
% Sai so 
e=qa-qd; de= dqa-dqd; 
% Tinh ma tran M 
M=Ms(q); 
% Tính ma tran C 
C=Cs(q,dq); 
% Tinh ma tran Gs 
G=Gs(q); 
% Tinh ma tran R 
 R=[eye(3,3);-inv(Phiz(q))*Phia(q)]; 
% Tinh dao ham ma tran R 
%dPhi=dPhiz(q,dq); 
dR=[zeros(3,3);inv(Phiz(q))*dPhiz(q,dq)*(inv(Phiz(q)))*Phia(q)-
inv(Phiz(q))*dPhia(q,dq)]; 
% Tinh ma tran Ma 
Ma=R'*M*R; Ca=R'*M*dR+R'*C*R; Ga=R'*G; 
Kp=2500*eye(3); Kd=150*eye(3); 
y=(Ma*ddqd+Ca*dqa+Ga)-Ma*(Kp*e+Kd*de); 
140 
% Ham tinh luat dieu khien robot Delta 3RRR 
function y=ControllerPD2(in) 
global R r a b anpha1 anpha2 anpha3 m1 m2 m3 IAy g 
R=0.266;r=0.04;a=0.3;b=0.8; anpha1=0; anpha2=2*pi/3; anpha3=4*pi/3; 
m1=0.42; 
m2=0.2; m3=0.75; IAy=m1*a*a/3; g=9.81; 
qd=[in(1),in(2),in(3)]'; % Vi tri khop chu dong mong 
muon; 
dqd=[in(7),in(8),in(9)]'; % Van toc khop chu dong mong 
muon; 
ddqd=[in(13),in(14),in(15)]'; 
s=[in(19),in(20),in(21),in(28),in(29),in(30)]'; % Vi tri toa 
do khop thuc te 
ds=[in(31),in(32),in(33),in(40),in(41),in(42)]'; % Van toc toa 
do khop thuc te 
q=[in(19),in(20),in(21)]'; x=in(28);y=in(29); z=in(30); 
dq=[in(31),in(32),in(33)]'; dx=in(40); dy=in(41); dz=in(42); 
qa=[in(19),in(20),in(21)]'; dqa=[in(31),in(32),in(33)]'; 
% Sai so 
e=q-qd; de= dq-dqd; 
% Tinh ma tran M 
M=Ms2(); 
% Tinh ma tran Gs 
G=Gs2(q); 
% Tinh dao ham ma tran Phiz2 
dPz=dPhiz2(q,dq,dx,dy,dz); 
P=inv(Phiz2(q,x,y,z)); 
% Tinh ma tran R 
Q=[eye(3,3);-P*Phia2(q,x,y,z)]; 
dR=[zeros(3,3);P*dPz*P*Phia2(q,x,y,z)-
P*dPhia2(q,x,y,z,dq,dx,dy,dz)]; 
% Tinh Ma, Ca 
Ma=Q'*M*Q; Ca=Q'*M*dR; Ga=Q'*G; 
Kp=2500*eye(3); Kd=150*eye(3); 
y=(Ma*ddqd+Ca*dqa+Ga)-Ma*(Kp*e+Kd*de); 
Luật điều khiển PID 
% Ham tinh luat dieu khien robot Delta 3RRR 
function y=ControllerPID(in) 
141 
global R r L1 L2 alpha1 alpha2 alpha3 m1 m2 m3 Ix Iy Iz I2x I2y I2z 
g 
%Dau vao 1 
R=0.266;r=0.04;L1=0.3;L2=0.8; alpha1=0; alpha2=2*pi/3; 
alpha3=4*pi/3; m1=0.42; 
m2=2*0.20; m3=0.75; Ix=0; Iz=m1*L1*L1/12; Iy=Iz; I2y=m2*L2*L2/12; 
I2z=I2y; g=9.81; I2x=0; 
qd=[in(1),in(2),in(3)]'; % Vi tri khop chu dong 
mong muon; 
dqd=[in(13),in(14),in(15)]'; % Van toc khop chu dong mong 
muon; 
ddqd=[in(25),in(26),in(27)]'; 
s=[in(37),in(38),in(39),in(40),in(41),in(42),in(43),in(44),in(45),in
(46),in(47),in(48)]'; % Vi tri toa do khop thuc te 
ds=[in(49),in(50),in(51),in(52),in(53),in(54),in(55),in(56),in(57),i
n(58),in(59),in(60)]'; % Van toc toa do khop thuc te 
Ie=[in(61),in(62),in(63)]' 
q=[in(37),in(38),in(39),in(40),in(41),in(42),in(43),in(44),in(45)]; 
dq=[in(49),in(50),in(51),in(52),in(53),in(54),in(55),in(56),in(57)]; 
qa=[in(37),in(38),in(39)]'; 
dqa=[in(49),in(50),in(51)]'; 
% Sai so 
e=qa-qd; de= dqa-dqd; 
% Tinh ma tran M 
M=Ms(q); 
% Tính ma tran C 
C=Cs(q,dq); 
% Tinh ma tran Gs 
G=Gs(q); 
% Tinh ma tran R 
 R=[eye(3,3);-inv(Phiz(q))*Phia(q)]; 
% Tinh dao ham ma tran R 
dR=[zeros(3,3);inv(Phiz(q))*dPhiz(q,dq)*(inv(Phiz(q)))*Phia(q)-
inv(Phiz(q))*dPhia(q,dq)]; 
% Tinh ma tran Ma 
Ma=R'*M*R; Ca=R'*M*dR+R'*C*R; Ga=R'*G; 
Kp=2500*eye(3); Kd=150*eye(3);Ki=150*eye(3); 
y=(Ma*ddqd+Ca*dqa+Ga)-Ma*(Kp*e+Kd*de+Ki*Ie); 
142 
% Ham tinh luat dieu khien robot Delta 3PRS 
function y=ControllerPID2(in) 
global R r a b anpha1 anpha2 anpha3 m1 m2 m3 IAy g 
R=0.266;r=0.04;a=0.3;b=0.8; anpha1=0; anpha2=2*pi/3; anpha3=4*pi/3; 
m1=0.42; 
m2=0.2; m3=0.75; IAy=m1*a*a/3; g=9.81; 
qd=[in(1),in(2),in(3)]'; % Vi tri khop chu dong mong 
muon; 
dqd=[in(7),in(8),in(9)]'; % Van toc khop chu dong mong 
muon; 
ddqd=[in(13),in(14),in(15)]'; 
s=[in(19),in(20),in(21),in(28),in(29),in(30)]'; % Vi tri toa 
do khop thuc te 
ds=[in(31),in(32),in(33),in(40),in(41),in(42)]'; % Van toc toa 
do khop thuc te 
Ie=[in(43),in(44),in(45)]'; 
q=[in(19),in(20),in(21)]'; x=in(28);y=in(29); z=in(30); 
dq=[in(31),in(32),in(33)]'; dx=in(40); dy=in(41); dz=in(42); 
qa=[in(19),in(20),in(21)]'; dqa=[in(31),in(32),in(33)]'; 
% Sai so 
e=q-qd; de= dq-dqd; 
% Tinh ma tran M 
M=Ms2(); 
% Tinh ma tran Gs 
G=Gs2(q); 
% Tinh dao ham ma tran Phiz2 
dPz=dPhiz2(q,dq,dx,dy,dz); 
P=inv(Phiz2(q,x,y,z)); 
% Tinh ma tran R 
Q=[eye(3,3);-P*Phia2(q,x,y,z)]; 
dR=[zeros(3,3);P*dPz*P*Phia2(q,x,y,z)-
P*dPhia2(q,x,y,z,dq,dx,dy,dz)]; 
% Tinh Ma, Ca 
Ma=Q'*M*Q; Ca=Q'*M*dR; Ga=Q'*G; 
Kp=2500*eye(3); Kd=150*eye(3);Ki=150*eye(3); 
y=(Ma*ddqd+Ca*dqa+Ga)-Ma*(Kp*e+Ki*Ie+Kd*de); 
Luật điều khiển trượt 
% Ham tinh luat dieu khien truot robot 3RRR 
function y=Controller_SMC(in) 
143 
global R r L1 L2 alpha1 alpha2 alpha3 m1 m2 m3 Ix Iy Iz I2x I2y I2z 
g 
%Dau vao 1 
R=0.266;r=0.04;L1=0.3;L2=0.8; alpha1=0; alpha2=2*pi/3; 
alpha3=4*pi/3; m1=0.42; 
m2=2*0.20; m3=0.75; Ix=0; Iz=m1*L1*L1/12; Iy=Iz; I2y=m2*L2*L2/12; 
I2z=I2y; g=9.81; I2x=0; 
qd=[in(1),in(2),in(3)]'; % Vi tri khop chu dong 
mong muon; 
dqd=[in(13),in(14),in(15)]'; % Van toc khop chu dong mong 
muon; 
ddqd=[in(25),in(26),in(27)]'; 
s=[in(37),in(38),in(39),in(40),in(41),in(42),in(43),in(44),in(45),in
(46),in(47),in(48)]'; % Vi tri toa do khop thuc te 
ds=[in(49),in(50),in(51),in(52),in(53),in(54),in(55),in(56),in(57),i
n(58),in(59),in(60)]'; % Van toc toa do khop thuc te 
q=[in(37),in(38),in(39),in(40),in(41),in(42),in(43),in(44),in(45)]; 
dq=[in(49),in(50),in(51),in(52),in(53),in(54),in(55),in(56),in(57)]; 
qa=[in(37),in(38),in(39)]'; 
dqa=[in(49),in(50),in(51)]'; 
% Sai so 
e=qa-qd; de= dqa-dqd; 
% Tinh ma tran M 
M=Ms(q); 
% Tính ma tran C 
C=Cs(q,dq); 
% Tinh ma tran Gs 
G=Gs(q); 
% Tinh ma tran R 
 R=[eye(3,3);-inv(Phiz(q))*Phia(q)]; 
% Tinh dao ham ma tran R 
dR=[zeros(3,3);inv(Phiz(q))*dPhiz(q,dq)*(inv(Phiz(q)))*Phia(q)-
inv(Phiz(q))*dPhia(q,dq)]; 
% Tinh ma tran Ma 
Ma=R'*M*R; Ca=R'*M*dR+R'*C*R; Ga=R'*G; 
% Tham so bo dieu khien 
Kpd=80*eye(3); 
Ks=80*eye(3); 
mu=80*eye(3); 
muy=de+mu*e; 
144 
y=(Ma*ddqd+Ca*dqd+Ga)-(Ma*mu*de+Ca*mu*e+Kpd*muy+Ks*2*atan(muy)/pi); 
% Ham tinh luat dieu khien robot Delta 3RRR 
function y=Controller_SMC2(in) 
global R r a b anpha1 anpha2 anpha3 m1 m2 m3 IAy g 
R=0.266;r=0.04;a=0.3;b=0.8; anpha1=0; anpha2=2*pi/3; anpha3=4*pi/3; 
m1=0.42; 
m2=0.2; m3=0.75; IAy=m1*a*a/3; g=9.81; 
qd=[in(1),in(2),in(3)]'; % Vi tri khop chu 
dong mong muon; 
dqd=[in(7),in(8),in(9)]'; % Van toc khop chu dong mong 
muon; 
ddqd=[in(13),in(14),in(15)]'; 
s=[in(19),in(20),in(21),in(28),in(29),in(30)]'; % Vi tri toa 
do khop thuc te 
ds=[in(31),in(32),in(33),in(40),in(41),in(42)]'; % Van toc toa 
do khop thuc te 
q=[in(19),in(20),in(21)]'; x=in(28);y=in(29); z=in(30); 
dq=[in(31),in(32),in(33)]'; dx=in(40); dy=in(41); dz=in(42); 
% Sai so 
e=q-qd; de= dq-dqd; 
% Tinh ma tran M 
M=Ms2(); 
% Tinh ma tran Gs 
G=Gs2(q); 
% Tinh dao ham ma tran Phiz2 
dPz=dPhiz2(q,dq,dx,dy,dz); 
P=inv(Phiz2(q,x,y,z)); 
% Tinh ma tran R 
Q=[eye(3,3);-P*Phia2(q,x,y,z)]; 
dR=[zeros(3,3);P*dPz*P*Phia2(q,x,y,z)-
P*dPhia2(q,x,y,z,dq,dx,dy,dz)]; 
% Tinh Ma, Ca 
Ma=Q'*M*Q; Ca=Q'*M*dR; Ga=Q'*G; 
% Bo dieu khien 
Kpd=400*eye(3); 
Ks=400*eye(3); 
mu=200*eye(3); 
muy=de+mu*e; 
y=(Ma*ddqd+Ca*dqd+Ga)-(Ma*mu*de+Ca*mu*e+Kpd*muy+Ks*atan(muy)/pi); 
145 
Luật điều khiển trượt sử dụng mạng nơ ron 
% Ham tinh luat dieu khien truot mang noron robot 3RRR 
function y=Controller_SMC_Noron(in) 
global R r L1 L2 alpha1 alpha2 alpha3 m1 m2 m3 Ix Iy Iz I2x I2y I2z 
g 
%Dau vao 1 
R=0.266;r=0.04;L1=0.3;L2=0.8; alpha1=0; alpha2=2*pi/3; 
alpha3=4*pi/3; m1=0.42; 
m2=2*0.20; m3=0.75; Ix=0; Iz=m1*L1*L1/12; Iy=Iz; I2y=m2*L2*L2/12; 
I2z=I2y; g=9.81; I2x=0; 
qd=[in(1),in(2),in(3)]'; % Vi tri khop chu dong mong muon; 
dqd=[in(13),in(14),in(15)]'; % Van toc khop chu dong mong muon; 
ddqd=[in(25),in(26),in(27)]'; 
s=[in(37),in(38),in(39),in(40),in(41),in(42),in(43),in(44),in(45),in
(46),in(47),in(48)]'; % Vi tri toa do khop thuc te 
ds=[in(49),in(50),in(51),in(52),in(53),in(54),in(55),in(56),in(57),i
n(58),in(59),in(60)]'; % Van toc toa do khop thuc te 
q=[in(37),in(38),in(39),in(40),in(41),in(42),in(43),in(44),in(45)]; 
dq=[in(49),in(50),in(51),in(52),in(53),in(54),in(55),in(56),in(57)]; 
noron=[in(61),in(62),in(63)]'; 
qa=[in(37),in(38),in(39)]'; 
dqa=[in(49),in(50),in(51)]'; 
% Sai so 
e=qa-qd; de= dqa-dqd; 
% Tinh ma tran M 
M=Ms(q); 
% Tính ma tran C 
C=Cs(q,dq); 
% Tinh ma tran Gs 
G=Gs(q); 
% Tinh ma tran R 
 R=[eye(3,3);-inv(Phiz(q))*Phia(q)]; 
% Tinh dao ham ma tran R 
dR=[zeros(3,3);inv(Phiz(q))*dPhiz(q,dq)*(inv(Phiz(q)))*Phia(q)-
inv(Phiz(q))*dPhia(q,dq)]; 
% Tinh ma tran Ma 
Ma=R'*M*R; Ca=R'*M*dR+R'*C*R; Ga=R'*G; 
% Bo dieu khien 
Kpd=80*eye(3); 
Ks=80*eye(3); 
mu=80*eye(3); 
146 
muy=de+mu*e; 
y=(Ma*ddqd+Ca*dqd+Ga)-(Ma*mu*de+Ca*mu*e+Kpd*muy+400*atan(muy)/pi -
2.1*noron); 
function y=Controller_SMC_Noron2(in) 
global R r a b anpha1 anpha2 anpha3 m1 m2 m3 IAy g 
R=0.266;r=0.04;a=0.3;b=0.8; anpha1=0; anpha2=2*pi/3; anpha3=4*pi/3; 
m1=0.42; 
m2=0.2; m3=0.75; IAy=m1*a*a/3; g=9.81; 
qd=[in(1),in(2),in(3)]'; % Vi tri khop chu dong mong 
muon; 
dqd=[in(7),in(8),in(9)]'; % Van toc khop chu dong mong 
muon; 
ddqd=[in(13),in(14),in(15)]'; 
s=[in(19),in(20),in(21),in(28),in(29),in(30)]'; % Vi tri toa 
do khop thuc te 
ds=[in(31),in(32),in(33),in(40),in(41),in(42)]'; % Van toc toa 
do khop thuc te 
q=[in(19),in(20),in(21)]'; x=in(28);y=in(29); z=in(30); 
dq=[in(31),in(32),in(33)]'; dx=in(40); dy=in(41); dz=in(42); 
qa=[in(19),in(20),in(21)]'; dqa=[in(31),in(32),in(33)]'; 
noron=[in(43),in(44),in(45)]'; 
% Sai so 
e=q-qd; de= dq-dqd; 
% Tinh ma tran M 
M=Ms2(); 
% Tinh ma tran Gs 
G=Gs2(q); 
% Tinh dao ham ma tran Phiz2 
dPz=dPhiz2(q,dq,dx,dy,dz); 
P=inv(Phiz2(q,x,y,z)); 
% Tinh ma tran R 
Q=[eye(3,3);-P*Phia2(q,x,y,z)]; 
dR=[zeros(3,3);P*dPz*P*Phia2(q,x,y,z)-
P*dPhia2(q,x,y,z,dq,dx,dy,dz)]; 
% Tinh Ma, Ca 
Ma=Q'*M*Q; Ca=Q'*M*dR; Ga=Q'*G; 
% Bo dieu khien 
Kpd=400*eye(3); 
Ks=400*eye(3); 
mu=200*eye(3); 
147 
muy=de+mu*e; 
y=(Ma*ddqd+Ca*dqd+Ga)-(Ma*mu*de+Ca*mu*e+Kpd*muy+400*atan(muy)/pi-
2.1*noron); 
5. Hàm con giải bài toán động lực học thuận 
T
T 
Nội dung các hàm Mô hình 1 Mô hình 2 Ghi 
chú 
1 Động lực học thuận mô 
hình robot chính xác 
function ydot=xvdot(in) 
2 Động lực học thuận mô 
hình robot có sai số 
function 
ydot=xvdot_error(in) 
function ydot=xvdot(in) 
global R r L1 L2 alpha1 alpha2 alpha3 m1 m2 m3 Ix Iy Iz I2x I2y I2z 
g 
%Dau vao 1 
R=0.266;r=0.04;L1=0.3;L2=0.8; alpha1=0; alpha2=2*pi/3; 
alpha3=4*pi/3; m1=0.42; 
m2=2*0.20; m3=0.75; Ix=0; Iz=m1*L1*L1/12; Iy=Iz; I2y=m2*L2*L2/12; 
I2z=I2y; g=9.81; I2x=0; 
% cac he so on dinh 
alpha=1/0.001; beta=sqrt(2)/0.001; %chinh xac cao thoi gian lau 
%alpha=100; beta=100*sqrt(2); 
q=[in(1),in(2),in(3),in(4),in(5),in(6),in(7),in(8),in(9)]'; 
x=in(10); y=in(11); z=in(12); 
s=[in(1),in(2),in(3),in(4),in(5),in(6),in(7),in(8),in(9),in(10),in(1
1),in(12)]'; 
dq=[in(13),in(14),in(15),in(16),in(17),in(18),in(19),in(20),in(21)]'
; 
ds=[in(13),in(14),in(15),in(16),in(17),in(18),in(19),in(20),in(21),i
n(22),in(23),in(24)]'; 
% Vec to momen dieu khien 
u1=in(25); u2=in(26); u3=in(27); 
u=[u1,u2,u3,0,0,0,0,0,0,0,0,0]'; 
% Tinh ma tran M 
M=Ms(q); 
% Tính ma tran C 
C=Cs(q,dq); 
% Tinh ma tran Gs 
148 
G=Gs(q); 
% Tinh ma tran R 
 R=[eye(3,3);-inv(Phiz(q))*Phia(q)]; 
% tinh ma tran Phi_s la dao ham cua f theo s 
 Phi=PhiT(q)'; % PhiT la ma tran da chuyen vi; 
% Tinh dao ham theo t cua ma tran PhiT 
 dPhi=dPhiT(q,dq)'; 
 H=forward_Kinematic(in); 
 F11=Phi*ds; 
% Vec to p1 
 p1=u-C*ds-G; 
% Vec to p3 co them on dinh hoa Baungart 
 p3=-dPhi*ds-2*alpha*Phi*ds-beta*beta*H; %+F2; %-
2*alpha*Phi*ds+beta*beta*F; 
% Tinh ma tran D 
 D=[R'*M;Phi]; 
 dds=inv(D)*[R'*p1;p3]; 
 ydot=[ds;dds]; 
function ydot=xvdot_error(in) 
global R r L1 L2 alpha1 alpha2 alpha3 m1 m2 m3 Ix Iy Iz I2x I2y I2z 
g 
%Dau vao 1 
R=0.266;r=0.04;L1=0.3;L2=0.8; alpha1=0; alpha2=2*pi/3; 
alpha3=4*pi/3; m1=0.42; 
m2=2*0.20; m3=0.75; Ix=0; Iz=m1*L1*L1/12; Iy=Iz; I2y=m2*L2*L2/12; 
I2z=I2y; g=9.81; I2x=0; 
% cac he so on dinh 
% alpha=1/0.005; beta=sqrt(2)/0.005; %chinh xac cao thoi gian lau 
alpha=40; beta=40*sqrt(2); 
q=[in(1),in(2),in(3),in(4),in(5),in(6),in(7),in(8),in(9)]'; 
x=in(10); y=in(11); z=in(12); 
s=[in(1),in(2),in(3),in(4),in(5),in(6),in(7),in(8),in(9),in(10),in(1
1),in(12)]'; 
dq=[in(13),in(14),in(15),in(16),in(17),in(18),in(19),in(20),in(21)]'
; 
ds=[in(13),in(14),in(15),in(16),in(17),in(18),in(19),in(20),in(21),i
n(22),in(23),in(24)]'; 
% Vec to momen dieu khien 
u1=in(25); u2=in(26); u3=in(27); 
u=[u1,u2,u3,0,0,0,0,0,0,0,0,0]'; 
t=in(28); 
% Tinh ma tran M 
149 
M=1.2*Ms(q); 
% Tính ma tran C 
C=1.2*Cs(q,dq); 
% Tinh ma tran Gs 
G=1.2*Gs(q); 
% Tinh ma tran R 
 R=[eye(3,3);-inv(Phiz(q))*Phia(q)]; 
% tinh ma tran Phi_s la dao ham cua f theo s 
 Phi=PhiT(q)'; % PhiT la ma tran da chuyen vi; 
% Tinh dao ham theo t cua ma tran PhiT 
 dPhi=dPhiT(q,dq)'; 
 H=forward_Kinematic(in); 
 F11=Phi*ds; 
da=0.5*[sin(20*t);cos(20*t);sin(20*t);sin(20*t);cos(20*t);sin(20*t);sin(2
0*t);cos(20*t);sin(20*t);sin(20*t);cos(20*t);sin(20*t)]; % Nhieu 
% Vec to p1 
 p1=u-C*ds-G-da; 
% Vec to p3 co them on dinh hoa Baungart 
 p3=-dPhi*ds-2*alpha*Phi*ds-beta*beta*H; %+F2; %-
2*alpha*Phi*ds+beta*beta*F; 
% Tinh ma tran D 
 D=[R'*M;Phi]; 
 dds=inv(D)*[R'*p1;p3]; 
 ydot=[ds;dds]; 
6. Sơ đồ điều khiển robot trên Simulink 
Các phương pháp điều khiển được xây dựng trên Simulink theo sơ đồ sau: 
Ta có bảng thống kê các file trên Simulink theo các phương pháp điều khiển 
như sau: 
Bảng thống kê các phương pháp điều khiển dựa trên mô hình robot 
150 
T
T 
Các luật điều khiển Mô hình 1 Mô hình 2 Ghi 
chú 
1 PD + biết chính xác tham số hệ ZPD.mdl ZPD2.mdl 
2 PD + không biết chính xác 
tham số hệ 
ZPD_er.mdl ZPD_er2.mdl 
3 PID + biết chính xác tham số 
hệ 
ZPID.mdl ZPID2.mdl 
4 PID + không biết chính xác 
tham số hệ 
ZPID_er.mdl ZPID_er2.mdl 
5 Trượt + biết chính xác tham số 
hệ 
ZSMC.mdl ZSMC2.mdl 
6 Trượt + không biết chính xác 
tham số hệ 
ZSMC_er.mdl ZSMC_er.mdl 
7 Trượt sử dụng mạng nơ ron + 
biết chính xác tham số hệ 
ZSMC_Noron.mdl ZSMC_Noron2.mdl 
8 Trượt sử dụng mạng nơ ron + 
không biết chính xác tham số 
hệ 
ZSMC_Noron_er.mdl ZSMC_Noron_er2.
mdl