Luận văn thạc sĩ Kỹ thuật điều khiển và tự động hóa: Xây dựng bản đồ sử dụng camera RGBD kết hợp IMU trên nền tảng ROS

5Hình 1-6 Kết quả đạt được trong nghiên cứu [8]: Đề tài này chỉ sử dụng camera RGBD và giải quyết bài toán SLAM trên nền hệ điều hành robot ROS.. Xây dựng bản đồ sử dụng camera RGBD kết

XÂY DỰNG BẢN ĐỒ SỬ DỤNG CAMERA RGBD

Chuyên ngành: Kỹ Thuật Điều Khiển Và Tự Động Hóa Mã số: 60520216

LUẬN VĂN THẠC SĨ

TP HỒ CHÍ MINH, tháng 12 năm 2015

Công trình được hoàn thành tại : Trường Đại học Bách Khoa – ĐHQG Tp.HCM

Cán bộ hướng dẫn khoa học: PGS.TS HUỲNH THÁI HOÀNG

CHỦ TỊCH HỘI ĐỒNG TRƯỞNG KHOA

Chuyên ngành: Kỹ Thuật Điều Khiển Và Tự Động Hóa

I TÊN ĐỀ TÀI:

XÂY DỰNG BẢN ĐỒ SỬ DỤNG CAMERA RGBD KẾT HỢP IMU TRÊN NỀN TẢNG ROS

II NHIỆM VỤ VÀ NỘI DUNG:

- Xây dựng mô hình cầm tay có khả năng xây dựng bản đồ môi trường trong nhàsử dụng camera RGBD và IMU

- Kiểm tra đánh giá mô hình, kết quả tích hợp IMU vào hệ thống

III NGÀY GIAO NHIỆM VỤ: 17/08/2015

IV NGÀY HOÀN THÀNH NHIỆM VỤ: 10/12/2015V CÁN BỘ HƯỚNG DẪN: PGS.TS Huỳnh Thái Hoàng

Tp HCM, ngày… tháng … năm 201…

CÁN BỘ HƯỚNG DẪN

PGS.TS Huỳnh Thái Hoàng

CHỦ NHIỆM BỘ MÔN ĐÀO TẠO

TS Trương Đình Châu TRƯỞNG KHOA ĐIỆN-ĐIỆN TỬ

60 52 02 16

LỜI CÁM ƠN

Trong quá trình thực hiện đề tài, tôi đã nhận được sự hướng dẫn, chỉ bảo tận tình của các thầy cô trong khoa Điện – Điện tử, đặc biệt là các thầy cô trong bộ môn Điều Khiển Tự Động, sự giúp đỡ của bạn bè cũng như sự động viên, khích lệ tinh thần từ phía gia đình

Tôi xin gửi đến thầy PGS.TS Huỳnh Thái Hoàng lời biết ơn sâu sắc vì đã dành thời gian quý báu để hướng dẫn, tạo điều kiện thuận lợi cũng như cho tôi những lời khuyên bổ ích để hoàn thành luận văn này

Cuối cùng tôi xin cám ơn cha mẹ và các anh chị em trong gia đình đã động viên và tạo điều kiện giúp tôi vượt qua những khó khăn trong suốt quá trình học tập và nghiên cứu vừa qua

TP Hồ Chí Minh, tháng 12/2015

VŨ MINH HOÀNG

của robot được ước lượng với sai số nhỏ hơn trường hợp chỉ có thông tin từ camera RGBD

ABSTRACT

This thesis build up a mobile model that can localize and build map for indoor environment The target of this thesis is implement a Kalman filter in order to combine odometry information from RGBD camera and angle information from IMU on Robot Operation System framework The experimental results show that the integration of angle information from IMU to the system make it work more accuracy In case of combination of camera and IMU, the estimated trajectory of robot has error smaller than the system that works with only camera

LỜI CAM KẾT

Tôi xin cam kết luận văn này là do tôi biên soạn Các nội dung biên soạn có tham khảo từ các nguồn khác đều được trích dẫn và chú thích đầy đủ Tất cả các kết quả đánh giá thực nghiệm đều do chính bản thân tôi tự làm ra, hoàn toàn không sao chép từ bất kỳ một tài liệu hoặc công trình nghiên cứu nào khác

Nếu tôi không thực hiện đúng các cam kết nêu trên, tôi xin hoàn toàn chịu trách nhiệm trước kỷ luật của nhà trường cũng như pháp luật Nhà nước

VŨ MINH HOÀNG

DANH SÁCH VIẾT TẮT vii

DANH MỤC HÌNH ẢNH viii

DANH MỤC BẢNG BIỂU xi

CHƯƠNG 1 GIỚI THIỆU 1

Tổng quan 1

Tổng quan các nghiên cứu liên quan 4

MỤC TIÊU CỦA LUẬN VĂN 12

Mục tiêu 12

Nhiệm vụ 14

Sơ lược về nội dung luận văn 14

CHƯƠNG 2 LÝ THUYẾT CÁC THUẬT TOÁN 16

Sơ đồ khối hệ thống: 16

Trích đặc trưng ảnh (feature detection): 17

Đánh dấu và ghép các điểm đặc trưng (feature description and matching)17 Tính toán vị trí mới của camera: 18

Iterative Closest Point (ICP) 20

Tối ưu bản đồ: 21

Phát hiện vòng kín: 21

Bộ lọc Kalman 24

Tổng quan về bộ lọc Kalman 24

CHƯƠNG 3 HỆ ĐIỀU HÀNH ROBOT ROS 29

Giới thiệu tổng quan hệ điều hành ROS 29

Thư viện Client 44

Tổng quan về thư viện Client 44

Các thư viện client chính 44

Các thư viện client Chuyên biệt 45

CHƯƠNG 4 MÔ HÌNH THỰC NGHIỆM 46

Phần cứng 46

Camera RGBD Asus Xtion Pro Live 46

Razor IMU 47

Phần mềm 47

Quỹ đạo chuyển động của camera: 58

Thống kê sai số của quỹ đạo chuyển động trong bảng sau: 62

CHƯƠNG 5 KẾT LUẬN 63

Các kết quả đã đạt được 63

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

TÀI LIỆU THAM KHẢO 64

LÝ LỊCH TRÍCH NGANG 67

DANH SÁCH VIẾT TẮT

INS: Inertial Navigation System IMU: Inertial Measurement Unit MEMS: Microelectromechanical systems GPS: Global Positioning System

tốt nhưng bài báo chỉ áp dụng trên môi trường 2D 5

Hình 1-6 Kết quả đạt được trong nghiên cứu [8]: Đề tài này chỉ sử dụng camera RGBD và giải quyết bài toán SLAM trên nền hệ điều hành robot ROS 6

Hình 1-7 Nghiên cứu [5] Dora robot, VIRGILE HÖGMAN - Royal Institute of Technology (KTH), Stockholm, Sweden Kết quả đạt được tốt Nghiên cứu này xây dựng odometry của robot bằng tín hiệu encoder kết hợp với hình ảnh từ camera Kinect 6

Hình 1-8 Kết quả từ nghiên cứu [5] 7

Hình 1-9 Nghiên cứu [7] sử dụng phương pháp SIFT để trích đặc trưng ảnh, sử dụng chu trình RANSAC và giải thuật Iterative Closest Point để xác định odometry, sử dụng TORO, một công cụ tối ưu phục vụ các nghiên cứu SLAM nhằm thực hiện việc xác định vòng lặp kín (loop closure detection) Kết quả đạt được tốt 8

Hình 1-10 Layout hệ thống của nghiên cứu [11] 9

Hình 1-11 Kết quả đạt được của nghiên cứu [11] 9

Hình 1-12 Mô hình thực nghiệm trong nghiên cứu [12] 10

Hình 1-13 Kết quả đạt được của nghiên cứu [12] 11

Hình 2-1 Sơ đồ khối cách giải quyết bài toán SLAM sử dụng camera RGBD 16

Hình 2-2 Đánh dấu và ghép đặc trưng 18

Hình 2-3 Các điểm node tượng trưng cho vị trí và các cạnh tượng trưng cho phép chuyển vị trí 21

Hình 2-4 Tối ưu lại các node khi có thêm ràng buộc 23

Hình 2-5 Sơ đồ hệ thống ước lượng 25

Hình 3-1 Một hệ robot được xây dựng trên ROS 29

Hình 3-2 So sánh khối lượng khoa học giữa việc sử dụng ROS hay không dùng 30

Hình 3-8 Quan hệ giữa action – topic 36

Hình 3-9 Mô tả phương thức hoạt động của service 37

Hình 3-10 Sơ đồ mô tả phương thức tổ chức trong ROS 38

Hình 3-11 Mô hình giao tiếp cơ bản trong ROS 38

Hình 3-12 Giao tiếp giữa các nodes 39

Hình 3-13 Sơ đồ mô tả phương thức tổ chức ROS: repository với stack và các thành phần khác của ROS 41

Hình 3-14 ROS universe và repository 41

Hình 3-15 Mô tả phương thức hoạt dộng server – client 42

Hình 3-16 Tổng quan về lớp ứng dụng 44

Hình 4-1 Camera Asus Xtion Pro Live 46

Hình 4-2 Board Razor IMU 47

Hình 4-3 Sơ đồ khối hệ thống được thực hiện trong luận văn 48

Hình 4-4 Hệ tọa độ của mô hình 51

Hình 4-5 Sơ đồ nodegraph SLAM với camera Asus Xtion Pro Live 52

Hình 4-6 Sơ đồ nodegraph SLAM với camera Asus Xtion Pro Live kết hợp IMU 53

Hình 4-7 Bản đồ xây dựng được khi chỉ sử dụng camera Asus Xtion Pro Live 54

Hình 4-8 Bản đồ xây dựng được khi sử dụng camera Asus Xtion Pro Live kết hợp board IMU 56

Hình 4-9 Hình ảnh thực tế căn phòng dùng thí nghiệm 56

Hình 4-10 Góc roll của hệ thống 57

DANH MỤC BẢNG BIỂU

Bảng 1 - Sai số khi cho robot di chuyển với các quỹ đạo khác nhau của nghiên cứu [12]

11

Bảng 2 - So sánh các loại camera RGBD trên thị trường 12

Bảng 3 - Tổng hợp chức năng các phương pháp xử lý đặc trưng 18

Bảng 4 - Công thức bộ lọc EKF 27

Bảng 5 - Thống kê sai số quỹ đạo chuyển động 62

nhiều lựa chọn cho các nhà phát triển dựa vào mục đích xây dựng mô hình (chủ yếu là do đặc điểm môi trường hoạt động)

Trong các bài toán về robot tự hành, bài toán về định vị robot được xem là một bài toán nền tảng để có thể thực hiện các tác vụ thông minh kế tiếp Bài toán định vị là bài toán ước lượng hệ tọa độ gắn với robot bằng cách thu thập các thông tin từ cảm biến, trong đó, thông tin từ môi trường hoạt động (bản đồ) là có thể biết trước hoặc không

Với môi trường hoạt động không biết trước thì bài toàn định vị robot sẽ bao gồm 2 thành phần chính: xây dựng bản đồ của môi trường xung quanh và định vị trí của nó trên bản đồ mới xây dựng được đó Quá trình xử lý đồng thời cả 2 bài toán trên cùng lúc được gọi là SLAM (Simultaneous Localization And Mapping)

Với môi trường ngoài trời (outdoor) việc định vị có thể được thực hiện sử dụng GPS (với ngưỡng sai số cho phép) để robot sử dụng cho việc dẫn hướng Tuy nhiên đối với môi trường trong nhà (indoor) thì việc sử dụng GPS là không khả thi do môi trường hoạt động nhỏ khiến sai số từ các hệ thống GPS thông dụng là quá lớn hoặc môi trường kín nơi không có tín hiệu GPS thì cần giải pháp khác phù hợp hơn Ngoài GPS, các cảm biến thường được sử dụng trong bài toán SLAM là Encoder, IMU, camera và cảm biến khoảng cách Một vài đặc điểm của các loại cảm biến nói trên sẽ được học viên trình bày tiếp sau đây:

Encoder: Thiết bị này được dùng rộng rãi nhờ vào tính đơn giản và rẻ tiền Từ giá trị

xung của encoder và đường kính biết trước của bánh xe ta sẽ tính được quãng đường

Xây dựng bản đồ sử dụng camera RGBD kết hợp IMU trên nền tảng ROS

chuyển động chung cho robot Rõ ràng phương pháp này phụ thuộc nhiều vào kết cấu cơ khí của robot và không thể áp dụng cho các hệ robot khác nhau Phương pháp này còn một nhược điểm nữa là nó chỉ định vị được trong không gian 2D

Hình 1-1 Encoder Omron E6C2

IMU: Thiết bị này được sử dụng rộng rãi từ khi công nghệ MEMS

(microelectromechanical systems) ra đời khiến giá thành giảm và việc ứng dụng nó vào robot di động trở nên khả thi và dễ dàng hơn Đầu tiên ta phải tính giá trị gia tốc chuyển động của robot dựa vào hiệu số giữa giá trị thu được từ cảm biến Accelerometer và hình chiếu của vector trọng trường Trái Đất lên robot Sau đó ta tích phân hai lần để thu được quãng đường di chuyển Phương pháp này có ưu điểm lớn là ta có thể phát triển thuật toán định vị độc lập với robot Tuy nhiên việc tích phân hai lần trên giá trị gia tốc sẽ gây ra sai số tích lũy lớn dần theo thời gian do đó đặt ra yêu cầu phải tích hợp thêm cảm biến để bù lại sai số này

Hình 1-3 Từ trái qua phải: Elphel353, bumblebee2

Cảm biến khoảng cách: sử dụng các thiết bị như cảm biến siêu âm, laser scanner sẽ

cho phép định vị robot nhờ vào ước lượng chuyển động tương đối giữa robot so với vật cản hay môi trường Cảm biến siêu âm (ultra sonic), hay laser scanner tính khoảng cách từ robot tới vật cản dựa vào tích thời gian truyền-phản hồi của sóng và vận tốc truyền sóng Một loại cảm biến khác dùng structure-light như Microsoft Kinect, Asus Xtion Pro hay Creative Senz3D hoạt động với một máy chiếu các tia sáng hồng ngoại kiểu các pixel ra không gian trước mặt sau đó camera hồng ngoại đọc về và dựa vào nguyên lí giống stereo camera để so sánh ảnh nhận được và ảnh đã calib để so sánh sẽ cho phép tính ra được độ sâu tương ứng với từng pixel trên ảnh

Hình 1-4 Từ trái qua phải: cảm biến siêu âm HC-SR04, laser scanner SICK, RGBD

camera Kinect, RGBD camera Asus Xtion Pro Live

Để giải quyết bài toán SLAM ta có thể kết hợp các thiết bị trên tùy thuộc vào yêu cầu hoạt động Trên thế giới đã có nhiều đề tài như:

Xây dựng bản đồ sử dụng camera RGBD kết hợp IMU trên nền tảng ROS

- Sử dụng laser scanner như đề tài [1] - Sử dụng cảm biến siêu âm như đề tài [2][3] - Sử dụng camera RGBD (Kinect) như đề tài [4][5][6][7][8] - Kết hợp encoder và cảm biến siêu âm như [9]

- Kết hợp encoder và stereo camera như đề tài [10] Cảm biến siêu âm có nhược điểm là độ chính xác không cao và khoảng cách cũng không xa (ví dụ cảm biến siêu âm giá rẻ HC-SR04 chỉ cho độ tin cậy trong khoảng từ 2->300 cm) Stereo camera hoặc laser scanner cho kết quả rất tốt do có thông tin về độ sâu ảnh chính xác với khoảng cách xa, tuy nhiên vấn đề gặp phải khi sử dụng các thiết bị này là mặt chi phí Đối với các ứng dụng nhằm mục đích thương mại hóa như máy hút bụi tự động hoặc robot tự hành hoạt động trong kho hàng, UAV…, độ chính xác không đòi hỏi quá khắc khe nhưng chi phí phải chấp nhận được Khi đó việc sử dụng camera RGBD thực sự hiệu quả

Với mong muốn thực hiện một thiết bị có khả năng xây dựng bản đồ môi trường xung quanh giá rẻ, sử dụng các thiết bị thông dụng trên thị trường, học viên lựa chọn camera RGBD Thiết bị này cung cấp đủ thông tin để định vị robot và nhỏ gọn, thích hợp với các ứng dụng di động Ngoài ra có thể dễ dàng kết hợp thiết bị này với các loại cảm biến khác nhằm mục đích cải thiện quỹ đạo chuyển động của robot, cải thiện khả năng định vị của robot

Tổng quan các nghiên cứu liên quan

Một số đề tài về SLAM trên thế giới sử dụng camera RGBD:

Hình 1-5 Nghiên cứu [4]: SLAM dựa vào Kinect và giải thuật Gmapping cho kết

quả tốt nhưng bài báo chỉ áp dụng trên môi trường 2D

Xây dựng bản đồ sử dụng camera RGBD kết hợp IMU trên nền tảng ROS

Hình 1-6 Kết quả đạt được trong nghiên cứu [8]: Đề tài này chỉ sử dụng camera RGBD và giải quyết bài toán SLAM trên nền hệ điều hành robot ROS

Hình 1-7 Nghiên cứu [5] Dora robot, VIRGILE HÖGMAN - Royal Institute of Technology (KTH), Stockholm, Sweden Kết quả đạt được tốt Nghiên cứu này xây

dựng odometry của robot bằng tín hiệu encoder kết hợp với hình ảnh từ camera

Kinect

Hình 1-8 Kết quả từ nghiên cứu [5]

Xây dựng bản đồ sử dụng camera RGBD kết hợp IMU trên nền tảng ROS

Hình 1-9 Nghiên cứu [7] sử dụng phương pháp SIFT để trích đặc trưng ảnh, sử dụng chu trình RANSAC và giải thuật Iterative Closest Point để xác định odometry,

sử dụng TORO, một công cụ tối ưu phục vụ các nghiên cứu SLAM nhằm thực hiện việc xác định vòng lặp kín (loop closure detection) Kết quả đạt được tốt

Với các đề tài trong nước thì học viên chỉ tìm được 2 đề tài thực hiện SLAM sử dụng camera RGBD Kinect:

Nghiên cứu thứ nhất có tên [11] “Indoor Mobile Navigation Using ROS”, Thien-Minh Nguyen-Pham trong hội nghị The 2014 FEEE Student Research Conference (FEEE-SRC 2014)

Hình 1-10 Layout hệ thống của nghiên cứu [11]

Hình 1-11 Kết quả đạt được của nghiên cứu [11]

Nội dung nghiên cứu là giải quyết bài toán định vị robot với thông tin về môi trường hoạt động (bản đồ) được cung cấp trước Mô hình được xây dựng dựa trên hệ điều hành robot ROS với cảm biến hình ảnh là camera Kinect thế hệ 1 Odometry được xây dựng dựa trên tín hiệu trả về từ 2 encoder của 2 bánh xe robot Giải thuật định vị sử dụng là Monte Carlo Localization Kết quả của nghiên cứu đạt được là robot có thể tiếp cận vị trí đích đặt trước trên bản đồ với khả năng tránh né vật cản không biết trước trên đường

Nghiên cứu thứ hai là một luận văn đại học có tên [12] “Xác định đường đi của robot dung Kinect trên nền tảng ROS” của sinh viên Lại Thành Phước năm 2013

Xây dựng bản đồ sử dụng camera RGBD kết hợp IMU trên nền tảng ROS

Hình 1-12 Mô hình thực nghiệm trong nghiên cứu [12]

Đề tài này có mục tiêu là xác định quỹ đạo chuyển động trong môi trường chưa có thông tin (không có bản đồ) Đề tài sử dụng thuật toán Shi-Tomasi và Iterative Closest Point để xác định Odometry thông qua tín hiệu trả về từ camera Kinect thế hệ 1 Trong báo cáo của đề tài không trình bày thuật toán sử dụng để phát hiện vòng kín Kết quả của đề tài là tốt với luận văn đại học

Hình 1-13 Kết quả đạt được của nghiên cứu [12]

Bảng 1 – Bảng sai số khi cho robot di chuyển với các quỹ đạo khác nhau của

nghiên cứu [12]

Trường hợp Quỹ đạo Quãng

đường Sai số (m) Chi tiết

3 Hình chữ nhật 9 m

X= 0.08

Y = 0.28

Hình chữ nhật 2.5m x 1m

Xây dựng bản đồ sử dụng camera RGBD kết hợp IMU trên nền tảng ROS

MỤC TIÊU CỦA LUẬN VĂN

Mục tiêu

Với tình hình nghiên cứu trên thế giới và trong nước đã trình bày ở trên cùng với mong muốn thực hiện một ứng dụng xây dựng bản đồ có tính di động, học viên lựa chọn các thiết bị cảm biến và công cụ thực hiện như sau:

1 Thiết bị cảm biến được lựa chọn là camera RGBD Điểm yếu của loại camera này là tầm hoạt động không rộng, chỉ được trong khoảng cách 5m Những năm gần đây, xu hướng nghiên cứu ứng dụng loại camera này phát triển nhanh chóng do giá thành không quá cao mà thông tin nhận được lại khá chi tiết, đủ để áp dụng lên các mô hình nghiên cứu thuật toán

Sau đây là bảng so sánh cảm biến một số loại camera RGBD trên thị trường:

Bảng 2 – So sánh các loại camera RGBD trên thị trường

Creative Senz3D

ASUS Xtion Pro Live

Kinect thế hệ 1

Kinect thế hệ 2 Độ phân

giải video RGB

1280x720 @30fps

1280x1024 @30fps

640x480 @30fps

1920x1080 @30fps Độ phân

giải camera độ sâu

320x240 @30fps

640x480 @30fps

320x240 @30fps 13

bit độ sâu

512x424 @30fps 13

bit độ sâu Khoảng

cách sử dụng

0.15m ~ 1m 0.8m ~ 3.5m 0.8m ~ 4m 0.8m ~ 4m

Góc nhìn 60ov 70oh 45ov 58oh 43ov 57oh 60ov 70oh

bổ sung thêm một bộ IMU nhằm cung cấp thêm thông tin khi tính toán odometry của robot

Để tiếp cận cách giải quyết bài toán SLAM nhanh chóng, học viên sẽ sử dụng hệ điều hành robot ROS Cộng đồng sử dụng ROS cung cấp nhiều project đã thực hiện về bài toán SLAM thích hợp để tham khảo và phát triển, cải tiến giải thuật Lợi ích thiết thực của việc sử dụng ROS là có tính kế thừa Điều này sẽ rút ngắn thời gian nghiên cứu lại những điều đã được nghiên cứu

Đề tài sẽ phát triển giải thuật dựa trên ROS và nghiên cứu “An evaluation of the D SLAM system” thực hiện bởi Felix Endres, Jurgen Hes, Nikolas Engelhard, Jurgen Sturm, Daniel Cremers, Wolfram Burgard trình bày trong hội nghị Robotics and Automation (ICRA) May 2012 Nghĩa là đề tài sẽ sử dụng các thuật toán được đề cập trong nghiên cứu trên nhưng sử dụng camera Asus Xtion Pro live và bổ sung thêm IMU để tính toán odometry Cuối cùng đề tài sẽ so sánh kết quả đạt được (odometry và bản đồ) trong 2 trường hợp tính toán odometry với chỉ camera và khi có thêm cảm biến IMU

Xây dựng bản đồ sử dụng camera RGBD kết hợp IMU trên nền tảng ROS

 Thực hiện lại giải thuật SLAM (Visual SLAM) trong nghiên cứu “An evaluation of the RGB-D SLAM system”

 Hiện thực nghiên cứu bằng cách xây dựng một mô hình cầm tay có khả năng xây dựng bản đồ 3D của môi trường indoor không có thông tin trước trên nền tảng ROS

 Kiểm tra và đánh giá việc bổ sung thêm thông tin của IMU cải thiện việc xây dựng odometry của robot như thế nào

Nhiệm vụ

 Tìm hiểu hệ điều hành robot ROS

 Tìm hiểu và nghiên cứu các giải thuật được sử dụng trong nghiên cứu “An evaluation of the RGB-D SLAM system” thực hiện bởi Felix Endres, Jurgen Hes, Nikolas Engelhard, Jurgen Sturm, Daniel Cremers, Wolfram Burgard hội nghị Robotics and Automation (ICRA) May 2012

 Tìm hiểu và nghiên cứu bộ lọc cho cảm biến IMU

 Tìm hiểu và nghiên cứu bộ lọc Kalman mở rộng để ước lượng vị trí robot kết hợp thông tin từ camera RGBD và IMU

 Áp dụng các giải thuật vào mô hình thực nghiệm và đánh giá kết quả thực nghiệm

Sơ lược về nội dung luận văn

Luận văn gồm 5 chương với nội dung các chương như sau: Chương 1: Giới thiệu Giới thiệu tổng quan về các hướng nghiên cứu giải quyết bài toán định vị và lập bản đồ trên thế giới cũng như trong nước Đồng thời chọn mục tiêu và cách giải quyết bài toán SLAM của luận văn này

Xây dựng bản đồ sử dụng camera RGBD kết hợp IMU trên nền tảng ROS

Chương này sẽ trình bày một cách tóm tắt các thuật toán được sử dụng để giải quyết bài toán SLAM Chương này chỉ trình bày một cách tóm tắt các thuật toán được sử dụng trong bài toán SLAM sử dụng camera RGBD theo nghiên cứu [8] vì học viên khi hiện thực mô hình sẽ sử dụng một ROS package cung cấp đầy đủ các thuật toán được trình bày dưới đây Vì vậy học viên không đi sâu nghiên cứu chi tiết từng thuật toán

Sơ đồ khối hệ thống:

Sơ đồ giải thuật cho bài toán SLAM:

Trích đặc trưng ảnhẢnh RGB

Ảnh độ sâu

đánh dấu điểm đặc trưng, ghép các điểm đặc trưng trong các frame ảnh liên tiếp

Tạo các điểm ảnh đặc trưng trong môi trường 3D(ghép thông tin từ ảnh RGB và ảnh độ

sâu)

Tính toán vị trí mới với thông tin môi

trường 3DTối ưu bản đồ (tìm

vòng kín)Point cloud

data

Hình 2-1 Sơ đồ khối cách giải quyết bài toán SLAM sử dụng camera RGBD

Trong sơ đồ trên thì các đặc trưng ảnh được xử lý qua 2 bước là trích đặc trưng ảnh (feature detection) và đánh dấu điểm đặc trưng (feature description) Trích đặc trưng ảnh nghĩa là tìm các điểm hoặc vùng trong ảnh có sự thay đổi về độ sáng (intensity) mạnh như một cạnh hoặc một góc sự vật trong ảnh, sau đó lấy điểm trung tâm của

ít thay đổi do góc xoay và cường độ sáng, các đặc trưng phân biệt với nhau, thời gian tính toán nhanh, và ổn định Các phương pháp trích đặc trưng ảnh thường được sử dụng là:

Harris Corner [14]: giải thuật trích đặc trưng các điểm góc (corner detector) SIFT [15]: Scalar Invariant Feature Transform

SURF [16]: Speeded Up Robust Feature NARF [17]: Normal Aligned Radual Feature ORB [18]: Oriented FAST and Rotated BRIEF (Binary Robust Independent Elementary Features)

Đánh dấu và ghép các điểm đặc trưng (feature description and matching)

Các phương pháp đánh dấu đặc trưng thường sử dụng là: SIFT: Scalar Invariant Feature Transform

SURF: Speeded Up Robust Feature NARF: Normal Aligned Radual Feature ORB: Oriented FAST and Rotated BRIEF (Binary Robust Independent Elementary Features)

Tổng hợp chức năng các phương pháp trên:

Xây dựng bản đồ sử dụng camera RGBD kết hợp IMU trên nền tảng ROS

Bảng 3 –Tổng hợp chức năng các phương pháp xử lý đặc trưng

Trích đặc trưng Đánh dấu đặc trưng

Tính toán vị trí mới của camera:

Từ các cặp điểm đặc trưng giữa 2 frame ảnh liên tiếp, các thuật toán cho phép ta ước lượng được độ dịch chuyển tương đối của camera Các thông số cần tối ưu bao gồm 9 thành phần của ma trận xoay 3x3 và 3 thành phần vector tịnh tiến chuyển các đặc trưng trên frame ảnh hiện tại về frame ảnh trước đó Giải thuật ước lượng thường dùng là Gauss-Newton cho việc tối thiểu hóa hàm mục tiêu sai số bình phương tối

cặp điểm đặc trưng Trong mỗi vòng lặp, một số cặp điểm đặc trưng sẽ được chọn ngẫu nhiên và tính toán một phép dịch chuyển Phép dịch chuyển này sau đó sẽ được sử dụng tính toán cho toàn bộ các cặp điểm đặc trưng và tính sai số Vòng lặp trên được thực hiện nhiều lần và giữ lại phép dịch chuyển cho sai số nhỏ nhất cho đến khi hết số lần lặp cho phép Phép dịch chuyển tìm được không phải là chính xác nhất nhưng là phép tốt nhất với sai số nhỏ nhất Phép dịch chuyển này sẽ được sử dụng cho thuật toán tiếp sau để có phép dịch chuyển chính xác nhất

Sau đây là tóm tắt chu trình RANSAC được dùng để tìm sự dịch chuyển của camera

Require: initial pairs of 3D points (origin, destination) bestT ransform, bestInliers ← ∅

Define the number of iterations N for iteration = 1 to N do

samples ← pickup randomly k pairs (origin, destination) Compute currentTransform from samples

inliers ← ∅ for all pairs of points do

projectedi ← projectPoint(origin , currentTransform) error ← computeDistance(projectedi − destinationi)

Xây dựng bản đồ sử dụng camera RGBD kết hợp IMU trên nền tảng ROS

if error < threshold then

inliers ← inliers + pairi end if

end for Count number of inliers and compute mean error if currentTransform is valid then

Recompute currentTransform from inliers if currentTransform better than bestTransform then

bestTransform ← currentTransform bestInliers ← inliers

end if end if

end for return bestTransform, bestInliers

Iterative Closest Point (ICP)

Giải thuật ICP được giới thiệu bởi Zhang ở bài báo [19] cho kết quả là độ dịch chuyển tối ưu cần thiết để tối thiểu khoảng cách giữa 2 bộ dữ liệu điểm Ví dụ có 2 bộ điểm (pi,qi) Sử dụng giải thuật này để tìm kiếm độ dịch chuyển tối ưu gồn phép tuyến tính T và phép quay R để biến đổi từ bộ điểm pi sang bộ điểm qi.Vấn đề có thể được diễn tả thông qua công thức tối thiểu bình phương khoảng cách giữa các bộ điểm kế cận

độ tương thích giữa nhiều frame ảnh liên tiếp Phương pháp này giúp giảm sai số tích

lũy trong hệ thốngError! Reference source not found

Các giải thuật này đã được phát triển nhiều và đạt đến độ chính xác cao Các phương pháp tối ưu bản đồ sẽ tối thiểu sai số nhờ vào ràng buộc giữa các vị trí camera qua các frame ảnh Ta gọi các vị trí của camera là các điểm node và sự dịch chuyển vị trí (biểu hiện qua ma trận chuyển vị trí (transformation matrix)) là các cạnh nối các điểm node đó

Hình 2-3 Các điểm node tượng trưng cho vị trí và các cạnh tượng trưng cho phép

Xây dựng bản đồ sử dụng camera RGBD kết hợp IMU trên nền tảng ROS

đi đến thành phố mới để du lịch Người khách này không có bản đồ và bắt đầu xuất phát từ khách sạn A Vừa đi vừa vẽ lại quãng đường mình đi trên giấy, người khách không có thông tin chính xác về độ dài quãng đường hay giá trị góc tại các khúc cong Người khách chỉ có các thông tin trên với sai số Đến một lúc nào đó người khách sẽ quay về lại khách sạn A bằng một con đường khác Khi đó, nếu bản đồ người khách vẽ được là hoàn toàn chính xác thì bản đồ sẽ là một vòng kín, nhưng vì các thông tin người khách ước lượng tại mỗi thời điểm là không chính xác nên điều này khó xảy ra Tuy nhiên bây giờ người khách đã có thêm một thông tin đáng giá khác là vị trí cuối mà người khách ước lượng chính là vị trí bắt đầu Vậy bản đồ có thể được chỉnh sửa với thông tin mới

Áp dụng ý tưởng trên vào bài toán SLAM thì khi robot di chuyển một quãng đường không biết trước sẽ xảy ra trường hợp robot quay trở lại địa điểm đã đi qua Robot sẽ nhận biết điều này nhờ phát hiện các điểm đặc trưng trong frame ảnh mới giống một trong các frame ảnh cũ Trong trường hợp đó ta sẽ có thêm một ràng buộc về sự thay đổi vị trí tương đối của robot giữa frame ảnh mới và frame ảnh trước Từ đó phương pháp tối ưu bản đồ trên sẽ cho ta được bản đồ tối ưu hơn với ràng buộc mới được thêm vào

Hình 2-4 Tối ưu lại các node khi có thêm ràng buộc

Xây dựng bản đồ sử dụng camera RGBD kết hợp IMU trên nền tảng ROS

Bộ lọc Kalman

Tổng quan về bộ lọc Kalman

Các bộ lọc Kalman dựa vào giải thuật ước lượng Kalman bao gồm hai phần: đầu tiênlà dự báo giá trị (prediction) dựa vào dữ liệu trước đó, sau đó là hiệu chỉnh lại (correction) nhờ vào các giá trị thực tế vừa đo Bộ lọc này được phát triển ban đầu từ nhu cầu của việc theo dõi quỹ đạo và dẫn đường cho các hệ thống trong không gian của NASA Sau này nóđược áp dụng rộng rãi vào các hệ thống khác như xử lí ảnh, điều khiển…

Các vấn đề của việc hiện thực một bộ lọc Kalman gồm: Chỉ ra mô hình toán của hệ thống

Ước lượng được các covariance của nhiễu: do mô hình & đo lường

Xét hệ thống tuyến tính tổng quát được biễu diễn bởi hệ phương trình trạng thái sau:

Trong đó:

A, B, C, D: các ma trận hằng (phân biệt với hệ phi tuyến là các hàm số)

xk: giá trị trạng thái của hệ thống tại thời điểm kT

yk: ngõ ra của hệ thống tại thời điểm kT wk: nhiễu do mô hình hay nhiễu quá trình (process noise) vk: nhiễu do đo lường (measurement noise)

Để tìm ngõ ra y của hệ thống đối với các ngõ vào u khác nhau và các trạng thái x khác nhau của hệ thống ta phải thiết lập được hệ phương trình trên Tuy nhiên việc tính toán có thể dẫn ra kết quả sai với thực tế do:

1 Mô hình toán sai vì tuyến tính hóa bỏ qua các thành phần bậc cao, bỏ qua các đại lượng như nhiệt độ, ma sát…

2 Nhiễu do đo đạc từ cảm biến hay phương pháp đo… Do đó yêu cầu đặt ra là cần có các các bộ lọc hay giải thuật ước lượng để tối ưu hóa giá trị tính toán Các bước tính toán trong giải thuật Kalman như sau:

Với: A, B: các hệ số của phương trình vi phân tuyến tính

Xây dựng bản đồ sử dụng camera RGBD kết hợp IMU trên nền tảng ROS

P: ma trận covariance của nhiễu ước lượng được (Pvà Plà giá trị trước và sau) Q: ma trận covariance của nhiễu do quá trình (hiệp phương sai)

R: ma trận covariance của nhiễu do đo lường K: ma trận độ lợi

H: ma trận độ nhạy xác định quan hệ tuyến tính giữa tín hiệu của hệ thống và tín hiệu ước lượng

Một thực tế là rất nhiều các hệ thống thực đều là phi tuyến do đó khi áp dụng Kalman

cho các đối tượng này ta phải dùng bộ lọc Kalman mở rộng (Extended-Kalman Filter)

Mô hình động học của đối tượng chuyển động bất kì:

f( , )h( )

Có các giá trị E x t( k 1) và var( (x tk 1)) Các giá trị đo lường được mô hình hóa như sau: