Nâng cao độ chính xác định vị robot bằng phương pháp tổng hợp dữ liệu cảm biến lập mã quang với bộ lọc Kalman mở rộng

Đ
ẠI
HỌC QUỐC GIA HÀ
NỘI
TRƯỜNG ĐẠI HỌC CÔNG
NGHỆ
NGUY
ỄN XUÂN HẢI
NÂNG CAO Đ
Ộ CHÍNH XÁC ĐỊNH VỊ ROBOT
B
ẰNG PHƯƠNG PHÁP TỔNG HỢP DỮ LIỆU
C
ẢM BIẾN LẬP MÃ QUANG VỚI BỘ LỌC
KALMAN M
Ở RỘNG
LUẬN VĂN TH
Ạ
C
SĨ CÔNG NGH
Ệ ĐIỆN TỬ
- VI
ỄN THÔNG
Hà Nội -
2014
Đ
ẠI
HỌC QUỐC GIA HÀ
NỘI
TRƯỜNG ĐẠI HỌC CÔNG
NGHỆ

NGUYỄN XUÂN HẢI
NÂNG CAO Đ
Ộ CHÍNH XÁC ĐỊNH VỊ ROBOT BẰNG
PHƯƠNG PHÁP TỔNG HỢP DỮ LIỆU CẢM BIẾN LẬP
MÃ QUANG V
ỚI BỘ
L
ỌCKALMAN MỞ RỘNG
Ngành: Công nghệ Điện tử - Viễn thông
Chuyên ngành: Kỹ thuật Điện tử
Mã số: 60520203
LUẬN VĂN TH
Ạ
C
SĨ CÔNG NGH
Ệ ĐIỆN TỬ
- VI
ỄN THÔNG
NGƯ
ỜI HƯỚNG DẪN KHOA HỌC: PGS.TS TRẦN QUANG VINH
Hà Nội -
2014
1
LỜI CAM ĐOAN
Trong thời gian thực hiện luận văn, tôi có tham khảo một số tài liệu từ nhiều
nguồn khác nhau như đ
ã li
ệt kê trong phần tài liệu tham khảo. Những phần tham khảo
này đều đ
ã đư

ợc đánh dấu cụ thể trong luận văn. Các phần còn lại, tôi xin cam đoan là
do tôi tự tìm hiểu để viết ra. Nếu có bất kỳ sự sai xót nào, tôi xin chịu hoàn toàn trách
nhiệm.
Hà Nội, ngày 02 tháng 06 năm 2014
Nguyễn Xuân Hải
2
LỜI CẢM ƠN
Để hoàn thành Luận văn Thạc s
ĩ c
ủa mình, em xin gửi lời cảm ơn chân thành
tới các thầy cô trường Đại học Công Nghệ - Đại học Quốc Gia Hà Nội đ
ã truy
ền đạt
những kiến thức quý báu cho em trong suốt quá trình học tập và hoàn thành Luận văn
Thạc s
ĩ.
Em xin gửi lời cảm ơn tới PGS.TS Trần Quang Vinh, người trực tiếp chỉ bảo,
hướng dẫn em trong suốt quá trình nghiên cứu và hoàn thành Luận văn Thạc s
ĩ.
Cuối cùng, tôi xin chân thành cảm ơn gia đ
ình, b
ạn bè và đồng nghiệp đ
ã
đ
ộng
viên, khuyến khích tôi trong suốt quá trình học tập và nghiên cứu.
Xin chân thành cảm ơn.
Hà Nội, ngày 02 tháng 06 năm 2014
Nguyễn Xuân Hải
3

MỤC LỤC
LỜI CAM ĐOAN 1
LỜI CẢM ƠN 2
DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ 5
MỞ ĐẦU 6
CHƯƠNG 1: TỔNG QUAN LÝ THUYẾT VỀ ROBOT DI ĐỘNG 8
1.1. Robot thông minh 8
1.2. Robot di động thông minh 9
CHƯƠNG 2: PHƯƠNG PHÁP TỔNG HỢP CẢM BIẾN VỚI BỘ LỌC KALMAN 12
2.1. Phương pháp tổng hợp cảm biến dùng cho robot di động 12
2.2. Phương pháp tổng hợp cảm biến với bộ lọc Kalman 13
CHƯƠNG 3: MỘT SỐ CẢM BIẾN HIỆN ĐẠI VÀ PHƯƠNG PHÁP ĐỊNH VỊ
ROBOT DI ĐỘNG 20
3.1. Phân loại các 20
3.1.1. Cảm biến lập mã trục xoay (cảm biến lập mã quang) 20
3.1.2. Cảm biến chỉ hướng (heading sensor) 21
3.1.3. Cảm biến siêu âm đo khoảng cách đến vật thể gần 22
3.1.4. Cảm biến đo xa laser (Laser Range Finder - LRF) 23
3.1.5. Cảm biến ảnh và thị giác máy 24
3.2. Một số phương pháp định vị cho robot di động 25
3.2.1. Phương pháp dead-reckoning hay odometry 25
3.2.2. Hệ thống dẫn đường cột mốc chủ động 25
3.2.3. Hệ thống dẫn đường toàn cầu GPS 27
3.2.4. Dẫn đường bằng vật mốc 27
3.2.5. Định vị dựa trên bản đồ 28
CHƯƠNG 4: NÂNG CAO ĐỘ CHÍNH XÁC ƯỚC LƯỢNG VỊ TRÍ CHO ROBOT
BẰNG PHƯƠNG PHÁP TỔNG HỢP CẢM BIẾN 32
4.1. Xây dựng mô hình
đ
ộng học của robot 32

4.2. Tính toán các ma trận trong vòng lặp đệ quy Kalman 34
CHƯƠNG 5: THỰC NGHIỆM VÀ THẢO LUẬN 37
4
5.1. Thiết lập thực nghiệm 37
5.2. Kết quả thực nghiệm áp dụng bộ lọc Kalman cho tổng hợp dữ liệu của một
cảm biến lập mã quang 38
5.3. Tổng hợp cảm biến từ phép đo odometry với cảm biến chỉ hướng 40
5.3.1. Cập nhật các tham số cho các ma trận. 40
5.3.2. Hiệu chỉnh chính xác góc quay qua bộ lọc Kalman 41
KẾT LUẬN 43
DANH MỤC TÀI LIỆU THAM KHẢO 44
5
DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ
Hình 1.1. Các bộ phận của một robot 8
Hình 1.2. Các khối cơ bản trong robot di động 10
Hình 2.1. S
ơ đ
ồ thuật toán bộ lọc Kalman tuyến tính rời rạc 15
Hình 2.2. Sơ đồ thuật toán bộ lọc Kalman mở rộng 18
Hình 3.1.
Đĩa có khe ch
ắn sáng và tín hiệu ra bộ lập mã quang 21
Hình 3.2a. Mô-đun cảm biến từ-địa bàn CMPS03 22
Hình 3.2b. Nguyên lý hoạt động của cảm biến từ-địa bàn. 22
Hình 3.3a. Các cảm biến siêu âm 23
Hình 3.3b. Nguyên lý hoạt động của cảm biến siêu âm 23
Hình 3.4. Một số loại máy quét laser của công ty SICK 23
Hình 3.5. Thời gian bay của tia laser 24
Hình 3.6. Phép
đo d

ựa trên 3 góc. 26
Hình 3.7. Các thành phần của hệ thống GPS 27
Hình 3.8. Ph
ương pháp xác đ
ịnh vị trí dựa vào bản đồ 29
Hình 3.9. Mô hình robot của Schiele và Crowley vị trí đặt tại hành lang 30
Hình 4.1. Thế (tọa độ và hướng) và các tham số của robot 32
Hình 5.1.
Đư
ờng đi thực của robot (blue line) cùng các đường đi nhận được chỉ qua
phép đo (green) và đường ước tính khi sử dụng thêm EKF (brown) 38
Hình 5.2. Các độ lệch của đường đi nhận được qua phép đo và đường ước tính khi sử
dụng EKF so với đường đi thực của robot 40
Hình 5.3.
Đ
ộ lệch theo các phương x, y,
θ gi
ữa vị trí ước lượng và vị trí thực 39
Hình 5.4. Hiệu quả EKF trong hai trường hợp điều khiển robot đi theo đúng dạng
đường mong muốn khi có và không có EKF. 41
Hình 5.5. Quỹ đạo ước tính của robot với các cấu hình EKF khác nhau 42
6
MỞ ĐẦU
Cùng với sự phát triển mạnh mẽ của khoa học kỹ thuật ngày nay, ngành
robotics đóng vai tr
ò r
ất quan trọng, tiên phong và làm nền tảng cho các l
ĩnh v
ực khoa
học công nghệ và tự động hoá khác.

Nhiều ứng dụng của các ngành nghề, l
ĩnh v
ực khác nhau đ
òi h
ỏi cần phải trang
bị thiết bị hoàn toàn tự động làm việc theo chương tr
ình hay do con ngư
ời điều khiển
như robot.
Robot công nghiệp, thường là các tay máy (arm-robot) trong các hệ thống sản
xuất tự động. Robot trong các ứng dụng này được đặt tại một vị trí cố định, làm việc
trong môi trường nhất định biết trước.
Thời gian gần đây, bắt đầu bằng các xe tự hành trong v
ũ tr
ụ, xuất hiện một loại
robot khác là robot thông minh. Một loại trong số đó là các robot di động hoạt động tự
trị (autonomous). Các robot di dộng với khả năng giám sát môi trường xung quanh để
đưa ra các quyết định cần thiết trong quá trình hoạt động thực hiện công việc được
giao, cụ thể là tự động đi tới đích chính xác và an toàn. Trong hệ thống này, robot thu
nhận thông tin qua các cảm biến, xử lý dữ liệu thu được và tự đưa ra các quyết định.
Quá trình dẫn đường của các robot di động thông minh thường được phân tích
thành 4 giai đoạn: cảm nhận, định vị, vạch đường đi và điều khiển chuyển động .
Giai đoạn định vị là rất quan trọng, nó cho phép trả lời câu hỏi: robot đang ở
đâu? để từ đó có thể vạch ra các quyết định dẫn đường. Để có được câu trả lời này, bộ
phận điều khiển robot cần phải có được các dữ liệu môi trường (qua các cảm biến
ngoại) hay dữ liệu về bản thân robot (qua các cảm biến nội) trong giai đoạn cảm nhận.
Vì hệ thống robot chứa đựng nhiều yếu tố ngẫu nhiên có thể gây nên các sai số
c
ũng như không th
ể có loại cảm biến nào đủ hoàn hảo để có thể chỉ dùng nó là đủ để

thực hiện định vị và lập bản đồ một cách chính xác. Lý do đó tạo nên một số phương
pháp tăng độ chính xác cho quá trình
đ
ịnh vị. Một trong số đó là phương pháp tổng
hợp dữ liệu cảm biến (sensor data fusion) để nâng cao độ chính xác định vị robot.
Vì l
ĩnh v
ực tổng hợp cảm biến rất rộng, bản luận văn này chỉ hạn chế nghiên
cứu phương pháp tổng hợp dữ liệu cảm biến với bộ lọc Kalman mở rộng để nâng cao
độ chính xác định vị cho robot di động.
Ngoài phần mở đầu và kết luận, bản luận văn có nội dung chính như sau:
- Chương 1: Tr
ình bày các nét cơ b
ản về robot di động
7
- Chương 2: Tổng quan về phương pháp tổng hợp cảm biến và bộ lọc Kalman
- Chương 3: Một số cảm biến hiện đại và phương pháp định vị robot di động
- Chương 4: Nâng cao độ chính xác ước lượng vị trí robot bằng tổng hợp cảm
biến với bộ lọc Kalman mở rộng
- Chương 5: Kết quả thực nghiệm và thảo luận
8
CHƯƠNG 1: TỔNG QUAN LÝ THUYẾT VỀ ROBOT DI ĐỘNG
1.1. Robot thông minh
Do nhu cầu của con người ngày càng đ
òi h
ỏi ứng dụng rộng rãi các ph
ương ti
ện
tự động, xu hướng tạo ra những thiết bị tự động có tính linh hoạt cao đang h
ình thành.

Các thiết bị này đang thay thế dần các máy tự động cứng chỉ đáp ứng một việc nhất
định. Điều này dẫn đến nhu cầu ứng dụng robot để tạo ra các hệ thống tự động linh
hoạt.
Thuật ngữ robot được sinh ra từ trên sân khấu, không phải trong phân xưởng
sản xuất. Những robot xuất hiện lần đầu tiên trên ở NewYork vào ngày 09/10/1922
trong vở “Rossum’s Universal Robot” của nhà soạn kịch người Tiệp Karen Kapek viết
năm 1921, c
òn t
ừ “robot” là cách gọi tắt của từ “robota” - theo tiếng Tiệp có ngh
ĩa là
công việc lao dịch.
Những robot thực sự có ích được nghiên cứu để đưa vào những ứng dụng trong
công nghiệp, y học, đời sống, quân sự, không gian…
Từ những năm 80, nhất là vào những năm 90, do áp dụng rộng rãi các tiến bộ
kỹ thuật về vi xử lý và công nghệ thông tin, số lượng robot đ
ã gia tăng v
ới nhiều tính
năng vượt bậc. Chính vì vậy mà robot đ
ã có m
ột vị trí quan trọng trong những ứng
dụng công nghiệp, y học, đời sống, quân sự, không gian…
Các bộ phận cơ bản của một robot có thể được mô tả như trên h
ình 1.1
gồm: bộ
phận cảm nhận, bộ điều khiển và bộ chấp hành.
Hình 1.1. Các bộ phận củ a một robot.
Bộ phận cảm nhận có nhiệm vụ thu thập thông tin từ môi trường dựa trên số
liệu các cảm biến đặt trên robot. Môi trường có thể là biết trước hoặc không biết trước.
9
Bộ điều khiển có nhiệm vụ xử lý các thông tin thu được từ bộ cảm nhận. Bộ

phận điều khiển thường là các máy tính gồm các bộ xử lý và bộ nhớ cùng các giao
diện ghép nối vào/ra.
Bộ chấp hành thực hiện các lệnh từ bộ điều khiển để tác động lại môi trường.
Robot thông minh là một hệ thống có thể thực hiện những thao tác, hành vi độc
lập trên cơ sở các biến môi trường thu được mà không theo sự sắp đặt cụ thể nào
trước. Mức độ thông minh có thể từ đơn giản đến phức tạp. Những robot nhận dạng
vật thể, lựa chọn sản phẩm theo một đặc tính nào đó, những robot tránh vật cản, nhận
dạng giọng nói… là những robot thông minh.
Nếu chương tr
ình đi
ều khiển được nạp toàn bộ từ máy tính PC vào bộ nhớ của
robot, được lưu trữ theo kiểu cố định để cho phép một khi khởi động, robot có thể hoạt
động độc lập không cần người điều khiển thì
đó là
robot hoạt động kiểu tự quản trị.
1.2. Robot di động thông minh
Luận văn này tập trung nghiên cứu tìm hiểu một trong các loại robot là loại
robot di động thông minh, còn gọi là robot di động hoạt động tự quản trị.
Có một số sự khác biệt quan trọng giữa các yêu cầu của việc lắp đặt robot cố
định truyền thống với các yêu cầu của một hệ thống robot di động. Một trong những
mối quan tâm hàng đầu là sự không biết trước môi trường vận hành của robot di động.
Đối với các hệ thống robot cố định, người ta thường xây dựng (thiết kế) một không
gian làm việc nhất định để thực hiện công việc và robot cố định thường thực hiện các
công việc lặp đi lặp lại trong môi trường xác định trước. Đối với các hệ thống robot di
động, việc nhận biết được môi trường làm việc là một yếu tố quyết định tới các “hành
động” của robot, chỉ khi nhận biết được đầy đủ các thông tin về môi trường xung
quanh thì robot di
đ
ộng mới có thể thích ứng được trong các môi trường làm việc khác
nhau.

Robot di động phải có một số bộ phận chuyển động. Chuyển động có thể dưới
dạng bánh xe, chân, cánh hoặc một số cơ cấu khác. Việc lựa chọn cơ cấu chuyển động
là dựa vào chức năng của robot và các công việc của robot cần phải thực hiện. Trong
nhiều môi trường làm việc công nghiệp, bánh xe là dạng chuyển động thích hợp nhất.
Đối với các hệ thống nghiên cứu ứng dụng khác, chân hoặc cánh có thể giúp cho robot
di động chuyển động được trên địa hình mà robot không có khả năng đi qua.
Một nhân tố quan trọng trong thiết kế của các hệ thống robot di động là khả
năng mang theo các thiết bị. Thiết bị bao gồm nguồn điện c
ũng như t
ất cả các phần
cứng là các bộ cảm biến và các bộ xử lý mà robot
đòi h
ỏi. Các thiết bị lắp đặt của
10
robot cố định có thể được kết nối trực tiếp với các nguồn điện và bộ xử lý, không gian
làm việc thường được xác định trước và có thể được kiểm soát. Hầu hết các robot di
động đều có một trọng tải nhất định, điều này có thể dẫn tới những sự giới hạn trong
thiết kế của hệ thống, đặc biệt trong các ứng dụng hay bị giới hạn về kích cỡ và cân
nặng.
Các thành phần cơ bản, thông dụng trong các hệ thống robot di động sử dụng
trong thực tế được chỉ ra như trên hình 1.2.
Hình 1.2. Các kh
ối cơ bản trong robot di động
.
Các khối cơ bản cho các hệ thống robot di động bao gồm các bộ phận cảm biến,
bộ phận dẫn đường, các mô-đun lập kế hoạch, điều khiển và các bộ chấp hành dùng để
di chuyển robot và tác động tới môi trường của nó. Trong khi một số các phần cụ thể
của robot di động sẽ phụ thuộc vào mục đích ứng dụng, một hệ thống robot di dộng
điển hình
đòi h

ỏi phải có tất cả các khối trên thì mới vận hành được. Thông tin về
trạng thái môi trường xung quanh của robot được cung cấp với các cảm biến nội và
cảm biến ngoại. Thông tin này được bộ xử lý trung tâm (có cài đặt sẵn các chương
trình, thuật toán dẫn đường ) sử dụng để ước lượng trạng thái của hệ thống, không chỉ
bao gồm trạng thái của bản thân robot mà cả trạng thái của môi trường xung quanh nó.
Ước lượng này được sử dụng để lập kế hoạch hoạt động của robot và phát ra các lệnh
cho các cơ cấu chấp hành của robot.
11
Thông tin về trạng thái của hệ thống được cung cấp bởi các cảm biến nội và
cảm biến ngoại. Thông tin này được một khối dẫn đường (có thể là các môđun phần
cứng hoặc phần mềm, nhưng thường là một chương tr
ình ph
ần mềm có khả năng tính
toán) sử dụng để ước lượng trạng thái của hệ thống. Tín hiệu này sau đó được mô đun
lập kế hoạch và điều khiển sử dụng để phát ra các lệnh gửi tới các bộ phận thao tác của
robot.
Thiết kế của các hệ thống robot di động phần nhiều sẽ bị chi phối bởi mục đích
ứng dụng. Không có giới hạn về số lượng các kết hợp giữa các thiết bị thao tác, các bộ
phận cảm biến và các thuật toán điều khiển cho các hệ thống robot di động. Hơn nữa,
luồng thông tin trong phạm vi hệ thống sẽ thay đồi tuỳ thuộc vào quá trình thực thi, sẽ
có một số thiết kế robot khác đáng kể so với những robot trình bày trong hình 1.2.
Trong một số trường hợp, dữ liệu từ các bộ phận cảm biến được trực tiếp cung cấp cho
bộ lên kế hoạch và điều khiển, cho phép hệ thống phản ứng nhanh hơn với những thay
đổi mạnh mẽ của môi trường.
Do không có loại cảm biến nào là hoàn hảo và thích hợp cho mọi ứng dụng, nên
ngoài việc mong đợi các thành tựu của công nghệ vật liệu sẽ cho ra đời các loại cảm
biến tốt hơn th
ì vi
ệc nghiên cứu áp dụng các thuật giải tổng hợp dữ liệu các cảm biến
là cần thiết. Điều đó càng cho thấy sự cần thiết phải gắn kết hữu cơ giữa phần cứng và

phần mềm một cách hợp lý cho một robot di động thông minh. Vì vậy việc tập trung
vào xây dựng phát triển các giải thuật tổng hợp cảm biến (sensor fusion) trên cơ sở
thiết kế lắp ráp phần cứng của các robot di động thông minh tự quản trị là hết sức cần
thiết. Chương tiếp theo trình bày về tổng hợp cảm biến.
12
CHƯƠNG 2: PHƯƠNG PHÁP TỔNG HỢP CẢM BIẾN VỚI
BỘ LỌC KALMAN
2.1. Phương pháp tổng hợp cảm biến dùng cho robot di động
Robot dùng số liệu thu được từ các cảm biến để xác định trạng thái của nó trong
môi trường, trên cơ sở đó mà ra các quyết định cho các nhiệm vụ dẫn đường như định
vị và lập bản đồ. Tuy nhiên, chưa thể có loại cảm biến nào hoàn hảo để thực hiện
nhiệm vụ này một cách thật chính xác được. Vì vậy kỹ thuật tổng hợp cảm biến
(sensor fusion) đ
ã
được sử dụng, đó là việc sử dụng đồng thời các dữ liệu từ một cảm
biến hay từ nhiều cảm biến rồi lấy thông tin hợp nhất ra để tạo nên bức tranh về trạng
thái môi trường chính xác hơn. Một phương pháp tổng hợp cảm biến hiệu quả nhất để
ước tính đồng thời các giá trị đo của các cảm biến đó là việc dùng bộ lọc Kalman mở
rộng (EKF) với các phép tính ma trận. Tổng hợp cảm biến có thể được định ngh
ĩa như
sau: đó là quá tr
ình k
ết hợp các dữ liệu nhận được từ nguồn các cảm biến để có được
kết quả thông tin với một ý ngh
ĩa t
ốt hơn so với khi sử dụng các nguồn này riêng lẻ.
Vì dữ liệu cảm biến có nhiều loại nên có tác giả đ
ã dùng thu
ật ngữ “tổng hợp
thông tin” như một thuật ngữ tổng thể cho tổng hợp các loại dữ liệu. Có thể định ngh

ĩa
thuật ngữ tổng hợp thông tin như sau [11]:
Tổng hợp thông tin bao gồm lý thuyết, và các công cụ hình thành, được sử dụng
để khai thác sự đồng vận của thông tin từ nhiều nguồn (cảm biến, cơ sở dữ liệu, thông
tin thu thập bởi con người, …) để đưa ra quyết định hay hành động với một ý ngh
ĩa t
ốt
hơn về mặt chất lượng hoặc số lượng, về độ chính xác, chắc chắn,…so với khi sử dụng
một nguồn đơn không khai thác sự đồng vận.
Bằng cách xác định như một tập con của tổng hợp thông tin, thuật ngữ tổng hợp
cảm biến có thể được định ngh
ĩa như sau:
Tổng hợp cảm biến là quá trình kết hợp các dữ liệu nhận được để có được kết
quả thông tin với một ý ngh
ĩa t
ốt hơn so với khi sử dụng các nguồn này riêng lẻ.
Ngoài các định ngh
ĩa như trên, m
ột số tác giả còn phân loại tổng hợp cảm biến
ra hai vấn đề là tổng hợp trực tiếp, và tổng hợp gián tiếp [10]. Tổng hợp trực tiếp có
ngh
ĩa là t
ổng hợp các dữ liệu cảm biến từ một tập hợp các quá trình thu thập giá trị dữ
13
liệu cảm biến của các cảm biến đồng nhất hoặc không đồng nhất. Trong khi đó tổng
hợp gián tiếp sử dụng nguồn thông tin như là một kiến thức tiên nghiệm về môi
trường. Do đó, tổng hợp đa cảm biến mô tả các hệ thống tổng hợp trực tiếp, còn lại
tổng hợp thông tin bao gồm các qui trình tổng hợp gián tiếp.
Về nguyên tắc, một hệ thống sử dụng nhiều nguồn dữ liệu cảm biến sẽ đưa ra
được một kết quả có ý ngh

ĩa hơn là t
ừ một nguồn dữ liệu đơn.
Xét một ví dụ đơn giản là cần định vị chính xác vị trí và hướng của robot khi nó
di chuyển trong một không gian chưa biết trước. Nếu chỉ sử dụng một camera ảnh thì
có thể xác định khá chính xác góc hướng của robot, nhưng không thể xác định chính
xác khoảng cách từ robot đến một vật cản nào đó. Nhưng nếu kết hợp dữ liệu từ
camera này với dữ liệu nhận được qua các cảm biến siêu âm hay máy đo xa laser th
ì có
thể xác định được chính xác không những góc hướng của robot mà còn cả khoảng
cách từ robot đến đối tượng. Một ví dụ tiếp theo là bằng cách sử dụng các bộ lập mã
trục xoay trong phương pháp odometry cho phép định vị robot khá đơn giản. Tuy
nhiên, với mô hình hệ thống khá đơn giản như vậy, các giá trị định vị gặp phải các sai
số tích l
ũy
bị gây nên do các kết cấu cơ khí được thiết kế không thể thật sự hoàn hảo
trong các robot. Nếu áp dụng phương pháp tổng hợp cảm biến bằng việc xử lý các số
liệu đo được qua bộ lọc Kalman mở rộng sẽ cho phép cải thiện đáng kể độ chính xác
định vị kiểu này.
2.2. Phương pháp tổng hợp cảm biến với bộ lọc Kalman
Bộ lọc Kalman sử dụng mô hình toán học cho lọc các tín hiệu bằng cách sử
dụng các phép đo với một số lượng đáng kể các sai số thống kê và có hệ thống.
Phương pháp được phát triển bởi Kalman và Bucy vào năm 1960 [3,8].
Thông thường, một bộ lọc Kalman tổng hợp dữ liệu đo được trong một khoảng
thời gian liên tiếp, cung cấp một ước tính khả năng xảy ra lớn nhất của một tham số.
Nó c
ũng có th
ể liên kết với các đầu vào của đa cảm biến thành một vector của các
trạng thái bên trong có chứa các tham số quan tâm, miễn là chúng chỉ phụ thuộc tuyến
tính giữa các đầu vào và các trạng thái của hệ thống [2]. Bộ lọc sử dụng thuật toán thời
gian rời rạc để loại bỏ nhiễu từ các tín hiệu của cảm biến để tạo ra được dữ liệu tổng

hợp, ví dụ ước tính các các giá trị làm trơn của vị trí, vận tốc, và gia tốc tại một dãy
14
các điểm trong quỹ đạo. Mô hình bộ lọc Kalman chuẩn được suy ra từ 2 phương trình
tuyến tính.
Phương tr
ình th
ứ nhất mô tả động học của hệ thống:
1
. .
k k k k
x A x B u w
+
= + +
(2.1)
Trong đó
k
x
là véc tơ trạng thái của hệ thống tại thời điểm k. A là ma trận trạng
thái không suy biến. Véc tơ
k
u
miêu tả đầu vào hệ thống tại thời điểm k, w
k
là một biến
ngẫu nhiên biểu diễn cho nhiễu hệ thống.
Phương tr
ình tuy
ến tính thứ hai mô tả quan sát nhiễu của hệ thống, với phép đo
m
z ∈ℜ

:
.
k k k
z H x v= +
(2.2)
Trong mỗi phần tử của vec tơ
k
z
chứa một quan sát cảm biến tại thời điểm k.
Ma trận H liên quan tới các phép đo trạng thái bên trong, v
k
biểu diễn nhiễu đo.
Các biến ngẫu nhiên w
k
và v
k
được giả định là nhiễu quá trình và nhiễu đo; độc
lập với nhau, ồn trắng và có phân bố xác suất chuẩn:
~ (0, ) ~ (0, ) ( ) 0
T
k k k k i j
w N Q v N R E w v =
Q
k
là các ma trận hiệp phương sai nhiễu đầu vào; R
k
là ma trận hiệp phương sai
của nhiễu đo.
Bài toán bộ lọc Kalman chính là tìm giá trị ước lượng của trạng thái x khi biết
được sự biến thiên của nó qua một đại lượng z đo được phụ thuộc tuyến tính vào x.

Hay nói cách khác, dựa trên các số liệu đo z, có thể tìm
đư
ợc một hệ số Kalman K
trong mỗi chu kỳ lấy mẫu tín hiệu đo thuộc một vòng lặp đệ quy gọi là bộ lọc Kalman,
sao cho giá trị ước lượng trạng thái của hệ gần với giá trị thực nhất.
Như vậy, quá trình thực hiện của bộ lọc Kalman được chia thành 2 giai đoạn là
giai đoạn dự đoán (ước tính, đánh giá) và giai đoạn hiệu chỉnh. Độ khuếch đại
Kalman K
k
sẽ được tính toán trước tiên, sau đó vector trạng thái và ma trận lỗi tương
quan
k
P
sẽ được cập nhật từ các đánh giá bước trước
ˆ
k
x
−
và
−
k
P
.
15
Để khởi động bộ lọc Kalman, người ta phải cung cấp một ước tính tiên nghiệm
0
ˆ
x
−
và một ước tính hiệp phương sai của nó

0
P
−
.
0
P
−
có thể được khởi tạo với một ước
tính giá trị ban đầu không chính xác, khi đó ứng dụng tiếp theo của bộ lọc Kalman sẽ
cho
−
k
P
tiếp cận gần với giá trị chính xác của nó.
1
)(
−−−
+=
k
T
kkk
T
kkk
RHPHHPK
(2.3)
ˆ ˆ ˆ
( )
k k k k k k
x x K z H x
− −

= + −
(2.4)
−
−=
kkkk
PHKIP )(
(2.5)
Trong giai đoạn dự đoán, các đánh giá và sai số của chúng sẽ được dự đoán
trước về mặt thời gian:
1
ˆ ˆ
k k k
x Ax Bu
−
+
= +
(2.6)
1
T
k k k k
P AP A Q
−
+
= +
(2.7)
Nếu giá trị của Q
k
được chọn là lớn hơn chính giá trị thật của nó, bộ lọc Kalman
sẽ đặt độ tin cậy vào các giá trị đo lường hơn là hệ thống. Khi đó, các kết quả đánh giá
sẽ phụ thuộc sai số của phép đo. Nếu giá trị Q

k
chọn nhỏ hơn chính nó, độ trễ thời gian
sẽ được tăng lên. Khi Q
k
nhỏ hơn rất nhiều so với giá trị thật, bộ lọc sẽ phân kỳ dẫn
đến kết quả là không ổn định về mặt tính toán, như vậy việc chọn giá trị Q
k
thông
thường dựa trên thực nghiệm.
0 0
ˆ
,Px
− −
k
z
Hình 2.1. Sơ đồ thuật toán bộ lọc Kalman tuyến tính rời rạc.
Tính hệ số khuếch đại Kalman
1
)(
−−−
+=
k
T
kkk
T
kkk
RHPHHPK
Cập nhật các đánh giá
ˆ ˆ ˆ
( )

( )
k k k k k k
k k k k
x x K z H x
P I K H P
− −
−
= + −
= −
Dự đoán các đánh giá
1
1
ˆ ˆ
k k k
T
k k k k
x Ax Bu
P AP A Q
−
+
−
+
= +
= +
16
Trong robot di động, bộ lọc Kalman được sử dụng để hiệu chỉnh giá trị ước
lượng tọa độ và góc hướng dựa trên các phép đo có nhiều dữ liệu đầu vào khác nhau
[2]. Tuy nhiên như đ
ã mô t
ả phần trên, bộ lọc Kalman giải quyết các vấn đề phổ biến

của ước tính trạng thái
m
x∈ℜ
mà quá trình
đi
ều khiển thời gian rời rạc phụ thuộc vào
phương tr
ình sai phân ng
ẫu nhiên tuyến tính. Nhưng điều gì sẽ xảy ra, nếu qúa trình
ước tính hoặc mối quan hệ giữa các phép đo với quá trình
đi
ều khiển là phi tuyến? Để
giải quyết bài toán này, bộ lọc Kalaman mở rộng viết tắt là EKF (extended Kalman
filter) đ
ã được phát triển. Bằng việc khai triển chuỗi Taylor tại lân cận m
ột điểm n
ào
đó trên qu
ỹ đạo, ta có thể tuyến tính hóa sự dự đoán xung quanh dự đoán hiện tại sử
d
ụng một phần dẫn xuất của quá tr
ình và chức năng đo lường để đưa vào tính toán dự
đoán. Đ
ể thực hiện, ta bắt đầu hiệu chỉnh từ bộ lọc Kalman rời rạc. Hãy giả đ
ịnh rằng
phương tr
ình tr
ạng thái của hệ thống (2.1) của
ước tính trạng thái
m

x∈ℜ
, nhưng quá
trình
đi
ều khiển thời gian rời rạc phụ thuộc vào phương tr
ình sai phân ng
ẫu nhiên phi
tuyến tính:
1
( , , )
k k k k
x f x u w
+
=
(2.8)
Và phép đo
m
z ∈ℜ
:
( , )
k k k
z h x v=
(2.9)
Các biến ngẫu nhiên w
k
và v
k
c
ũng đ
ư

ợc giả định là nhiễu quá trình và nhiễu đo
như (2.1) và (2.2), trong trường hợp này hàm phi tuyến, f, ở (2.8) liên quan đến trạng
thái ở bước thời gian hiện tại k đến trạng thái ở bước thời gian tiếp theo k+1. Nó bao
gồm như các tham số, u
k
và nhiễu quá trình, w
k
. Hàm phi tuyến, h, của phép đo trong
phương tr
ình (2.9) bi
ểu diễn mối liên quan giữa trạng thái x
k
với phép đo z
k.
Thực tế, không biết được các giá trị riêng lẻ của nhiễu w
k
và v
k
tại mỗi bước
thời gian. Tuy nhiên, có thể tính gần đúng vec-tơ trạng thái và vec-tơ đo lường:
1
( , ,0)
k k k
x f x u
+
=
 
(2.10)
và:
17

( ,0)
k k
z h x=


(2.11)
Để dự đoán quá tr
ình các m
ối quan hệ vi phân phi tuyến và phép đo, chúng ta
bắt đầu viết lại các phương tr
ình tuy
ến tính hóa xung quanh phương tr
ình (2.10) và
(2.11).
1 1
ˆ
( ) W
k k k k k
x x A x x w
+ +
≈ + − +

ˆ
( )
k k k k k
z z H x x Vv≈ + − +

(2.12)
(2.13)
trong đó:

-
k
x
và
k
z
là các vec-tơ trạng thái và phép đo thực.
-
k
x

và
k
z

là các vec-tơ trạng thái và phép đo xấp xỉ từ (2.10) và (2.11).
-
ˆ
k
x
ước tính trạng thái hậu nghiệm tại thời điểm k.
- Các biến ngẫu nhiên w
k
và v
k
c
ũng đư
ợc giả định là nhiễu quá trình và nhiễu
đo như (2.10) và (2.11).
- A là Jacobi của đạo hàm riêng của f theo x:

ij ( 1) ( 1)
ˆ
| ( , ,0);
i
k k
j
f
A x u
x
− −
∂
=
∂
- W là Jacobi của đạo hàm riêng của f theo w:
ij ( 1) ( 1)
ˆ
W | ( , ,0);
w
i
p k k
j
f
− −
∂
=
∂
x u
- H là Jacobi của đạo hàm riêng của h theo x:
ij
| ( ,0);

i
k
j
h
H x
x
∂
=
∂

- V là Jacobi của đạo hàm riêng của h theo v:
ij
ˆ
| ( ,0);
i
k
j
h
V v
v
∂
=
∂
18
z
k
Hình 2.2. Sơ đồ thuật toán bộ lọc Kalman mở rộng.
Tương tự như bộ lọc Kalman rời rạc, quá trình thực hiện của bộ lọc Kalman mở
rộng c
ũng đư

ợc chia thành 02 giai đoạn là giai đoạn dự đoán (ước tính, đánh giá) và
hiệu chỉnh như trên h
ình 2.2.
1. Bước dự đoán với các phương tr
ình c
ập nhật:
-
-1 -1
ˆ ˆ
( , )
k k k
x f x u=
(2.14)
-
-1 -1
T T
k k k k k k k
P A P A W Q W= +
(2.15)
Trong đó
-
ˆ
n
k
x ∈ℜ
là ước lượng trạng thái tiên nghiệm tại thời điểm k cho
biết giá trị trước quá trình tại thời điểm k.
-
ˆ
k

P
là ma trận hiệp phương sai của sai số dự báo trạng thái.
Q
k-1
là ma trận hiệp phương sai nhiễu đầu vào phụ thuộc vào độ lệch
chuẩn của nhiễu.
2. Bước hiệu chỉnh với các phương tr
ình c
ập nhật phép đo:
- - -1
( )
T T T
k k k k k k k k k
K P H H P H V R V= +
(2.16)
1 1
ˆ
,
k k
x P
− −
Tính hệ số khuếch đại
Kalman
- - -1
( )
T T T
k k k k k k k k k
K P H H P H V RV= +
Cập nhật các đánh giá
- -

-
ˆ ˆ
( - ( ,0))
( - )
k k k k k
k k k k
x x K z h x
P I K H P
= +
=
Dự đoán các đánh giá
-
-1 -1
-
-1 -1
ˆ ˆ
( , )
k k k
T T
k k k k k k k
x f x u
P A P A W Q W
=
= +
19
- -
ˆ ˆ
( - ( ,0))
k k k k k
x x K z h x= +

(2.17)
-
( - )
k k k k
P I K H P=
(2.18)
Trong đó
ˆ
n
k
x ∈ℜ
là ước tính trạng thái hậu nghiệm tại thời điểm k, giá trị thu
được sau khi đo lường z
k
.
R
k
là ma trận hiệp phương sai của nhiễu được ước lượng từ nhiễu trong phép
đo.
Qua những phân tích ở trên chúng ta thấy rằng bộ lọc Kalman mở rộng (EKF)
rời rạc đã
đ
ạt được nhiều kết quả mong muốn [9]. Tuy nhiên, mô hình tuyến tính của
hệ thống không phải lúc nào c
ũng kh
ả thi. Vì vậy EKF đ
ã đư
ợc dẫn xuất từ các
phương tr
ình ng

ẫu nhiên phi tuyến cho mô hình hệ thống.
Việc ứng dụng EKF cho robot di động rất đa dạng, ví dụ như điều khiển bánh
xe vi sai cho robot di động, hay ứng dụng EKF cho tổng hợp đa cảm biến để nâng cao
khả năng ước lượng trạng thái của robot di động. Các nội dung nghiên cứu này sẽ
được đề cập cụ thể trong chương 4 và chương 5. Thêm một lợi thế trong một môi
trường đa cảm biến với các ma trận đo lường giống hệt nhau, các phép đo có thể được
xử lý một cách riêng biệt để có được độ phức tạp tính toán thấp hơn. Trong trường hợp
các ma trận đo không giống nhau là thích hợp sử dụng hợp nhất vec tơ đầu vào chứa
các thông tin của tất cả các cảm biến. Thêm một lý do quan trọng khi tổng hợp đa cảm
biến có các loại cảm biến khác nhau, do thời gian xử lý trên mỗi cảm biến khác nhau,
thời gian cập nhật trên mỗi cảm biến khác nhau, nhưng vẫn có thể mô hình hóa được
với các dạng thông tin của EKF, và do đó có thể tổng hợp được nhiều cảm biến khác
nhau, tăng số chiều dữ liệu.
20
CHƯƠNG 3: MỘT SỐ CẢM BIẾN HIỆN ĐẠI VÀ PHƯƠNG PHÁP ĐỊNH
VỊ ROBOT DI ĐỘNG
Định vị là việc xác định vị trí tọa độ và hướng của robot trong môi trường.
Khâu định vị-lập bản đồ sử dụng cơ sở dữ liệu từ khâu cảm nhận để làm khớp với dữ
liệu bản đồ đ
ã bi
ết trước hoặc được lập từ cơ sở dữ liệu đó. Như đ
ã th
ảo luận ở trên,
kết quả của việc tổng hợp cảm biến có thể áp dụng đến tất cả các khâu kể trên, trong
đó khâu định vị là quan trọng. Chương này sẽ giới thiệu một số cảm biến hiện đại và
dùng chúng trong một số phương pháp định vị cho robot di động.
3.1. Phân loại các cảm biến dùng trong robot di động
Cảm biến (sensor) là những thiết bị có khả năng cảm nhận những đại lượng
điện và không điện, chuyển đổi chúng trở thành những tín hiệu phù hợp với những
thiết bị thu nhận tín hiệu ở đầu ra có thể đo và xử lý đ

ư
ợc.
Tùy theo các đặc điểm kỹ thuật, cảm biến có thể được chia thành nhiều loại
khác nhau.
Một trong những cách phân loại đó là cảm biến được chia làm hai loại: thụ
động và tích cực. Cảm biến thụ động thường dựa vào các thông số môi trường để cung
cấp phương tiện cho việc quan sát trong khi đó, cảm biến tích cực phải phát năng
lượng vào môi trường hoặc để thay đổi nó hoặc để tăng cường nó rồi thu thập dữ liệu
được phản xạ về.
Tùy vào tính chất mục đích sử dụng các cảm biến còn
đư
ợc phân thành: cảm
biến nội, cảm biến ngoại.
Cảm biến nội được đặt trong bản thân robot, đó là các cơ cấu điện, điện tử, cơ
khí, thủy lực… để nhận các thông tin phản hồi về vị trí của các bộ phận trên robot.
Cảm biến ngoại là các cảm biến giúp robot giao tiếp được với môi trường bên
ngoài, phản ánh các thông tin về môi trường xung quanh tới bộ điều khiển robot. Ví
dụ, cảm biến đo xa siêu âm cho phép robot ước lượng khoảng cách từ nó đến các đối
tượng.
3.1.1. Cảm biến lập mã trục xoay (cảm biến lập mã quang)
Các bộ lập mã quang
đư
ợc dùng cho phương pháp đo qu
ãng đư
ờng đi của robot
(phương pháp này thường gọi là odometry hay dead reckoning) [5], trong đó robot đo
một tín hiệu đang phát ra trong nó.
21
Bộ mã hóa quang đã trở thành thiết bị phổ biến để đo tốc độ góc và vị trí bên
trong một động cơ mô-tô hoặc trục của bánh xe hoặc một cơ cấu lái. Trong robot di

động, bộ mã hóa
đư
ợc sử dụng để điều khiển vị trí hoặc tốc độ của bánh xe và những
chỗ nối động cơ - bánh lái khác.
Một bộ mã hóa quang bao gồm một nguồn sáng, một đ
ĩa ch
ắn sáng được gắn cố
kết với trục mô tơ quay bánh xe robot. Đĩa chắn được rạch các khe sáng cho phép tia
sáng lọt qua tới một bộ thu nhạy quang phía bên kia mặt đ
ĩa. Khi mô tơ quay làm cho
đ
ĩa ch
ắn sáng cùng các khe sáng quay theo. Kết quả là tạo nên các xung sáng đập tới
bộ nhạy quang làm phát ra các xung điện đưa đến mạch điện tử. Như vậy, khi trục
xoay mô tơ quay, số lượng tia sáng đập vào bộ nhạy thay đổi tạo nên các xung điện
mỗi khi có sự thẳng hàng của khe với nguồn chiếu sáng. Biết toàn bộ số khe sáng trên
đ
ĩa (cũng l
à trên 1 vòng quay), bi
ết được tỷ số truyền bánh răng từ trục mô tơ với trục
bánh xe robot, bằng việc đếm số xung nhận được trong 1 giây thì hoàn toàn xác
đ
ịnh
được vận tốc góc hay vận tốc dài của bánh chuyển động xe robot. Độ phân giải góc
nhỏ nhất có thể dễ dàng tính được từ tốc độ CPR (Cycles per Revolution) của bộ mã
hóa. Một loại bộ mã hóa trong l
ĩnh v
ực robot di động có thể đến 2000 CPR, trong khi
bộ mã hóa quang công nghiệp có thể dễ dàng chế tạo những bộ mã hóa với 10000
CPR. Trong giới hạn băng thông yêu cầu, bộ mã hóa

đ
ủ nhanh để đếm tốc độ trục
quay đ
ã đư
ợc dự kiến. Hình 3.1 là bộ lập mã quang từng bước loại vuông pha 2 kênh.
Hình 3.1.
Đĩa có khe chắn sáng và tín hiệu ra bộ lập mã quang
.
3.1.2. Cảm biến chỉ hướng (heading sensor).
Cảm biến cho phép đo góc hướng của robot so với phương từ trường của quả
đất. Cảm biến được tích hợp trong một mô-đun cảm biến từ-địa bàn CMPS03 của hãng
Philips (trên hình 3.2a) hoạt động dựa trên nguyên tắc hiệu ứng Hall với độ phân giải
góc là 0,1° [12]. Nguyên tắc hoạt động như biểu diễn trên hình 3.2b, mô-đun có 2 trục
hướng x và y.
22
Hình 3.2a. Mô-đun c
ảm biến từ
-đ
ịa bàn
CMPS03.
Hình 3.2b. Nguyên lý ho
ạt
đ
ộng
c
ủa cảm biến từ
-đ
ịa bàn
.
Cảm biến cung cấp số liệu đo cường độ từ trường theo các trục hướng này, từ đó

có thể xác định được góc lệch của mô-đun so với phương từ trường quả đất khi nó được
gắn cố kết với robot. Một vi xử lý tích hợp trong mạch điện tử của mô-đun cung cấp
các số đo này ra theo chuẩn truyền thông nối tiếp RS-232C. Cảm biến này có ưu điểm
rất phổ biến và dễ sử dụng, có thể đo trực tiếp góc quay (hướng) của robot. Tuy nhiên
vì lệ thuộc vào từ trường của trái đất nên nó có nhược điểm thường không chính xác
khi hoạt động trong vùng mà từ trường cục bộ của trái đất bị ảnh hưởng do các vật
nhiễm từ gần robot gây nên.
3.1.3. Cảm biến siêu âm đo khoảng cách đến vật thể gần
Có 2 phương pháp đo khoảng cách từ cảm biến đến vật: hoặc phát ra xung
(chùm xung) rời rạc hoặc một sóng liên tục tới bề mặt vật cản rồi thu nhận xung phản
xạ hoặc sóng phản xạ. Tính thời gian đi về (trong trường hợp xung phát rời rạc) hoặc
sự sai pha giữa sóng phát ra và sóng phản xạ (trong trường hợp phát sóng liên tục) cho
phép biết được quãng
đư
ờng đi-về của sóng tức là khoảng cách đến vật. Các xung rời
rạc dùng cho phép đo thường được phát ra từ các nguồn phát siêu âm, sóng radio RF
hoặc phát quang. Do đó các thông số liên quan trong dải tốc độ âm thanh trong không
khí (cỡ 0,3 m/ms) tới tốc độ ánh sáng (0,3 m/ns). Quãng
đư
ờng đi-về bằng d = vt.
Sóng âm hoạt động với các tần số khác nhau tùy vảo môi trường ứng dụng
(trong nước là tần số âm, hạ âm, trong không khí là tần số siêu âm…). Những hiệu ứng
cần chú ý khi đo với cảm biến siêu âm là: dội (echo), cạnh bên (side lobe), phản xạ
(speculat reflection) và xuyên âm (cross-talk) và phải chú ý rằng chùm tia siêu âm có
góc mở hình nón. Cảm biến siêu âm do tốc độ đo nhanh và giá thành rẻ được sử dụng
rất nhiều cho đo khoảng cách trong nhà (in-door).
Cảm biến siêu âm cho các thông tin về khoảng cách đến vật cản với thời gian
23
thu thập số liệu nhanh. Nguyên tắc hoạt động của nó được chỉ ra trong hình 2.3b. Một
chùm sóng siêu âm được phát ra từ cảm biến và sau một thời gian nào đó thu được

sóng phản xạ từ vật cản. Biết thời gian này và vận tốc sóng siêu âm truyền trong môi
trường thì có thể xác định được khoảng cách từ cảm biến đến vật.
Hình 3.3a. Các c
ảm biến si
êu âm
[13].
Hình 3.3b. Nguyên lý ho
ạt động
c
ủa cảm
bi
ến siêu âm
.
Cảm biến siêu âm có ưu điểm là có giá thành thấp, dễ sử dụng và có lợi thế tạo
ra kết quả nhanh hơn so với các thiết bị đo khác. Tuy nhiên, kết quả đo khoảng cách
của cảm biến siêu âm bị một số hạn chế cơ bản do chùm tia siêu âm bị tán xạ thành
hình nón chứ không phải là một tia hẹp. Độ chính xác của kết quả đo không ổn định do
bị ảnh hưởng bởi các hiện tượng như: tiếng dội (echo), búp sóng bên (side lobe), phản
xạ gương (specular reflection), xuyên âm (cross-talk) của chùm sóng siêu âm nên khó
xác định được kích thước của vật.
3.1.4. Cảm biến đo xa laser (Laser Range Finder - LRF)
Máy đo khoảng cách bằng tia laser là một thiết bị sử dụng tia lase để quét và
tìm khoảng cách đến vật thể. Có nhiều loại máy quét lase trên thị trường, một ví dụ 2
loại khác nhau của máy quét lase như h
ình 4.
Hình 3.4. M
ột số loại máy quét laser của công ty SICK
[14].