Hoạch định quỹ đạo cho robot di động dùng thuật toán PSO

iii
LỜI CM T

Đềătàiă“HoạchăđịnhăquỹăđạoăchoărobotădiăđộngădùngăthuậtătoánăPSO”ăđãăhoàn thành
với sựăhướngădẫnătậnătìnhăcủaăTS. Ngô Văn Thuyên (Trưởngăphòng quảnălýăkhoaă
họcă&ăquanăhệăquốcătếă- TrườngăSưăphạmăKỹăthuậtăTp.HồăChíăMinh).ăTrướcăsựătậnă
tìnhăđóătôi xinăbàyătỏălòngăbiếtăơnăsâuăsắcăvàălờiăcảmăơnăchânăthànhănhấtăđếnăthầyă
Ngô Văn Thuyên, ngườiăđãătậnătìnhăhướngădẫnăvàăgiúpăđỡătôi rấtănhiềuătrongăsuốtă
thờiăgianăthựcăhiệnăđềătài.
Tôi xin gửiălờiăcámăơn đếnăquýăthầyăcôătrườngăĐạiăHọcăSưăPhạmăKỹăThuậtătp.ăHồă
Chí Minh,ă Đạiă Họcă Báchă Khoaă tp.ă Hồă Chíă Minh,ă Đạiă Họcă Quốcă Tếă đãă tậnă tìnhă
hướngă dẫnă vàătruyềnă đạtă nhữngă kiếnăthức,ăphươngă phápă nghiênăcứuăvàăcácăkinhă
nghiệmătrongăsuốtăhaiănĕmăhọc.ăNhữngăkiếnăthứcăvàăkinhănghiệm nàyăchínhălàănềnă
tảngăgiúpăngườiăthựcăhiệnăhoànăthànhătốtăluậnăvĕnănày.
Tôi xinăgửiă lờiăcảmăơnă đếnăBanăGiámăHiệu,ă cùngăquýăthầyăcôă trườngă Cao Đẳngă
NghềăĐồngăThápăđãăgánhăvácăcôngăviệcăcủaătôi vàătạoăđiềuăkiệnăthuậnălợiăchoătôi
tham gia và hoàn thành khóa học này.
Tôi xinăgửiălờiăcảmăơnăđếnăquýăanhăchịăcácăkhóaăCaoăhọcătrước,ăbạn bè, ngườiăthână
vàăgiaăđìnhăđãătậnătìnhăgiúpăđỡ,ăđộngăviên tôi trongăsuốtăkhóaăhọcăcǜngănhưătrongă
quáătrìnhăthựcăhiệnăluậnăvĕn.

T.p.HồăChíăMinh,ăthángă04 nĕmă2013
Họcăviênăthựcăhiện

v
TÓM TT

H l 
 
thúc mà .
     

khai            
(Road Map)
               


 

          

“Hoạch định quỹ đạo cho robot di động dùng thuật toán
PSO” p D*_PF.
  D* và             
   
 
.


ách an toàn.


vi
ABSTRACT

Path planning is one of the important problems of mobile robot, its task is to find an
optimal path from its starting point to end point without colliding with obstacles.
There are many methods for mobile robot path planning such as: potential field
method (PF), the road map approach (RM), or genetic algorithm (GA)
Disadvantages of those methods are trapped local minima (PF), increase of the cost
of the path length (RM) or complexity (GA) due to the crossover and mutation
techniques.

Recent path planning solutions combining algorithm to find the shortest path D *
with the above methods result in avoiding local minima traps and the path length is
shorter. However, the robot's path has not been smoothed. The robot is swung when
it travels near the obstacles or when it moves along a narrow path. To overcome this
limitation, the proposed method named "Planning for mobile robot using PSO
algorithm" on the basis of inherited method D *_PF is used in this thesis.
The D* and PF algorithms are used to find the shortest path for the robot to move
from its starting position to its desired position without colliding to the obstacles.
Then the PSO algorithm is used to find the angular and linear velocities for the
robot to move from one way point to the other along the desired path generated by
D* algorithm.
The algorithm is simulated on Player /Stage using C++ language. The simulation
results show the path of the robot is very smooth and the robot to the goal safely.



MC LC

 i
 iii
 iv
 v
Chương 1: TNG QUAN 1
 1
1.2.  5
1.3.  5
Chương 2: C S Lụ THUYT 7
 7
2.2.1. Robot 2 bánh 7
2.2.2. Robot 3 bánh 9

2.2.3. Robot 4 bánh 10
 11
 11
 13
 14
2.3.1.  14
2.3.2.  16
2.3.3.  19
2.4. Các gi 21

*
21

*
24
 29
2.5.1.  29
 34
 35
Chương 3: HOCH ĐNH CHUYN ĐNG CHO ROBOT DI ĐNG DÙNG
THUT TOÁN PSO 37
 37
 37
3.3. Tìm  39
 41
 45
Chương 4: KT QU MÔ PHNG 46
 46
 46
 46

 48
 49
 50
 50


*
52
 56
Chương 5: KT LUN VÀ HNG PHÁT TRIN CA Đ TÀI 63
5.1.   63
5.2.  63
 63
 64
TÀI LIU THAM KHO 65

 68

DANH SÁCH CÁC HÌNH

Hình 1.1. Một số hình ảnh robot di động 2
Hình 1.2. Robot di chuyển bằng bằng bánh xe. 2
Hình 2.1. Cấu trúc robot 2 bánh 8
Hình 2.2. Cấu trúc robot với điều kiện ràng buộc nonholonomic trong hệ tọa độ
toàn cục 9
Hình 2.3. Cấu trúc robot 3 bánh 10
Hình 2.4. Cấu trúc ca robot di động 4 bánh 10
Hình 2.5. Phơng pháp đồ thị trực quan (Visibility graph) 12
Hình 2.6. Sơ đồ Voronoid 13
Hình 2.7. Phơng pháp chia ô 13

Hình 2.8. Đờng đi tránh vật cản ca thuật toán Bug 1 15
Hình 2.9. Đờng đi tránh vật cản ca thuật toán Bug 2 15
Hình 2.10. Đờng đi tránh vật cản ca thuật toán DistBug 15
Hình 2.11. Phơng pháp trờng thế năng 16
Hình 2.12. Phân bố xác xuất vật cản dùng cảm biến lấy mẫu liên tục 20
Hình 2.13. Mô tả bản đồ và đờng đi ca thuật toánt A* 22
Hình 2.14. Khoảng cách giữa các ô lới 23
Hình 2.15. Mô tả bản đồ và đờng đi ca thuật toán D* 25
Hình 2.16. Sự kết hợp ca ba trọng số di chuyển 30
Hình 2.17. Sự di chuyển ca cá thể trong PSO 31
Hình 3.1. Lu đồ mô tả hoạt động ca giải thuật hoạch định đờng đi 38
Hình 3.2. Mô phỏng thuật toán D* với vật cản tĩnh 40
Hình 3.3. Mô phỏng thuật toán D* với vật cản tĩnh và động 40
Hình 3.4. Dẫn đờng giữa hai điểm 41
Hình 4.1. Mô hình Client/Sever ca Player 47
Hình 4.2. Kết nối giữa Player/Stage và Robot 49
Hình 4.3. Đờng đi ca robot sử dụng phơng pháp PF tiếp cận đợc mục tiêu 51
Hình 4.4. Đờng đi ca robot sử dụng phơng pháp PF không tiếp cận mục tiêu 52
Hình 4.5. Đờng đi ca robot sử dụng kết hợp thuật toán D* với phơng pháp PF
thoát khỏi bẫy cực tiểu cục bộ 53
Hình 4.6. Biểu đồ vận tốc góc điều khiển robot với vật cản chữ U 54
Hình 4.7. Đờng đi ca robot sử dụng kết hợp thuật toán D* với phơng pháp PF
cha tối u 54
Hình 4.8. Đờng đi ca robot sử dụng kết hợp thuật toán D* với phơng pháp PF
cha tối u với vật cản phc tạp 55
Hình 4.9. Biểu đồ vận tốc góc điều khiển robot với vật cản phc tạp 56
Hình 4.10. Đờng đi ca robot sử dụng thuật toán PSO 57
Hình 4.11. Biểu đồ vận tốc góc điều khiển robot với vật cản chữ U 58
Hình 4.12. Biểu đồ so sánh vận tốc góc điều khiển robot phơng pháp PSO và
PF_D* 58

Hình 4.13. Đờng đi ca robot sử dụng thuật toán PSO với thông số khác nhau . 59
Hình 4.14. Đờng đi ca robot sử dụng thuật toán PSO với vật cản phc tạp 60
Hình 4.15. Biểu đồ vận tốc góc điều khiển robot với vật cản phc tạp 61
Hình 4.16. Biểu đồ so sánh vận tốc góc điều khiển robot phơng pháp PSO và
PF_D* 61

Ch


1
Chương 1
TNG QUAN
1.1. Gii thiu


 [1].
obot  ngày càng c hoàn
thi       
và 
     
nhan              
 



 
 [4].


. 

 



  
. 


Ch


2








a. Robot tự hành thám hiểm sao hoả b. Robot hút bụi Trilobite

Hình 1.1. Một số hình ảnh robot di động


a. Robot di chuyển bằng bánh xe b. Robot di chuyển bằng bánh xe
dạng dĩa đa hớng
Hình 1.2. Robot di chuyển bằng bằng bánh xe.

Ch

3
 robot  
tâm là làm nào  r tôi   âu
    và robot có
  r
.
Theo Lagoudakis, Michail G [14
các "task 

 Mapping : 


 Positioning : 

 Path planning "): 

 Obstacle avoidance       :    


 Dn đưng đi cho robot di
động”

(global path planning) và
  nning) robot


   
:  (Potential Field) [30],

Ch


4
     (Road Map) [5]      
(Genetic Algorithms) [23] PSO (Particle Swarm Optimization) [3],
[11], [12] 
Phương pháp bn đ đưng (Road Map)
  Road Map kt ni các vùng không gian trng ca robot trong mt
mng thng hong cong gi là các b 
pháp bn  i viu khin robot phi thc s chính xác, vic tìm
kii chi vng rng ln có nhiu vt cn.
Phương pháp trưng th năng (Potential Field)

 



  



   

Phương pháp thut toán di truyn (Genetic Algorithms)
   
 evolutionary), thô     
 Chính vì các quá trình trên nên trong


.
Phương pháp PSO (Particle Swarm Optimization)

Ch


5



. 

               

 
 
1.2 . Mc tiêu vƠ gii hạn đ tƠi
       
 
 D*_PF           u 
.
 

- N  
các ph, 
tìm 
-  
tPSO (Particle Swarm Optimization).
- K              
Player/Stage .

1.3 . Nội dung đ tƠi
Chơng 1: Tổng quan
 
mobile robot,      

Ch


6

                

Chơng 2: Cơ sở lý thuyết
 
l
 :        ap),     
Decomposition). Các t toán Bug

 A*, g D*. T trình bày .
Chơng ̀: Hoạch định chuyển động cho robot di động dùng thuật toán PSO
 là     các 
               
 
và bài toán di  ).
Chơng 4: Kết quả mô phỏng
          robot mã
Player/Stage  
  và
    r       . K  mô
  ,  

 .
Chơng 5: Kết luận và hớng phát triển của đề tài
n  
ra t và 
.


Ch 


7
Chương 2
C S Lụ THUYT
2.1. Mô t mô hình toán hc ca một số robot di động


khác nhau [4], [6
ba  bánh, 3
bánh và robot 4 bánh.
2.2.1. Robot 2 bánh
Robot di               
 


 
cao.







  


b


Ch 


8


v
U





(2.1)

y
x
P








(2.2)

( , )xy


là góc 
v
là 





Hình 2.1. Cấu trúc robot 2 bánh

L
(R+ )
2
R
V


(2.3)

L
(R- )
2

L
V


(2.4)
:
R
V

L
V


.

Ch 


9




2
RL
VV
v


(2.5)

RL
VV
L



(2.6)



Hình 2.2. Cấu trúc robot với điều kiện ràng buộc nonholonomic trong hệ tọa độ toàn cục


cos 0
sin 0 .
01
x
v
y
























(2.7)


 
os sin os =0
x
sin c x y c
y
   


  


฀
฀฀
฀

(2.8)
2.2.2. Robot 3 bánh

v



Ch 


10

()
cos ( ) 0
()
( ) sin ( ) 0 .
()
01
()
xt
t
vt
y t t
t
t























฀
(2.9)

( ) ( ) os (t)
s
v t v t c


,
()
( ) sin (t)
s
vt

t
d




Hình 2.3. Cấu trúc robot 3 bánh
2.2.3. Robot 4 bánh

Hình 2.4. Cấu trúc ca robot di động 4 bánh

v


Ch 


11
cos sin 0
sin cos 0 .
0 0 1
x
v
y
























(2.10)
2.2. Các phương pháp lp k hoạch đưng đi
B5], [7], [8] 
r 




g
 :
2.2.1. Phơng pháp bản đồ đờng đi (Road Map)
  Road Map kt ni các vùng không gian trng ca robot trong mt
mng thng hong cong 1D gi là các b 

Road Map c chp nhn vi nhng kit ngt:
 th trc quan (Visibility graph) và bi Voronoi (Voronoi diagram). Trong
 th tr bám sát theo vt c
cui cùng là kt qu  dài ngn nh Voronoi
ng càng cách xa vt cn càng tt.
 Visibility graph
PVisibility graph (hình 2.5) m. Th nht, nó cc k
nhanh và hiu qu     t vt c    chm và
không hiu qu bi vng có m vt
cn cao. Th hai là có nhiu thiu sót tim n: Kt qu    c bi
Visibility graph có th làm cho robot b chm vào vt c

Ch 


12
Gic vt cn lên mt khong bng vi bán kính ca
robot, hou chnh kt qu  ho
vt cn ra mt khong cách u này s hy sinh s t 
dài cVisibility graph.

Hình 2.5. Phơng pháp đồ thị trực quan (Visibility graph)
 Voronoi
   Voronoid (Voronoid diagram). S  Voronoid (hình 2.6) là
 nh da trên khong cách ta robot và vt
cn trong b. Ti
ng t t dài. Voronoid diagram có mt hn ch là s gii hn v
khong cách ca cm bin v trí. Vì thun khong cách t
gia robot và vt cng nên các cm bin có tm hong ngn ca
robot s sai khi cm nhn v ng xung quanh.

Tuy nhiên, mt lm quan trng cVoronoid diagram so vi hu
ht các k thut tránh vt cn khác là kh c thi. Cho mt k ho
 c lp b   Voronoid diagram, mt robot vi các cm bin
kho    c siêu âm, có th   ng Voronoid trong môi
ng thc bng cách s dng thun ca Voronoid diagram: Robot
 cc tiu cc b mà cm bin cc. H thng
u khin này s gi    ng Voronoid, u này s gim
thiu kh 

Ch 


13

Hình 2.6. Sơ đồ Voronoid
2.2.2. Phơng pháp chia ô (Cell decomposition)
ng cchia ô là phân bit rõ gia khu vc hình hc, hoc các ô
là nhng vùng trng và vùng b chim gi bi vt cn. Có hchia ô là
chia ô chính xác và chia ô gần đúng.
Hình 2.7a mô t chia ng biên cc
nh da trên gii hn hình hc ca vt cn. Các ô kt qu có th là nhng ô
hoàn toàn trng hoc hoàn toàn b chim gi, vì vy k hong
c thc hin gi trên. Bt li chính c
chia ô chính xác là s ng các ô, do s tính toán khi lp k ho
thuc vào m  phc tp ca vt cn tng. Vì vy, s biu din
s ph thu l t cng. Do s phc tp khi thc
hin nên k thut chia ô c s dng trong các ng dng robot t
hành mc dù nó là s la ch c vì vn gi c nguyên trng môi
ng.

a). Chia ô chính xác. b). Chia ô 
Hình 2.7. Phơng pháp chia ô


Ch 


14
 Chia ô gần đúng
Chia ô g (hình 2.7b) là mt trong nhng k thut ph bin nhc s
dng cho vic lp k hong ph bin nht c
chia ô c c nh (fixed-size cell decompositionc ca ô không
ph thum ca vt cng, và các lp có th b
mt do s không chính xác t nhiên ca các m  m li ln nht ca
chia ô chính xác là tính toán rn khi lp k ho
2.3. Các phương pháp tránh vt cn
2.3.1. Thuật toán Bug

 
S

G
mà

[28], [ 29].
 

1
H



1
L

1
L


.
   mà
     
 
 
1
H

     
1
L

9
  
 
 2.

Ch 


15

Hình 2.8. Đờng đi tránh vật cản ca thuật toán Bug 1

Hình 2.9. Đờng đi tránh vật cản ca thuật toán Bug 2

10
 1 và Bug 

Hình 2.10. Đờng đi tránh vật cản ca thuật toán DistBug

Ch 


16
 1
  2, Bug 
        

u điê
̉
m va

ha
̣
n chê

của thuật toán Bug


 


2.3.2. Phơng pháp trờng thế năng (Potential field methods)
 (PF) [30] 
(gradient 



              11).

.

Hình 2.11. Phơng pháp trờng thế năng

Ch 


17



 
( , )q x y
. Tr
()Uq

( ) ( ) ( )
att rep
U q U q U q
(2. 11)



()
att
Uq

q
t.

()
rep
Uq

q



( ) ( ) ( )
att repi
i
U q U q U q

(2. 12)

()
repi
Uq

i
.

r

( ) ( ) ( ) ( ) ( )
att ep att rep
F q U q U q F q F q
     
(2. 13)
Thế hút từ mục tiêu


2
1
( ) ( )
2
att att goal
U q k d q

(2. 14)


att
k


()
goal goal
d q q q

q
t
goal
q

.


Ch 


18
( ) ( )
att att goal
U q k q q  
(2.15)
()
att
Uq
     
q
 
goal
q


( ) ( )
att att att goal
F U q k q q    
(2.16)
 
(2.16


 


Thế đẩy từ vật cản




( ) ( )
rep repi
i
U q U q

(2.17)
 

i


0
0
0
1 1 1
, ( )
U ( )
2 ( )
0 , ( )
obsti obsti
repi
obsti
obsti
k d q d

q
d q d
d q d











(2.18)


()
obsti
dq

q

i
;