Mô hình hóa và điều khiển robot rắn

LUẬNăVĂNăTHẠCăSĨă GVHD:ăTS.ăNGUYỄNăMINHăTÂM

HVTH: NGUYỄNăVĂNăPHƯC iv

TÓM TẮT
Lunăvĕnă“Mô Hình Hoá Và Điều Khin Robot Rắn”ăgii thiệu về việc xây
dựngăphơngătrìnhăđng lực học và mô hình cho robot rắn, nghiên cứuăphơngăphápă
điều khin chuynăđng của robot rắn chuynăđng theoăđờng cong Serpenoid. Tối
uăcácăthôngăsố của mô hình.
Điều khin chuynăđng của robot rắn bằngăcáchăđiều khin:ăhng, vn tốc
và góc phi. Thiết kế các b điều khinăPIDăđ điều khin chuynăđng của robot rắn
và tốiăuăcácăthôngăsố PID sử dụng giải thut bầyăđànăPSO.ăXâyădựng mô hình hoá
và mô phỏng sử dụng phần mềm matlab 7.6.
So sánh kết quả giữa 2 giải thut tốiău các thông số PID sử dụng PSO và
giải thut gen duy truyền GA. Đánhăgiáăkết quả của 2 giải thut.
Thi công mô hình thực nghiệm cho robot rắn,ăđiều khin từ máy tính thông
qua cổng USB











LUẬNăVĂNăTHẠCăSĨă GVHD:ăTS.ăNGUYỄNăMINHăTÂM

HVTH: NGUYỄNăVĂNăPHƯC v

ABSTRACT
Thesis “Modelling and Controling of Snake Robot” presents the
establishment of the motivation equation and model for snake robot, studies some
methods of controlling snake robot's movements according to Serpenoid curve and
optimizes parameters of the model.
Some motion control of the snake robot is introduced such as direction,
speed and

angle. Also, the design of PID controller of operating the snake robot's
movements with parameters optimized by a particle swarm optimization approach
(PSO) is presented. Besides, a simulation for snake robot will be conducted on
Matlab 7.6.
The results between two algorithms optimized PID parameters using PSO
and genetic algorithm GA will be compared, and the results of the two algorithms
should be evaluated.
The experimental model for the snake robot is conducted by controlling from
a computer via the USB port.

LUẬNăVĂNăTHẠCăSĨă GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC vi

MC LC

TRANG TA TRANG
Quyếtăđịnhăgiaoăđề tài
Lý lịch cá nhân i
Lờiăcamăđoană ii
Lời cámăơn iii

Tóm tắt iv
Mục lục vi
Danh sách các chữ viết tắt ix
Danh sách các hình x
Chơng 1. TNG QUAN
1.1 Tổng quan về robot và các kết quả nghiên cứuăđưăcôngăbố 1
1.1.1 Tổng quan về robot: 1
1.1.2 Các kết quả nghiên cứuăđưăcôngăbố 2
1.2 Mụcăđíchăcủaăđề tài 3
1.3 Nhiệm vụ củaăđề tài và gii hnăđề tài 4
1.3.1 Nhiệm vụ 4
1.3.2 Gii hn 4
1.4 Phơngăphápănghiênăcứu 4
Chơng 2. MÔ HÌNH HOÁ ROBOT RN
2.1 Lực ma sát nht của robot rắn 6
2.2 Phơngătrìnhăchuynăđng 9
2.3 Phânălyăđng lực học 14
Chơng 3.PHNG PHÁP ĐIU KHIN CHUYN ĐNG CA ROBOT
RN
3.1 Đờng cong Serpenoid 17
3.2 Sự di chuyn hình rắn 21
LUẬNăVĂNăTHẠCăSĨă GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC vii

3.3 Hiệu suất chuynăđng 24
Chơng 4. GII THUẬT TI U BY ĐÀN
4.1 Lịch sử phát trin 28
4.2 Các khái niệmăcơăbản trong giải thut bầyăđàn 31
4.3 Mô tả thut toán 31

4.4 Những vấnăđề cần quan tâm khi xây dựng giải thut PSO 34
4.4.1 Mã hóa cá th 34
4.4.2 Khởi to quần th banăđầu 36
4.4.3 Hàm thích nghi (hàm mục tiêu) 37
4.4.4 Hàm vn tốc v 37
4.4.5 Cp nht vị trí tốt nhất cho cả quần th 38
4.5 Đặcăđim và ứng dụng của giải thut PSO 40
4.5.1 Đặcăđim 40
4.5.2 ng dụng 41
4.6 Hiệu chỉnh b điều khin PID bằng giải thut bầyăđàn 41
Chơng 5. XỂY DNG H THNG ĐIU KHIN ROBOT RN TRÊN
MATLAB
5.1 Thiếtăkếăbăđiềuăkhinărobotărắnă 43
5.1.1 Băđiềuăkhinăđịaăphơng 44
5.1.2ăBăđiềuăkhinăvòngăngoài 45
5.2 XâyădựngăphơngătrìnhătoánăhọcătrênăMatlabă 45
5.3 Xâyădựngămôăhìnhăchoăbăđiềuăkhinăđịaăphơng 56
5.4 Xâyădựngămôăhìnhăchoăbăđiềuăkhinăvòngăngoài 57
5.4.1 B điều khin vn tốc 57
5.4.2 B điều khinăhng 58
5.5 Xây dựng b điều khin rắn trên matlab 58
5.6 KếtăquảămôăphỏngăsửădụngăgiảiăthutăPSO 59
5.7 Kếtăquảămôăphỏngăhệăthngăđiềuăkhinărobotărắn 61
LUẬNăVĂNăTHẠCăSĨă GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC viii

Chơng 6.SO SÁNH PHNG PHÁP ĐIU KHIN PID DÙNG GII
THUẬT PSO VÀ GII THUẬT GA
6.1ăPhơngăphápăđiều khin PID dùng giải thut GA 67

6.1.1 Thiếtăkếăbăđiềuăkhinăđịaăphơngă 67
6.1.2 Thiếtăkếăbăđiềuăkhinăhngăvàăvnătốcă 68
6.2 Kếtăquảămôăphỏngă 70
6.3 Phơngăphápăđiều khin PID dùng giải thut PSO 75
6.4 So sánh kết quả củaăphơngăphápăđiều khin PID dùng giải thut PSO và giải
thutăGAăkhiăthayăđổi vn tốc 79
6.5 SoăsánhăkếtăquảăcủaăphơngăphápăđiềuăkhinăPID dùngăgiảiăthutăPSO vàăgiảiă
thutăGA khiămôiătrờngăthayăđổi 84
6.6 So sánhă kếtăquảăcủaăphơngăphápăđiềuă khinăPID dùngăgiảiăthută PSO và
giảiăthutăGA khi cácăthôngăsốăthayăđổi 85
6.7ăNhnăxétăkếtăquả 86
Chơng 7. THIT K VÀ THI CÔNG MÔ HÌNH ROBOT RN
7.1 Chọnăđngăcơăchoărobotărắn 87
7.1.1 Gii thiệu về đngăcơăDynamixelăAX-12A 87
7.1.2 Đặc tính kỹ thut củaăđngăcơăDynamixelăAX-12A 88
7.2 Thiết kế cơăkhí 89
7.3 Thiết kế mch giao tiếp giữa robot và máy tính 93
Chơng 8. KT LUẬN VÀ KIN NGH
8.1 Những kết quả đtăđợc 95
8.2 Những mặt hn chế: 95
8.3ăHng phát trin củaăđề tài 95
Tài liu tham kho 97
Ph lc 1 99
Ph lc 2 100
LUẬNăVĂNăTHẠCăSĨă GVHD: TS. NGUYỄN MINH TÂM
HVTH: NGUYỄNăVĂNăPHƯC ix

DANH SÁCH CÁC CH VIT TT

PID (Proportional–Integral–Derivative): Viết tắt của ba thành phần cơ bản

có trong bộ điều khiển (khuếch đại (P)), tích phân (I), và vi phân(D)
PSO (Particle swarm optimization): Tối ưu hoá bầy đàn
GA (Genetic Algorithms): Thut toán di truyền
LUẬNăVĂNăTHẠCăSĨă GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC x

DANH SÁCH CÁC HÌNH

HÌNH TRANG
Hình 1.1: Các robot rắnăđưăđợc công bố 3
Hình 2.1: Robot rắn gồmănăđon, (n ậ 1) khp 5
Hình 2.2: Đon vi phân của khâu thứ i 6
Hình 2.3: Phân tích lựcătácăđngălênăđon thứ I của robot rắn 9
Hình 3.1a: Đờng cong serpernoid vi bă=ă2πăvàăcă=ă0 18
Hình 3.1b: Đờng cong serpernoid vi a =
�
2
và c = 0 18
Hình 3.1c: Đờng cong serpernoid vi a =
�
2
và b = 10π 19
Hình 3.2: Đờng cong serpernoid đợc xấp xỉ bởiă4ăđon thẳng 20
Hình 3.3a: Chuyn đng hình rắnă(ă=ă0) 22
Hình 3.3b: Chuyn đng hình rắnă(ă=ă10ădeg) 23
Hình 3.4: Đồ thị của tốcăđ trungăbìnhăave(v)ătheoăωăvàă 23
Hình 3.5: Đồ thị của tốcăđ góc trung bình ave()ătheoăωăvàă 24
Hình 3.6: Sự kết hợp tốiăuătốcăđ vàănĕngălợng 26
Hình 3.7: Các thông số tốiăuă(α,ă,ăω) 26

Hình 3.8: Quan hệ giữaăđonăăvàăsố đon n của Robot rắn 27
Hình 3.9: Đồ thị αăvi tỉ số c
t

/ c
n

27
Hình 4.1: Mô tả kiếnătìmăđờng 29
Hình 4.2: Luăđồ giải thut của thut toán PSO 33
Hình 4.3: Cá th biu din mt biu thức toán học 36
Hình 4.4: Chuyn đng cá th 38
Hình 4.5: B điều khin PID bằng giải thut bầyăđàn 41
Hình 4.6: Luăđồ giải thut của hệ thốngăđiều khin PSO-PID 42
Hình 5.1: Cấu trúc hệ thốngăđiều khin 44
Hình 5.2: Cấu trúc b điều khinăđịaăphơng 44
Hình 5.3: Sơăđồ khối tổng quát của Robot rắn 51
LUẬNăVĂNăTHẠCăSĨă GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC xi

Hình 5.4: Sơăđồ chi tiết khốiă“SnakeăRobot” 52
Hình 5.5: Sơăđồ b điều khinăđịaăphơng 56
Hình 5.6: Sơăđồ chi tiết b điều khin C
Φ
56
Hình 5.7: Sơăđồ mô phỏng b điều khin vn tốc ( C
υ
) của Robot Rắn 57
Hình 5.8: Sơăđồ mô phỏng b điều khin vn tốc ( C

si
) của Robot Rắn 58
Hình 5.9: Sơăđồ hệ thốngăđiều khin robot rắn dùng PID sử dụng giải thut PSO 58
Hình 5.10: Đồ thị hàm thích nghi của quá trình tốiău 60
Hình 5.11:ăĐồ thị hàm Kp1, Ki1 và Kd1 trong quá trình tốiău 60
Hình 5.12: Kết quả mô phỏng robot rắn viă
*
=1ă(m/s)ăvàă
*
=0 (rad) 62
Hình 5.13: Vị trí của robot rắn viă
*
=1ă(m/s)ăvàă
*
=0 (rad) 62
Hình 5.14: Kết quả mô phỏng robot rắn viă
*
=1ă(m/s)ăvàă
*
=
�
4
(rad) 63
Hình 5.15: Vị trí của robot rắn viă
*
=1ă(m/s)ăvàă
*
=
�
4

(rad) 64
Hình 5.16: Kết quả mô phỏng robot rắn viă
*
=1ă(m/s)ăvàă
*
=
�
2
(rad) 65
Hình 5.17: Vị trí của robot rắn viă
*
=1ă(m/s)ăvàă
*
=
�
2
(rad) 66
Hình 6.1: Sơăđồ mô phỏng b điều khin C
Φ
trên Matlab 67
Hình 6.2: Sơăđồ mô phỏng b điều khin C
υ
trên Matlab 69
Hình 6.3: Sơăđồ mô phỏng b điều khin C
ξ
trên Matlab 69
Hình 6.4: Sơăđồ hệ thốngăđiều khin robot rắn dùng PID sử dụng giải thut GA 70
Hình 6.5: Kết quả mô phỏng robot rắn viăυă=ă1m/s,ăă=ă0ărad 71
Hình 6.6: Vị trí của robot rắn viăυă=ă1m/s,ăă=ă0ărad 72
Hình 6.7: Kết quả mô phỏng robot rắn viăυă=ă1m/s,ăă=ăπ/4ărad 73

Hình 6.8: Vị trí của robot rắn viăυă=ă1m/s,ăă=ăπ/4ărad 73
Hình 6.9: Kết quả mô phỏng robot rắn viăυă=ă1m/s,ăă=ăπ/2ărad 74
Hình 6.10: Vị trí của robot rắn viăυă=ă1m/s,ăă=ăπ/2ărad 74
Hình 6.11: Sơăđồ hệ thốngăđiều khin robot rắn dùng giải thut PSO 75
Hình 6.12: Kết quả mô phỏng robot rắn vi 
*
=1 (m/s) và 
*
=0 (rad) 76
Hình 6.13: Vị trí của robot rắn viă
*
=1ă(m/s)ăvàă
*
=0 (rad) 76
Hình 6.14: Kết quả mô phỏng robot rắn viă
*
=1ă(m/s)ăvàă
*
=
�
4
(rad) 77
LUẬNăVĂNăTHẠCăSĨă GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC xii

Hình 6.15: Vị trí của robot rắn viă
*
=1ă(m/s)ăvàă
*

=
�
4
(rad) 77
Hình 6.16: Kết quả mô phỏng robot rắn viă
*
=1ă(m/s)ăvàă
*
=
�
2
(rad) 78
Hình 6.17: Vị trí của robot rắn viă
*
=1ă(m/s)ăvàă
*
=
�
2
(rad) 79
Hình 6.18: Kết quả mô phỏng robot rắn viă
*
=0.5ă(m/s)ăvàă
*
= 0 (rad) 80
Hình 6.19: Kết quả mô phỏng robot rắn viă
*
=0.5ă(m/s)ăvàă
*
=

�
4
(rad) 80
Hình 6.20: Kết quả mô phỏng robot rắn viă
*
=0.5ă(m/s)ăvàă
*
=
�
2
(rad) 80
Hình 6.21: Kết quả mô phỏng robot rắn viă
*
=1 (m/s)ăvàă
*
=
�
2
(rad) 81
Hình 6.22: Kết quả mô phỏng robot rắn viă
*
=1ă(m/s)ăvàă
*
=
�
4
(rad) 81
Hình 6.23: Kết quả mô phỏng robot rắn viă
*
=1ă(m/s)ăvàă

*
= 0 (rad) 82
Hình 6.24: Kết quả mô phỏng robot rắn viă
*
=1.5ă(m/s)ăvàă
*
= 0 (rad) 82
Hình 6.25: Kết quả mô phỏng robot rắn viă
*
=1.5ă(m/s)ăvàă
*
=
�
4
(rad) 83
Hình 6.26: Kết quả mô phỏng robot rắn viă
*
=1.5ă(m/s)ăvàă
*
=
�
2
(rad) 83
Hình 6.27: Kết quả mô phỏng robot rắn viă
*
=1ă(m/s)ăvàă
*
= 0 (rad) 84
Hình 6.28: Kết quả mô phỏng robot rắn viă
*

=1ă(m/s)ăvàă
*
= 0 (rad) 84
Hình 6.29: Kết quả mô phỏng robot rắn viă
*
=1ă(m/s)ăvàă
*
= 0 (rad) 85
Hình 6.30: Kết quả mô phỏng robot rắn viă
*
=1ă(m/s)ăvàă
*
= 0 (rad) 85
Hình 7.1: Đng cơăDynamixelăAX-12 88
Hình 7.2: Mô hình tổng th của robot rắn 89
Hình 7.3a: Khp liên kết giữaăhaiăđngăcơ 90
Hình 7.3b: Sơăđồ lắpărápăhaiăđon của robot rắn 90
Hình 7.4: Khp nối trên của robot rắn 90
Hình 7.5: Khp nốiădi của robot rắn 91
Hình 7.6: Sơăđồ chân củaăđngăcơ 91
Hình 7.7: Sơăđồ liên kết giữaăcácăđngăcơ 91
Hình 7.8: Mặt trên của robot rắn 92
Hình 7.9: Mch chuynăđổi tín hiệu sang giao tiếp bán song công 93
Hình 7.10: Sơăđồ nguyên lý mch FT232 94
LUẬNăVĂNăTHẠCăSĨăăăăăăăăăăăăăăăăăăăăăăăăăăăăăăăă GVHD:ăTS.ăNGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 1

Chơng 1
TNG QUAN

1.1 Tng quan v robot và các kt qu nghiên cứu đư công b.
1.1.1 Tng quan v robot:
Trong quá trình phát trin củaăđấtănc nói chung, sự nghiệp công nghiệp
hoá và hiệnăđi hoá nói riêng là rất quan trọng.ăNc ta thuc nhữngănc kém phát
trin có nền công nghiệp sản xuất lc hu,ădoăđóăphơngăchâmăcủaăchúngătaălàăđi tắt
đónăđầu, ứng dụng thành tựu về khoa học công nghệ hiệnăđi của thế giiăđ đẩy
nhanhăgiaiăđon phát trin.ăĐiềuăđóăđòiăhỏi mỗiăchúngătaăđều phải nỗ lựcăđóngăgópă
công sức của mình. Việc học tp, nghiên cứu lý thuyết phải gắn liền vi thực tin
của cuc sống và sự phát trin của khoa học.
Trên thế gii gần nửa thế kỉ trở liă đâyăRobotăđưăđợc ứng dụng rng rãi
trong nhiềuălƿnhăvực sản xuất,ăđặc biệt là tự đng hoá sản xuất, vi uăthế đặc biệt
về tính công nghệ,ănĕngăxuất và hiệu quả sản xuất.
Vi cách di chuyn nhẹ nhàngăvàăđaădng, rắn cho thấy khả nĕngădiăchuyn
hợp lý trên nhiềuămôiătrờngăvàăđịa hình khác nhau. Vì vy những robot di chuyn
giống cách di chuyn của loài rắn trong tự nhiênăđangăđợc tích cực nghiên cứu,
chúng cho thấy tính thích nghi viăđi hình rất tốt. Vi cách di chuyn giốngănhă
loài rắn, robot này có th luồn lách qua các khe hẹp, có th di chuyn ở nhữngăđịa
hình không bằng phẳng hay lầy li, thm chí có th bơiă hoặc trèo lên cây. Vi
những khả nĕngăđó,ărobotărắnăngàyăcàngăđợc ứng dụng rng rãi trong nhiềuălƿnhă
vựcă nh:ă kim tra, noă vétăcácă đờng ống; tìm kiếm nn nhân trong các vụ hỏa
hon,ăđngăđất; dò thám trong quân sự. Chính vì vy, tôi chọnăđề tàiă“ăMô Hình Hoá
Và Điều Khiển Robot Rắn”ăđ nghiên cứu.
 Khóăkhĕnăchủ yếuătrongăđiều khin robot rắn là:
- Xây dựng mô hình robot rắn (nhiều biến)
- PhơngătrìnhăRobotărắn phi tuyến, phức tp
LUẬNăVĂNăTHẠCăSĨăăăăăăăăăăăăăăăăăăăăăăăăăăăăăăăă GVHD:ăTS.ăNGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 2

- Xây dựngăcơăchế chuynăđngănhărắn
- Các hệ số ma sát có th thayăđổi trong phm vi rng tuỳ thucăvàoăđịa hình.
- So vi robot di chuynătrênăbánhăxeăthìămôăhìnhăvàăcơăchế chuynăđng của
robot rắn phức tpăhơnănhiều.
1.1.2 Các kt qu nghiên cứu đư công b trong và ngoài nc
 Các kết quả công bố ngoàiănc:
- Nĕmă1996,ăhaiănhàăkhoa học Jim Ostrowski thucătrờngăđi học kỹ thut và
ứng dụng Pensylvania, Philadelphia, Hoa kỳ và Joel Burdick thuc học viện kỹ
thut và ứng dụng California, Hoa kỳ, viăđề tài nghiên cứuă“GaităKinematicsăforăaă
SerpentinăRobot”,ăđề tài dựa trên những ràng buc về đng họcăđ mô tả chuyn
đng của robot rắn và trình bày kết quả mô phỏng chuynăđng của robot này.
- Nĕmă2002,ătrờngăđi học kỹ thut King Mongkut, Thái Lan, viăđề tài nghiên
cứuă“AnalysisăandăDesignăofăAăMulti-LinkăMobileăRobot”ăđưăđaăra kết quả về sự
phânătíchăđng lực học của robot rắn.
- Nĕmă2007,ănhàăkhoaăhọc Seif Dalilsafaei thucăđi học Qazvin, Iran, viăđề
tàiă“DynamicăAnalyzeăofăsnakeărobot”,ăđưăđaăraăcácăkết quả về phânătíchăđng lực
học robot rắn và tính toán lực dọc và lực tiếp tuyến dọc theo thân của robot rắn.
- Nĕmă1975,ăviện kỹ thutăTokyoăđưănghiênăcứu và chế to ra mô hình robot rắn
có chiềuădàiă2ăm,ăđợc nối vi nhau bởi 20 khp chuynăđng song song vi mặt
đất và di chuyn vi vn tốcă40cm/s,ărobotănàyăđợcăđặt tên là ACM ậ III. Từ khi ra
đời, robot ACM ậ III không ngừngăđợc cải tiếnăđ choăraăđời những phiên bản tiếp
theo. Phiên bản mi nhất là robot rắn ACM ậ R5ăraăđờiăvàoănĕmă2006,ăngoàiănhững
tínhănĕngăvợt tri các phiên bản robot rắnătrcăđâyăvề kíchăthc, tốcăđ, sự linh
hotă…ă, robot này còn có khả nĕngăbơiătrongănc.



LUẬNăVĂNăTHẠCăSĨăăăăăăăăăăăăăăăăăăăăăăăăăăăăăăăă GVHD:ăTS.ăNGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 3

Mt số kết quả đưănghiênăcứu về lƿnhăvựcănàyăđợc công bố các hình ảnh bên
di:

Hình 1.1. Các Robot rắnăđưăđợc công bố
 Các kết quả công bố trongănc:
 nc ta hiện nay, việc nghiên cứu và chế to các loi robot chuynăđng
giống các loài vt trong tự nhiênăcũngăđưăcóăsự bắtăđầu, chẳng hnănhămôăhìnhă
robot cá củaănhómăsinhăviênăđi họcăSăphm kỹ thut thành phố Hồ Chí Minh. Tuy
nhiên việc nghiên cứu chỉ dừng li ở việc thực hiện mô hình, chứ chaăcóăsự nghiên
cứu sâu về đng lực họcăđ xây dựng mô hình toán họcăvàăcácăphơngăphápăđiều
khin tốiău.ă
1.2 Mc đích ca đ tài.
- Xây dựng mô hình robot rắn.
- Nghiên cứuăcơăchế điều khin chuynăđng của robot
- Xây dựng hệ thốngăđiều khin theo 2 cấp:ăĐiều khinăđịaăphơngăvàăđiều
khin vòng ngoài.
- Tốiăuăthôngăsố các b điều khin PID dùng giải thut PSO
- Mô phỏng hệ thốngăđiều khin
- Thiết kế và thi công mô hình robot rắn
- So sánh kết quả tốiăuăthôngăsố PID dùng giải thut PSO và giải thut GA
LUẬNăVĂNăTHẠCăSĨăăăăăăăăăăăăăăăăăăăăăăăăăăăăăăăă GVHD:ăTS.ăNGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 4

1.3 Nhim v ca đ tài và gii hn đ tài.
1.3.1 Nhim v:
Trongăđề tài này sẽ xây dựng mô hình cho robot rắn, nghiên cứuăphơngăthức
điều khin chuynăđng của robot rắn, thiết kế hệ thốngăđiều khin robot rắn, mô
phỏng chuynăđng của robot rắn trên phần mềm Matlab và thi công mô hình thực

nghiệm robot rắn .
1.3.2 Gii hn:
Đề tài này chỉ gii hn nghiên cứu sự di chuyn của robot rắn trong không gian
hai chiều, chỉ hotăđngătrênăđịa hình bằng phẳngăvàăkhôngăcóăchng ngi vt.
1.4 Phơng pháp nghiên cứu.
Nghiên cứu lý thuyếtă đồng thời mô phỏng chuynă đng của robot rắn trên
Matlab và thi công mô hình thực nghiệm robot rắn.

LUẬNăVĂNăTHẠCăSĨ GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 5

Chơng 2
MÔ HÌNH HOÁ ROBOT RN

Trongăchơngănày,ăchúngătaăsẽăxâyădựngămôăhìnhătoánăhọcăcủaărobotărắn.ăXétă
robotărắnăgồmănăđonăkếtănốiăviănhauăquaă(n-1)ăkhp.ăGiảăthiếtămỗiăđonăcóăkhốiă
lợngăphânăbốăđều.ăămỗiăkhpăcóămtăđngăcơătruyềnăđngăchoărobotă(hìnhă2.1).
Đềătàiănàyăchỉăxétărobotărắnădiăchuynătrongămặtăphẳngăhaiăchiều.ăTrongătrờngăhợpă
nàyăhệăthốngăcóă(n+2)ăbcătựădoă(ă(n-1)ăchoăhìnhădng,ăă2ăchoăvịătríăvàă1ăchoăhng).ă
Chúngătaăsẽăxâyădựngăphơngătrìnhăđngălựcăhọcăcủaăchuynăđngăviărobotărắnă
trongăhaiătrờngăhợpămaăsátănht và ma sát Coulomb.

Hình 2.1 Robot rắn gồmănăđon, n-1 khp

Robot gồmănăđon, tọaăđ trọng tâm của mỗiăđon là
 
ii
yx ,
, góc hợp bởi mỗi
đon viăphơngăngangălàă
i

, chiều dài của mỗiăđon là
i
l2
,
x

và
y

lầnălợt là
tọaăđ củaăđim trọngătâmătheoăphơngăxăvàăphơngăy.

LUẬNăVĂNăTHẠCăSĨ GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 6

2.1 Lc ma sát nht ca robot rn
Xétăđon thứ i của robot rắn mô tả trong hình 2.2.












Hình 2.2 Đon vi phân của khâu thứ i.
Trongăđó:
Chiều dài củaăđon thứ i là
i
l2
.
 
ii
yx ,
là tọaăđ của trọng tâm củaăđon thứ i.
i
f
là lực ma sát giữaăđon thứ i và mặt phẳngătrợt.
i
p


làăvectơăvn tốc củaăđon vi phân ds.
i
t
v
~
và
i
n
v
~
lầnălợt là vn tốcătheoăphơngătiếp tuyếnăvàăphơngăphápătuyến vi
đon thứ i.
s là khoảng cách từ đonăviăphânădsăđếnăđim trọng tâm củaăđon thứ i.
i

là góc hợp bởiăđon thứ i vi trục x.
Tọaăđ củaăđonăviăphânădsă(hìnhă1.2)ăđợc cho bởi

cos
sin
ii
i
ii
x
ps
y


   


   
   
(2.1.1)
Lấyăđo hàm
i
p
theo thờiăgian,ătaăđợcăvectơăvn tốc trong hệ trục x-y:
LUẬNăVĂNăTHẠCăSĨ GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 7

sin
cos
ii
ii
ii
x
ps
y




   

   
   





(2.1.2)
Thành phần tiếp tuyến và pháp tuyến củaăvectơăvn tốc
i
p

đợc cho bởi:

cos sin 0
sin cos
tt
ii
ii
i
i i i
nn
ii
vv
p
s
vv

  
   
   
  
   
   

   

   
   




(2.1.3)
Trongăđó:

cos sin
sin cos
t
i
i i i
i i i
n
i
v
x
y
v



   


   

   





(2.1.4)
Lực ma sát
df
bao gồm hai thành phần: thành phần tiếp tuyến
i
t
df
(song song
viăđon i) và thành phần pháp tuyến
i
n
df
(thẳng góc viăđon i):

0
0
t t t
i i i
i
n n n
i i i
df c v
dm
df c v
     


     
     
     


(2.1.5)
Trongăđóă
t
i
c
và
n
i
c
lầnălợt là hệ số maăsátătheoăphơngătiếp tuyếnăvàăphơngăă
pháp tuyến và
i
dm
là khốiălợng củaăđonăviăphânădsă(cóănghƿaălàă
.
2
i
i
i
m ds
dm
l

).
Trong hệ tọaăđ x ậ y, lực ma sát

df
có th viếtănhăsau:

cos sin
sin cos
xt
ii
ii
ii
yn
ii
df df
df df


   



   


   
   
(2.1.6)
Từ (2.1.3), (2.1.4), (2.1.5), (2.1.6) và lấyătíchăphânătrênăđon thứ i,ătaăđợc:
0
cos sin cos sin
sin cos sin cos
0

xt
ii
i i i i
i
i
i i i i
yn
i
ii
fc
x
m
fc
y
   
   
   


   

   

   

   
   

   



(2.1.7)
Cuối cùng , tổng moment ma sát xoắn quanh trọng tâm củaăđon thứ i là:

ii
i
ni
i
n
ii
i
l
i
l
i
ni
Jcc
lm
sdf





3
2
(2.1.8)
Vi
i
J

là moment quán tính củaăđon thứ i,
3
2
ii
i
lm
J 
.
LUẬNăVĂNăTHẠCăSĨ GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 8

Trongăđóăchiềuădơngăcủa
i

đợcăquiăc là chiềuălợngăgiácă(ăngợc chiều
kimăđồng hồ ).
Xét toàn b hệ thống gồmănăđon.ăVectơălực ma sát
f
(nx1) và moment xoắn


tácăđngălênărobotăđợc cho bởi:
zDf
T
f



(2.1.9)




D
(2.1.10)
Trongăđó:
 = 







= 


1


2
 




, 


làălựcămaăsátătheoăphơngăxăcủaăđonăthứăi.



= 




2
 




, 


làălựcămaăsátătheoăphơngăyăcủaăđonăthứăi.
 =


1

2
 



,  làămomentălựcămaăsátăcủaăđonăthứăi.

0

0
t
f
n
CM
D
CM




(2.1.11)
n
D C J


(2.1.12)
CS
SC








(2.1.13)
 
1

,,
t t t
n
C diag c c 
(2.1.14)
 
1
,,
n n n
n
C diag c c 
(2.1.15)
 
n
mmdiagM ,,
1

(2.1.16)
 
n
diagS


sin,,sin
1

(2.1.17)
 
n
diagC



cos,,cos
1

(2.1.18)
LUẬNăVĂNăTHẠCăSĨ GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 9

2.2 Phơng trình chuyn đng

Hình 2.3. Phân tích lựcătácăđngălênăđon thứ i của robot rắn

Xét khâu thứ i của robot rắn gồmănăđonă(xemăhìnhă2.3).ăTrongăđó:
-
i
f
và
i

là lực và moment do ma sát giữaăđon thứ i và mặt phẳngătrợt.
-
i
g
và
1i
g

là lựcătơngătácădoăcácăđon kế cn thứ (i-1) và (i+1).

-
i
u
và
1i
u

là moment củaăđng cơăở khp thứ i và i-1.
-
i

là góc hợp viăphơngăngangăcủaăđon thứ i
- Mỗi khâu thứ i có khốiălợng là
i
m
,chiều dài là
2
i
l
.
- Momentăquánătínhătácăđngălênăđon thứ i là
2
3
ii
i
ml
J 

Áp dụngăđịnh lut II Newton ta có:
Phơngătrìnhăchuynăđng tịnh tiến của robot rắn

1
1
i i x x x
i i i
i i y y y
i i i
m x f g g
m y f g g


  
  


(2.2.1)
Phơngătrìnhăchuynăđng quay của robot rắn
       
1
11
cos cos sin sin
i i i i i y i i y i i x i i x i i
i i i i
J l g l g l g l g u u
     


      

(2.2.2)
LUẬNăVĂNăTHẠCăSĨ GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 10

Áp dụng vào robot gồmănăđon ta có:
Phơngătrìnhăchuynăđng tịnh tiến của robot rắn
T
xx
T
yy
Mx f D g
My f D g




(2.2.3)
vàăphơngătrìnhăchuynăđng quay của robot rắn
T T T
xy
J S LA g C LA g D u


   

(2.2.4)
Trongăđó:
 
xnn
RA
1

11
11












 
,
 
xnn
RD
1
11
11















 

1
2
sin 0 0 0
0 sin 0
0 0 sin
n
S












   

,

1
2
cos 0 0 0
0 cos 0
0 0 cos
n
C












   


1
2
0 0 0
00
00
n
l
l

L
l








   

, vi
i
l
là
1
2
chiều dài củaăđon thứ i.
1
2
0 0 0
00
00
n
J
J
J
J









   

, vi
i
J
là moment quán tính củaăđon thứ i.
1
2
0 0 0
00
00
n
m
m
M
m









   

, vi
i
m
là khốiălợng củaăđon thứ i.
Robot rắn có n+2 bc tự do,ăvàăchúngătaăđưăsử dụng 3n biến ( x(nx1), y(nx1) và

(nx1)ă)ă đ mô tả chuynăđng củaănó.ăBênădi, chúng ta sẽ trìnhăbàyăphơngă
LUẬNăVĂNăTHẠCăSĨ GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 11

trình chuynăđngădi dng góc tuyệtăđối
 
T
n


1

và vị trí của trọng
tâm của robot rắn

1
1
1
1
1

n
ii
T
x
i
nT
y
ii
i
mx
m
e Mx
m
e My
my
m










  












(2.2.5)
Trongăđó:
 =

1  1


 

,



n
i
i
mm
1

 
n
T

n
Rxxx  
1
,
 
n
T
n
Ryyy  
1

Xem hình 2.1, xét khâu thứ i của robot rắn ta có:
   
   
1 1 1
1 1 1
cos cos 0
sin sin 0
i i i i i i
i i i i i i
x x l l
y y l l


  
  

   



   


(2.2.6)
Áp dụngăvàoărobotănăđon, ta có quan hệ x, y và

nhăsau:

cos 0
sin 0
Dx AL
Dy AL




(2.2.7)
Từ địnhănghƿaăcủa (2.2.5) và (2.2.7), ta có:

T
D
T
eM
m







(2.2.8)
cos
x
AL
Tx







(2.2.9)
sin
y
AL
Ty







(2.2.10)
Suy ra:
 
1
1 1 1TT
T M D DM D e


  




(2.2.11)
LUẬNăVĂNăTHẠCăSĨ GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 12

Giải nhữngăphơngătrìnhănàyătheoăxăvàăy,ăvàălấyăđo hàm theo thời gian, chúng
ta thấy rằng vn tốc tịnh tiếnăđợc cho bởi:
 
 
 
1
1 1 1
sin
sin
T
TT
x
x
AL
x T M D DM D AL e

  



  


  









(2.2.12)
 
 
 
1
1 1 1
cos
T
TT
y
y
AL
y T cos M D DM D AL e

  



  



   










(2.2.13)
Suy ra:
 







ENCNS
y
x
z
T

TT








(2.2.14)
Trongăđó:ă
 
 
1
-1 1
0
N=M ; ; 1 1
0
T
T T n
e
D DM D AL E e R
e



  




Sauăđóăphânătíchăphơngătrìnhăchuynăđng tịnh tiến (2.2.3) ra 2 phần: gia tốc
của trọng tâm


và lực ràng buc
x
g
và
y
g
. Nhân
1
TM

vào bên trái (2.2.3), ta
có:

11T
xx
T
x
x
DM f DM D g
Dx
Tx
ef
m
















(2.2.15)






















m
fe
gDDMfDM
yD
yT
y
T
y
T
y
y
11




(2.2.16)

Giải vi
x
g
và
y
g
, ta có:

   
 
   
 
1
1 2 1
1
1 2 1
T
xx
T
yy
g DM D AL C S DM f
g DM D AL S C DM f








  
  
 
 
(2.2.17)
LUẬNăVĂNăTHẠCăSĨ GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 13

Từ (2.2.7) ta có :

 
2
Dx AL C S



 

,
 
2
Dy AL S C



 

(2.2.18)
Từ (2.2.15) ta có :

m
fe
x
T
x




(2.2.19)
Thay (2.2.17) vào (2.2.4) và sử dụng (2.2.19), ta có:
fuDCF
T T2
£


(2.2.20)
fEm
T



(2.2.21)
Trongăđó:ă
C S HC C HS
   

(2.2.22)
Tổng hợpăcácăphơngătrìnhăchuynăđng từ (2.1.9), (2.1.10), (2.2.14), (2.2.20)
và (2.2.21), ta có:
2
0
0
00
T
T
RS

F
CD
u
mI
SQ



     
   

  
     
   


   
     
 

 
(2.2.23)
Trongăđó:
 
T
0£
£
00
T
f

T
T
RS
D
DE
SQ
E





   







(2.2.24)

 
xnn
RA
1
11
11













 
,
 
xnn
RD
1
11
11















 

0
0
e
E
e




,
 
n
T
Re  11 
,



n
i
i
mm
1

1
2

sin 0 0 0
0 sin 0
0 0 sin
n
S












   

,
1
2
cos 0 0 0
0 cos 0
0 0 cos
n
C













   


LUẬNăVĂNăTHẠCăSĨ GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 14

1
2
0 0 0
00
00
n
l
l
L
l









   

, vi
i
l
là
1
2
chiều dài củaăđon thứ i.
1
2
0 0 0
00
00
n
J
J
J
J









   

, vi
i
J
là moment quán tính củaăđon thứ i.
1
2
0 0 0
00
00
n
m
m
M
m








   

,vi
i
m

là khốiălợng củaăđon thứ i.
F J S HS C HC
   
  
,

£=
T
TT
S N C N





 
1
-1 1
N=M
TT
D DM D AL


,
 
1
1
H=
TT
LA DM D AL



,
C S HC C HS
   


0
0
t
f
n
CM
D
CM




,
n
D C J


,
CS
SC










1
2
0 0 0
00
00
t
t
t
t
n
c
c
C
c









   


, vi
t
i
c
là hệ số maăsátătheoăphơngătiếp tuyến củaăđon thứ i.
1
2
0 0 0
00
00
n
n
n
n
n
c
c
C
c









   


, vi
n
i
c
là hệ số maăsátătheoăphơngăphápăătuyến củaăđon thứ i.
2.3 Phân ly đng lc học (Dynamic Decoupling)
Trong phần này, chúng ta chứng tỏ rằngăphơngătrìnhăđng lực họcăđưăxâyădựng
trong phần trên có th đợc tách ra làm hai phần:ă đóă làă hìnhă dng chuynăđng
(moment xoắn ở khp

góc ở khp) và sự diăđng quán tính (góc ở khp

vị trí
LUẬNăVĂNăTHẠCăSĨ GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 15

vàăhng quán tính). Việcăphânălyănàyălàmăđơnăgiản việc phân tích và tổng hợp
chuynăđng hình rắn.
Đầu tiên chúng ta xét mt vài tính chất củaă đng lực học robot rắn. Từ định
nghƿaă(2.2.22) ta có:
0
T
CC
F C C

   
  
(2.3.1)

Vi
 
12
, , ,
n
diag
  

  



Đặt:

T
D
eF














(2.3.2)
Trongăđóă


làăvectơăcủaăgócătơngăđối giữaăđon kế cn và


làăđngălợng
góc trung bình. Giảiăphơngătrìnhă(2.3.2)ătheoă


ta có:

Ke

  



(2.3.3)
Trongăđó:ă

HCCHSSJF 
,
11T
K F D B


,
1

T
e Fe


,
ee




Từ (2.3.1) và (2.3.2) ta có:
 
   
22TT
T
D
DD
d
e F C C e F C
eF
dt



    



   



   
  


  
   

   



 

    



(2.3.4)

Trongăđó:

0
TT
e C C
  
  
  

(2.3.5)

Vì C là ma trnăđối xứng ghềnhă (skewăsymmetric).ă Thayă (2.3.5)ă vàoăphơngă
trình (2.2.20) ta có:
LUẬNăVĂNăTHẠCăSĨ GVHD: TS. NGUYỄN MINH TÂM

HVTH: NGUYỄNăVĂNăPHƯC 16


 
1
2T
£0
T
T
DF
F C f D u
e
  


    



 
(2.3.6)
hoặcătơngăđơngănhăsau:
 
BufCDF 
 T21
£



(2.3.7)
 
fe
T T
£


(2.3.8)
Trongăđóă
0
TT
eD 

Ta thấy moment xoắnă đầuă vàoă uă điều khin hình dng


của robot rắn qua
phơngătrìnhă(2.3.7),ălực ma sát và moment xoắnăđiều khin

và

của robot
rắnăquaăphơngătrìnhă(2.2.21)ăvàă(2.3.8).Trongătrờng hợp sử dụng mô hình ma sát
nht,ăphơngătrìnhăchuynăđng có th biu dinănhăsau:
0
Re
0
0


















































K
S
Re
QeS
See
mI
T
T
T
TT
(2.3.9)
 

BuSRCDF 




21
(2.3.10)
Trongăđó:ă
Ke

  



,
1 T
B DF D


,
11T
K F D B



1
T
e Fe



,
ee




Viăphơngătrìnhăchuynăđngăđưăphânăly,ăcóăth xây dựngăphơngăphápăđiều
khin sao cho:
1. Moment xoắn ở khpăuăđiều khin hình dáng

của robot rắn
2. Hình dáng

điều khin

và

của robot rắn.
Trong phần sau, chúng ta sẽ xây dựngăphơngăphápăđiều khin chuynăđng của
robot rắn.