(Luận văn thạc sĩ) Xây dựng bản đồ 2D cho Robot di động sử dụng Point Cloud Library

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT
THÀNH PHỐ HỒ CHÍ MINH

LUẬN VĂN THẠC SĨ
ĐÀO HỒNG PHONG

XÂY DỰNG BẢN ĐỒ 2D CHO ROBOT DI ĐỘNG
SỬ DỤNG POINT CLOUD LIBRARY

NGÀNH: KỸ THUẬT ĐIỆN TỬ - 605270
Hướng dẫn khoa học: TS. Ngơ Văn Thun

Tp. Hồ Chí Minh, năm 2014


MỤC LỤC
Chương 1: TỔNG QUAN .................................................................................1
1.1.

Giới thiệu .............................................................................................1

1.2.

Kết quả nghiên cứu trong và ngoài nước ............................................4

1.3.

Mục tiêu của đề tài ..............................................................................7

1.4.

Nội dung thực hiện của đề tài ..............................................................7

Chương 2: CƠ SỞ LÝ THUYẾT ......................................................................8
2.1.

Giới thiệu chung về Kinect Xbox 360 và Point Cloud Library ..........8

2.1.1. Kinect Xbox 360 ............................................................................8
2.1.2. Point Cloud Library .......................................................................9
2.2.

Các bước ghép đám mây ...................................................................11

2.3.

Ghi dữ liệu đám mây mới sử dụng phương pháp dời tọa độ .............13

2.3.1. Chuyển từ tham chiếu cục bộ thành tham chiếu toàn cục dùng ma
trận biến đổi ......................................................................................................13
2.3.2. Khởi tạo offset..............................................................................15
2.4.

Ghi đám mây sử dụng ICP (Iterative Closest Points) .......................16

2.4.1. Thuật toán ICP .............................................................................16
2.4.2. Ghép đám mây điểm chồng một phần sử dụng ICP ....................19
2.5.

Ghi đám mây sử dụng SAC-IA .........................................................20

2.6.

Quyết định lồng đám mây điểm ........................................................22

2.7.

Xử lí cuối cùng ..................................................................................23

2.8.

Chuẩn bị cho lần lặp tiếp theo ...........................................................23

2.9.

Kỹ thuật lọc đám mây để giảm số lượng điểm ảnh ...........................24

I


2.9.1. Lọc thông dải ...............................................................................24
2.9.2. Giảm số lượng mẫu (Down Sampling) ........................................25
2.9.3. Loại bỏ Outliers ...........................................................................25
2.9.4. Xây dựng lại bề mặt .....................................................................26
2.10.

Kỹ thuật xác định các cặp điểm tương đồng giữa hai ảnh .............26

2.11.

Kỹ thuật xác định vị trí 3D (Pose) tương đối giữa hai đám mây ...28

2.11.1. Tối ưu hóa ma trận biến đổi giữa hai đám mây điểm ................29
2.11.2. Xác định ma trận biến đổi tốt nhất [3] .......................................30
Chương 3: THUẬT TOÁN THỰC HIỆN.......................................................32
3.1.

Khởi tạo đám mây điểm đầu tiên ......................................................34

3.2.

Giảm số lượng điểm ảnh cho mỗi đám mây điểm ảnh ......................34

3.3.

Tìm điểm đặc trưng của đám mây .....................................................35

3.4.

Tìm điểm tương đồng giữa hai đám mây ..........................................36

3.5.

Tìm ma trận biến đổi giữa hai đám mây điểm ảnh............................39

3.6.

Ghép nhiều đám mây điểm khác nhau sử dụng thuật toán SAC-IA và

ICP

...........................................................................................................40
3.7.

Chiếu đám mây xuống nền nhà .........................................................42

Chương 4: KẾT QUẢ......................................................................................43
4.1.

Thi công phần cứng và phần mềm sử dụng.......................................43

4.2.

Giảm số lượng điểm ảnh ...................................................................45

4.3.

Thực hiện ghép hai đám mây điểm ảnh với góc xoay là 50o ............46

4.4.

Thực hiện ghép hai đám mây khi robot tịnh tiến về phía trước ........47

4.5.

Thực hiện ghép liên tiếp nhiều đám mây và chiếu xuống nền tạo bản

đồ 2D

...........................................................................................................49

II


Chương 5: KẾT LUẬN ...................................................................................51
5.1.

Kết quả đạt được................................................................................51

5.2.

Hướng phát triển của đề tài ...............................................................51

III


PHỤ LỤC: DANH SÁCH CÁC HÌNH
HÌNH

TRANG

Hình 1.1: Robot di chuyển dựa vào cảm biến .................................................................................................... 1
Hình 1.2: Hiện tượng chồng ảnh của 3 frame ảnh trên cùng 1 hệ trục của Kinect ........................................... 2
Hình 1.3: Các đám mây rời rạc trong khơng gian .............................................................................................. 3
Hình 1.4: Ví dụ về các góc nhìn khác nhau ở các vị trí khác nhau ...................................................................... 3
Hình 1.5: Các đám mây đã được ghép lại thành một tổng thể.......................................................................... 3
Hình 1.6: Dùng xe ơ tơ được trang bị thiết bị định vị GPS để lập bản đồ .......................................................... 4
Hình 1.7. Dùng robot được trang bị đầu đọc RFID và các thẻ tag để lập bản đồ .............................................. 4
Hình 1.8: Định vị robot di động sử dụng vật mốc nhân tạo ............................................................................... 5
Hình 1.9: Robot tự hành tránh vật cản .............................................................................................................. 5

Hình 1.10: Scan 3D một đối tượng..................................................................................................................... 6
Hình 1.11: Bản đồ 3D mơi trường trong nhà ..................................................................................................... 6
Hình 2.1: Thiết bị Kinect ..................................................................................................................................... 8
Hình 2.2: Logo và một số Module quan trọng của PCL ...................................................................................... 9
Hình 2.3: Cấu trúc của một tập tin *.pcd ......................................................................................................... 10
Hình 2.4: Các bước thực hiện ghi một cặp dữ liệu đám mây điểm ảnh ........................................................... 12
Hình 2.5: Bản đồ kết quả của cộng các đám mây sau khi xử lí roto-translation (a) và sau khi xử lí ghi dữ liệu
dùng SAC-IA và ICP (b) ..................................................................................................................................... 12
Hình 2.6: Lưu đồ xây dựng lại bản đồ .............................................................................................................. 13
Hình 2.7: Quá trình chuyển đổi đám mây tham chiếu ..................................................................................... 14
Hình 2.8: Hệ tọa độ của camera Kinect và chung quanh ................................................................................. 14
Hình 2.9: Hệ tọa độ toàn cục và mối liên quan với hệ tọa độ của Kinect......................................................... 15
Hình 2.10: Lưu đồ ghi dữ liệu chỉ sử dụng ICP................................................................................................. 16
Hình 2.11: Hai đám mây điểm chồng lấp một phần ........................................................................................ 19
Hình 2.12: Lưu đồ ghi đám mây sử dụng SAC-IA và ICP .................................................................................. 21

IV


Hình 2.13 : Lưu đồ tìm tính năng cần thiết cho sắp xếp SAC-IA ....................................................................... 21
Hình 2.14: Quy trình quyết định lồng một đám mây đã được ghi vào trong bản đồ ...................................... 23
Hình 2.15 Các bước lọc một đám mây điểm ảnh ............................................................................................. 24
Hình 2.16: Mơ tả lọc dạng Voxel Grid .............................................................................................................. 25
Hình 2.17: Các cặp điểm tương đồng giữa hai đám mây điểm ảnh ................................................................ 27
Hình 2.18: Áp dụng Box filter, 1 bước trong quá trình tăng tốc xác định điểm đặc trưng của SURF .............. 28
Hình 2.19: Mô tả đặc trưng thông qua hướng và thông tin của các vùng lân cận .......................................... 28
Hình 2.20: Đặc trưng của điểm ảnh ................................................................................................................. 28
Hình 2.21: Minh họa việc dùng RANSAC áp dụng cho việc tìm đường thẳng trong mặt phẳng ...................... 30
Hình 2.22: Minh họa kết quả ghép 2 đám mây sau khi áp dụng Ransac và Levenberg–Marquardt để xác định
ma trận biến đổi .............................................................................................................................................. 32

Hình 3.1: Lưu đồ tổng quát thực hiện của đề tài ............................................................................................. 33
Hình 3.2: Lưu đồ thuật toán khởi tạo đám mây điểm đầu tiên ....................................................................... 34
Hình 3.3: Đám mây điểm ảnh thực tế thu được từ camera và dữ liệu của đám mây ..................................... 34
Hình 3.4: Lưu đồ lọc đám mây dùng Voxel Grid............................................................................................... 35
Hình 3.5: Lưu đồ tìm điểm đặc trưng của đám mây ........................................................................................ 36
Hình 3.6: Lưu đồ điều khiển robot xoay phải 50o ............................................................................................. 37
Hình 3.7: Vùng trùng lắp giữa hai đám mây điểm ảnh.................................................................................... 37
Hình 3.8: Tìm độ rộng của vùng giao nhau giữa hai frame ảnh ...................................................................... 38
Hình 3.9: Tìm chiều cao của vùng giao nhau giữa hai frame ảnh .................................................................... 38
Hình 3.10: Lưu đồ xác định cặp điểm tương đồng giữa hai đám mây ............................................................ 39
Hình 3.11: Hàm biến đổi điểm ảnh ở đám mây gốc đến điểm ảnh ở đám mây đích ...................................... 40
Hình 3.12: Lưu đồ ghép liên tiếp nhiều đám mây điểm ảnh dùng SAC-IA kết hợp ICP .................................... 40
Hình 3.13: Bề rộng của frame ảnh khi robot cách tường 1 mét ...................................................................... 41
Hình 3.14: Chiếu một điểm ảnh xuống nền nhà ............................................................................................... 42
Hình 3.15: Lưu đồ thực hiện chiếu đám mây điểm ảnh xuống mặt phẳng nền nhà ........................................ 42
Hình 4.1: Robot di động được dùng trong đề tài ............................................................................................. 43

V


Hình 4.2: Board điều khiển robot và máy tính sử dụng trong đề tài ............................................................... 44
Hình 4.3: Hai đám mây điểm ảnh và vật mốc tự nhiên (ghế) trước khi ghép .................................................. 46
Hình 4.4: Ma trận biến đổi từ đám mây đích vào đám mây nguồn................................................................. 46
Hình 4.5: Ghép hai đám mây điểm ảnh dùng SAC-IA và ICP ............................................................................ 47
Hình 4.6: Hai đám mây điểm ảnh được ghép thành công với vật mốc tự nhiên là chiếc ghế ......................... 47
Hình 4.7: Vật mốc ở xa (ảnh trái) và vật mốc .................................................................................................. 48
Hình 4.8: Frame sau khi tịnh tiến (ảnh phải) là một ........................................................................................ 48
Hình 4.9: Hai đám mây điểm ảnh được ghép thành công trong trường hợp robot tịnh tiến về phía trước
chạm tường ..................................................................................................................................................... 48
Hình 4.10: Hai đám mây điểm ảnh được ghép thành công trong trường hợp robot tịnh tiến về phía trước có

vật mốc ............................................................................................................................................................ 49
Hình 4.11: Các vị trí dừng chụp đám mây điểm ảnh cho trường hợp ghép liên tiếp ....................................... 49
Hình 4.12: Kết quả ghép liên tiếp nhiều đám mây và chiếu xuống mặt phẳng nền nhà.................................. 50

VI


PHỤ LỤC: DANH SÁCH CÁC BẢNG BIỂU
Bảng 2.1: Góc mở và tiêu cự của RGB và IR camera .......................................................................................... 9
Bảng 4.1: Thông số cơ bản của robot di động ................................................................................................. 44
Bảng 4.2: Bảng mã lệnh điều khiển cơ bản...................................................................................................... 44
Bảng 4.3: Thông số cài đặt giao tiếp cổng USB COM ...................................................................................... 45
Bảng 4.4: Bảng số lượng điểm ảnh mỗi frame sau mỗi lần lọc ....................................................................... 45

VII


Chương 1

TỔNG QUAN
1.1. Giới thiệu
Điều hướng cho robot di động di chuyển theo quỹ đạo mong muốn được nghiên
cứu từ rất lâu, đã đạt được nhiều thành công lớn. Phương pháp điều hướng cổ điển là
dựa vào các loại cảm biến để tính tốn khoảng cách, hướng, gia tốc để tính tốn, quyết
định quỹ đạo di chuyển của robot như Hình 1.1. Bên cạnh những thành tựu đó, những
nhược điểm khách quan do sai số từ các cảm biến đưa về cho robot cũng khơng thể
loại bỏ, vì thế mục tiêu trên vẫn chưa được giải quyết triệt để. Việc điều khiển robot
chỉ dựa vào các cảm biến trên robot thôi vẫn chưa đủ để giải quyết vấn đề trên. Dần
dần, các nhà nghiên cứu nhận thấy rằng cần phải phối hợp, tương tác với môi trường
xung quanh, giúp robot thêm nhiều thơng tin để robot có thể “đưa ra quyết định” đúng

đắn nhằm di chuyển đến đích chính xác.
Một giải pháp khác cũng đã được nghiên cứu và phát triển là thực hiện nhận
dạng vật mốc dựa trên xử lý ảnh thu về từ camera Kinect [1]. Trong phương pháp
này, robot sẽ nhận dạng vật mốc nhân tạo hoặc vật mốc tự nhiên để xác định tọa độ
hiện tại, và từ đó giúp cho việc điều hướng robot dễ dàng hơn.

Hình 1.1: Robot di chuyển dựa vào cảm biến

1


Thế nhưng, quá trình xử lý ảnh để nhận dạng vật mốc chỉ đạt hiệu quả khi vật
mốc nằm trong vùng quét của camera, và sự điều hướng cho robot không đạt được
mức độ mịn mong muốn. Điều này mở thêm một hướng phát triển là cung cấp cho
robot một bản đồ 2D hoặc 3D, lúc này robot sẽ phối hợp dữ liệu hiện tại với bản đồ
được cung cấp sẵn để xác định tọa độ và xây dựng quỹ đạo tốt hơn.
Trong thế giới thực, để quan sát toàn bộ đối tượng, người ta thường quan sát với
nhiều góc nhìn, mỗi góc nhìn tương ứng với vị trí quan sát khác nhau. Xét đối với
Kinect, tương ứng với việc đặt Kinect ở các vị trí khác nhau để ghi nhận cùng 1 đối
tượng. Tuy nhiên, dữ liệu thu nhận được từ Kinect lại rời rạc, mỗi lần thực hiện chỉ
ghi nhận được thông tin ảnh màu RGB và ảnh độ sâu tại ví trí hiện tại. Khi ánh xạ vị
trí của các điểm ảnh này trong khơng gian 3D, tức là ánh xạ lên không gian 3D tương
ứng với vị trí đặt Kinect (hệ trục camera). Khi có nhiều hơn 1 frame, thì các điểm
được ánh xạ lên khơng gian 3D sẽ bị chồng chéo lên nhau. Với tình trạng chồng chéo
các frame ảnh như trong Hình 1.2 [3], chúng ta cần chuyển từng frame ảnh lên hệ
trục tọa độ trong thế giới thực. Nhưng vấn đề đặt ra ở đây là với từng frame rời rạc
thu được, thì làm thế nào để chuyển sang hệ trục tọa độ thế giới thực một cách chính
xác? Hướng giải quyết cho vấn đề này là các frame ảnh 2D hay đám mây điểm ảnh
3D sẽ được sắp xếp lại với nhau và ghép lại tạo thành một bản đồ toàn cục của mơi
trường xung quanh như trong Hình 1.3. Vấn đề sắp xếp nhiều đám mây điểm 3D khác

nhau vào một mơ hình hồn chỉnh được gọi là registration and alignment.

Hình 1.2: Hiện tượng chồng ảnh của 3 frame ảnh trên cùng 1 hệ trục của Kinect

2


Hình 1.3: Các đám mây rời rạc trong khơng gian
Hình 1.4 [16] là một ví dụ về sáu đám mây điểm thu được từ nhiều hướng nhìn
khác nhau. Vì mỗi một đám mây chỉ là một phần nhỏ của thế giới xung quanh, bắt
buộc phải tìm cách để ghép chúng lại với nhau, mà kết quả hồn thành như trong
Hình 1.5 [16]. Phương pháp dùng để ghép các đám mây chính là việc tìm kiếm các
điểm tương đồng trong bộ dữ liệu đầu vào, và ước tính các hàm biến đổi để xoay và
biến đổi bộ dữ liệu riêng biệt vào một hệ tọa độ tồn cục.

Hình 1.4: Ví dụ về các góc nhìn khác nhau ở các vị trí khác nhau

Hình 1.5: Các đám mây đã được ghép lại thành một tổng thể

3


1.2. Kết quả nghiên cứu trong và ngoài nước
Lập bản đồ có nhiều cách thức khác nhau như dùng thiết bị định vị tồn cầu
GPS gắn trên ơ tơ như Hình 1.6. Khi ơ tơ di chuyển, tọa độ hiện tại của ô tô luôn được
cập nhật gửi về máy chủ. Từ đó máy chủ sẽ tính tốn và vẽ được bản đồ 2D của môi
trường mà ô tô di chuyển. Tuy nhiên, độ chính xác của thiết bị GPS không tốt, sai số
từ vài mét cho đến vài chục mét. Do đó, phương pháp định vị dùng GPS chỉ thích hợp
với việc lập bản đồ ở quy mơ lớn.

Hình 1.6: Dùng xe ô tô được trang bị thiết bị định vị GPS để lập bản đồ
Một phương pháp khác là lập bản đồ dựa vào công nghệ RFID như trong Hình
1.7. Ở phương pháp này, robot sẽ xác định vị trí của mình và lập bản đồ dựa trên
khoảng cách đọc thẻ tag của bộ Reader và góc mở của vùng phủ sóng từ hai ăng ten
Reader. Từ đó, robot sẽ tính tốn được chiều dài và góc để tính tốn cho việc lập bản
đồ. Tuy nhiên, do khoảng cách đọc của bộ Reader không cao nên không thể điều
hướng robot tốt được, mặt khác, bắt buộc phải gắn thẻ tag ở những vị trí có khoảng
cách đặt trước nằm trong giới hạn của bộ Reader.

Hình 1.7. Dùng robot được trang bị đầu đọc RFID và các thẻ tag để lập bản đồ
Một đề tài khác sử dụng camera Kinect Xbox 360 là thực hiện nhận dạng vật
mốc nhân tạo [1] trong môi trường trong nhà. Đề tài này sử dụng phần mềm mã nguồn

4


mở phổ biến OpenCV phát huy khả năng nhận biết độ sâu của camera. Do robot chỉ
nhận dạng vật mốc trong vùng nhìn của camera thơi, nên bắt buộc các vật mốc nhân
tạo này phải bố trí theo một trật tự định trước (bố trí theo màu, khoảng cách giữa các
vật mốc như trong Hình 1.8). Do đó, việc xử lí thơng tin bị giới hạn trong vùng nhìn
của camera cũng chính là nhược điểm của nghiên cứu này.

Hình 1.8: Định vị robot di động sử dụng vật mốc nhân tạo
Trong Robot tự hành tránh vật cản sử dụng thiết bị Kinect [2], tác giả Nguyễn
Hồng Đức và Nguyễn Văn Đức đã trình bày phương pháp điều hướng tránh vật cản
dùng phương pháp xử lý ảnh dùng camera Kinect, và phần mềm thực hiện là PCL.
Giới hạn của đề tài này cũng chính là chỉ xử lí tránh vật cản trong vùng nhìn của robot
mà thơi.

Hình 1.9: Robot tự hành tránh vật cản

Trong 3D is here: Point Cloud Library (PCL) [7], tác giả Radu Bogdan Rusu
trình bày về phần mềm PCL và các tính năng của nó. Bên cạnh đó, trong Robust
registration of 2D and 3D point sets [8], tác giả Andrew trình bày phương pháp ghi
tập dữ liệu điểm ảnh và thuật toán ICP dùng để ghép các tập dữ liệu điểm ảnh.

5


Một nghiên cứu khác cũng sử dụng camera Kinect sử dụng phần mềm mã nguồn
mở Point Cloud Library để thực hiện scan 3D [4] một vật thể trong không gian.
Nghiên cứu này thực hiện chuyển vật thể thành các điểm ảnh rời rạc trong khơng gian
3D, hay cịn gọi là đám mây điểm ảnh, có gốc tọa độ là camera, sau đó xoay dần vật
thể và tiếp tục thu thập đám mây điểm ảnh và cuối cùng ghép chúng lại với nhau dựa
trên các điểm tương đồng tạo thành vật thể 3D. Giới hạn của nghiên cứu này là vật
thể được nhận dạng trong mơi trường (nền) là khơng đổi.

Hình 1.10: Scan 3D một đối tượng
Một nghiên cứu gần đây nhất là Implementing Kinect Sensor for Building 3D
Maps of Indoor Environments [14], tác giả Wael R. Abdulmajeed và cộng sự đã xây
dựng thành công bản đồ 3D của môi trường trong nhà. Với kết quả này, tác giả đã
nhấn mạnh ưu điểm của camera Kinect, kích thước bản đồ rất gần với kích thước
thực, và ít kém chi phí hơn so với phương pháp dùng cảm biến laser hay sonic.

Hình 1.11: Bản đồ 3D môi trường trong nhà

6


1.3. Mục tiêu của đề tài
Đề tài này trình bày phương pháp ghép các đám mây điểm ảnh trong không gian

và thực hiện chiếu đám mây toàn cục hay bản đồ 3D này xuống mặt phẳng nền nhà
tạo ra đám mây 2D. Để đạt được mục tiêu tạo ra bản đồ 2D này, robot sẽ được điều
hướng theo quỹ đạo định trước. Robot sẽ được tính tốn khoảng cách giới hạn dựa
vào tốc độ di chuyển và góc quay giới hạn theo thơng số góc mở của camera, ghi
nhận các đám mây điểm ảnh trong từng vị trí mà robot đi qua.
Kết quả của đề tài này nhằm phối hợp với phương pháp nhận dạng vật mốc tự
nhiên để điều hướng cho robot di động đạt hiệu quả cao hơn trong lần điều hướng
tiếp sau, hoặc dùng để cung cấp cho robot khác cũng được điều hướng trong môi
trường cũ này.
1.4. Nội dung thực hiện của đề tài
Quyển luận văn này trình bày phương pháp thực hiện ghép nhiều đám mây điểm
ảnh, tính tốn giá trị giới hạn và kết quả thực nghiệm, sau đó đánh giá kết quả thực
nghiệm đạt được để đề ra hướng phát triển. Chương 2 trình bày cơ sở lý thuyết về
khởi tạo đám mây, phương pháp lọc và giảm số lượng điểm ảnh, tìm và ghép đám
mây điểm ảnh dựa vào cặp điểm tương đồng giữa hai đám mây. Thuật tốn thực hiện
được trình bày chi tiết ở chương 3 dựa trên nền tảng cơ sở lý thuyết ở chương 2. Nội
dung thực hiện của đề tài được trình bày ở chương 4 và hướng phát triển được trình
bày ở chương 5.

7


Chương 2

CƠ SỞ LÝ THUYẾT
Nội dung chương 2 trình bày về thông số cơ bản của camera Kinect được dùng
trong đề tài, phần mềm Point Cloud Library để tính tốn, ghép các đám mây điểm
ảnh và cơ sở lý thuyết để thực hiện việc ghép các đám mây điểm ảnh.
2.1. Giới thiệu chung về Kinect Xbox 360 và Point Cloud Library
2.1.1. Kinect Xbox 360

Camera Kinect là sản phẩm của Microsoft dựa trên công nghệ camera được phát
triển bởi PrimeSense, những sản phẩm đầu tiên được bán tại Bắc Mỹ vào ngày 4 tháng
11 năm 2010 [17]. Sản phẩm Kinect Xbox 360 (minh hoạ trong Hình 2.1) được dùng
vào mục đích nghiên cứu xử lý ảnh 3D, phát hiện cử chỉ (gesture recognition), bám
theo người (body tracking) và nhiều mục đích khác.
Đối với mỗi điểm ảnh trong hình ảnh sâu sắc về độ phân giải 640x480 pixel
được cung cấp một giá trị độ sâu trong khoảng 0-2048 (11bit). Để sử dụng các thông
tin này, một mối quan hệ giữa giá trị cảm biến và khoảng cách thực tế đã được thành
lập.

Hình 2.1: Thiết bị Kinect
Hai camera RGB và IR được đặt cách nhau 2.5 cm nên có chút khác nhau ở
khung hình thu về từ hai camera. Để đảm bảo khung hình RGB có thể chứa được

8


khung hình IR, người ta thiết kế góc mở của RGB camera lớn hơn. Điều này cũng
dẫn đến tiêu cự của RGB camera nhỏ hơn. Các thông số cơ bản của Kinect được trình
bày trong Bảng 2.1 [13].
Bảng 2.1: Góc mở và tiêu cự của RGB và IR camera
Tính năng
Vùng nhìn

VGA

IR camera

Ngang

~62o

~58o

Dọc

~48o

~44o

Chéo

~72o

~69o

525

580

Tiêu cự (pixels)
2.1.2. Point Cloud Library

Point Cloud Library (PCL) là thư viện hỗ trợ cho n-D Point Cloud và cho việc
xử lý ảnh trong không gian 3D. Thư viện được xây dựng với nhiều giải thuật như lọc
(filtering), khôi phục bề mặt (surface reconstruction), phân vùng (segmentation), ước
lượng đặc tính vật (feature estimation),... PCL có thể dùng trên nhiều platform như
Linux, MacOS, Windows và Android. Để đơn giản cho việc phát triển, PCL được
chia ra thành nhiều thư viện nhỏ và có thể biên dịch một cách riêng lẻ. Phiên bản mới
nhất là PCL 1.6 đưa ra vào ngày 17 tháng 07 năm 2012. PCL hồn tồn miễn phí cho

việc nghiên cứu hay phát triển các sản phẩm thương mại hóa.
PCL là sự kết hợp của nhiều module nhỏ (Hình 2.2 [18]). Những module này
thực chất cũng là các thư viện thực hiện các chức năng riêng lẻ trước khi được PCL
đóng gói. Các thư viện cơ bản của PCL là Eigen, Flann, Boost, VTK, CminPack.

Hình 2.2: Logo và một số Module quan trọng của PCL

9


- Eigen: một thư viện mở hỗ trợ cho các phép tốn tuyến tính, được dùng trong
hầu hết các tính toán toán học của PCL.
- Flann: (Fast Library for Approximate Nearest Neighbors) hỗ trợ cho việc tìm
kiếm nhanh các điểm lân cận trong không gian 3D.
- Boost: giúp cho việc chia sẻ con trỏ trên tất cả các module và thuật toán trong
PCL để tránh việc sao chép trùng lặp dữ liệu đã được lấy về trong hệ thống.
- VTK: (Visualization Toolkit) hỗ trợ cho nhiều platform trong việc thu về dữ
liệu 3D, hỗ trợ việc hiển thị, ước lượng thể tích vật thể.
- CMinPack: một thư viện mở giúp cho việc giải quyết các phép tốn tuyến tính
và khơng tuyến tính.
Dữ liệu 3D của đám mây điểm ảnh được lưu trữ dưới tập tin có đi là .pcd.
Định dạng này gồm 2 phần là header và phần dữ liệu. Hình 2.3 là một ví dụ cho cấu
trúc của một tập tin .pcd.

Hình 2.3: Cấu trúc của một tập tin *.pcd

10


2.2. Các bước ghép đám mây

Quá trình ghép hai đám mây điểm ảnh được bắt đầu khi xuất hiện đám mây
điểm ảnh mới, lúc này đám mây điểm ảnh cũ được xem như là đám mây toàn cục và
đám mây mới sẽ được sắp xếp, ghép vào đám mây toàn cục. Q trình được lặp lại
khi có dữ liệu từ đám mây mới. Cứ như thế, dữ liệu ban đầu sẽ trở thành dữ liệu toàn
cục, bản đồ 3D, của môi trường mà robot di chuyển. Các bước để thực hiện việc ghép
đám mây được trình bày như Hình 2.4 [16], giải thích cụ thể cho từng bước như sau:
- Bước 1: Chọn một đám mây điểm ảnh làm gốc. Trong frame gốc này, hệ trục
tọa độ của camera lúc này cũng chính là hệ trục tọa độ của mơi trường thực.
- Bước 2: thu thập đám mây điểm ảnh mới, thực hiện kiểm tra với tất cả các đám
mây điểm ảnh cũ xem có nhiều điểm tương đồng nhất.
- Bước 3: loại bỏ các cặp khơng có độ sâu, lọc bớt các cặp khơng chính xác so
với các cặp khác.
- Bước 4: ước lượng vị trí tương đối giữa hai đám mây điểm ảnh trong không
gian, ước lượng hàm chuyển đổi sao cho đạt được khoảng cách tối thiểu giữa các cặp
điểm tương đồng ở trên.
- Bước 5: thực hiện xắp xếp, ghép đám mây mới vào đám mây cũ sử dụng hàm
chuyển đổi vừa tìm ở bước 4.
Phương pháp chỉ áp dụng roto-translation và cộng tất cả các đám mây sẽ để lại
một bản đồ có nhiều lỗi như trong Hình 2.5 [6]. Để cải thiện kết quả trên, một q
trình xử lí ghi dữ liệu đám mây được đề ra. Quá trình này gồm hai quá trình khác
nhau. Q trình xử lí thứ nhất sử dụng phương pháp ICP (Iterative Closest Point),
q trình xử lí thứ hai áp dụng hai quá trình ghi dữ liệu khác nhau là SAC-IA (Sample
Consensus Initial Alignment) và ICP. Sau khi khởi tạo bản đồ, cho mỗi đám mây
mới, chúng tôi ghi dữ liệu với bản đồ trước đó và quyết định ghi dữ liệu là tốt, đủ để
đưa vào trong bản đồ, nếu không sẽ thực hiện lại bước lọc đám mây.

11


Hình 2.4: Các bước thực hiện ghi một cặp dữ liệu đám mây điểm ảnh

a) Tổng trực tiếp;

b)Tổng sau khi xử lí ghi dữ liệu

Hình 2.5: Bản đồ kết quả của cộng các đám mây sau khi xử lí roto-translation (a)
và sau khi xử lí ghi dữ liệu dùng SAC-IA và ICP (b)

12


Hình 2.6: Lưu đồ xây dựng lại bản đồ
2.3. Ghi dữ liệu đám mây mới sử dụng phương pháp dời tọa độ
Đối với mỗi quá trình ghi dữ liệu mới, một đám mây mới thu được bằng cách
đọc đám mây trước đó và tùy thuộc vào vị trí của camera khi mà đám mây được thu.
Khi xử lí được thơng tin này tức là tính được khoảng cách di chuyển và góc quay của
robot, thì robot có thể chuyển đổi đám mây mới này thành tham chiếu toàn cục.
2.3.1. Chuyển từ tham chiếu cục bộ thành tham chiếu toàn cục dùng ma trận
biến đổi
Để ghép nhiều đám mây khác lại với nhau tạo thành bản đồ chung, chúng phải
ở trong cùng một tham chiếu. Hình 2.7 [6] trình bày hai bước của quá trình chuyển
đổi này. Đám mây điểm thu được sẽ được tham chiếu với hệ tọa độ của camera Kinect
Rkinect. Điểm gốc của nó nằm ở điểm cảm biến của camera Kinect và các trục của nó
xk, yk và zk được định nghĩa như trong Hình 2.8 [6]. Hệ thống tham chiếu của robot
Rrobot, cũng là một hệ tọa độ di động. Do camera được gắn cố định trên robot và khơng
điều khiển góc ngẩng, quay của riêng camera, nên tọa độ của cả hai Rkinect và Rrobot là
trùng nhau trên cùng một hệ tham chiếu.

13

K

Pi

Offsets ban
đầu
(xi, yi, i)

Hàm biến
đổi tồn cục
K

Pi

Hình 2.7: Q trình chuyển đổi đám mây tham chiếu
từ Kinect thành tham chiếu tồn cục

Hình 2.8: Hệ tọa độ của camera Kinect và chung quanh
R

Áp dụng phép biến đổi TK để chuyển đổi một đám mây từ hệ tọa độ của camera
Kinect Rkinect thành hệ tọa độ của robot Rrobot. Một khi các đám mây là trong một hệ
tham chiếu cục bộ thích hợp, nó là cần thiết để chuyển đổi đám mây sang hệ tham
chiếu toàn cục, sẽ cho phép làm việc với các đám mây khác. Rglobal là một hệ tọa độ
cố định có cùng gốc tọa độ và định hướng hơn Rrobot khi vị trí của robot là

14

 x0 , y0 ,0    0,0,0  . Hình 2.9 [6] cho thấy mối quan hệ giữa ba hệ tọa độ khác
nhau.

Hình 2.9: Hệ tọa độ tồn cục và mối liên quan với hệ tọa độ của Kinect
G

Áp dụng phép biến đổi TR để chuyển đổi đám mây từ hệ tọa độ robot Rrobot
G

thành hệ tọa độ toàn cục Rglobal. Phép biến đổi TR thực chất là một ma trận như
Phương trình (2.1) dưới đây.

 cos G   sin G 

sin G  cos G 
G
TR  
 0
0

0
 0

0
0
1
0

xG 


yG 
0

1

( 2.1)

2.3.2. Khởi tạo offset
Hệ tọa độ của camera và hệ tọa độ của robot được định hướng sao cho xr luôn
luôn tương ứng với zk. Điều này khơng phải ln ln chính xác. Thực sự Kinect có
một mức độ tự do, cho phép thay đổi định hướng của zk. Sự di chuyển này được thực
hiện bằng tay và định vị trí khác nhau liên tiếp trong khoảng 6o. Trong trường hợp zk
không song song với sàn nhà, bắt buộc ban đầu phải căn chỉnh xr. Sự căn chỉnh này

15


tùy thuộc vào góc quay đơn của ωk quanh xk, và hàm biến đổi được định nghĩa là Toffset
như sau:

Toffset

0
0
1
0 cos    sin  
k
k

0 sin k  cos k 


0
0
0

0
0

0

1

(2.2)

Phương pháp này địi hỏi phải có trước thông số về sự di chuyển của robot trong
hai trường hợp cơ bản là chuyển động tịnh tiến và chuyển động xoay trịn. Nếu các
thơng số này khơng chính xác thì quá trình ghép hai đám mây điểm ảnh lại với sẽ dẫn
đến kết quả không mong muốn như bị trùng lắp hoặc bị mất điểm ảnh.
2.4. Ghi đám mây sử dụng ICP (Iterative Closest Points)
Quá trình ghi dữ liệu đầu tiên được thiết kế để thay thế cho phương pháp ở trên
Mục 2.3 được mơ tả trong Hình 2.10 [6]. Nó bao gồm một liên kết duy nhất giữa bản
đồ hiện tại và đám mây mới sẽ được chèn vào bằng cách sử dụng thuật toán Iterative
Closest Points.
Đám mây mới: đám
mây ICP nguồn

Bản đồ trước: đám
mây ICP đích

Sắp xếp dùng ICP

Điểm Fitness
Hình 2.10: Lưu đồ ghi dữ liệu chỉ sử dụng ICP
2.4.1. Thuật toán ICP
Thuật toán ICP được biết đến đó là q trình sắp xếp hai tập dữ liệu đám mây
điểm bằng cách tối thiểu khoảng cách Euclidean giữa các điểm tương đồng. Thuật
tốn này tìm kiếm cặp điểm 3D gần nhất trong nguồn và đích và định nghĩa chúng là

16


tương đồng nếu khoảng cách giữa chúng nhỏ hơn một khoảng cách đặt trước. Sau đó
sẽ ước lượng một hàm biến đổi cố định để tối thiểu hóa khoảng cách giữa chúng và
lặp lại cho đến khi sự khác biệt giữa biến đổi liên tục là nhỏ hơn so với giới hạn cho
phép hoặc cho đến khi việc lặp lại đạt tới số lượng tối đa.
Mơ tả thuật tốn ICP để ghi và sắp xếp một đám mây điểm D vào một đám mây
M gồm ba bước như sau [11]:
1. Chọn điểm tương đồng wi,j trong khoảng {0,1}
2. Tối thiểu hàm biến đổi xoay R và hàm tịnh tiến t
Nm Nd

E  R, t    wi , j mi   Rd j  t 

2

(2.3)

i 1 j 1

3. Lặp lại bước 1 và bước 2 cho đến khi đạt được hội tụ hoặc đạt đến giá trị lặp

Nếu hai đám mây M và D có kích thước giống nhau thì Phương trình (2.1) được
viết lại như sau:

E  R, t  

1
N

N


i 1

mi   Rd j  t 

2

(2.4)

Tính trọng tâm của những điểm được tìm thấy

1
cm 
N

N

1
mi ; cd 


N
i 1

N

d
i 1

i

Và M '  m'i  mi  cm 1,..., N ; D '  d 'i  d i  cd 1,..., N
Lúc này, Phương trình (2.2) được viết lại như sau:
1 N
E  R, t    m'i  Rd 'i   t  cm  Rcd 
N i 1
~

2

Đặt t  t  cm  Rcd , thay vào Phương trình (2.3) ta khai triển thành:

17

(2.5)