ĐẠI HỌC QUỐC GIA HÀ NỘI

TRƯỜNG ĐẠI HỌC CÔNG NGHỆ

LÊ VIỆT NAM

ROBOT VẼ BẢN ĐỒ ĐỊA HÌNH

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

Hà Nội – 2015


2
ĐẠI HỌC QUỐC GIA HÀ NỘI

TRƯỜNG ĐẠI HỌC CÔNG NGHỆ
LÊ VIỆT NAM

ROBOT VẼ BẢN ĐỒ ĐỊA HÌNH
Ngành:

Công nghệ thông tin

Mã số:

60.48.01.03

Chuyên ngành:

Kỹ thuật phần mềm

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

NGƯỜI HƯỚNG DẪN KHOA HỌC: PGS. TS. PHẠM BẢO SƠN

Hà Nội – 2015


3

Lời cam đoan

Tôi xin cam đoan:
(i)

Luận văn này là sản phẩm nghiên cứu của tôi,

(ii)

Số liệu trong luận văn được điều tra trung thực,

(iii)

Tôi xin chịu trách nhiệm về nghiên cứu của mình.
Học viên

Lê Việt Nam


4


LỜI CẢM ƠN
Tôi xin bày tỏ lòng biết ơn sâu sắc đến thầy hướng dẫn của tôi, Phó Giáo sư-Tiến
sĩ Phạm Bảo Sơn. Thầy đã cho tôi cơ hội quý giá để theo đuổi nghiên cứu lĩnh

vực mình yêu thích. Trong suốt quá trình thực hiện luận văn, thầy đã tận tình

hướng dẫn cho tôi, đồng thời thầy đã cung cấp những tài liệu và thiết bị cần thiết
để tôi có thể hoàn thành luận văn của mình. Đó là một vinh dự cho tôi khi nhận
được bằng Thạc sĩ.

Tiếp đến, tôi xin chân thành cảm ơn các thầy cô giáo trong Khoa Công nghệ
Thông tin, Đại học Công nghệ - Đại học Quốc gia Hà Nội đã truyền đạt cho tôi

những kiến thức và kinh nghiệm vô cùng quí báu trong quá trình học tập và nghiên
cứu.

Tôi cũng muốn cảm ơn các bạn cùng lớp và các đồng nghiệp đã cho tôi những lời
động viên, những hỗ trợ và góp ý chuyên môn quý báu.

Cuối cùng, tôi xin cảm ơn gia đình, bạn bè, những người đã luôn bên cạnh ủng
hộ và động viên tôi.


5

MỤC LỤC
Lời cam đoan.............................................................................................................. 3
Danh mục các ký hiệu, viết tắt .................................................................................... 7
Danh mục hình vẽ ...................................................................................................... 8


CHƯƠNG 1: MỞ ĐẦU .............................................................................................. 9
1.1.

Giới thiệu bài toán lập bản đồ ........................................................................ 9

1.3.

Một số thuậtngữ cơ bản ............................................................................... 10

1.2.
1.4.

Lịch sử của SLAM ...................................................................................... 10

Bài toán SLAM ........................................................................................... 11

CHƯƠNG 2: CƠ SỞ LÝ THUYẾT ......................................................................... 18
2.1.

Lọc Kalman................................................................................................. 18

2.1.1. Ý tưởng Kalman .......................................................................................... 18
2.1.2. Lọc Kalman và SLAM ................................................................................ 19

2.1.3. Nhiều trắng, đường cong Gausian, quá trình tuyến tính ............................... 19
2.1.4. Phát triển một bộ lọc đơn giản ..................................................................... 20
2.1.5. Phát triển lọc Kalman cho robot chuyển động ............................................. 25
2.1.6. Phát triển lọc Kalman với môi trường thực tế cho SLAM ............................ 28


2.1.7. Bộ lọc Kalman mở rộng .............................................................................. 32
2.1.8. Hạn chế của kĩ thuật lọc Kalman ................................................................. 33

2.2.

FastSLAM ...................................................................................................... 35

2.2.1. Cách tiếp cận SLAM bằng Bayes ................................................................ 35

2.2.2. Thuật toán FastSLAM ................................................................................. 38


6
2.2.3. Fast SLAM và particle ................................................................................ 39
2.2.4. Ước lượng Pose ........................................................................................... 39

2.2.5. Ước lượng feature ....................................................................................... 40

2.2.6. Vòng lặp đóng với FastSLAM ..................................................................... 41

2.3.

DP-SLAM ...................................................................................................... 43

2.3.1. Mô tảthuật toán DP-SLAM ......................................................................... 43
2.3.2. Bản đồ phân bố Particle ............................................................................... 44

2.3.3. DP-SLAM trong thực tế .............................................................................. 45

CHƯƠNG 3: THỰC NGHIỆM ................................................................................ 47

3.1.
3.2.

Mô hình thực nghiệm .................................................................................. 47
Robot Lego NXT......................................................................................... 48

3.2.1.

Giới thiệu.............................................................................................. 48

3.2.3.

Chương trình điều khiển ....................................................................... 50

3.2.2.
3.3.
3.4.

Lắp ráp robot ........................................................................................ 49

Chương trình trên Smartphone .................................................................... 51
Chương trình giám sát trên PC .................................................................... 54

KẾT LUẬN .............................................................................................................. 57
TÀI LIỆU THAM KHẢO ........................................................................................ 59


7

Danh mục các ký hiệu, viết tắt

STT

Từ viết tắt

1

SLAM

2

DP-SLAM

3

KF

4

Ý nghĩa
Simultaneous Localization And Mapping – Đồng
thời định vị và lập bản đồ

Distributed Particle for Simultaneous Localization
And Mapping – Phân phối particle cho Đồng thời
định vị và lập bản đồ

Kalman Filter – Bộ lọc Kalman

EKF


Extended Kalman filter – Bộ lọc Kalman mở rộng

6

US sensor

Ultrasonic sensor

7

API

8

PC

5

9

IR sensor

OS

Infrared sensor – Cảm biến hồng ngoại

Application Programming Interface – Giao diện lập
trình ứng dụng

Personal Computer – Máy tính cá nhân

Operating System - Hệ điều hành


8

Danh mục hình vẽ
Hình 1.1 Tìm kiếm feature trong mô hình định vị ..................................................... 13

Hình 1.2 Cập nhật vị trí ước lượng trong mô hình định vị......................................... 14

Hình 1.3 Mô hình lập bản đồ .................................................................................... 15
Hình 1.4 Sơ đồ thuật toán SLAM cơ bản .................................................................. 17

Hình 2.1 Mô phỏng robot trong không gian một chiều.............................................. 20
Hình 2.2 Phân bố xác suất giá trị cảm biến trong không gian một chiều ................... 21
Hình 2.3 Phân bố xác suất kết hợp từ nhiều cảm biến ............................................... 23

Hình 2.3 Ảnh hưởng của chuyển động đến ước lượng trạng thái............................... 27

Hình 2.4 Một ví dụ về đóng vòng lặp ....................................................................... 34

Hình 2.5 Mạng Bayesian đơn giản dùng cho SLAM ................................................. 35
Hình 2.6 Minh họa d-khả ly ...................................................................................... 36
Hình 2.7 Mạng Bayesian liên quan đến vị trí robot ................................................... 37

Hình 2.8 Lựa chọn particle tạm thời theo mật độ xác suất......................................... 40

Hình 2.9 Cây particle ................................................................................................ 44

Hình 2.10 Bản đồ phân bố particle............................................................................ 45

Hình 2.11 Bản đồ DP-SLAM với 9000 particle (Nguồn Eliazar và Parr) .................. 46

Hình 3.1. Các thành phần của hệ thống ..................................................................... 47

Hình 3.2 Các thành phần chính của robot Lego ........................................................ 48

Hình 3.3 Sơ đồ lắp ráp Robot Lego .......................................................................... 49
Hình 3.4 Robot sau khi được lắp ráp......................................................................... 50

Hình 3.5 Cách bố trí Smartphone .............................................................................. 52
Hình 3.6 Giao diện chương trình chạy trên mobile ................................................... 54
Hình 3.7 Giao diện chương trình trên máy PC .......................................................... 55

Hình 3.8 Robot vẽ bản đồ trong phòng ..................................................................... 56


9

1.1.

CHƯƠNG 1: MỞ ĐẦU

Giới thiệu bài toán lập bản đồ

Bài toán lập bản đồ là một trong những vấn đề được nghiên cứu chủ yếu của ngành

Robot học. Xét một robot đơn giản sử dụng một bộ bánh xe có gắn động cơ với các
thiết bị vật lý để kiểm soát tốc độ, hướng di chuyển và những cảm biến để thu thập

thông tin về môi trường. Nhiệm vụ của robot nàylà di chuyển để lập bản đồ những nơi

con người không thể hoặc không muốn tiếp cận, đồng thời robot tự xác định được vị trí

của nó so với những đối tượng xung quanh. Kỹ thuật để robot có thể thực hiện được
nhiệm vụ này được gọi là SLAM.

Một ví dụ đơn giản về robot lập bản đồ là sử dụng robot có sự điều khiển của con người.

Thông qua camera gắn trên robot, người điều khiển có thể tiếp nhận thông tin để lập
bản đồ những nơi không thể tiếp cận. Trong trường hợp khác, lĩnh vực nghiên cứu về
SLAM của ngành robot học đang cố gắng tìm ra phương pháp để robot có thể hoạt động
tự động mà không cần bất kỳ sự trợ giúp nào của con người.

Việc giải bài toán robot lập bản đồ sẽ giúp con người nắm được thông tin những nơi có

địa hình rất phức tạp hoặc có cấu trúc không ổn định. Kỹ thuật SLAM giúp cho robot
có thể hoạt động được ở nơi có địa hình thường xuyên thay đổi và cập nhật lại thông tin
mới nhất về bản đồ khu vực này.

Robot vẽ bản đồ còn được sử dụng để thăm dò những nơi con người không thể tiếp cận

được như dưới lòng biển sâu, vùng bị thiên tai thảm họa, nơi có chiến sự, hành tinh
khác,…

Mục tiêu của luận văn này là nghiên cứu các kỹ thuật hiện tại và đưa ra một số cải tiến
để tăng hiệu quả cho bài toán SLAM tự động. Luận văn sẽ đề cập đến bộ lọc Kalman,

xương sống của các thuật toán SLAM hiện tại. Tiếp đến luận văn sẽ trình bày những
phương pháp mở rộng Fast SLAM và DP SLAM. Cuối cùng là phần thực nghiệm trên
robot Lego NXT và những kết quả đạt được.



10
1.2.

Lịch sử của SLAM

SLAM là một lĩnh vực chính nhưng tương đối mới của robot học. Mãi đến giữa năm

1980, Smith và Durant –Whyte phát triển một thể hiện của sự không chắc chắn trong
định vị. Đây là một bước tiến lớn, quan trọng trong việc tìm kiếm một giải pháp thực tế
chứ không phải là lý thuyết cho định vị robot. Bài báo của Smith và Durrant-Whyte

cung cấp một nền tảng cho việc tìm kiếm cách hàn gắn lỗi. Ngay sau đó một bài báo

của Cheesman, Chatila, và Crowley đã chứng minh sự tồn tại mối tương quan giữa các
lỗi của vị trí feature do sai sót trong chuyển độngảnh hưởng đến tất cả các định vị. Đây
là bước cuối cùng trong bài toán SLAM như chúng ta biết ngày nay.
1.3.

Một số thuậtngữ cơ bản

Trong mục này, tôi sẽ đưa ra những giải thích ngắn cho nhữngthuật ngữđược sử dụng
nhiều trong luận văn.

- Thuật ngữ “feature” được dùng để chỉ một điểm cụ thể trong môi trường mà có thể
được phát hiện bởi một robot di động.

- Thuật ngữ “map” dùng để chỉ vector các vị trí feature. Trong thực tế các feature có

thể là những vật thểnằm rải rác trong môi trường xung quanh robot, feature cũng có thể


là bức tường hoặc các góc cạnh của địa hình. Khi một feature được tìm thấy, nó sẽ
không tồn tại bền vững, bất cứ khi nào các robot thay đổi vị trí, nó mất theo dõi tất cả
các feature đã phát hiện và sẽcần phải tìm thấy chúng một lần nữa.

Một điều kiện cần để thực hiện bài toán lập bản đồlà robot phải ước lượng được khoảng
cách từ nó đến các feature tìm được. Khoảng cách này có thể được ước lượng bằng
nhiều cách khác nhau. Nếu một feature là kích thước vật lý đã biết, kích cỡ ước lượng

được của feature có thể chỉ ra khoảng cách. Một phương pháp khác là sử dụng nhiều
camera và kĩ thuật ảnh nổi để đo khoảng cách đường chéo [15]. Ngoài ra, có thể đo

khoảng cách bằng cảm biến hồng ngoại (IR sensor) hoặc cảm biến siêu âm (Ultrasonic


11
sensor)… Việc ước lượng khoảng cách là một bài toán nằm ngoài phạm vi của luận văn
này.

- Thuật ngữ “beacon” dùng để chỉ một feature trong môi trường đã được robot biết
trước. Becon được robot sử dụng cho việc định vị hoặc hiệu chỉnh sai số.

- Các thuật ngữ “pose” và “robot pose” được sử dụng đồng nghĩa để chỉ cách robot
định vị. Điều này bao gồm bất kì các biến liên quan đến chính robot. Trong trường hợp
đơn giản, pose là một vector 3 thành phần (x,y, ɵ) trong đó x,y là tọa độ của robot trong
không gian 2 chiều và ɵ là hướng của robot. Trong thực tế, pose có thể được biểu diễn
phức tạp hơn.

- “Odometry” là việc sử dụng dữ liệu thu thập được từ các cảm biến chuyển động để


ước tính sự thay đổi ở vị trí theo thời gian. Odometry được một số robot, bao gồm cả
robot có chân và robot có bánh, sử dụngđể ước tínhtương đối vị trí của chúngso với một
vị trí bắt đầu.
1.4.

Bài toán SLAM

Trong mục 1.2, lịch sử Robot SLAM đã ghi nhận bước phát triển quan trọng trong

nghiên cứu SLAM là việc chứng minh rằng các lỗi tại vị trí feature thu được bởi tác
nhân có tương quan với nhau. Mối tương quan tồn tại bởi vì một lỗi trong việc định vị
sẽ có tác động chung vào vị trí của tất cả các feature, chúng ta sẽ thảo luận chi tiết hơn
về vấn đề này trong chương về bộ lọc Kalman.

Hiểu và sử dụng mối quan hệ giữa lỗi trong định vị feature và vị trí robot là vấn đề cốt

lõi của việc nghiên cứu về SLAM, nó là động lực chính đằng sau việc giải quyết cả định

vị và lập bản đồ. Trong chương này sẽ xem xét tách riêng 2 vấn đề định vị và lập bản
đồ riêng để phát triển sâu bài toán SLAM và tiếp tục làm rõ động lực sau việc tìm kiếm
một giải pháp đồng thời.


12
1.4.1. Mô hình định vị
Giả sử chúng ta có một robot di động trong môi trường có các feature tương ứng với
mỗi điểm trong môi trường đó và các robot có thể tính toán khoảng cách và hướng đi.

Tiếp tục giả sử rằng sau khi khởi tạo, robot không thể biết chính xác vị trí và hướng của
mình.


Hãy tưởng tượng rằng robot bắt đầu ở vị trí đã biết x0, và nó đã biết trước thông tin về
một tập P bao gồm một vài vị trí feature.

Khi robot di chuyển một khoảng cách cho trước theo một hướng nhất định (tức là các

vector chuyển động u1) đến một vị trí gần x1,các feature cần phải được dịch chuyển để
xác định pose mới của robot. Nếu các thiết bị chuyển động là hoàn toàn chính xác các

feature có thể được thu lại bằng cách giả sử họ đã di chuyển ngược chiều với u1. Tuy
nhiên do các chuyển động không chính xác, nên các robot phải tìm kiếm các feature.
Việc tìm kiếm này được thực hiện bằng cách bắt đầu tìm ở gần vị trí mong đợi của
feature và dần mở rộng ra ngoài. Điều này được thể hiện ở hình 1.1


13

X0
P0
P1

U1

X1

Chuyển động thực tế

Chuyển động dự đoán

(Tìm kiếm P1 mở rộng từ vị trí mong

đợi)

Quan sát thực tế

Quan sát đự đoán

Vị trí thực của feature

Vị trí dự đoán của feature

Hình 1.1 Tìm kiếm feature trong mô hình định vị
Chú ý x1 đó cũng như các vector chuyển động tương ứng với ước lượng. Một khi feature

được thu lại, vị trí ước lượng mới của robot được cập nhật là x2. Quá trình này được
thực hiện như hình 1.2.


14

X0

P1

P0

X1
X2

(Vị trí ước lượng được cập nhật
dựa trên những quan sát mới)


Véc tơ cập nhật vị trí
Quan sát thực tế

Vị trí thực của feature

Hình 1.2 Cập nhật vị trí ước lượng trong mô hình định vị
1.4.2. Mô hình lập bản đồ
Quá trình lập bản đồ, trong một nghĩa nào đótrái ngược mô hình định vị được đề cập ở

phần 1.4.1. Trong lập bản đồ, chúng ta giả định các robot biết chính xác nó mọi lúc, có

nghĩa là, giả sử hệ thống chuyển động là hoàn hảo và không có một lỗi GPS. Những gì
robot thiếu trong khi lập bản đồ là cách biết được vị trí tuyệt đối của feature. Nhận thấy

rằng trái ngược với định vị, các robot không bắt đầu với một thế giới quan thực, các

robot có thể xác định vị trí các feature để xây dựng một bản đồ, z0 = {z00, z01... z0n}
trong đó zij là dự toán feature thứ j tại bước thứ i. Tất nhiên, z0 sẽ chỉ chứa xấp xỉ trong
những vị trí feature thực. Khi tác nhân di chuyển, bản đồ có thể được thực hiện chính
xác hơn.

Xét robot tương tự như mục 1.4.1, bắt đầu tại x0 và di chuyển theo vector u1 đến vị tríx1.
Các robot sẽ một lần nữa cần phải thu được tập các feature, cũng giống như nó từng
làm trong mô hình định vị, nhưng lúc này các vị trí nhận được của các feature sẽ chuyển

từ các vị trí dự kiến do cảm biến chính xác hơn là odometry không chính xác (Odometry


15

là việc sử dụng dữ liệu từ các cảm biến chuyển động để ước tính sự thay đổi củavị trí
theo thời gian). Cũng giống như trước, các feature có thể được đặt lại bằng cách tìm

gần nơi mà robot sẽ tìm thấy chúng. Mỗi lần điều này được thực hiện các robot sẽ xây
dựng một bản đồ mớiz1, bao gồm các ví trí mới của các feature trong z0

Z01

Z00

X0

P1

X1
Z11

P0

Z10
Chuyển động
Quan sát

Vị trí thực của feature

Vị trí dự đoán của feature

Hình 1.3 Mô hình lập bản đồ
Để tạo ra bản đồ mới z2, có kết hợp các thông tin của z0 và z1 nhưng chỉ chứa một


trường hợp của mỗi feature, các robot có thể chọn một trong nhiều lựa chọn. Một cách

để giảm thiểu tổng khoảng cách giữa hai vị trí nhận được của mỗi feature pj (các vị trí
nhận đượclà z0j là thành phần của z0, và z1j là thành phần của z1) và vị trí mong đợi
mới của feature pi (z2j là thành phần của z2). Vấn đề này trong ví dụ hai chiều được
thể hiện là số lượng lựa chọn bất kỳ điểm nào trên đường nối z0i và z1i cho mỗi feature

pi (Biết rằng: z0i và z1i là vị trí nhận được trái ngược nhau của feature i).Phương pháp

cơ bản này có thể chỉ tốt trong việc lập bản đồ, mặc dù nó không phải là tối ưu. Một lựa
chọn khác để giảm thiểu bình phương khoảng cách chứ không phải là tổng số khoảng

cách là bộ lọc Kalman (KF). Trong Chương 2, chúng ta sẽ thấy rằng giải pháp này là


16
khá hiệu quả.
Phương pháp mà chúng ta lựa chọn để kết hợp các bộ cảm biến mới, có vẻ như an toàn
giả sử rằng zj nói chung sẽ cải thiện. Khi robot di chuyển và đọc nhiều hơn, dự toán
trong zj sẽ có xu hướng ngày càng trở nên chính xác miễn là chúng ta làm cho một số

giả định về lỗi chấp nhận được. Đặc biệt chúng ta phải giả sử sẽ hiệu chỉnh được cảm
biến.

1.4.3. Bài toán mô phỏng
Những thuật toán được mô tả ở các mục trên đều yêu cầu đầu vào không có trong thực

tế. Bài toán định vị trả về kết quả là pose của robot nhưng yêu cầu vị trí của những

feature(tức là một bản đồ)lại cần phải chính xác như là đầu vào ở mỗi bước lặp của

thuật toán. Ngược lại, thuật toán lập bản đồ tạo ra một bản đồ nhưng cần một poseđể
làm đầu vào. Thực tế thường không có đầu vào như vậy.

Đầu vào của mỗi quá trình là đầu ra của yêu cầu khác mà chúng có liên quan và có thể

được kết hợp thành một phương pháp duy nhất. Nếu cả hai quá trình được sử dụng cùng
nhau sẽ có hi vọng cho Robot chuyển hướngmà không cần bản đồ trướcđó hoặc độ
chính xác của GPS/ odometry. Chúng ta xem xét giải pháp sau đây.

Giả sử một robot bắt đầu từ một số vị trí đã biết. Nó sử dụng tiến trình được mô tả trong

phần 1.4.2 – Mô hình lập bản đồ để xây dựng một bản đồ ban đầu. Sau đó nó di chuyển
đến một vị trí mới và cập nhật vị trí mong muốn như được mô tả trong 1.4.1 – Mô hình

định vị. Cuối cùng mới tính được pose tạo ra một bản đồ mới bằng cách kết hợp với
bản đồ gốc như được mô tả trong phần 1.4.2. Bằng cách lặp lại quá trình này, ta cung
cấp một bản đồ cho đầu vào đối với chức năng định vịvà một vị trí cho đầu vào đối với

các chức năng lập bản đồ. Đây là một giải pháp khả thi cho bài toán SLAM. Sơ đồ hình
1.4cho thấy ý tưởng SLAM cơ bản, chú ý đến thứ tự mà quá trình này được thực hiện.


17
1: Khởi tạo bản đồ z0

2: Di
chuyển,
cập nhật
vị trí x1
5: Cập

nhật bản
đồ thành
z, sử
dụngx, z0

3: Tạobản
đồ z1
4: Cập nhật vị
trí thành x, sử
dụng bản đồ z1

Chuyển động dự đoán
Quan sát
Cập nhật pose hoặc feature
Vị trí thực của feature
Vị trí mong muốn
của feature

Hình 1.4 Sơ đồ thuật toán SLAM cơ bản
Ngoài ra, chúng ta cũng cần chú ý tới việc định vị kết hợp với thuật toán lập bản đồ
có đầu vào không hoàn toàn chính xác. Trong phần 1.4.1 - Mô hình Định vị, việc tính

toán vị trí chiếc xe giống như trong thực tế.Điều này có thể xác định được bởi vì các
pose của robot luôn tính được bằng việc sử dụng vị trí chính xác của feature. Tương

tự như vậy, chúng ta giả sử các thuật toán lập bản đồ sẽ chỉ hội tụ khi các pose của
robot luôn biết chính xác.

Trong phương pháp này cần hai bước là định vị và lập bản đồ. Theo mô tả ở trên,


robot di chuyển đồng thời cập nhật pose và cuối cùng là lập bản đồ. Điều này cho
phép các robot sử dụng pose của xe để tạo ra bản đồ mới tốt hơn. Nếu robot thay vì
di chuyển, lập bản đồ rồi mới định vị thì nó có thể sử dụng bản đồ mới để tạo ra một

định vị tốt hơn. Ta thấy rằng thông tin mới luôn có giá trị được bỏ qua trong bước thứ
hai. Pose của xe được tính toán và lập bản đồ gần đúng làm ảnh hưởng đến bản đồ
khác. Điều này dẫn đến một phương pháp toán học chặt chẽ đầu tiên cho bài toán
SLAM: bộ lọc Kalman.


18

CHƯƠNG 2: CƠ SỞ LÝ THUYẾT
2.1. Lọc Kalman
Bộ lọc Kalman (KF) được phát triển bởi R.E. Kalman, bài báo nổi bật về vấn đề này

được xuất bản vào năm 1960 [8]. Lọc Kalman là định vị cụ thể và lập bản đồ đã rất
nổi tiếng ngay sau khi Kalman xuất bản bài báo của mình trong năm 1960. Tuy nhiên,

KF khôngđược sử dụng để thể hiện cho toàn bộ trạng thái trên thế giới (và vì thế

không được sử dụng như một giải pháp hoàn chỉnh cho bài toán SLAM) cho đến năm
1988.

Bộ lọc Kalman là một thuật toán được sử dụng để xử lý dữ liệu và ước lượng giá trị

của các biến số. Trong bài toán SLAM, các giá trị biến được ước lượng sẽ bao gồm

các pose của robot và các feature hay là những trạng thái của thế giới. Các dữ liệu


được xử lý có thể bao gồm vị trí xe, đầu vào cho thiết bị chuyển động, cảm biến đọc

và cảm biến chuyển động của robot di động. Như vậybộ lọc Kalman có thể sử dụng

tất cả các dữ liệu có sẵn để ước lượng đồng thời pose của robot và tạo ra một bản đồ
feature. Welch và Bishop giải thích rằng ước lượng trạng thái được cung cấp bởi các

bộ lọc Kalman sẽ sử dụng bất kỳ thông tin có sẵn để giảm thiểu trung bình sai số bình
phương của các ước lượng liên quan.
2.1.1. Ý tưởng Kalman
Trong phần 1.4.2 - Mô hình lập bản đồ, chúng ta xem xét việc lựa chọn vị trí mong
muốn của feature dựa vào việc giảm thiểu lỗi bình phương. Vấn đề này về cơ bản là

dựa vào lọc Kalman. Trong thực tế, nếu chúng ta bắt đầu bằng cách tạo ra một phương

pháp toán học của giảm thiểu lỗi bình phương trong một tình huống đơn giản, chúng
ta có thể mở rộng các phương pháp để phù hợp với SLAM. Luận văn này trình bày
các phương pháp cho bài toán SLAM.

Phần 2.1.4 - Phát triển bộ lọc đơn giản sẽ xây dựng một bộ lọc Kalman đơn giản cho

bài toán ước lượng tĩnh gần như tương tự nhưng đơn giản hơn các bài toán SLAM.
Trong các phần đang xử lý, các bộ lọc từ phần 2.1.4 sẽ được điều chỉnh để mô tả bài
toán SLAM.


19
2.1.2. Lọc Kalman và SLAM
Kỹthuật KF là một lựa chọn phổ biến cho các nhà nghiên cứu SLAM. Một phần là do


KF có khả năng kết hợp tất cả các phép đo có sẵn và cung cấp một ước lượng bình
phương của quá trình trạng thái. Hơn nữa, trong bài toán SLAM, SLAM sẽ là bình
thường nếu các cảm biến của robot có nhiễutự do, xử lý có thể là mục đích quan trọng

của KF. Cuối cùng, KF hoạt động tốt trong thực tế và vì vậy các nhà nghiên cứu tiếp
tục sử dụng nó.

2.1.3. Nhiều trắng, đường cong Gausian, quá trình tuyến tính

Nhiễu trắng là một tín hiệu ngẫu nhiên có mật độ phân bố công suất không đổi trên

miền tần số. Tín hiệu này có tên là nhiễu trắng vì nó có tính chất tương tự với ánh

sáng trắng. Chúng ta không thể tạo ra nhiễu trắng theo đúng lý thuyết vì theo định
nghĩa của nó, nhiễu trắng có mật độ phổ công suất phân bố trong khoảng tần số vô
hạn và do vậy nó cũng phải có công suất vô hạn. Tuy nhiên, trong thực tế, chúng ta

chỉ cần tạo ra nhiễu trắng trong khoảng băng tần của hệ thống mà chúng ta đang xem

xét. KF hoạt động trong việc giả sử rằng thiết bị truyền động cụ thể nào hoặc lỗi cảm
biến sẽ không hiển thị giá trị của bất kỳ lỗi nào.

Phân bố Gausian: Một phân bố Gaussian của nhiễu có thể đạt được. Một phân bố
Gaussian của nhiễu nghĩa là bất cứ lúc nào mật độ xác suất của biên độ của nhiễu hệ
thống có dạng đường cong hình chuông. Theo định lý giới hạn trung tâm, một đường

cong Gauss phát sinh bất cứ khi nào với nhiều biến ngẫu nhiên độc. Ví dụ, nếu chúng

ta làm thử nghiệm với một bộ cảm biến khoảng cách để xác định phương sai của việc
đọc được thực hiện bởi các cảm biến, việc đọc cảm biến xác định một đường cong


Gaussian tương ứng với khoảng cách thực sự. KF có thể làm điều nàyvà chúng ta
phải có khả năng giả định một phân phối Gaussian của bất kỳ nhiễu nào.

Giả định thực hiện bởi các bộ lọc Kalman thực sự có thể gây ra vấn đề cho các nhà
nghiên cứu SLAM. Giả định này là các mô hình quy trình là tuyến tính mặc dù nó

không đúng với bài toán SLAMcó tình huống phức tạp nhất. Tuyến tính của một quá

trình là thước đo sự phức tạp của nó. Trong phòng thí nghiệm với sàn nhà cứng, mô

hình xử lý cho robot di động có thể là tuyến tính. Nếu các robot đặt trong trường


20
ngoài trời, nơi các mảnh vỡ có thể chặn một bánh xe hoặc năng lượng robot thả ra để
thiết bị truyền động mất hiệu lực khi đó mô hình xử lý sẽ là phi tuyến. Giả sử điều
khiển đầu vào là một vector giá trị. Chúng ta sẽ dùng một mô hình xử lý tuyến tính

nếu nó có thể mô tả vị trí có khả năng nhất của robot như là một hàm tuyến tính của
các giá trị bổ sung vào vị trí cũ.

Ví dụ, đầu vào thiết bị truyền động là một giá trị tương ứng với năng lượng gửi đến
động cơ cho một lượng thời gian. Nếu chúng ta luôn có thể nhân giá trị đầu vào của
thiết bị truyền động bởi một số không đổi và thêm nó vào vị trí trước đó để có được
vị trí mới có khả năng nhất của robot, sau đó các mô hình xử lý là tuyến tính. Ngược

lại, các mô hình xử lý là phi tuyến. Các Kalman Filter vẫn còn hữu ích trong nhiều
tình huống mà các mô hình xử lý gần như tuyến tính, nhưng sau đó chúng ta sẽ muốn


có một giải pháp KF các mô hình xử lý phi tuyến. Các giải pháp sẽ được phát triển
trong phần Bộ lọc Kalman mở rộng.

2.1.4. Phát triển một bộ lọc đơn giản

Trước tiên, chúng ta xem xét ví dụ 1 chiều. Giả sử một robot di động được giới hạn

trong một đường thẳng trên mặt đất. Nó thực hiện một quan sát z0, xác định một
feature duy nhất p0và sử dụng quan sát này để thực hiện một ước lượng khoảng cách
giữa các robot và các feature là x0

Hình 2.1 Mô phỏng robot trong không gian một chiều
Giả sử thử nghiệm trước đã cho chúng ta một ước lượng độ chính xác của cảm biến.
Sẽ hợp lý khi cho rằng mật độ xác suất sẽ xấp xỉ Gaussian như phần giải thích trong

phần 2.3 Các khái niệm: nhiễu trắng, đường cong Gaussian, và quá trình tuyến tính.


21
Bây giờ chúng ta có thể khẳng định rằng, tất cả giá trị bằng nhau, ước lượng của x0
có nhiều khả năng hơn mọi ước lượng khác, và lỗi cụ thể hoặc không đúng, var0.

Hình 2.2 Phân bố xác suất giá trị cảm biến trong không gian một chiều
Giả sử rằng robot đọc lần thứ hai với một số cảm biến khác chứ không di chuyển
ngay lập tức, điều này cho phép chúng ta phát triển theo cách kết hợp thông tin mới

trước khi bài toán trở nên phức tạp. Trong lần đọc mới z1, ước lượng chiếc xe mới là

x1 với một phương sai mới var1. Bây giờ, robot của chúng ta ở vị trí: x0 hay x1? Chúng
ta có thể tin tưởng vào cảm biến với độ chính xác tốt hơn, tức là phương sai thấp hơn

- nhưng điều này có thể sẽ bỏ qua giá trị được cung cấp bởi các cảm biến khác. Ngược
lại, nếu tính trung bình giữa hai cách tính thì sẽ bỏ qua độ sự khác nhau về độ chính
xác giữa các cảm biến(var0 khác var1).

Trong việc tìm kiếm một cách tính thích hợp của việc ước lượng x theo trọng số mỗi
phép đo, x được cập nhật theo cách sau:

x <- (var1/(var0 + var1)) x0 + (var0/(var0 + var1))x1

(2.1)

Trong đó var0,var1 là phương sai cho các ước lượng x0, x1
Giả sử var1 lớn hơn var0. Ta thấy trọng số lớn hơn được đặt cho x0, điều này hợp với
mong muốn vì chúng ta biết rằng x0 là ước lượng dựa trên các cảm biến chính xác
hơn. Phương pháp này cho phép chúng ta ước tính trọng số tỷ lệ nghịch với phương
sai. Do đó,giá trị kết quả giảm thiểu các lỗi bình phương đối với các ước lượng.

Với ước lượng mới x, xem xét các phương sai var của x. Chúng ta mong đợi var sẽ
phụ thuộc vào hai phương sai trước, nhưng nó nên nhỏ hơn cả hai, tức là var

22
(var0, var1). Nếu đây không phải là trường hợp mong đợi, nếu var> var0, sau đó ta kết
hợp thông tin từ các cảm biến thứ hai thì coi như đã bị mất thông tin. Miễn là các cảm
biến được hiệu chỉnh đúng không xảy ra. Tuy nhiên, theo như giải thích trong mục

2.3 - Các khái niệm: Nhiễu trắng, đường cong Gaussian và quá trình tuyến tính, hiệu
chuẩn thích hợp của cảm biến là hợp lý.

Người ta cũng mong chờ var đểcải thiện nhanh hơn phương sai trước nếu các cảm

biến của hai robot có độ chính xác là như nhau. Đó là: (min (var1, var0) - var) / (var1
+ var0) là tối đa khi var1 = var0. Để thấy điều này, hãy tưởng tượng hai bộ cảm biến
độc lập đo mà đo khoảng cách như nhau. Nếu hai bộ cảm biến chính xác như nhau,

chúng ta có thể có ước lượng tốt hơn đáng kể (trung bình) và kết hợp thông tin đó

bằng cách chọn một trong cái cụ thể. Nếu một cảm biến là chính xác hơn nhiều so với
các khác, mặt khác, không có nhiều thông tin có thể đạt được bằng cách kết hợp các

bộ cảm biến không chính xác (mặc dù cần phải có một số đạt được thông tin nhỏ nếu
việc đọc là thích hợp).

Phương sai mới có thể được thống kê xác định như sau:
1/var<- 1/var1 + 1/var0

(2.2)

Ta thấy rằng phương trình cập nhật này làm mẫu cho cách chúng ta dự kiến phương

sai để xử lý:var< min(var1,var0) cho bất ký phương sai var1 và var0, (min(var1,var0)
– var) / (var1 + var0) nhỏ nhất khi var1 = var0.

Hình2.3 cho thấy sự phân bố mật độ xác suất có hình chuông xác định bởi sự kết hợp
x0 và var0 theo x1 và var1 được xác định bởi(2.1) và(2.2)


23

Hình 2.3 Phân bố xác suất kết hợp từ nhiều cảm biến

Ước lượng tối đa cho z0, z1, var0 và var1

Các phương trình cập nhật mà chúng ta đã mô tả trong phần này đã rất gần với một

ứng dụng cơ bản của bộ lọc Kalman. Tất cả những gì còn lại là đại số và xác định lại
các điều kiện. Trước tiên, để thiết lập các phương trình thích hợp cho SLAM đệ quy,
chúng ta hãy khái quát quá trình cập nhật có giá trị đối với bất kỳ bước i:
Công thức (1) trở thành:
xi = (vari-1/(vars + vari-1)) xs + (vars/(vars + vari-1))xi-1

(2.3)

Trong đó:
-

xi là ước lượng vị trí cập nhật tại thời điểm i

-

xs là một ước lượng vị trí dựa trên việc đọc của cảm biến mới

-

xi-1 là ước lượng vị trí cuối cùng (tức là vị trí đoán tốt nhất dựa trên việc đọc

-

vari-1 là sự thay đổi của xi-1

cảm biến i-1màchúng ta đã thực hiện cho đến nay)

24
-

var là phương sai của xs

Phương trình này là cách kết hợp nhiều cách đọc của cảm biến độc lập. Cập nhật

không đúng sẽ được giải quyết ngay. Sau đó ta sẽ xem xét cách sắp xếp lại để làm
cho phương trình cập nhật phản ánh đúng chuẩn dạng lọc Kalman, bắt đầu với (2.3)

xi=xi-1+Ki*[xs-xi-1]

(2.4)

Trong đó :
(2.5)
Ước lượng xi là xi-1 được chuyển theo sự khác biệt giữa xi-1 và một số cảm biến có
ước lượng gốc xs. Ước lượng mới xs theo ki có liên quan đến sự thay đổi của các ước
lượng mới mới cho sự thay đổi của ước lượng cũ. Phương trình (2.4) trong mẫu lọc

Kalman. Phương trình cập nhật phương sai. Khái quát phương trình (2.2), chúng ta
có:

1/vari = 1/vars + 1/vari-1
Phiên bản bộ lọc Kalman của phương trình cập nhật phương sai:


25

Cuối cùng, sử dụng (2.5), ta được phương trình sau:
vari= vari-1- Ki* vari-1

(2.6)

Bài toán ước lượng tĩnh trên đơn giản hơn bài toán SLAM thực tế vớiba khía cạnh
quan trọng:
-

Trước tiên, các robot trong ví dụ trên không di chuyển thực sự. Chúng ta cần

tính đến việc xe di chuyển và lỗi khi di chuyển (Điều này tương đối đơn giản và được
xử lý trong phần tiếp theo)
-

Thứ hai, bộ lọc Kalman, chúng ta đã phát triển cho đến nay chỉ xem xét vị trí

robot liên quan đến một feature riêng biệt. Với nhiều feature chúng ta cần phải cập
nhật nhiều giá trị sau mỗi lần đọc thay vì chỉ một.
-

Thứ ba, môi trường trong ví dụ trên là một chiều. Bất kỳ môi trường SLAM

thực tế phải biểu diễn ít nhất trong không gian 2 chiều.

2.1.5. Phát triển lọc Kalman cho robot chuyển động
Trong phần 2.4 Phát triển một bộ lọc đơn giản, chúng ta có thể thực hiện tốt ước
lượngbằng cách theo dõi và cập nhật hai giá trị: ước lượng vị trí xi và ước lượng vari.

Đểđiều khiển đầu vào được thực hiện bởi các robot, ta cần tìm ra cách điều khiển đầu
vào ảnh hưởng đến hai giá trị.