tìm hiểu và xây dựng phần mềm hỗ trợ bài toán tìm đường đi ngắn nhất tránh vật cản cho xe tự hành trong không gian 2d

THÁI QUỐC THẮNG
BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƢỜNG ĐẠI HỌC LẠC HỒNG
* * *





LUẬN VĂN THẠC SĨ CÔNG NGHỆ THÔNG TIN

THÁI QUỐC THẮNG






TÌM HIỂU VÀ XÂY DỰNG PHẦN MỀM HỖ TRỢ
BÀI TOÁN TÌM ĐƢỜNG ĐI NGẮN NHẤT
TRÁNH VẬT CẢN CHO XE TỰ HÀNH
TRONG KHÔNG GIAN 2D






LUẬN VĂN THẠC SĨ CÔNG NGHỆ THÔNG TIN



KHÓA 3




Đồng Nai - Năm 2013
BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƢỜNG ĐẠI HỌC LẠC HỒNG
* * *



THÁI QUỐC THẮNG



TÌM HIỂU VÀ XÂY DỰNG PHẦN MỀM HỖ TRỢ
BÀI TOÁN TÌM ĐƢỜNG ĐI NGẮN NHẤT
TRÁNH VẬT CẢN CHO XE TỰ HÀNH
TRONG KHÔNG GIAN 2D



Chuyên ngành: CÔNG NGHỆ THÔNG TIN
Mã ngành: 60 48 02 01

LUẬN VĂN THẠC SĨ CÔNG NGHỆ THÔNG TIN


NGƢỜI HƢỚNG DẪN KHOA HỌC:
TS. VŨ ĐỨC LUNG




Đồng Nai - Năm 2013
LỜI CẢM ƠN
Tôi xin gởi lời cảm ơn chân thành và sâu sắc nhất đến TS. Vũ Đức Lung,
ngƣời Thầy đã tận tình hƣớng dẫn tôi trong suốt quá trình thực hiện luận văn cao
học và tạo mọi điều kiện để tôi có thể hoàn thành luận văn này.
Tôi xin gởi lời cảm ơn đến khoa Khoa Công Nghệ Thông Tin, Đại Học Lạc
Hồng; cùng Khoa Kỹ Thuật Máy Tính của Đại học Công Nghệ Thông Tin thuộc
Đại học Quốc Gia TP.HCM và Thầy cô trong khoa trong thời gian qua đã góp ý, hỗ
trợ cũng nhƣ tạo điều kiện thuận lợi cho tôi trong suốt thời gian làm luận văn.
Tôi xin cảm ơn gia đình đã động viên và tạo điều kiện tốt nhất để tôi có thể theo
đuổi việc học tập và nghiên cứu. Tôi gởi lòng tri ân đến tất cả bạn bè, những ngƣời
đã động viên, thăm hỏi cũng nhƣ đã giúp đỡ thiết thực giúp tôi hoàn tất luận văn
này
LỜI CAM ĐOAN
Tôi cam đoan: Quyển luận văn tốt nghiệp này ngoại trừ các kết quả tham khảo
từ các công trình nhƣ đã ghi rõ trong luận văn, các công việc trình bày trong luận
văn này là công trình nghiên cứu thực sự của cá nhân, đƣợc thực hiện trên cơ sở
nghiên cứu lý thuyết, kiến thức kinh điển, nghiên cứu khảo sát tình hình thực tiễn và
dƣới sự hƣớng dẫn khoa học của Tiến sĩ Vũ Đức Lung.
Một lần nữa, tôi khẳng định về sự trung thực của lời cam kết trên.
Đồng Nai, ngày 25 tháng 10 năm 2013

Ngƣời cam đoan


Thái Quốc Thắng
TÓM TẮT LUẬN VĂN
Trong những năm gần đây, robot tự hành (hoạt động không cần sự trợ giúp
của con ngƣời, có thể đƣợc điều khiển hoặc theo dõi từ xa thông qua máy tính) đã
và đang rất phát triển từ quân sự tới dân sự, giúp giảm thiểu đáng kể nguy hại và
công sức cho con ngƣời. Một trong những vấn đề chính của robot tự hành là làm
nhƣ thế nào để xử lý thông tin về môi trƣờng xung quanh và đƣa ra hành vi đúng
đắn nhất. Một robot có thể có rất nhiều hành vi khi hoạt động tự hành, và hành vi cụ
thể mà luận văn này hƣớng đến là hoạch định đƣờng đi ngắn nhất cho robot trong
môi trƣờng có nhiều vật cản từ một điểm đầu đến một điểm đích mong muốn nào
đó. Sau khi đƣờng đi tốt nhất đã đƣợc hoạch định, trong quá trình di chuyển robot
có thể gặp một số tác động ngoại gây cho việc bám đƣờng di chuyển thiếu chính
xác. Vì vậy, luận văn cũng trình bày một phƣơng pháp giúp robot di chuyển một
cách chính xác theo một đƣờng đi đã định trƣớc.
Về mặt lý thyết, luận văn tìm hiểu tổng quan các phƣơng pháp tìm đƣờng đi
ngắn nhất cho robot, trong đó phƣơng pháp tìm đƣờng đi ngắn nhất cho robot dựa
việc xây dựng một đồ thị tầm nhìn (visibility graph) với độ phức tạp O(n
2
logn) kết
hợp với thuật toán Dijkstra đƣợc mô tả chi tiết. Thêm vào đó việc điều khiển robot
di chuyển chính xác theo đƣờng đi này đƣợc hiện thực dựa vào lý thuyết bộ điều
khiển vi tích phân tỷ lệ (Proportional Integral Derivative - PID), một trong những lý
thuyết đƣợc sử dụng rộng rãi trong lĩnh vực điều khiển tự động.
Về mặt hiện thực, một môi trƣờng kiểm thử đƣợc xây dựng để kiểm tra tính
khả thi của lý thuyết bao gồm những vật cản giả lập và một robot Lego Mindstorm
NXT đƣợc điều khiển thông qua máy tính. Kết quả thực nghiệm cho thấy robot có
thể di chuyển từ một điểm đầu đến một điểm đích mong muốn theo đƣờng đi ngắn

nhất mà không va chạm vào các vật cản một cách chính xác. Đồng thời trong mô
hình kiểm thử một camera quan sát cũng đƣợc lắp đặt trên robot, ghi nhận lại những
hình ảnh quan sát trên đƣờng đi và gởi về cho máy tính điều khiển nhằm mục đích
phục vụ cho việc xử lý thông tin và phát triển luận sau này.

MỤC LỤC
Trang
Trang phụ bìa
Lời cảm ơn
Lời cam đoan
Tóm tắt luận văn
Mục lục
Danh mục các chữ cái viết tắt
Danh mục bảng
Danh mục hình
Lời mở đầu 1
Chƣơng 1: GIỚI THIỆU ĐỀ TÀI 2
1.1. Lý do thực hiện đề tài 2
1.2. Nội dung đề tài 4
1.3. Tóm lƣợc những kết quả đạt đƣợc 4
1.4. Cấu trúc luận văn 5
Chƣơng 2: CÁC CƠ SỞ LÝ THUYẾT LIÊN QUAN 7
2.1. Tổng quát bài toán hoạch định đƣờng đi (path planning) 7
2.1.1. Giới thiệu bài toán hoạch định đƣờng đi 7
2.1.2. Các vấn đề liên quan khi giải quyết bài toán path planning 8
2.1.2.1. Không gian làm việc (Work space): 8
2.1.2.2. Không gian cấu hình (Configuration space): 8
2.1.2.3. Không gian tự do (Free space): 9
2.1.2.4. Vật di chuyển: 9
2.1.2.5. Vật cản (Obstacle): 10

2.1.2.6. Đƣờng cong đƣợc rút ngắn nhất: 10
2.2. Tổng quan các giải pháp cho bài toán path planning theo visibility
graph 12
2.3 Tìm hiểu giải thuật tìm đƣờng đi ngắn nhất với độ phức tạp O(n
2
logn) 14
2.4. Tìm đƣờng đi ngắn nhất trong visibility graph với giải thuật Dijkstra 18
Chƣơng 3: TÌM HIỂU LEGO MINDSTORM NXT, MICROSOFT ROBOTICS
DEVELOP STUDIO 22
3.1. Tìm hiểu Lego Mindstorm NXT 22
3.1.1. Giới thiệu 22
3.1.2. Lego Mindstorm NXT 23
3.1.3. Intelligent Brick NXT 25
3.1.4. Bluetooth trên Brick NXT 26
3.1.5. Lập trình với Brick NXT 27
3.1.5.1. NXT-G 27
3.1.5.2. C# và Microsoft Robotics Developer Studio 28
3.1.5.3. Các ngôn ngữ khác 28
3.2. Microsoft robotics develop studio 29
3.2.1. Giới thiệu 29
3.2.2. Lập trình 31
3.2.3.1. Các ngôn ngữ lập trình dạng viết code 31
3.2.3.2. Visual Programming Language (VPL) 31
3.2.3. Các robot đƣợc hỗ trợ 31
Chƣơng 4: TÌM HIỂU BỘ ĐIỀU KHIỂN PID CHO ROBOT LEGO
MINDSTORMS 32
4.1. Giới thiệu tổng quan về Bộ điều khiển PID 32
4.2. Các khâu chức năng trong bộ điều khiển PID 35
4.2.1. Khâu tỷ lệ “P” trong PID 35
4.2.1.1. Các mức công suất từ P đến mô tơ thực tế 39

4.2.1.2. Code mã giải cho bộ điều khiển P 39
4.2.1.3. Tóm tắt bộ điều khiển tỷ lệ “P” 42
4.2.2. Khâu tích phân “I” trong bộ điều khiển PID 42
4.2.2.1. Tổng thời gian chạy của error. 43
4.2.2.2. Code mã giải cho bộ điều khiển PI 44
4.2.3. Khâu vi phân “D” của bộ điều khiển PID 45
4.2.3.1. Code mã giải cho bộ điều khiển PID 47
4.2.3.2. Điều chỉnh bộ điều khiển PID 48
4.3. Kết luận: 49
Chƣơng 5: HIỆN THỰC VÀ ĐÁNH GIÁ KẾT QUẢ 50
5.1. Mục tiêu kiểm tra 50
5.2. Tổng quan thiết kế 51
5.3. Hiện thực các hoạt động của xe tự hành trên mô hình thật 53
5.3.1. Chƣơng trình tính đƣờng đi ngắn nhất của xe tự hành: 53
5.3.2. Chƣơng trình điều khiển, giao tiếp với xe tự hành 55
5.3.3. Xây dựng chức năng bám đƣờng đi chính xác cho robot Lego
Mindstorm NXT dựa trên PID: 59
5.3.4. Xây dựng chức năng xác định vị trí hiện tại của xe tự hành 61
5.3.5. Xây dựng chức năng ghi hình ảnh trên đƣờng đi của xe tự hành 63
5.4. Kết luận 64
Chƣơng 6: KẾT QUẢ ĐẠT ĐƢỢC VÀ HƢỚNG PHÁT TRIỂN 65
6.1. Kết quả đạt đƣợc 65
6.2. Hƣớng phát triển đề tài 66
TÀI LIỆU THAM KHẢO
DANH MỤC CÁC CHỮ CÁI VIẾT TẮT
AI Artificial Intelligent
CCR Concurrency and Coordination Runtime
DOF Degrees Of Freedom
DSS Decentralized Software Services
MRDS Microsoft Robotics Develop Studio

PID Proportional Integral Derivative
SOA Service Oriented Architecture
VLSI Very Large Scale Integrated
VPL Visual Programming Language
VSE Visual Simulation Environment

DANH MỤC BẢNG
Bảng 3.1. Một số ngôn ngữ có thể lập trình với Lego Mindstorm NXT 28
Bảng 4.1. Code mã giải cho bộ điều khiển P 40
Bảng 4.2. Code mã giải cho bộ điều khiển PI 44
Bảng 4.3. Code mã giải cho bộ điều khiển PID 47
Bảng 4.4. Giá trị K’ của phƣơng pháp Ziegler-Nichols 49
Bảng 5.1. Chƣơng trình điều khiển Lego Mindstorm NXT bằng C# 57
Bảng 5.2. PID theo phƣơng pháp điều chỉnh Ziegler–Nichols 60

DANH MỤC HÌNH
Hình 2.1. Vật cản dạng đa giác lõm 10
Hình 2.2. Đƣờng cong đƣợc rút ngắn 10
Hình 2.3. Một đƣờng cong đƣợc rút ngắn bằng đƣờng cắt 11
Hình 2.4. G là tập hợp các đoạn đƣờng đơn giản. G
v
là tập các đƣờng kết nối
giữa mỗi đỉnh (visibility graph) 12
Hình 2.5. Visibility graph đƣợc tạo từ một tập các đa giác 13
Hình 2.6. Ví dụ một đƣờng đi ngắn nhất trong visibility graph 14
Hình 2.7. Ví dụ về các cạnh giao nhau và cây tìm kiếm tƣơng ứng 15
Hình 2.8. Điểm quét đƣợc bắt đầu với một nửa đƣờng thẳng p 15
Hình 2.9. Một số ví dụ trong đó r chứa nhiều đỉnh 16
Hình 3.1. RCX 23
Hình 3.2. NXT 23

Hình 3.3. Các dạng robot thông dụng của họ Lego Mindstorm NXT 24
Hình 3.4. Các cảm biến trong bộ Lego Mindstorm NXT 25
Hình 3.5. Khả năng kết nôi với Bluetooth của Lego Mindstorm NXT 26
Hình 3.6. Môi trƣờng lập trình trên NXT-G 27
Hình 4.1. Minh họa điểm đơn giản của robot với tất cả các chi tiết cần thiết 32
Hình 4.2. Cho thấy giá trị của ánh sáng màu trắng và màu đen 34
Hình 4.3. Phân chia ánh sáng thành ba phần 34
Hình 4.4. Minh họa ba mức độ của đƣờng đi 35
Hình 4.5. Minh họa cách biến đổi trục để cân bằng giá trị error 36
Hình 5.1. Tổng quan về mô hình kiểm thử với xe tự hành 52
Hình 5.2. Giao diện mô phỏng 2D môi trƣờng chƣa vật cản 54
Hình 5.3. Ví dụ một tập tin .gw trong thƣ viện LEDA hỗ trợ tạo đồ thị 54
Hình 5.4. Hình ảnh tìm đƣờng di ngắn nhất trong chƣơng trình demo 55
Hình 5.5. Kết quả kiểm tra thực tế 59
Hình 5.6. Quỹ đạo mong muốn và tập các đƣờng đi của robot 60
Hình 5.7. Giao diện màn hình chính của chƣơng trình CSTracker6 62
Hình 5.8. Ảnh thí nghiệm với robot Mindstorm NXT đƣợc chụp từ camera 62
Hình 5.9. Giao diện chƣơng trình camera 64
1
LỜI MỞ ĐẦU
Con ngƣời thƣờng có thói quen dự định một việc gì đó trƣớc khi làm và hầu
nhƣ con ngƣời biết cần những hành động nào để đạt đƣợc những dự định đó. Để
giúp máy tính làm việc nhƣ con ngƣời, nghĩa là biết những hành động nào có thể đi
đến mục tiêu, máy tính cần đƣợc cung cấp một lƣợng tri thức. Tri thức ở đây rất đa
dạng, máy tính “hiểu” đƣợc môi trƣờng xung quanh nó nhƣ thế nào và các thao tác
cần làm là việc rất khó khăn. Một máy tính có những trang thiết bị hiện đại nhất
hiện nay vẫn chƣa thể cảm nhận hết những thay đổi của môi trƣờng. Tuy nhiên, với
sự phát triển mạnh mẽ của công nghệ thông tin những năm gần đây, máy tính đã có
thể ghi nhận những tri thức liên quan và xử lý nó trong một số bài toán cụ thể đơn
giản.

Cùng với xu hƣớng này, vấn đề cụ thể mà luận văn hƣớng đến là xe tự hành và
máy tính; cung cấp cho những hệ thống này một lƣợng tri thức cần thiết về môi
trƣờng mà nó sẽ hoạt động trong đó cũng nhƣ sinh ra một tập các hành vi cần cho
một mục tiêu cụ thể nào đó. Trong luận văn này, xe tự hành di chuyển với mục tiêu
là xuất phát từ điểm đầu đến điểm đích nào đó cho trƣớc với khoảng cách ngắn nhất
và không va chạm vào các vật cản trên đƣờng đi. Một chƣơng trình máy tính sẽ
nhận lƣợng tri thức biết trƣớc về môi trƣờng, xử lý và sinh ra một tập các đƣờng đi
dự đoán trƣớc cho xe tự hành. Thêm vào đó, xe tự hành luôn đƣợc điều khiển để
đảm bảo di chuyển theo đúng tập đƣờng đã quy hoạch. Đề tài thực hiện trong luận
văn là những bƣớc đi đầu tiên trong việc hƣớng đến một hệ thống xe tự hành có đủ
khả năng tự vận động và di chuyển trong không gian phức tạp với khả năng thu
nhận tri thức cũng nhƣ xử lý nó.
Phẩn việc cụ thể trong luận văn này là tổng quan lại cơ sở lý thuyết của bài
toán quy hoạch đƣờng đi (path planning), cụ thể là phƣơng pháp dùng visibility
graph. Trong đó, luận văn đóng góp giải thuật xây dựng visibility graph với độ phức
tạp O(n
2
logn) và khối chức năng điều khiển xe tự hành di chuyển chính xác theo
đƣờng đi đã đƣợc hoạch định – dùng ý tƣởng từ bộ điều khiển PID (Proportional
Integral Derivative). Bên cạnh đó một mô hình kiểm thử bao gồm một sân giả lập
với nhiều vật cản và một robot Lego Mindstorm đƣợc xây dựng để kiểm tra tính khả
thi của giải thuật.
2
Chƣơng 1:
GIỚI THIỆU ĐỀ TÀI
1.1. Lý do thực hiện đề tài
Các xe tự hành không ngƣời lái rất hữu ích không chỉ trong các hoạt động
quân sự mà còn trong đời sống dân sự, giúp giảm nguy cơ và tiết kiệm năng lƣợng
cho con ngƣời một cách đáng kể. Phần quan trọng nhất trong xe tự hành không
ngƣời lái là làm thế nào để xe tự hành tự động tìm một đƣờng đi đến điểm đích một

cách chính xác [22] và việc tìm kiếm đƣờng đi đó đƣợc gọi là quy hoạch đƣờng
(path planning). Nói chung, mục tiêu chính của quy hoạch đƣờng là để tìm ra một
đƣờng đi tốt nhất thỏa một số điều kiện nhƣ: khoảng cách ngắn nhất, thời gian ngắn
nhất, tiêu thụ năng lƣợng ít nhất mà một đối tƣợng có thể di chuyển dọc theo đƣờng
đi từ một điểm khởi đầu đến điểm kết thúc trong không gian hai chiều hoặc nhiều
chiều, đồng thời phải tránh đƣợc một tập hợp các vật cản. Không gian có thể là tĩnh
(các vật cản cố định và đƣợc biết trƣớc) hoặc động (các vật cản không cố định hoặc
không đƣợc biết trƣớc). Quy hoạch đƣờng chủ yếu có ba phƣơng pháp tiếp cận: dựa
trên tìm kiếm (Search-based), dựa trên lấy mẫu (Sampling-based) và tìm đƣờng tổ
hợp (Combinatorial-planning) [19]. Đặc biệt với Combinatorial-planning thì việc
tìm đƣờng đi phải thông qua việc xây dựng một tập các đƣờng có thể đi (roadmap)
và một trong những roadmap phổ biến là
visibility graph
. Việc xây dựng một tập
các đƣờng có thể đi là nhiệm vụ chính trong toàn bộ quá trình. Tuy nhiên, với một
số ứng dụng thực tế, ví dụ nhƣ: môi trƣờng có nhiều vật cản thì giai đoạn này tốn
rất nhiều thời gian. Nhiều phƣơng pháp đã đƣợc đƣa vào để giảm độ phức tạp từ
O(n
3
) xuống O(n
2
logn). Tuy nhiên, kết quả O(nlogn) chỉ là một gợi ý lý thuyết [15].
Các tác giả của phƣơng pháp này cũng khuyến cáo rằng nó là khá khó khăn để đạt
đƣợc độ chính xác O(nlogn) trong khi thực hiện, bởi vì cấu trúc dữ liệu của
O(nlogn) quá phức tạp. Mặc dù tiêu tốn nhiều thời gian nhƣng phƣơng pháp tìm các
tập đƣờng có thể đi vẫn hữu ích trong các ứng dụng tìm một đƣờng đi đến điểm
đích cho xe di chuyển tự động một cách chính xác. Phƣơng pháp này không yêu cầu
thời gian thực hoặc trong môi trƣờng tĩnh với số lƣợng vật cản trung bình, bởi vì nó
đơn giản, trực quan và có thể thực hiện đƣợc. Ngoài ra, phƣơng pháp này có thể
3

đƣợc áp dụng trong nhiều lĩnh vực khác nhƣ CAD/CAM, xử lý hình ảnh, VLSI,
v.v…
Ở Việt Nam, trong những năm gần đây, một số đề tài cũng đã tìm hiểu và áp
dụng lý thuyết của bài toán path planning vào các thiết bị tự hành trong thực tế.
Luận văn cao học “Nghiên cứu và xây dựng các giải thuật tìm đƣờng đi ngắn nhất
tránh vật cản cho thiết bị không ngƣời lái” của Trần Thị Nhƣ Nguyệt, Trƣờng Đại
Học Bách Khoa TP. Hồ Chí Minh, năm 2011 đã tìm hiểu các giải thuật xây dựng
visibility graph của nhánh Combinatorial-planning với nhiều độ phức tạp khác
nhau, đồng thời đƣa ra một vài phƣơng pháp cải tiến cho quá trình xây dựng
visibility graph và hiện thực kết quả trên xe tự hành thật [3]. Tuy nhiên phần mềm
ứng dụng vẫn chƣa đầy đủ và chƣa có thể áp dụng đƣợc trong thực tế.
Thêm vào đó, có các đề tài khác nhƣ “Lập lộ trình chuyển động cho robot”
của Trần Duy Quang, đồ án môn học, Trƣờng Đại Học Khoa Học Tự Nhiên, năm
2011 [2]; Luận văn cao học “Một số phƣơng pháp chính xác lập lộ trình chuyển
động cho Robot” của Nguyễn Thị Thu Thủy, Trƣờng Đại Học Thái Nguyên, năm
2008 [1] và luận văn cử nhân “Nghiên cứu Planning để giải bài toán xác định lộ
trình” của Trần Thủy Tiên và Trần Hồng Thái, Trƣờng Đại Học Khoa Học Tự
Nhiên, năm 2003 [4] phần lớn tập trung vào tìm hiểu, nghiên cứu lý thuyết; chƣa
xây dựng các ứng dụng hỗ trợ một xe tự hành thật.
Từ những lý do trên, luận văn “Tìm hiểu và xây dựng phần mềm hỗ trợ bài
toán tìm đƣờng đi ngắn nhất tránh vật cản cho xe tự hành trong không gian 2D” này
sẽ tập trung tìm hiểu lý thuyết của bài toán tìm đƣờng theo hƣớng combinatorial-
planning, cụ thể là phƣơng pháp dùng visibility graph với độ phức tạp giải thuật là
0(n
2
logn). Và trên cơ sở kế thừa từ luận văn cao học [3], luận văn cũng sẽ xây dựng
thêm các khối chức năng cần thiết để hoàn chỉnh phần mềm ứng dụng của [3] và
hiện thực thêm khối chức năng bám đƣờng khi di chuyển giúp xe tự hành di chuyển
theo đƣờng đi định trƣớc một cách chính xác kết hợp với các giải thuật tìm đƣờng
có thể đƣa vào thực tế sử dụng dễ dàng. Thêm vào đó, luận văn cũng xây dựng thêm

khối chức năng truyền nhận hình ảnh giữa xe tự hành và máy tính điểu khiển nhằm
phục vụ cho việc xử lý thông tin sau này.
4
1.2. Nội dung đề tài
Các nội dung chính trong đề tài gồm:
- Tìm hiểu tổng quát bài toán path planning.
- Tìm hiểu tổng quan về các giải pháp cho bài toán tìm đƣờng trên mặt
phẳng, với các đặc tính cụ thể:
+ Vật cản đa giác lồi và lõm;
+ Vật cản tĩnh không di chuyển;
+ Đối tƣợng di chuyển trong không gian có kích thƣớc (hình tròn).
- Tìm hiểu các giải thuật liên quan đến visibility graph với độ phức tạp O(n
3
)
và O(n
2
logn).
- Ứng dụng giải thuật Dijkstra để xây dựng đƣờng đi ngắn nhất.
- Tìm hiểu Bộ điều khiển PID cho robot Lego Mindstorms.
- Tìm hiểu Lego Mindstorm NXT và Microsoft Robotics Develop Studio.
- Trên những cơ sở lý thuyết đó, đề tài hoàn chỉnh các khối chức năng cần
thiết để hoàn chỉnh phần mềm có thể áp dụng vào xe tự hành thật. Các khối
chức năng cụ thể dự kiến là:
+ Xây dựng chức năng điều khiển xe tự hành di chuyển chính xác theo
đƣờng đi ngắn nhất từ phần mềm.
+ Xây dựng chức năng truyền hình ảnh trên đƣờng đi về máy tính điều
khiển.
1.3. Tóm lƣợc những kết quả đạt đƣợc
Với những nội dung và phƣơng pháp thực hiện đƣợc liệt kê trên, sau thời gian
tìm hiểu và hiện thực, đề tài sẽ đạt đƣợc các kết quả:

 Về lý thuyết:
- Hệ thống hóa các lý thuyết về bài toán tìm đƣờng đi ngắn nhất trong đó
chú trọng phƣơng pháp dùng visibility graph.
- Hiểu cách giải bài toán path planning theo visibility graph với các độ
phức tạp O(n
3
) và O(n
2
logn).
- Sau khi visibility graph đƣợc tạo ra, sẽ dùng giải thuật Dijkstra để tính
đƣờng đi ngắn nhất từ điểm A tới điểm B nào đó.
- Tìm hiểu Lego Mindstorm NXT; Microsoft Robotics Develop Studio
5
- Bộ điều khiển PID cho robot Lego Mindstorms.
 Về mặt sản phẩm:
- Xây dựng một phần mềm giao diện trực quan chạy trên máy tính trong
đó có:
+ Khối chức năng điều khiển xe tự hành di chuyển chính xác theo đƣờng
đi ngắn nhất từ phần mềm.
+ Khối chức năng truyền hình ảnh trên đƣờng đi về máy tính điều khiển.
- Xe tự hành nhận lệnh và đƣờng đi ngắn nhất mà mình sẽ đi và di chuyển
theo đƣờng đi đó.
- Một xe tự hành thật và một sân trên đó có chứa các vật cản sẽ đƣợc xây
dựng để kiểm thử kết quả trong thực tế.
 Về mặt xe tự hành:
- Trên đƣờng di chuyển, xe tự hành sử dụng một camera quan sát, tín hiệu
quan sát đƣợc gởi trả về máy tính điều khiển.
- Xe tự hành có lắp thêm vòng tròn màu đỏ ở phía để xác định vị trí hiện
hành của xe.
- Xe tự hành đƣợc kết nối không dây bluetooth với máy tính để truyền dữ

liệu cho máy tính hay nhận các chỉ thị từ máy tính để chạy trên sân tránh
các vật cản từ điểm đầu đến điểm đích theo đƣờng đi ngắn nhất.
1.4. Cấu trúc luận văn
Các chƣơng còn lại của luận văn bao gồm:
Chƣơng 2: Các cơ sở lý thuyết liên quan.
2.1 Tổng quát bài toán path planning.
2.2. Tổng quan các giải pháp cho bài toán path planning theo visibility
graph.
2.3. Tìm hiểu giải thuật tìm đƣờng đi ngắn nhất với độ phức tạp
O(n
2
logn).
2.4. Giải thuật Dijkstra cho một đƣờng đi ngắn nhất.
Chƣơng 3: Lego Mindstorm NXT và Microsoft Robotics Develop Studio.
3.1. Tìm hiểu Lego Mindstorm NXT.
3.2. Microsoft Robotics Develop Studio.
6
Chƣơng 4: Tìm hiểu bộ điều khiển PID cho robot Lego Mindstorms.
4.1. Giới thiệu tổng quan về Bộ điều khiển PID Controller
4.2. Các khâu chức năng trong bộ điều khiển PID
4.3. Kết luận
Chƣơng 5: Hiện thực và đánh giá kết quả
5.1. Mục tiêu kiểm tra.
5.2. Tổng quan thiết kế.
5.3. Hiện thực.
5.3.1. Môi trƣờng và đối tƣợng cụ thể đƣợc sử dụng kiểm tra.
5.3.2. Hiện thực các hoạt động của xe tự hành trên mô hình thật.
5.3.2.1. Chƣơng trình tính đƣờng đi ngắn nhất của robot.
5.3.2.2. Xây dựng khối chức năng bám đƣờng đi chính xác cho
robot Lego Mindstorm NXT dựa trên PID.

5.3.2.3. Chƣơng trình điều khiển, giao tiếp với xe tự hành.
5.3.2.4. Xây dựng chức năng xác định vị trí hiện tại của xe tự
hành.
5.4. Kết luận.
Chƣơng 6: Kết luận và hƣớng phát triển.
7
Chƣơng 2:
CÁC CƠ SỞ LÝ THUYẾT LIÊN QUAN
2.1. Tổng quát bài toán hoạch định đƣờng đi (path planning)
2.1.1. Giới thiệu bài toán hoạch định đƣờng đi
Path planning đôi khi đƣợc gọi là “Motion planning” là hành động của việc
tìm kiếm một đƣờng đi để đi từ vị trí A đến vị trí B. Path planning là một bài toán
tìm đƣờng đi đã đƣợc áp dụng nhiều trong các lĩnh vực nhƣ sinh tin học,
CAD/CAM, layout, xử lý ảnh,… và đặc biệt rất nhiều trong lĩnh vực robot. Bài toán
đặt ra là phải tìm ra đƣờng đi tốt nhất theo một tiêu chuẩn tối ƣu nào đó, sao cho
một đối tƣợng có thể di chuyển dọc theo đƣờng này từ điểm bắt đầu đến điểm kết
thúc và không va chạm vào các vật cản trên đƣờng đi trong môi trƣờng hai hoặc
nhiều chiều. Các vật cản, điểm xuất phát và điểm đích là những đối tƣợng có kích
thƣớc và hình dạng khác nhau và motion planning là một trong những nhiệm vụ
quan trọng trong việc kiểm soát thông minh của một robot di chuyển tự động. Nó
thƣờng đƣợc phân tách ra thành path planning và trajectory planning. Path planning
là dùng để tạo ra một đƣờng đi tự do có sự va chạm trong một môi trƣờng với các
vật cản và tối ƣu hóa nó thông qua một số tiêu chí [20]. Trajectory planning là lịch
trình di chuyển của một robot, giúp robot di chuyển dọc theo đƣờng đi đã đƣợc lập
kế hoạch trƣớc.
Có 2 phƣơng pháp đã đƣợc đề xuất để giải quyết các vấn đề qui hoạch động
của một robot di động. Phƣơng pháp thứ nhất là Off-line planning là một địa hình
mà khi robot di chuyển trong đó đƣợc xem nhƣ tĩnh (điểm xuất phát và đích đến là
biết trƣớc và cố định, kích thƣớc các vật cản không thay đổi, đƣợc cố định và biết
trƣớc) hay còn gọi là tìm đƣờng tĩnh toàn cục. Phƣơng pháp thứ hai là On-line

planning, tập trung vào việc giải quyết những sự không chắc chắn khi robot di
chuyển trong môi trƣờng động hay còn gọi là tìm đƣờng động cục bộ. Trong một số
tài liệu, On-line path planning cũng đƣợc nghiên cứu với tên gọi navigation
problem.
8
2.1.2. Các vấn đề liên quan khi giải quyết bài toán path planning
Giữa path planning và path mapping (bản đồ đƣờng đi) thì không nên nhằm
lẫn bởi vì path mapping tác động đến đặc tính của môi trƣờng đƣờng đi mà robot đi
qua và có thể giúp nó xác định đƣợc vị trí cần đi đến. Về mặt nghiên cứu, path
planning đƣợc phân biệt dựa trên việc tính toán đƣờng đi thông qua biểu hiện bởi
tham số thời gian. Do các robot khi di chuyển có những ảnh hƣởng động học riêng
đến từng loại robot, nên đƣờng đi (path) phải đƣợc tính toán và kết hợp với nhiều
yếu tố khác để giải quyết bài toán path planning:
2.1.2.1. Không gian làm việc (Work space):
Không gian làm việc (work space) là một mặt phẳng hai chiều gồm một
robot  đƣợc xem nhƣ một điểm di động và một tập S = {P
1
,… , P
t
} của các vật
cản đa giác đƣợc đặt ở những vị trí cố định trƣớc. Việc xác định vị trí hoặc hình
dạng của robot có thể đƣợc xác định bằng vector dịch chuyển. Robot di chuyển
đƣợc thông qua một vector (x, y) bằng R(x, y). Vị trí của robot đƣợc xác định bởi
các tham số tƣơng ứng với số bậc tự do (gọi là DOF - Degrees Of Freedom) của
robot. Khi robot di chuyển trên mặt phẳng không gian 2 chiều thì có bậc là 2,
nếu số bậc tự do là 3 thì robot có thể di chuyển và quay trên mặt phẳng không
gian 3 chiều. Tham số để robot di chuyển trong không gian 3 chiều thì cao hơn.
Nhƣng do thời gian và điều kiện nên trong luận văn này chỉ đề cập đến robot di
chuyển trong mặt phẳng không gian 2 chiều.
2.1.2.2. Không gian cấu hình (Configuration space):

Không gian mà các tham số của robot  có thể di chuyển đƣợc gọi là
không gian cấu hình (configuration space). Khái niệm này đƣợc giới thiệu bởi
Lozano-Perez [23]. Không gian cấu hình đƣợc ký hiệu là C(). Điểm p trong
không gian cấu hình thì tƣơng ứng với một vị trí nhất định (p) của robot trong
không gian làm việc. Hình dạng của robot trên mặt phẳng cũng đƣợc xác định
đến không gian làm việc. Tuy nhiên, nó đƣợc dùng để phân biệt hai khái niệm:
không gian làm việc và không gian cấu hình. Không gian làm việc là không gian
mà trong đó các robot có thể xoay tròn và di chuyển đƣợc trong phạm vi đã đƣợc
xác định trƣớc; Không gian cấu hình là không gian chứa các tham số của robot.
Robot đƣợc xem là điểm hình tròn trong không gian làm việc, tƣơng ứng bởi
9
một điểm trong không gian cấu hình và bất kỳ điểm nào trong không gian cấu
hình đều tƣơng ứng với một số vị trí của robot trong không gian làm việc.
Nhƣ vậy, không phải tất cả vị trí của robot trong không gian làm việc thì
đều có thể đi đƣợc (nơi mà các robot giao nhau với một trong những vật cản
trong tập S). Do đó, điểm tƣơng ứng trong không gian cấu hình không thể xảy ra.
Phần của không gian cấu hình bao gồm những điểm này đƣợc gọi là không gian
cấu hình vùng cấm hay không gian vùng cấm. Không gian vùng cấm đƣợc ký
hiệu là C
forb
(R, S). Phần còn lại của không gian cấu hình, bao gồm những điểm
tƣơng ứng với những vị trí tự do nơi mà các robot không cắt bất kỳ vật cản nào
thì đƣợc gọi là không gian cấu hình tự do hay không gian tự do và nó đƣợc ký
hiệu là C
free
(R,S). Những vị trí của robot đƣợc ánh xạ đến những điểm trong
không gian cấu hình, tƣơng tự nhƣ một vật cản P trong không gian làm việc
đƣợc ánh xạ tới một tập các điểm p trong không gian cấu hình sao cho (p) cắt
nhau với P. Tập kết quả đó đƣợc gọi là không gian cấu hình vật cản hay C-
obstacle của P.

2.1.2.3. Không gian tự do (Free space):
Không gian dùng để thiết lập các cấu hình tránh va chạm với những vật cản
đƣợc gọi là C
free
hay còn gọi không gian tự do. Phần bù của C
free
trong C đƣợc
gọi là vật cản hoặc khu vực vùng cấm.
Thông thƣờng, khu vực vùng cắm là rất khó để tính toán một cách rõ ràng
hình dạng của C
free
. Tuy nhiên, việc thử nghiệm có đƣợc hay không thì hình
dạng đã cho trong C
free
cũng là hiệu quả. Trƣớc tiên, sự di chuyển về phía trƣớc
của robot sẽ xác định vị trí hình học của robot, và thử nghiệm sẽ cho thấy sự có
sự va chạm hay không nếu hình dạng của robot va chạm với hình dạng của môi
trƣờng.
2.1.2.4. Vật di chuyển:
Cần phải xét đến hình dạng của vật di chuyển trong bài toán đƣợc thể hiện
nhƣ thế nào khi nó di chuyển từ điểm xuất phát đến điểm đích, vì nó có ảnh
hƣởng rất lớn đến quá trình tính toán trong quá trình xây dựng một đƣờng đi tốt
nhất. Ngoài ra cũng phải xét thêm vật di chuyển có phải là chất điểm hay có kích
thƣớc hoặc có bao nhiêu bậc tự do, Trong luận văn này, vật di chuyển đƣợc
10
xem là một đối tƣợng di chuyển trong không gian có kích thƣớc (hình tròn).
2.1.2.5. Vật cản (Obstacle):
Các đa giác lồi hoặc đa giác lõm và các điểm đầu là S các điểm cuối là D
thuộc mặt phẳng R(2). Các điểm S và D không nằm trong phần lõm của đa giác
lõm nhƣ Hình (2.1). Đa giác lồi là đa giác nằm trên nữa mặt phẳng đƣợc tạo bởi

một cạnh bất kỳ của đa giác đó. Đối với đa giác lõm sẽ quy về đa giác lồi.

Hình 2.1 - Vật cản dạng đa giác lõm [9]
Khi xây dựng bài toán tìm đƣờng đi thì hình dạng của các vật cản cũng
đƣợc quan tâm đến, nghĩa là hình dạng vật cản có đƣợc chỉ rõ trƣớc trong input
hay không. Ví dụ, vật cản có là đa giác không, nếu là đa giác thì là đa giác lồi
hay lõm.
2.1.2.6. Đường cong được rút ngắn nhất:
Mục
tiêu chính là tìm đƣờng đi ngắn nhất tránh va chạm vào vật cản từ
điểm bắt đầu (P
start
) đến điểm kết thúc (P
goal
) của Robot R. Chú ý rằng chỉ
cần có một đƣờng đi ngắn nhất trong tất cả các đƣờng đi đƣợc xem là ngắn.
Để có đƣợc một đƣờng đi ngắn nhất tồn tại, điều quan trọng cần lƣu ý đến
các vật cản là các tập mở. Điều này có nghĩa là các robot đƣợc phép di
chuyển men theo các vật cản.

Hình 2.2 - Đường cong được rút ngắn [16]
11
Nhìn vào cấu trúc của đƣờng đi, xét các đƣờng cong trong Hình 2.2, dễ
dàng thấy rằng đƣờng đi này có thể đƣợc rút ngắn theo đƣờng gạch nối. Có
hai đặc điểm của đƣờng đi rút ngắn cần lƣu ý:
Đặc điểm thứ
nhất, đƣờng đi rút ngắn bao gồm các đoạn đƣờng thẳng
mà không gồm các đoạn đƣờng cong. Đặc điểm này là do phần cong của
đƣờng đi có thể đƣợc thay thế bằng một đoạn thẳng mà đoạn thẳng vẫn tồn
tại trong không gian tự do. Nếu vật cản là các đa giác và đƣờng đi là đƣờng

cong thì có một điểm p trên vị trí đƣờng cong của đƣờng đi đó. Nhƣ vậy, vị
trí trung tâm của đƣờng cong tập trung tại điểm p và hoàn toàn chứa trong
không gian tự do. Khi đó, một phần của đƣờng đi sẽ chứa bên trong vòng
tròn và có thể đƣợc rút ngắn bằng cách thay thế nó bằng một đoạn nối hai
điểm trên đƣờng biên của vòng tròn. Ý tƣởng này đƣợc minh họa trong
Hình 2.3.

Hình 2.3 - Một đường cong được rút ngắn bằng đường cắt [16]
Đặc điểm thứ hai, đƣờng đi chứa các đỉnh của những vật cản. Nguyên
lý này cũng tƣơng tự nhƣ đặc điểm thứ nhất. Nếu có một đỉnh v trên đƣờng
đi thì sẽ tạo ra một vòng tròn tâm tại v và hoàn toàn nằm trong không gian
tự do, đƣờng đi có thể đƣợc rút ngắn bằng cách thay thế một phần của
đƣờng đi đƣợc thực hiện lần lƣợt tại v bởi một đoạn đƣờng đi ngắn hơn. Rõ
ràng đây sẽ là trƣờng hợp, khi v là một đỉnh vật cản và ranh giới những
đƣờng biên của các vật cản. Đặc điểm này đƣợc thể hiện theo một cách
chính xác trong Bổ đề 2.1 sau đây.
Bổ đề 2.1
: Bất kỳ đường đi ngắn nhất giữa P
start
và P
goal
trong một tập
S là các vật cản đa giác rời nhau thì sẽ có một đường đi đa giác mà bên
trong là các đỉnh của S [10].
12
Để tìm đƣợc một đƣờng đi ngắn nhất, thông qua việc xây dựng từ tập
các đƣờng có thể đi (roadmap) và một trong những roadmap phổ biến là
visibility graph.
2.2. Tổng quan các giải pháp cho bài toán path planning theo visibility graph
Một đồ thị tầm nhìn, hay còn gọi là một visibility graph có hình ảnh nhƣ trong

hình 2.4 và 2.5.
Việc xây dựng một visibility graph có thể hiểu nhƣ một tiến trình mà dữ liệu
đầu vào là là đồ thị G đƣợc mô tả bằng một tập hợp các vật cản trong một không
gian đa chiều, và dữ liệu đầu ra là đồ thị G
v
mô tả khả năng hiển thị giữa mỗi đỉnh
và mọi đỉnh khác trong G. Visibility đƣợc xác định nhƣ là một đƣờng thẳng có thể
đi giữa hai đỉnh mà không cắt bất kỳ cạnh khác trong đồ thị G. Bằng cách này, hai
đỉnh nhìn thấy nhau đƣợc cho là không bị cản trở bởi bất kỳ vật cản nào, và một
đƣờng thẳng đƣợc vẽ giữa chúng và đƣa vào đồ thị G
v
.
Một ví dụ về visibility graph trong không gian mặt phẳng 2 chiều cho tập các
đoạn thẳng đƣợc thể hiện trong hình 2.4 và visibility graph cho tập các đa giác đƣợc
thể hiện trong hình 2.5


G G
v

Hình 2.4 - G là tập hợp các đoạn đường đơn giản và Gv là tập các đường kết nối giữa
mỗi đỉnh (visibility graph) [9]
Tập các vật cản trong đồ thị G có thể bao gồm nhiều kiểu hình dạng khác nhau
nhƣ: thẳng, tròn, đoạn thẳng, đa giác lồi, đa giác lõm hoặc thông thƣờng nhất là các
đa giác đơn lẻ. Đã có nhiều thuật toán khác nhau đƣợc phát triển dựa trên giả thuyết
mà trong đó các kiểu hình dạng của vật cản đƣợc cho phép. Nhƣng trong phần này
chỉ tập trung vào các đa giác lồi lõm đơn lẻ mà không cắt nhau.
13

Hình 2.5 - Visibility graph được tạo từ một tập các đa giác

Ký hiệu tập các vật cản là S, các nút của visibility graph là các đỉnh của S và
các cạnh của visibility graph là một cung giữa hai nút v và w nếu nó nhìn thấy
mọi đỉnh khác. Nghĩa là, nếu đoạn





hoặc là biên của vật cản hoặc là không cắt
vật cản trong S sẽ trở thành một cạnh của visibility graph. Nói cách khác, hai đỉnh
mà nhìn thấy mọi đỉnh khác thì gọi là visible, và đoạn nối chúng là một cạnh tầm
nhìn (visibility edge). Lƣu ý, hai điểm trên cùng một cạnh của đa giác thì luôn
luôn thấy mọi đỉnh khác.
Nếu bài toán thêm vào hai đỉnh P
start
và P
goal
cho mục đích xác định một
đƣờng tối ƣu nào đó giữa đỉnh này,

P
start
và P
goal
sẽ đƣợc đƣa vào tập đỉnh trong
visibility graph và các cạnh visibility edge giữa hai đỉnh này và và các đỉnh khác
cũng đƣợc thiết lập.
Để tính toán một đƣờng đi ngắn nhất từ P
start
đến P

goal
, có thể áp dụng giải
thuật sau:
ShortestPath
(S, P
start
, P
goal
)
Đầu vào: một tập S của các đa giác rời nhau, 2 điểm P
start
và P
goal
trong
không gian tự do.
Đầu ra: Đường đi ngắn nhất mà không va chạm với đường nối giữa hai
điểm P
start
và P
goal
.

1. G
vis

VisibilityGraph









2. Gán mỗi cung (v,w) trong G
vis
một trọng số, là chiều dài của đoạn






3. Sử dụng giải thuật Dijkstra để tính đường đi ngắn nhất giữa điểm







14
Nhƣ vậy để giải quyết bài toán path planning sử dụng visibility graph có 2 vấn
đề cần giải quyết:
(1) Tạo ra một visibility graph.
(2) Dùng giải thuật Dijkstra để tìm đƣờng đi ngắn nhất của robot trên
visibility graph đã tạo ra (có thể sử dụng các giải thuật tìm đƣờng đi ngắn
nhất trên đồ thị có trọng số khác nhƣ: Bellman-Ford, A* search, Floyd-
Warshall)

Hình 2.6 - Ví dụ một đường đi ngắn nhất trong visibility graph [16]
2.3 Tìm hiểu giải thuật tìm đƣờng đi ngắn nhất với độ phức tạp O(n
2
logn)
Nhƣ trình bày trong phần 2.2, để tính visibility graph của S, cần phải tìm tất
cả các cặp đỉnh khác mà chúng có thể thấy nhau đƣợc. Thực hiện bằng cách quan
sát những đoạn thẳng nối giữa cặp đỉnh để kiểm tra xem chúng có cắt nhau với
bất kỳ vật cản nào không, cách tiếp cận này dẫn đến thời gian chạy của giải thuật
O(n
3
). Phần này trình bày giải thuật giúp giảm thời gian xây dựng một visibility
graph xuống còn
O(n
2
logn).
Quá trình xây dựng một visibility graph đƣợc trình bày gồm các bƣớc:
VisibilityGraph(
S
)
Đầu vào : một tập hợp S là các vật cản đa giác rời nhau
Đầu ra : Đồ thị tầm nhìn G
vis
(S)

Khởi tạo một đồ thị G = (V, E) trong đó V là tập các đỉnh của đa giác
trong tập S và E = 0
for
mỗi đỉnh v

V

do
W


VisibleVertices
(v, S)
cho mỗi w

W, thêm vòng cung (v, w) to E
return
G
Việc xác định hai đỉnh có “nhìn thấy” (visible) với nhau hay không đƣợc