Thiết kế quỹ đạo cho xe tự hành và ứng dụng trên mô hình robot 3 bánh
- 50 trang
- file .pdf
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI
LUẬN VĂN TỐT NGHIỆP
Thiết kế quỹ đạo cho xe tự hành và
ứng dụng trên mô hình robot 3 bánh
CAO VĂN HOÀNG
[email protected]
Kỹ thuật Điều khiển và Tự động hóa
Giảng viên hướng dẫn: TS. Nguyễn Thu Hà
Chữ ký của GVHD
Bộ môn: Điều khiển tự động
Viện: Điện
HÀ NỘI, 6/2020
i
ĐỀ TÀI TỐT NGHIỆP
Thiết kế quỹ đạo cho xe tự hành và ứng dụng trên mô hình robot 3 bánh
Giáo viên hướng dẫn
Ký và ghi rõ họ tên
ii
Lời cảm ơn
Tôi xin bày tỏ lòng biết ơn sâu sắc đến TS. Nguyễn Thu Hà đã tận tình
hướng dẫn, chia sẻ kinh nghiệm giúp đỡ để tôi hoàn thiện luận văn này.
Tôi xin chân thành cảm ơn bạn Nguyễn Ngọc Huy và Nguyễn Ngọc Tú đã
nhiệt tình hỗ trợ trong quá trình nghiên cứu và triển khai ứng dụng.
Tôi xin trân trọng cảm ơn các thầy, cô giáo Viện Điện đã giúp đỡ, tạo điều
kiện trong quá trình học tập tại trường.
Tôi xin chân thành cảm ơn gia đình, đồng nghiệp là những người luôn bên
cạnh, hỗ trợ về mặt tinh thần, chia sẻ một phần khó khăn trong quá trình học tập
nghiên cứu.
Một lần nữa xin chân thành cảm ơn!
iii
Tóm tắt nội dung luận văn
1. Lý do chọn đề tài
Sự phát triển của kỹ thuật tự động với những robot được áp dụng
trong nhiều lĩnh vực như quân sự, y tế, công nghiệp, dân dụng... Với những
nhu cầu phong phú như vậy thì robot tự hành là một phần không thể thiếu.
Với mong muốn giải quyết bài toán cơ bản của robot tự hành là lập bản đồ,
xây dựng bản đồ và bám quỹ đạo từ đó ứng dụng lên mô hình thực tế, ở đây
là robot 3 bánh. Tôi đã chọn đề tài “Thiết kế quỹ đạo cho xe tự hành và ứng
dụng trên mô hình robot 3 bánh”
2. Các vấn đề nghiên cứu
- Xây dựng bản đồ với mô hình xác suất động học, xác suất cảm biến
khoảng cách.
- Xác định vị trí robot và vật cản cố định sử dụng thuật toán Bayes, Particle
Filter, FlastFLAM.
- Lập bản đồ với phương pháp A*.
- Xây dựng mô hình động lực học và mô hình động học của robot. Và thiết
kế bộ điều khiển chuyển động.
- Xây dựng bộ điều khiển bám quỹ đạo theo phương pháp “Path Following
with the Pure Pursuit Algorithm”.
- Ứng dụng thuật toán, thực thi thuật toán trên máy tính với ROS-robot
Operating System.
- Thiết kế và xây dựng mô hình robot tự hành sử dụng cảm biến LIDAR và
máy tính nhúng Raspberry.
3. Kết quả
Đồ án đã giải quyết khá hoàn chỉnh bài toán của robot tự hành. Xây
dựng được bản đồ sử dụng cảm biến laser LIDAR, lập quỹ đạo cho robot
dùng giải thuật A*, điều khiển bám quỹ đạo và điều khiển cho robot tự tránh
vật cản. Kết quả thực tế khá tốt, robot bám sát quỹ đạo và thực thi lệnh hiệu
quả. Tuy nhiên vẫn có một số hạn chế như tốn nhiều thời gian để lập bản
đồ, hạn chế của cảm biến khiến robot không thể tránh được những vật cản
dưới tầm quét của sensor. Đây là cũng tiền đề cho các nghiên cứu sau này
để áp dụng điều khiển robot.
Sinh viên thực hiện
Ký và ghi rõ họ tên
iv
MỤC LỤC
CHƯƠNG 1. LỜI MỞ ĐẦU................................................................................ 2
1.1 Lý do chọn đề tài ........................................................................................ 2
1.2 Tổng quan về bài toán thiết kế robot tự hành ............................................ 2
1.3 Đối tượng nghiên cứu................................................................................. 3
1.4 Cấu trúc của đồ án ...................................................................................... 3
CHƯƠNG 2. CƠ SỞ LÝ THUYẾT .................................................................... 5
2.1 Mô hình thuật toán cho bài toán lập bản đồ và xác định vị trí ................... 5
Mô hình động học xác suất ......................................................... 5
Mô hình xác suất cảm biến khoảng cách .................................... 7
2.2 Thuật toán lọc Bayes .................................................................................. 9
Giới thiệu thuật toán .................................................................................. 9
2.3 Particle Filter ............................................................................................ 10
2.4 Thuật toán FastSLAM .............................................................................. 11
2.5 Lập quỹ đạo robot .................................................................................... 11
Phương pháp A star ................................................................... 11
Mô phỏng thuật toán và đánh giá .............................................. 12
2.6 Bộ điều khiển chuyển động (motion control) .......................................... 13
Mô hình toán học ...................................................................... 13
Xây dựng hệ trục tọa độ. ........................................................... 13
Mô hình động học ..................................................................... 15
Thiết kế bộ điều khiển chuyển động ......................................... 15
2.7 Bộ điều khiển bám quỹ đạo Pure pursuit. ................................................ 16
Giới thiệu về phương pháp........................................................ 16
Cơ sở lý thuyết. ......................................................................... 17
Phân tích tính ổn định ............................................................... 22
Thực hiện bộ điều khiển............................................................ 25
Mô phỏng bộ điều khiển path following pure pursuit. ............. 27
CHƯƠNG 3. ỨNG DỤNG ................................................................................ 29
3.1 Giới thiệu về Robot Operating System .................................................... 29
Ưu điểm của ROS ..................................................................... 29
Mô hình ROS ............................................................................ 29
3.2 Cấu trúc phần mềm .................................................................................. 32
v
3.3 Cấu trúc phần cứng .................................................................................. 33
3.4 Thiết kế chi tiết phần cứng ....................................................................... 34
3.5 Kết quả thực nghiệm ................................................................................ 38
Mô hình robot tự hành .............................................................. 39
Bài toán lập bản đồ.................................................................... 39
Bài toán điều khiển lập quỹ đạo và bám quỹ đạo ..................... 40
Bài toán lập quỹ đạo.................................................................. 40
Bài toán robot tránh vật cản ...................................................... 40
Bài toán tự xác định vị trí trong bản đồ .................................... 41
KẾT LUẬN VÀ THẢO LUẬN HƯỚNG PHÁT TRIỂN ............................... 42
Tài liệu tham khảo ............................................................................................. 43
vi
DANH MỤC HÌNH VẼ
Hình 1.1 Mô hình thiết kế robot tự hành ............................................................... 3
Hình 2.1 Mô hình xác suất động học của robot chuyển động................................ 5
Hình 2.2 Mô hình odometry ................................................................................... 6
Hình 2.3 Thành phần của mô hình cảm biến khoảng cách ................................... 8
Hình 2.4 Mô hình khoảng cách đã kết hợp ............................................................ 9
Hình 2.5 Mô hình robot 3 bánh ............................................................................ 14
Hình 2.6 Sơ đồ điều khiển bộ điều khiển chuyển động. ...................................... 16
Hình 2.7 Mô hình động học của robot ................................................................. 17
Hình 2.8 Mô tả bám quỹ đạo cho robot ............................................................... 18
Hình 2.9 Bám quỹ đạo với đường thẳng .............................................................. 19
Hình 2.10 Bám quỹ đạo dàng đường tròn ............................................................ 20
Hình 2.11 Mô tả hình học phương pháp PP ......................................................... 21
Hình 2.12 đồ thị biểu diễn giá trị của L phụ thuộc vào γp ................................. 25
Hình 2.13 Sơ đồ điều khiển bám quỹ đạo PP ...................................................... 25
Hình 2.14 Mô tả hình học bộ điều khiển PP ........................................................ 26
Hình 2.15 Kết quả mô phỏng bám quỹ đạo theo 3 giá trị của lookahead distance
.............................................................................................................................. 28
Hình 3.1 Sơ đồ ROS Filesystem .......................................................................... 30
Hình 3.2 Tương tác giữa các Node trong ROS .................................................... 31
Hình 3.3 Lưu đồ thuật toán dẫn đường cho robot ................................................ 32
Hình 3.4 Cấu trúc phần mềm thực nghiệm .......................................................... 33
Hình 3.5 Sơ đồ khối cấu trúc phần cứng .............................................................. 33
Hình 3.6 Sơ đồ nguồn LDO 3V3 cho MCU. ....................................................... 34
Hình 3.7 Sơ đồ nguồn BOOST 5V-2A cho máy tính nhung RASPI. .................. 34
Hình 3.8 Sơ đồ nguồn BOOST 8.3V cho động cơ. .............................................. 35
Hình 3.9 hình ảnh thực tế GY-521 ....................................................................... 35
Hình 3.10 Sơ đồ nguyên lý mạch cầu L298 ......................................................... 36
Hình 3.11 hình ảnh mô phỏng phần cứng ............................................................ 36
Hình 3.12 Cơ chế làm việc của RPLIDAR A1 .................................................... 37
Hình 3.13 Hình ảnh thực tế Raspberry PI 3B ...................................................... 37
Hình 3.14 Mô hình sản phẩm robot tự hành ........................................................ 39
Hình 3.15 Kết quả bài toán lập bản đồ ................................................................. 39
Hình 3.16 Kết quả bám quỹ đạo PP ..................................................................... 40
Hình 3.17 Kết quả bài toán lập quỹ đạo ............................................................... 40
Hình 3.18 Quá trình tránh vật cản của robot ........................................................ 41
Hình 3.19 Kết quả bài toán tự xác định vị trí ....................................................... 41
vii
DANH MỤC HÌNH VẼ
Bảng 2.1 Kết quả phương pháp A* ..................................................................... 12
Bảng 2.2 Mô tả các khối trong sơ đồ điều khiển chuyển động ........................... 16
Bảng 2.3 Các khối trong sơ đồ điều khiển bám quỹ đạo .................................... 27
Bảng 2.4 Tham số bộ điều khiểm PP .................................................................. 27
Bảng 2.5 Tham số động học robot ...................................................................... 27
Bảng 3.1 Cấu trúc khối nguồn............................................................................. 34
1
CHƯƠNG 1. LỜI MỞ ĐẦU
1.1 Lý do chọn đề tài
Sự phát triển mạnh mẽ của kỹ thuật tự động hóa ngày nay không những được
áp dụng trong công nghiệp, mà nó cũng đã được phát triển nhiều ứng dụng khác
nhau trong cuộc sống hằng ngày, trong các ứng dụng y tế, quân sự. Những robot
tự động không còn chỉ đảm nhận những việc thay thế con người tham gia những
nhiệm vụ khó khăn và nguy hiểm như như hoạt động trong những môi trường khắc
nghiệt, vận chuyển vật liệu trong môi trường độc hại, trinh sát… Chúng đã và đang
được ứng dụng nhiều vào hoạt động của cuộc sống hằng ngày như giải trí, chăm
sóc sức khỏe, logistics,…. Với một mức chi phí rẻ hơn nhiều so với người và có
độ chính xác, ổn định cao.
Với những yêu cầu phong phú như vậy cũng có rất nhiều loại robot tự hành
khác nhau tương ứng như robot di động kiểu chân sinh học, robot di động kiểu
bánh xích, robot di động kiểu bánh xe… Tùy hình thái khác nhau nhưng các robot
tự hành đều phải có một chức năng cơ bản là phát hiện và tránh các vật cản trong
quá trình chuyển động, đồng thời thực hiện các nhiệm vụ hiện tại như di chuyển,
bám mục tiêu. Trước mỗi vật cản mà robot phát hiện được, nó cần đưa ra quyết
định vòng tránh như thế nào nhưng vẫn đảm bảo nhiệm vụ di chuyển theo quỹ đạo,
bám mục tiêu.
Bài toán tránh vật cản cho rô bốt tự hành đã được nhiều nhà khoa học trên
thế giới quan tâm và giải quyết trong thập niên vừa qua. Với mục đích xây dựng
kế thừa và ứng dụng những cơ sở khoa học vào thực tế. Tôi đã chọn đề tài “Thiết
kế quỹ đạo cho xe tự hành và ứng dụng trên mô hình robot 3 bánh”
1.2 Tổng quan về bài toán thiết kế robot tự hành
Để có thể di chuyển tự động một cách thông minh và không cần sự tác động
từ con người thì các robot tự hành kiểu bánh xe sẽ được trang bị các phần tử chấp
hành, cảm biến, bộ xử lý… Nghiên cứu về robot tự hành được chia ra làm 4 bài
toán chính: lập bản đồ và xác định vị trí, thiết kế quỹ đạo, điều khiển bám quỹ đạo,
điều khiển chuyển động.
Vấn đề lập bản đồ và định vị là bài toán phối kết hợp các cảm biến để ước
lượng vị trí, hướng, vận tốc, và gia tốc của robot và các vật thể của môi trường
trong thời gian thực
Thiết kế quỹ đạo là một bài toán quan trọng trong lĩnh vực robot tự hành, đặc
biệt trong môi trường có nhiều vật cản. Bộ điều khiển cần xây dựng quỹ đạo tối ưu
để robot có thể đi từ điểm A tới B mà không vướng vật cản với quãng đường ngắn
nhất. Việc xây dựng một bản đồ hiệu quả sẽ tiết kiệm năng lượng cũng như thời
gian, từ đó mang lại hiệu quả kinh tế cho người sử dụng. Vậy nên thiết kế quỹ đạo
có thể được xem như một dạng của bài toán tối ưu trong điều kiện có các ràng buộc
nhất định.
2
Hình 1.1 Mô hình thiết kế robot tự hành
Bài toán thiết kế quỹ đạo có thể được chia làm hai lớp: thiết kế quỹ đạo toàn
cục và thiết kế quỹ đạo cục bộ. Trong thiết kế quỹ đạo toàn cục, toàn bộ thông tin
về môi trường hoạt động của robot di động đều được biết, bởi vậy quỹ đạo được
thiết kế tĩnh từ trước và môi trường không có sự thay đổi theo thời gian. Trong khi
đó, trong thiết kế quỹ đạo cục bộ, chỉ một phần thông tin về môi trường được biết
hoặc không có chút thông tin nào về môi trường. Trong phương pháp thiết kế quỹ
đạo cục bộ, quỹ đạo luôn được thiết kế tự động dựa theo thông tin cảm biến về các
vật cản được thu thập online. Bởi vậy, phương pháp thiết kế quỹ đạo cục bộ là phù
hợp hơn và có ý nghĩa thực tiễn hơn phương pháp toàn cục, bởi vì môi trường
thường thay đổi theo thời gian và rất khó để được mô tả chính xác từ trước.
Sau khi quỹ đạo được thiết kế, vấn đề tiếp theo là điều khiển robot bám quỹ
đạo đã được thiết kế. Bài toán điều khiển bám quỹ đạo và điều khiển chuyển động
cực kỳ quan trọng trong lĩnh vực robot tự hành, bởi vì hiệu năng của các luật điều
khiển ảnh hưởng trực tiếp đến hiệu quả của các ứng dụng robot di động trong sản
xuất và đời sống.
1.3 Đối tượng nghiên cứu
Nhằm thực thi các luật điều khiển được đề xuất, robot di động 3 bánh xe được
lựa chọn làm đối tượng nghiên cứu. Với kiểu 3 bánh xe này bao gồm 2 bánh chủ
động điều khiển vi phân, 01 bánh thụ động được sử dụng để làm điểm tựa tạo thế
cân bằng trọng lực.
1.4 Cấu trúc của đồ án
Cấu trúc của đồ án được thể hiện với các nội dung như sau:
Phần 1: Lời mở đầu
Phần 2: Cơ sở lý thuyết
o Bài toán lập bản đồ và xác định vị trí
3
o Bài toán lập quỹ đạo cho robot
o Mô hình hóa đối tượng và thiết bị điều khiển
Phần 3: Ứng dụng
o Thiết kế robot tự hành
o Kết quả thực nghiệm
Kết luận
4
CHƯƠNG 2. CƠ SỞ LÝ THUYẾT
2.1 Mô hình thuật toán cho bài toán lập bản đồ và xác định vị trí
Xác suất trong robotics là một hướng tiếp cận mới cho robot. Ý tưởng là ước
lượng trạng thái từ dữ liệu cảm biến. Đa số các ứng dụng robot, việc điều khiển
robot thực thi theo yêu cầu là tương đối dễ nếu các thông tin về trạng thái của robot
là chính xác. Ví như điều khiển robot chuyển động là dễ dàng nếu ta xác định được
vị trí của robot cũng như các vật cản trong môi trường chuyển động. Tuy nhiên các
thông tin này thường không thể đo đạc một cách trực tiếp, robot sẽ tiếp nhận các
thông tin này một cách gián tiếp qua thông tin kết hợp các cảm biến. Cảm biến
trạng thái có thể bị nhiễu do tác động của ngoại cảnh nên cần phục hồi bởi phép
ước lượng trạng thái từ các dữ liệu nhận được.
Mô hình động học xác suất
Mô hình động học đóng vai trò là mô hình chuyển trạng thái trong robot di
động [1]. Mô hình được diễn tả thông qua biểu thức sau:
Trong đó và đều là vị trí robot và là lệnh điều khiển. Mô hình diễn tả
phân phối nghiệm qua phương trình động học của robot khi robot ở vị trí và
thực hiện lệnh điều khiển
Hình 2.1 Mô hình xác suất động học của robot chuyển động
Hình 2.1 là ví dụ 2 mô tả mô hình động học cho robot chuyển động. Cả 2
bắt đầu từ vị trí và lệnh điều khiển được biểu diễn qua các đường thẳng ở
hình (a) và đường xung vuông như hình (b). Phân phối ( | , ) được biểu
diễn chính là vùng xám. Vị trí nào càng tối thì càng gần vị trí thật. Ta thấy với hình
(b) lệnh điều khiển phức tạp hơn, dẫn đến trong quá trình chuyển động robot có
sai lệch nhiều hơn, nên phân bố biểu diễn cũng mờ hơn so với hình (a).
Có 2 loại mô hình động học là: mô hình theo vận tốc và mô hình theo
odometry (odometry là những thông tin đo được từ cảm biến encoder như độ dịch
hay góc quay). Trên thực tế, mô hình sử dụng odometry được sử dụng nhiều hơn.
Mô hình odometry được mô tả như sau:
5
Hình 2.2 Mô hình odometry
Tại thời điểm t - 1 có =( ) , thời điểm t ta có =( ′ ′ ′)
Ta có các thông số:
: góc tạo bởi robot thời điểm t -1 và đường nối tâm của robot ở 2 thời điểm t
và t -1
: khoảng cách giữa tâm của robot 2 khoảng thời gian t và t -1
: góc tạo bởi robot thời điểm t và đường nối tâm của robot ở 2 thời điểm t và
t -1
Vậy mô hình theo odometry sẽ được xác định như sau:
Với đầu vào là vị trí thực tại thời điểm t – 1: , lệnh điều khiển = đưa robot
từ vị trí đặt =( ) tới vị trí đặt =( ′ ′ ′) và vị trí giả thuyết đạt
được tại thời điểm t:
Motion_model_odometry( , , ):
′ ′ PT 2.1
= 2( − , − )−
′ ′ PT 2.2
= ( − ) +( − )
′ PT 2.3
= − −
= ( ′− ) +( ′− ) PT 2.4
′ PT 2.5
= − −
= ( − , + ) PT 2.6
= ( − , + ( PT 2.7
+ ))
= ( − , + ) PT 2.8
Return .. .
Với prob là hàm phân phối chuẩn
Prob_normal_distribution(a,b) =
√
6
Để phục vụ cho bài toán xác định vị trí, ta cần thuật toán trích xuất từ mô hình
động học xác suất với odometry đã cho.
Với đầu vào là vị trí robot tại thời điểm t – 1 : và lệnh điều khiển = ,
đầu ra là vị trí , ta có:
Sample_motion_model_odometry( , ):
′ ′ PT 2.9
= 2( − , − )−
′ PT 2.10
= − −
= − ( + ) PT 2.11
= − ( + ) PT 2.12
′ PT 2.13
= + ( + )
′ PT 2.14
= + +
Return = ( ′, ′, ′)
Với hàm sample từ phân phối chuẩn có công thức như sau:
Sample(b) = ∑ (−1,1)
Các thông số , , đặc trưng cho nhiễu đo của từng đại lượng.
Mô hình xác suất cảm biến khoảng cách
Mô hình cảm biến cùng với mô hình động học là hai yếu tố quan trọng nhất
trong thuật toán xác suất này. Có thể coi đây chính là mô hình quan sát trong các
hệ thống điều khiển tự động [2].
Mô hình được định nghĩa là phân phối có điều kiện ( | , ) với là vị trí
của robot, l là tập hợp các landmark của môi trường (hay chính là bản đồ), là
phép đo tại thời điểm t.
Mỗi mô hình là sự kết hợp của 4 loại nhiễu đo: nhiễu đo nhỏ, lỗi do vật thể
không mong muốn, lỗi do không thể xác định vật thể và lỗi ngẫu nhiên không thể
giải thích.
Nhiễu đo nhỏ là nhiễu đo do hạn chế về độ phân giải của cảm biến dẫn đến
sai lệch một chút trong khoảng cách đến vật thể. Nhiễu này thường được mô hình
bởi phân phối Gauss hẹp và được định nghĩa bởi ℎ ( | , ).
∗
Với là giá trị chính xác của khoảng cách, ta có:
∗
(
) if 0 ≤ ; ≤ ,
PT 2.15
ℎ ( | , )= ℎ
0 otherwise
Nhiễu đo do những vật thể không mong muốn: Môi trường xung quanh
robot là môi trường động, trong khi đó bản đồ là cố định. Do vậy, những vật thể
không có trong bản đồ sẽ tạo ra những sai lệch khác. Ví dụ như là sự di chuyển
7
của con người trong bản đồ, để khắc phục việc này, có thể coi nó là nhiễu đo.
Nhiễu này sẽ được mô hình hóa bởi phân phối theo hàm mũ và được định nghĩa
bởi ℎ ( | , )
ℎ if 0 ≤ ≤ ∗ PT 2.16
ℎ ( | , )= ℎ
0 otherwise
∗
Với là khoảng cách thật của vật thể không xác định.
Nhiễu do không xác định được vật thể là sensor bị ảnh hưởng bởi vật thể
đen, hấp thụ ánh sáng, hoặc là chạy trong điều kiện ánh sáng quá sáng. Khi đó,
cảm biến sẽ trả về giá trị lớn nhất có thể là . Mô hình trong trường hợp này sẽ
là:
1 if z = z PT 2.17
( | , )=
0 otherwise
Nhiễu do những yếu tố ngẫu nhiên không thể giải thích: Loại nhiễu này sẽ
được mô hình hóa bằng phân phối đồng nhất.
1 PT 2.17
if 0 ≤ ≤z
_ ( _ ^ │ _ , )= z
0 ℎ
Bốn phân phối của bốn loại nhiễu đo được mô tả như hình dưới đây
Hình 2.3 Thành phần của mô hình cảm biến khoảng cách
Bốn phân phối trên sẽ kết hợp với nhau bởi một trọng số trung bình của các
trọng số sau , , , sao cho tổng 4 trọng số là 1 như hình dưới
8
Hình 2.4 Mô hình khoảng cách đã kết hợp
Công thức của mô hình như sau:
( | , ) PT 2.18
ℎ ℎ
ℎ ⎛ ℎ ( | , )⎞
( | , )= ⎜ ( | , )⎟
⎝ ( | , )⎠
2.2 Thuật toán lọc Bayes
Giới thiệu thuật toán
Thuật toán lọc Bayes được dùng phổ biến trong ước lượng trạng thái của
robot. Thuật toán này là một phương pháp xác suất cho việc ước lượng một hàm
mật độ xác suất chưa biết bằng hồi quy theo thời gian dựa vào đo lường và mô
hình xử lý toán học. Ước lượng trạng thái x của hệ được cho bởi quan sát z và lệnh
điều khiển u bằng cách tính toán kì vọng của nó. Kì vọng của trạng thái nào lớn
nhất thì robot đang ở trạng thái đó [3].
Mục tiêu của thuật toán: Xác định kì vọng của trạng thái x tại thời điểm t:
( )= ( | : , : ) PT 2.19
Theo định lý Bayes ta có:
( )= ( | , : , : ) ( | : , : ) PT 2.20
Tiếp tục biến đổi, theo Markov và áp dụng tổng xác suất, ta được
( )= ( | ) ( | : , : ) PT 2.21
PT 2.22
= ( | ) ( | , : , : ) ( | : , : )
PT 2.23
= ( | ) ( | , ) ( | : , : )
PT 2.24
= ( | ) ( | , ) ( | : , : )
9
PT 2.25
= ( | ) ( | , ) ( )
Ta thấy được biểu thức hồi quy, kì vọng của trạng thái x tại thời điểm t phụ thuộc
vào kì vọng của nó ở các trạng thái trước đó.
Từ đây, ta có được thuật toán như sau:
Bước 1: Là bước dự đoán (prediction) kì vọng của trạng thái x tại thời điểm t dựa
trên kì vọng của x tại thời điểm t – 1 và mô hình xác suất động học.
PT 2.26
( )= ( | , ) ( )
Bước 2: Là bước xác định kì vọng chính xác (correction). Kết hợp mô hình quan
sát (đo lường) thông qua cảm biến khoảng cách và kì vọng dự đoán ở bước trước
thông qua luật Bayes.
( )= ( | ) ( ) PT 2.27
2.3 Particle Filter
Particle Filter là thuật toán giúp ước lượng được vị trí của robot. Với nền tảng
là thuật toán lọc Bayes, Particle có đầu vào là một tập các trạng thái được trích
xuất từ mô hình xác suất động học ở thời điểm t – 1, luật điều khiển u tại thời điểm
t và phép đo tại thời điểm t.
Đầu ra chính là tập trạng thái thời điểm t. Số lượng phần tử trọng tập trạng thái đầu
vào càng nhiều sẽ càng chính xác, các phần tử sẽ càng gần vị trí thật [4].
Thuật toán được mô tả như sau:
PF( , , , ):
= =∅ PT 2.28
(Bước dự đoán trong lọc Bayes)
Lặp m = 1 đến M (số phần tử):
[ ] [ ] PT 2.29
∼ ( | , )
[ ] [ ]
= ( | , ) PT 2.30
[ ] [ ] PT 2.31
+= ,
(Bước xác định chính xác)
Lặp từ 1 đến M:
[] [] [] PT 2.32
= .
[]
Thêm vào
Return
10
2.4 Thuật toán FastSLAM
Ta có mô hình xác suất của động học robot trong môi trường không hề có vật
cản nào cả. Trên thực tế, robot luôn chuyển động trong một môi trường gồm rất
nhiều vật thể cả động lẫn tĩnh. Ta định nghĩa bản đồ l là tập hợp các yếu tố trong
môi trường và đặc biệt chính là tập hợp tất cả các vật thể, vật cản cố định hay di
chuyển. Với một robot khi được đặt vào một môi trường mới, hầu hết các thông
tin như vị trí của nó và vị trí các vật thể đều không được xác định rõ ràng, điều này
đòi hỏi robot vừa phải ước lượng vị trí của nó và đồng thời ước lượng vị trí các vật
thể cố định xung quanh. Từ đó, thuật toán SLAM (Simultaneous localization and
mapping) được áp dụng để thực hiện vấn đề trên [5].
Mô hình robot với thuật toán FastSLAM sẽ thay đổi như sau:
( : , : | : , : )= ( : | : , : ). ( : | : , : ) PT 2.33
= ( : | : , : ). ∏ ( | : , : )
Với l là bản đồ hay tập hợp các landmark (vật cản cố định)
Thuật toán SLAM được dựa trên Particle Filter và được thực hiện như sau:
Bước 1: Khởi tạo tập particles. Mỗi particle sẽ gồm vị trí robot và tập hợp các
landmarks. Với bài toán SLAM, vị trí ban đầu khởi tạo sẽ tại vị trí x = y = 0
Bước 2: Thực hiện lệnh điều khiển trên toàn các particle. Ta nhận thấy biểu thức
trên có thể chia làm 2 phần:
- ( : | : , : ) đại diện cho ước lượng vị trí (localization)
- ∏ ( | : , : ) đại diện cho map (các vật cản cố định)
Các bước tiếp theo ta thực hiện đồng thời ước lượng vị trí và ước lượng vật cản cố
định thông qua dữ liệu cảm biến khoảng cách.
Bước 3: Tính toán các trọng số với vị trí robot
Bước 4: Xác định lại vị trí robot và update vị trí các landmark mới (nếu có)
Bước 5: Quay lại bước 2.
2.5 Lập quỹ đạo robot
Bài toán hoạch định đường đi cho robot (path planning) là một bước quan
trọng trong robot tự hành và xe tự lái, giúp cho robot di chuyển tới điểm đích một
cách tự động và theo một cách tối ưu. Thuật toán thường được áp dụng hiện nay
cho path planning là giải thuật tìm kiếm A* [6].
Phương pháp A star
A* lưu giữ một tập các lời giải chưa hoàn chỉnh, nghĩa là các đường đi qua
đồ thị, bắt đầu từ nút xuất phát. Tập lời giải này được lưu trong một hàng đợi ưu
tiên (priority queue). Thứ tự ưu tiên gán cho một đường đi được quyết định bởi
hàm ( ) = ( ) + ℎ( ).
Trong đó, ( ) là chi phí của đường đi cho đến thời điểm hiện tại, nghĩa là
tổng trọng số của các cạnh đã đi qua. ℎ( ) là hàm đánh giá heuristic về chi phí
nhỏ nhất để đến đích từ . Ví dụ, nếu "chi phí" được tính là khoảng cách đã đi
11
qua, khoảng cách đường chim bay giữa hai điểm trên một bản đồ là một đánh giá
heuristic cho khoảng cách còn phải đi tiếp. Hàm ( ) có giá trị càng thấp thì độ
ưu tiên của càng cao (do đó có thể sử dụng một cấu trúc heap tối thiểu để cài đặt
hàng đợi ưu tiên này).
function A*(điểm_xuất_phát,đích)
var đóng:= tập rỗng
var q:= tạo_hàng_đợi(tạo_đường_đi(điểm_xuất_phát))
while q không phải tập rỗng
var p:= lấy_phần_tử_đầu_tiên(q)
var x:= nút cuối cùng của p
if x in đóng
continue
if x = đích
return p
bổ sung x vào tập đóng
foreach y in các_đường_đi_tiếp_theo(p)
đưa_vào_hàng_đợi(q, y)
return failure
Trong đó, các_đường_đi_tiếp_theo (p) trả về tập hợp các đường đi tạo bởi
việc kéo dài p thêm một nút kề cạnh. Giả thiết rằng hàng đợi được sắp xếp tự động
bởi giá trị của hàm f.
"Tập hợp đóng" ( đóng ) lưu giữ tất cả các nút cuối cùng của p (các nút mà các
đường đi mới đã được mở rộng tại đó) để tránh việc lặp lại các chu trình (việc này
cho ra thuật toán tìm kiếm theo đồ thị). Đôi khi hàng đợi được gọi một cách tương
ứng là "tập mở". Tập đóng có thể được bỏ qua (ta thu được thuật toán tìm kiếm
theo cây) nếu ta đảm bảo được rằng tồn tại một lời giải hoặc nếu
hàm các_đường_đi_tiếp_theo được chỉnh để loại bỏ các chu trình.
Mô phỏng thuật toán và đánh giá
Ta mô phỏng thuật toán tìm đường A* trên kích thước bản đồ (30x30). Kết
quả như bảng bên dưới, kết quả cho thấy việc đáp ứng là khá nhanh
Bảng 2.1 Kết quả phương pháp A*
startCell GoalCell BestPathSize BestPathCost ExecTimeBestPath
362 229 22 22.2 266.364
362 229 22 22.2 236.365
532 111 15 15.2 454.463
12
startCell GoalCell BestPathSize BestPathCost ExecTimeBestPath
532 111 15 15.2 504.368
88 591 24 25.8 148.899
88 591 24 25.8 147.449
2.6 Bộ điều khiển chuyển động (motion control)
Mô hình toán học
Có 2 loại mô hình được sử dụng trong phân tích hệ thống và thiết kế điều
khiển cho hệ robotic là mô hình động lực học và mô hình động học. Trong báo cáo
này sử dụng mô hình động học, một mô hình có đầu vào là điều khiển vận tốc, thay
vì điều khiển momen. Mô hình động học sẽ đơn giản hơn vì sẽ không liên quan
đến việc xác đinh chính xác các giá trị liên quan đối với mô hình như khối lượng,
hiệu suất cơ học… Trong một số trường hợp, việc đo đạc chính xác các thông số
trên là khó khăn và gần như không thể.
Đối với robot được truyền động bởi động cơ điện, các động cơ này thường
được điều khiển ổn định tốc độ bởi bộ PID. Bằng mối quan hệ động học giữa tốc
độ động cơ với chuyển động của robot, ta có thể kiểm soát chuyển động của robot
gián tiếp qua tốc độ của động cơ một cách dễ dàng. Trong trường hợp robot được
truyền động bởi động cơ điện, các động cơ này thường được điều khiển bởi bộ PID
ổn định tốc độ, bằng mối quan hệ động học giữa tốc độ động cơ với chuyển động
của robot, ta có thể kiểm soát chuyển động của Robot gián tiếp qua tốc độ của
động cơ, trong khi đó bài toán điều khiển tốc độ động cơ là bài toán tương đối đơn
giản, dễ chỉnh định tham số bộ điều khiển.
Mô hình động học của robot 3 bánh.
Xây dựng hệ trục tọa độ.
Để thuận tiện cho việc phân tích động học cho WMR, ta có 2 hệ trục tọa độ
được định nghĩa như sau:
Hệ trục tọa độ quán tính: Hệ trục cố định gắn với mặt đất, đại diện bởi { , }
Hệ trục tọa độ gắn xe: Hệ trục tọa độ gắn liền với thân xe và chuyển động
cùng với xe, đại diện bởi { , }
Hai hệ trục tọa độ được minh họa như hình dưới. Gốc tọa độ của hệ trục
gắn xe được định nghĩa là trung điểm A của đường nối tâm hai bánh xe. Trọng tâm
C của xe thuộc trục đối xứng của xe, cách gốc A một đoạn của d [7].
13
LUẬN VĂN TỐT NGHIỆP
Thiết kế quỹ đạo cho xe tự hành và
ứng dụng trên mô hình robot 3 bánh
CAO VĂN HOÀNG
[email protected]
Kỹ thuật Điều khiển và Tự động hóa
Giảng viên hướng dẫn: TS. Nguyễn Thu Hà
Chữ ký của GVHD
Bộ môn: Điều khiển tự động
Viện: Điện
HÀ NỘI, 6/2020
i
ĐỀ TÀI TỐT NGHIỆP
Thiết kế quỹ đạo cho xe tự hành và ứng dụng trên mô hình robot 3 bánh
Giáo viên hướng dẫn
Ký và ghi rõ họ tên
ii
Lời cảm ơn
Tôi xin bày tỏ lòng biết ơn sâu sắc đến TS. Nguyễn Thu Hà đã tận tình
hướng dẫn, chia sẻ kinh nghiệm giúp đỡ để tôi hoàn thiện luận văn này.
Tôi xin chân thành cảm ơn bạn Nguyễn Ngọc Huy và Nguyễn Ngọc Tú đã
nhiệt tình hỗ trợ trong quá trình nghiên cứu và triển khai ứng dụng.
Tôi xin trân trọng cảm ơn các thầy, cô giáo Viện Điện đã giúp đỡ, tạo điều
kiện trong quá trình học tập tại trường.
Tôi xin chân thành cảm ơn gia đình, đồng nghiệp là những người luôn bên
cạnh, hỗ trợ về mặt tinh thần, chia sẻ một phần khó khăn trong quá trình học tập
nghiên cứu.
Một lần nữa xin chân thành cảm ơn!
iii
Tóm tắt nội dung luận văn
1. Lý do chọn đề tài
Sự phát triển của kỹ thuật tự động với những robot được áp dụng
trong nhiều lĩnh vực như quân sự, y tế, công nghiệp, dân dụng... Với những
nhu cầu phong phú như vậy thì robot tự hành là một phần không thể thiếu.
Với mong muốn giải quyết bài toán cơ bản của robot tự hành là lập bản đồ,
xây dựng bản đồ và bám quỹ đạo từ đó ứng dụng lên mô hình thực tế, ở đây
là robot 3 bánh. Tôi đã chọn đề tài “Thiết kế quỹ đạo cho xe tự hành và ứng
dụng trên mô hình robot 3 bánh”
2. Các vấn đề nghiên cứu
- Xây dựng bản đồ với mô hình xác suất động học, xác suất cảm biến
khoảng cách.
- Xác định vị trí robot và vật cản cố định sử dụng thuật toán Bayes, Particle
Filter, FlastFLAM.
- Lập bản đồ với phương pháp A*.
- Xây dựng mô hình động lực học và mô hình động học của robot. Và thiết
kế bộ điều khiển chuyển động.
- Xây dựng bộ điều khiển bám quỹ đạo theo phương pháp “Path Following
with the Pure Pursuit Algorithm”.
- Ứng dụng thuật toán, thực thi thuật toán trên máy tính với ROS-robot
Operating System.
- Thiết kế và xây dựng mô hình robot tự hành sử dụng cảm biến LIDAR và
máy tính nhúng Raspberry.
3. Kết quả
Đồ án đã giải quyết khá hoàn chỉnh bài toán của robot tự hành. Xây
dựng được bản đồ sử dụng cảm biến laser LIDAR, lập quỹ đạo cho robot
dùng giải thuật A*, điều khiển bám quỹ đạo và điều khiển cho robot tự tránh
vật cản. Kết quả thực tế khá tốt, robot bám sát quỹ đạo và thực thi lệnh hiệu
quả. Tuy nhiên vẫn có một số hạn chế như tốn nhiều thời gian để lập bản
đồ, hạn chế của cảm biến khiến robot không thể tránh được những vật cản
dưới tầm quét của sensor. Đây là cũng tiền đề cho các nghiên cứu sau này
để áp dụng điều khiển robot.
Sinh viên thực hiện
Ký và ghi rõ họ tên
iv
MỤC LỤC
CHƯƠNG 1. LỜI MỞ ĐẦU................................................................................ 2
1.1 Lý do chọn đề tài ........................................................................................ 2
1.2 Tổng quan về bài toán thiết kế robot tự hành ............................................ 2
1.3 Đối tượng nghiên cứu................................................................................. 3
1.4 Cấu trúc của đồ án ...................................................................................... 3
CHƯƠNG 2. CƠ SỞ LÝ THUYẾT .................................................................... 5
2.1 Mô hình thuật toán cho bài toán lập bản đồ và xác định vị trí ................... 5
Mô hình động học xác suất ......................................................... 5
Mô hình xác suất cảm biến khoảng cách .................................... 7
2.2 Thuật toán lọc Bayes .................................................................................. 9
Giới thiệu thuật toán .................................................................................. 9
2.3 Particle Filter ............................................................................................ 10
2.4 Thuật toán FastSLAM .............................................................................. 11
2.5 Lập quỹ đạo robot .................................................................................... 11
Phương pháp A star ................................................................... 11
Mô phỏng thuật toán và đánh giá .............................................. 12
2.6 Bộ điều khiển chuyển động (motion control) .......................................... 13
Mô hình toán học ...................................................................... 13
Xây dựng hệ trục tọa độ. ........................................................... 13
Mô hình động học ..................................................................... 15
Thiết kế bộ điều khiển chuyển động ......................................... 15
2.7 Bộ điều khiển bám quỹ đạo Pure pursuit. ................................................ 16
Giới thiệu về phương pháp........................................................ 16
Cơ sở lý thuyết. ......................................................................... 17
Phân tích tính ổn định ............................................................... 22
Thực hiện bộ điều khiển............................................................ 25
Mô phỏng bộ điều khiển path following pure pursuit. ............. 27
CHƯƠNG 3. ỨNG DỤNG ................................................................................ 29
3.1 Giới thiệu về Robot Operating System .................................................... 29
Ưu điểm của ROS ..................................................................... 29
Mô hình ROS ............................................................................ 29
3.2 Cấu trúc phần mềm .................................................................................. 32
v
3.3 Cấu trúc phần cứng .................................................................................. 33
3.4 Thiết kế chi tiết phần cứng ....................................................................... 34
3.5 Kết quả thực nghiệm ................................................................................ 38
Mô hình robot tự hành .............................................................. 39
Bài toán lập bản đồ.................................................................... 39
Bài toán điều khiển lập quỹ đạo và bám quỹ đạo ..................... 40
Bài toán lập quỹ đạo.................................................................. 40
Bài toán robot tránh vật cản ...................................................... 40
Bài toán tự xác định vị trí trong bản đồ .................................... 41
KẾT LUẬN VÀ THẢO LUẬN HƯỚNG PHÁT TRIỂN ............................... 42
Tài liệu tham khảo ............................................................................................. 43
vi
DANH MỤC HÌNH VẼ
Hình 1.1 Mô hình thiết kế robot tự hành ............................................................... 3
Hình 2.1 Mô hình xác suất động học của robot chuyển động................................ 5
Hình 2.2 Mô hình odometry ................................................................................... 6
Hình 2.3 Thành phần của mô hình cảm biến khoảng cách ................................... 8
Hình 2.4 Mô hình khoảng cách đã kết hợp ............................................................ 9
Hình 2.5 Mô hình robot 3 bánh ............................................................................ 14
Hình 2.6 Sơ đồ điều khiển bộ điều khiển chuyển động. ...................................... 16
Hình 2.7 Mô hình động học của robot ................................................................. 17
Hình 2.8 Mô tả bám quỹ đạo cho robot ............................................................... 18
Hình 2.9 Bám quỹ đạo với đường thẳng .............................................................. 19
Hình 2.10 Bám quỹ đạo dàng đường tròn ............................................................ 20
Hình 2.11 Mô tả hình học phương pháp PP ......................................................... 21
Hình 2.12 đồ thị biểu diễn giá trị của L phụ thuộc vào γp ................................. 25
Hình 2.13 Sơ đồ điều khiển bám quỹ đạo PP ...................................................... 25
Hình 2.14 Mô tả hình học bộ điều khiển PP ........................................................ 26
Hình 2.15 Kết quả mô phỏng bám quỹ đạo theo 3 giá trị của lookahead distance
.............................................................................................................................. 28
Hình 3.1 Sơ đồ ROS Filesystem .......................................................................... 30
Hình 3.2 Tương tác giữa các Node trong ROS .................................................... 31
Hình 3.3 Lưu đồ thuật toán dẫn đường cho robot ................................................ 32
Hình 3.4 Cấu trúc phần mềm thực nghiệm .......................................................... 33
Hình 3.5 Sơ đồ khối cấu trúc phần cứng .............................................................. 33
Hình 3.6 Sơ đồ nguồn LDO 3V3 cho MCU. ....................................................... 34
Hình 3.7 Sơ đồ nguồn BOOST 5V-2A cho máy tính nhung RASPI. .................. 34
Hình 3.8 Sơ đồ nguồn BOOST 8.3V cho động cơ. .............................................. 35
Hình 3.9 hình ảnh thực tế GY-521 ....................................................................... 35
Hình 3.10 Sơ đồ nguyên lý mạch cầu L298 ......................................................... 36
Hình 3.11 hình ảnh mô phỏng phần cứng ............................................................ 36
Hình 3.12 Cơ chế làm việc của RPLIDAR A1 .................................................... 37
Hình 3.13 Hình ảnh thực tế Raspberry PI 3B ...................................................... 37
Hình 3.14 Mô hình sản phẩm robot tự hành ........................................................ 39
Hình 3.15 Kết quả bài toán lập bản đồ ................................................................. 39
Hình 3.16 Kết quả bám quỹ đạo PP ..................................................................... 40
Hình 3.17 Kết quả bài toán lập quỹ đạo ............................................................... 40
Hình 3.18 Quá trình tránh vật cản của robot ........................................................ 41
Hình 3.19 Kết quả bài toán tự xác định vị trí ....................................................... 41
vii
DANH MỤC HÌNH VẼ
Bảng 2.1 Kết quả phương pháp A* ..................................................................... 12
Bảng 2.2 Mô tả các khối trong sơ đồ điều khiển chuyển động ........................... 16
Bảng 2.3 Các khối trong sơ đồ điều khiển bám quỹ đạo .................................... 27
Bảng 2.4 Tham số bộ điều khiểm PP .................................................................. 27
Bảng 2.5 Tham số động học robot ...................................................................... 27
Bảng 3.1 Cấu trúc khối nguồn............................................................................. 34
1
CHƯƠNG 1. LỜI MỞ ĐẦU
1.1 Lý do chọn đề tài
Sự phát triển mạnh mẽ của kỹ thuật tự động hóa ngày nay không những được
áp dụng trong công nghiệp, mà nó cũng đã được phát triển nhiều ứng dụng khác
nhau trong cuộc sống hằng ngày, trong các ứng dụng y tế, quân sự. Những robot
tự động không còn chỉ đảm nhận những việc thay thế con người tham gia những
nhiệm vụ khó khăn và nguy hiểm như như hoạt động trong những môi trường khắc
nghiệt, vận chuyển vật liệu trong môi trường độc hại, trinh sát… Chúng đã và đang
được ứng dụng nhiều vào hoạt động của cuộc sống hằng ngày như giải trí, chăm
sóc sức khỏe, logistics,…. Với một mức chi phí rẻ hơn nhiều so với người và có
độ chính xác, ổn định cao.
Với những yêu cầu phong phú như vậy cũng có rất nhiều loại robot tự hành
khác nhau tương ứng như robot di động kiểu chân sinh học, robot di động kiểu
bánh xích, robot di động kiểu bánh xe… Tùy hình thái khác nhau nhưng các robot
tự hành đều phải có một chức năng cơ bản là phát hiện và tránh các vật cản trong
quá trình chuyển động, đồng thời thực hiện các nhiệm vụ hiện tại như di chuyển,
bám mục tiêu. Trước mỗi vật cản mà robot phát hiện được, nó cần đưa ra quyết
định vòng tránh như thế nào nhưng vẫn đảm bảo nhiệm vụ di chuyển theo quỹ đạo,
bám mục tiêu.
Bài toán tránh vật cản cho rô bốt tự hành đã được nhiều nhà khoa học trên
thế giới quan tâm và giải quyết trong thập niên vừa qua. Với mục đích xây dựng
kế thừa và ứng dụng những cơ sở khoa học vào thực tế. Tôi đã chọn đề tài “Thiết
kế quỹ đạo cho xe tự hành và ứng dụng trên mô hình robot 3 bánh”
1.2 Tổng quan về bài toán thiết kế robot tự hành
Để có thể di chuyển tự động một cách thông minh và không cần sự tác động
từ con người thì các robot tự hành kiểu bánh xe sẽ được trang bị các phần tử chấp
hành, cảm biến, bộ xử lý… Nghiên cứu về robot tự hành được chia ra làm 4 bài
toán chính: lập bản đồ và xác định vị trí, thiết kế quỹ đạo, điều khiển bám quỹ đạo,
điều khiển chuyển động.
Vấn đề lập bản đồ và định vị là bài toán phối kết hợp các cảm biến để ước
lượng vị trí, hướng, vận tốc, và gia tốc của robot và các vật thể của môi trường
trong thời gian thực
Thiết kế quỹ đạo là một bài toán quan trọng trong lĩnh vực robot tự hành, đặc
biệt trong môi trường có nhiều vật cản. Bộ điều khiển cần xây dựng quỹ đạo tối ưu
để robot có thể đi từ điểm A tới B mà không vướng vật cản với quãng đường ngắn
nhất. Việc xây dựng một bản đồ hiệu quả sẽ tiết kiệm năng lượng cũng như thời
gian, từ đó mang lại hiệu quả kinh tế cho người sử dụng. Vậy nên thiết kế quỹ đạo
có thể được xem như một dạng của bài toán tối ưu trong điều kiện có các ràng buộc
nhất định.
2
Hình 1.1 Mô hình thiết kế robot tự hành
Bài toán thiết kế quỹ đạo có thể được chia làm hai lớp: thiết kế quỹ đạo toàn
cục và thiết kế quỹ đạo cục bộ. Trong thiết kế quỹ đạo toàn cục, toàn bộ thông tin
về môi trường hoạt động của robot di động đều được biết, bởi vậy quỹ đạo được
thiết kế tĩnh từ trước và môi trường không có sự thay đổi theo thời gian. Trong khi
đó, trong thiết kế quỹ đạo cục bộ, chỉ một phần thông tin về môi trường được biết
hoặc không có chút thông tin nào về môi trường. Trong phương pháp thiết kế quỹ
đạo cục bộ, quỹ đạo luôn được thiết kế tự động dựa theo thông tin cảm biến về các
vật cản được thu thập online. Bởi vậy, phương pháp thiết kế quỹ đạo cục bộ là phù
hợp hơn và có ý nghĩa thực tiễn hơn phương pháp toàn cục, bởi vì môi trường
thường thay đổi theo thời gian và rất khó để được mô tả chính xác từ trước.
Sau khi quỹ đạo được thiết kế, vấn đề tiếp theo là điều khiển robot bám quỹ
đạo đã được thiết kế. Bài toán điều khiển bám quỹ đạo và điều khiển chuyển động
cực kỳ quan trọng trong lĩnh vực robot tự hành, bởi vì hiệu năng của các luật điều
khiển ảnh hưởng trực tiếp đến hiệu quả của các ứng dụng robot di động trong sản
xuất và đời sống.
1.3 Đối tượng nghiên cứu
Nhằm thực thi các luật điều khiển được đề xuất, robot di động 3 bánh xe được
lựa chọn làm đối tượng nghiên cứu. Với kiểu 3 bánh xe này bao gồm 2 bánh chủ
động điều khiển vi phân, 01 bánh thụ động được sử dụng để làm điểm tựa tạo thế
cân bằng trọng lực.
1.4 Cấu trúc của đồ án
Cấu trúc của đồ án được thể hiện với các nội dung như sau:
Phần 1: Lời mở đầu
Phần 2: Cơ sở lý thuyết
o Bài toán lập bản đồ và xác định vị trí
3
o Bài toán lập quỹ đạo cho robot
o Mô hình hóa đối tượng và thiết bị điều khiển
Phần 3: Ứng dụng
o Thiết kế robot tự hành
o Kết quả thực nghiệm
Kết luận
4
CHƯƠNG 2. CƠ SỞ LÝ THUYẾT
2.1 Mô hình thuật toán cho bài toán lập bản đồ và xác định vị trí
Xác suất trong robotics là một hướng tiếp cận mới cho robot. Ý tưởng là ước
lượng trạng thái từ dữ liệu cảm biến. Đa số các ứng dụng robot, việc điều khiển
robot thực thi theo yêu cầu là tương đối dễ nếu các thông tin về trạng thái của robot
là chính xác. Ví như điều khiển robot chuyển động là dễ dàng nếu ta xác định được
vị trí của robot cũng như các vật cản trong môi trường chuyển động. Tuy nhiên các
thông tin này thường không thể đo đạc một cách trực tiếp, robot sẽ tiếp nhận các
thông tin này một cách gián tiếp qua thông tin kết hợp các cảm biến. Cảm biến
trạng thái có thể bị nhiễu do tác động của ngoại cảnh nên cần phục hồi bởi phép
ước lượng trạng thái từ các dữ liệu nhận được.
Mô hình động học xác suất
Mô hình động học đóng vai trò là mô hình chuyển trạng thái trong robot di
động [1]. Mô hình được diễn tả thông qua biểu thức sau:
Trong đó và đều là vị trí robot và là lệnh điều khiển. Mô hình diễn tả
phân phối nghiệm qua phương trình động học của robot khi robot ở vị trí và
thực hiện lệnh điều khiển
Hình 2.1 Mô hình xác suất động học của robot chuyển động
Hình 2.1 là ví dụ 2 mô tả mô hình động học cho robot chuyển động. Cả 2
bắt đầu từ vị trí và lệnh điều khiển được biểu diễn qua các đường thẳng ở
hình (a) và đường xung vuông như hình (b). Phân phối ( | , ) được biểu
diễn chính là vùng xám. Vị trí nào càng tối thì càng gần vị trí thật. Ta thấy với hình
(b) lệnh điều khiển phức tạp hơn, dẫn đến trong quá trình chuyển động robot có
sai lệch nhiều hơn, nên phân bố biểu diễn cũng mờ hơn so với hình (a).
Có 2 loại mô hình động học là: mô hình theo vận tốc và mô hình theo
odometry (odometry là những thông tin đo được từ cảm biến encoder như độ dịch
hay góc quay). Trên thực tế, mô hình sử dụng odometry được sử dụng nhiều hơn.
Mô hình odometry được mô tả như sau:
5
Hình 2.2 Mô hình odometry
Tại thời điểm t - 1 có =( ) , thời điểm t ta có =( ′ ′ ′)
Ta có các thông số:
: góc tạo bởi robot thời điểm t -1 và đường nối tâm của robot ở 2 thời điểm t
và t -1
: khoảng cách giữa tâm của robot 2 khoảng thời gian t và t -1
: góc tạo bởi robot thời điểm t và đường nối tâm của robot ở 2 thời điểm t và
t -1
Vậy mô hình theo odometry sẽ được xác định như sau:
Với đầu vào là vị trí thực tại thời điểm t – 1: , lệnh điều khiển = đưa robot
từ vị trí đặt =( ) tới vị trí đặt =( ′ ′ ′) và vị trí giả thuyết đạt
được tại thời điểm t:
Motion_model_odometry( , , ):
′ ′ PT 2.1
= 2( − , − )−
′ ′ PT 2.2
= ( − ) +( − )
′ PT 2.3
= − −
= ( ′− ) +( ′− ) PT 2.4
′ PT 2.5
= − −
= ( − , + ) PT 2.6
= ( − , + ( PT 2.7
+ ))
= ( − , + ) PT 2.8
Return .. .
Với prob là hàm phân phối chuẩn
Prob_normal_distribution(a,b) =
√
6
Để phục vụ cho bài toán xác định vị trí, ta cần thuật toán trích xuất từ mô hình
động học xác suất với odometry đã cho.
Với đầu vào là vị trí robot tại thời điểm t – 1 : và lệnh điều khiển = ,
đầu ra là vị trí , ta có:
Sample_motion_model_odometry( , ):
′ ′ PT 2.9
= 2( − , − )−
′ PT 2.10
= − −
= − ( + ) PT 2.11
= − ( + ) PT 2.12
′ PT 2.13
= + ( + )
′ PT 2.14
= + +
Return = ( ′, ′, ′)
Với hàm sample từ phân phối chuẩn có công thức như sau:
Sample(b) = ∑ (−1,1)
Các thông số , , đặc trưng cho nhiễu đo của từng đại lượng.
Mô hình xác suất cảm biến khoảng cách
Mô hình cảm biến cùng với mô hình động học là hai yếu tố quan trọng nhất
trong thuật toán xác suất này. Có thể coi đây chính là mô hình quan sát trong các
hệ thống điều khiển tự động [2].
Mô hình được định nghĩa là phân phối có điều kiện ( | , ) với là vị trí
của robot, l là tập hợp các landmark của môi trường (hay chính là bản đồ), là
phép đo tại thời điểm t.
Mỗi mô hình là sự kết hợp của 4 loại nhiễu đo: nhiễu đo nhỏ, lỗi do vật thể
không mong muốn, lỗi do không thể xác định vật thể và lỗi ngẫu nhiên không thể
giải thích.
Nhiễu đo nhỏ là nhiễu đo do hạn chế về độ phân giải của cảm biến dẫn đến
sai lệch một chút trong khoảng cách đến vật thể. Nhiễu này thường được mô hình
bởi phân phối Gauss hẹp và được định nghĩa bởi ℎ ( | , ).
∗
Với là giá trị chính xác của khoảng cách, ta có:
∗
(
) if 0 ≤ ; ≤ ,
PT 2.15
ℎ ( | , )= ℎ
0 otherwise
Nhiễu đo do những vật thể không mong muốn: Môi trường xung quanh
robot là môi trường động, trong khi đó bản đồ là cố định. Do vậy, những vật thể
không có trong bản đồ sẽ tạo ra những sai lệch khác. Ví dụ như là sự di chuyển
7
của con người trong bản đồ, để khắc phục việc này, có thể coi nó là nhiễu đo.
Nhiễu này sẽ được mô hình hóa bởi phân phối theo hàm mũ và được định nghĩa
bởi ℎ ( | , )
ℎ if 0 ≤ ≤ ∗ PT 2.16
ℎ ( | , )= ℎ
0 otherwise
∗
Với là khoảng cách thật của vật thể không xác định.
Nhiễu do không xác định được vật thể là sensor bị ảnh hưởng bởi vật thể
đen, hấp thụ ánh sáng, hoặc là chạy trong điều kiện ánh sáng quá sáng. Khi đó,
cảm biến sẽ trả về giá trị lớn nhất có thể là . Mô hình trong trường hợp này sẽ
là:
1 if z = z PT 2.17
( | , )=
0 otherwise
Nhiễu do những yếu tố ngẫu nhiên không thể giải thích: Loại nhiễu này sẽ
được mô hình hóa bằng phân phối đồng nhất.
1 PT 2.17
if 0 ≤ ≤z
_ ( _ ^ │ _ , )= z
0 ℎ
Bốn phân phối của bốn loại nhiễu đo được mô tả như hình dưới đây
Hình 2.3 Thành phần của mô hình cảm biến khoảng cách
Bốn phân phối trên sẽ kết hợp với nhau bởi một trọng số trung bình của các
trọng số sau , , , sao cho tổng 4 trọng số là 1 như hình dưới
8
Hình 2.4 Mô hình khoảng cách đã kết hợp
Công thức của mô hình như sau:
( | , ) PT 2.18
ℎ ℎ
ℎ ⎛ ℎ ( | , )⎞
( | , )= ⎜ ( | , )⎟
⎝ ( | , )⎠
2.2 Thuật toán lọc Bayes
Giới thiệu thuật toán
Thuật toán lọc Bayes được dùng phổ biến trong ước lượng trạng thái của
robot. Thuật toán này là một phương pháp xác suất cho việc ước lượng một hàm
mật độ xác suất chưa biết bằng hồi quy theo thời gian dựa vào đo lường và mô
hình xử lý toán học. Ước lượng trạng thái x của hệ được cho bởi quan sát z và lệnh
điều khiển u bằng cách tính toán kì vọng của nó. Kì vọng của trạng thái nào lớn
nhất thì robot đang ở trạng thái đó [3].
Mục tiêu của thuật toán: Xác định kì vọng của trạng thái x tại thời điểm t:
( )= ( | : , : ) PT 2.19
Theo định lý Bayes ta có:
( )= ( | , : , : ) ( | : , : ) PT 2.20
Tiếp tục biến đổi, theo Markov và áp dụng tổng xác suất, ta được
( )= ( | ) ( | : , : ) PT 2.21
PT 2.22
= ( | ) ( | , : , : ) ( | : , : )
PT 2.23
= ( | ) ( | , ) ( | : , : )
PT 2.24
= ( | ) ( | , ) ( | : , : )
9
PT 2.25
= ( | ) ( | , ) ( )
Ta thấy được biểu thức hồi quy, kì vọng của trạng thái x tại thời điểm t phụ thuộc
vào kì vọng của nó ở các trạng thái trước đó.
Từ đây, ta có được thuật toán như sau:
Bước 1: Là bước dự đoán (prediction) kì vọng của trạng thái x tại thời điểm t dựa
trên kì vọng của x tại thời điểm t – 1 và mô hình xác suất động học.
PT 2.26
( )= ( | , ) ( )
Bước 2: Là bước xác định kì vọng chính xác (correction). Kết hợp mô hình quan
sát (đo lường) thông qua cảm biến khoảng cách và kì vọng dự đoán ở bước trước
thông qua luật Bayes.
( )= ( | ) ( ) PT 2.27
2.3 Particle Filter
Particle Filter là thuật toán giúp ước lượng được vị trí của robot. Với nền tảng
là thuật toán lọc Bayes, Particle có đầu vào là một tập các trạng thái được trích
xuất từ mô hình xác suất động học ở thời điểm t – 1, luật điều khiển u tại thời điểm
t và phép đo tại thời điểm t.
Đầu ra chính là tập trạng thái thời điểm t. Số lượng phần tử trọng tập trạng thái đầu
vào càng nhiều sẽ càng chính xác, các phần tử sẽ càng gần vị trí thật [4].
Thuật toán được mô tả như sau:
PF( , , , ):
= =∅ PT 2.28
(Bước dự đoán trong lọc Bayes)
Lặp m = 1 đến M (số phần tử):
[ ] [ ] PT 2.29
∼ ( | , )
[ ] [ ]
= ( | , ) PT 2.30
[ ] [ ] PT 2.31
+= ,
(Bước xác định chính xác)
Lặp từ 1 đến M:
[] [] [] PT 2.32
= .
[]
Thêm vào
Return
10
2.4 Thuật toán FastSLAM
Ta có mô hình xác suất của động học robot trong môi trường không hề có vật
cản nào cả. Trên thực tế, robot luôn chuyển động trong một môi trường gồm rất
nhiều vật thể cả động lẫn tĩnh. Ta định nghĩa bản đồ l là tập hợp các yếu tố trong
môi trường và đặc biệt chính là tập hợp tất cả các vật thể, vật cản cố định hay di
chuyển. Với một robot khi được đặt vào một môi trường mới, hầu hết các thông
tin như vị trí của nó và vị trí các vật thể đều không được xác định rõ ràng, điều này
đòi hỏi robot vừa phải ước lượng vị trí của nó và đồng thời ước lượng vị trí các vật
thể cố định xung quanh. Từ đó, thuật toán SLAM (Simultaneous localization and
mapping) được áp dụng để thực hiện vấn đề trên [5].
Mô hình robot với thuật toán FastSLAM sẽ thay đổi như sau:
( : , : | : , : )= ( : | : , : ). ( : | : , : ) PT 2.33
= ( : | : , : ). ∏ ( | : , : )
Với l là bản đồ hay tập hợp các landmark (vật cản cố định)
Thuật toán SLAM được dựa trên Particle Filter và được thực hiện như sau:
Bước 1: Khởi tạo tập particles. Mỗi particle sẽ gồm vị trí robot và tập hợp các
landmarks. Với bài toán SLAM, vị trí ban đầu khởi tạo sẽ tại vị trí x = y = 0
Bước 2: Thực hiện lệnh điều khiển trên toàn các particle. Ta nhận thấy biểu thức
trên có thể chia làm 2 phần:
- ( : | : , : ) đại diện cho ước lượng vị trí (localization)
- ∏ ( | : , : ) đại diện cho map (các vật cản cố định)
Các bước tiếp theo ta thực hiện đồng thời ước lượng vị trí và ước lượng vật cản cố
định thông qua dữ liệu cảm biến khoảng cách.
Bước 3: Tính toán các trọng số với vị trí robot
Bước 4: Xác định lại vị trí robot và update vị trí các landmark mới (nếu có)
Bước 5: Quay lại bước 2.
2.5 Lập quỹ đạo robot
Bài toán hoạch định đường đi cho robot (path planning) là một bước quan
trọng trong robot tự hành và xe tự lái, giúp cho robot di chuyển tới điểm đích một
cách tự động và theo một cách tối ưu. Thuật toán thường được áp dụng hiện nay
cho path planning là giải thuật tìm kiếm A* [6].
Phương pháp A star
A* lưu giữ một tập các lời giải chưa hoàn chỉnh, nghĩa là các đường đi qua
đồ thị, bắt đầu từ nút xuất phát. Tập lời giải này được lưu trong một hàng đợi ưu
tiên (priority queue). Thứ tự ưu tiên gán cho một đường đi được quyết định bởi
hàm ( ) = ( ) + ℎ( ).
Trong đó, ( ) là chi phí của đường đi cho đến thời điểm hiện tại, nghĩa là
tổng trọng số của các cạnh đã đi qua. ℎ( ) là hàm đánh giá heuristic về chi phí
nhỏ nhất để đến đích từ . Ví dụ, nếu "chi phí" được tính là khoảng cách đã đi
11
qua, khoảng cách đường chim bay giữa hai điểm trên một bản đồ là một đánh giá
heuristic cho khoảng cách còn phải đi tiếp. Hàm ( ) có giá trị càng thấp thì độ
ưu tiên của càng cao (do đó có thể sử dụng một cấu trúc heap tối thiểu để cài đặt
hàng đợi ưu tiên này).
function A*(điểm_xuất_phát,đích)
var đóng:= tập rỗng
var q:= tạo_hàng_đợi(tạo_đường_đi(điểm_xuất_phát))
while q không phải tập rỗng
var p:= lấy_phần_tử_đầu_tiên(q)
var x:= nút cuối cùng của p
if x in đóng
continue
if x = đích
return p
bổ sung x vào tập đóng
foreach y in các_đường_đi_tiếp_theo(p)
đưa_vào_hàng_đợi(q, y)
return failure
Trong đó, các_đường_đi_tiếp_theo (p) trả về tập hợp các đường đi tạo bởi
việc kéo dài p thêm một nút kề cạnh. Giả thiết rằng hàng đợi được sắp xếp tự động
bởi giá trị của hàm f.
"Tập hợp đóng" ( đóng ) lưu giữ tất cả các nút cuối cùng của p (các nút mà các
đường đi mới đã được mở rộng tại đó) để tránh việc lặp lại các chu trình (việc này
cho ra thuật toán tìm kiếm theo đồ thị). Đôi khi hàng đợi được gọi một cách tương
ứng là "tập mở". Tập đóng có thể được bỏ qua (ta thu được thuật toán tìm kiếm
theo cây) nếu ta đảm bảo được rằng tồn tại một lời giải hoặc nếu
hàm các_đường_đi_tiếp_theo được chỉnh để loại bỏ các chu trình.
Mô phỏng thuật toán và đánh giá
Ta mô phỏng thuật toán tìm đường A* trên kích thước bản đồ (30x30). Kết
quả như bảng bên dưới, kết quả cho thấy việc đáp ứng là khá nhanh
Bảng 2.1 Kết quả phương pháp A*
startCell GoalCell BestPathSize BestPathCost ExecTimeBestPath
362 229 22 22.2 266.364
362 229 22 22.2 236.365
532 111 15 15.2 454.463
12
startCell GoalCell BestPathSize BestPathCost ExecTimeBestPath
532 111 15 15.2 504.368
88 591 24 25.8 148.899
88 591 24 25.8 147.449
2.6 Bộ điều khiển chuyển động (motion control)
Mô hình toán học
Có 2 loại mô hình được sử dụng trong phân tích hệ thống và thiết kế điều
khiển cho hệ robotic là mô hình động lực học và mô hình động học. Trong báo cáo
này sử dụng mô hình động học, một mô hình có đầu vào là điều khiển vận tốc, thay
vì điều khiển momen. Mô hình động học sẽ đơn giản hơn vì sẽ không liên quan
đến việc xác đinh chính xác các giá trị liên quan đối với mô hình như khối lượng,
hiệu suất cơ học… Trong một số trường hợp, việc đo đạc chính xác các thông số
trên là khó khăn và gần như không thể.
Đối với robot được truyền động bởi động cơ điện, các động cơ này thường
được điều khiển ổn định tốc độ bởi bộ PID. Bằng mối quan hệ động học giữa tốc
độ động cơ với chuyển động của robot, ta có thể kiểm soát chuyển động của robot
gián tiếp qua tốc độ của động cơ một cách dễ dàng. Trong trường hợp robot được
truyền động bởi động cơ điện, các động cơ này thường được điều khiển bởi bộ PID
ổn định tốc độ, bằng mối quan hệ động học giữa tốc độ động cơ với chuyển động
của robot, ta có thể kiểm soát chuyển động của Robot gián tiếp qua tốc độ của
động cơ, trong khi đó bài toán điều khiển tốc độ động cơ là bài toán tương đối đơn
giản, dễ chỉnh định tham số bộ điều khiển.
Mô hình động học của robot 3 bánh.
Xây dựng hệ trục tọa độ.
Để thuận tiện cho việc phân tích động học cho WMR, ta có 2 hệ trục tọa độ
được định nghĩa như sau:
Hệ trục tọa độ quán tính: Hệ trục cố định gắn với mặt đất, đại diện bởi { , }
Hệ trục tọa độ gắn xe: Hệ trục tọa độ gắn liền với thân xe và chuyển động
cùng với xe, đại diện bởi { , }
Hai hệ trục tọa độ được minh họa như hình dưới. Gốc tọa độ của hệ trục
gắn xe được định nghĩa là trung điểm A của đường nối tâm hai bánh xe. Trọng tâm
C của xe thuộc trục đối xứng của xe, cách gốc A một đoạn của d [7].
13