Luận văn thạc sĩ Kỹ thuật điều khiển và tự động hóa: Hệ thống định vị tích hợp dùng thị giác lập thể và quán tính

chính của học viên là xây dựng được phần mềm định vị thời gian thực dùng công nghệ xử lí ảnh stereo kết hợp hệ thống dẫn đường quán tính INS, tần số cập nhật ngõ ra là 100Hz bao gồm vị t

-

ĐẶNG ANH TÙNG

HỆ THỐNG ĐỊNH VỊ TÍCH HỢP DÙNG THỊ GIÁC LẬP THỂ VÀ QUÁN TÍNH

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 07 năm 2015

Cán bộ hướng dẫn khoa học : TS Nguyễn Vĩnh Hảo Ký tên

Cán bộ chấm nhận xét 1 : TS Võ Công Phương Ký tên

Cán bộ chấm nhận xét 2 : TS Ngô Mạnh Dũng Ký tên

Luận văn thạc sĩ được bảo vệ tại Trường Đại học Bách Khoa, ĐHQG Tp HCM, ngày 14 tháng 07 năm 2015

Thành phần Hội đồng đánh giá luận văn thạc sĩ gồm: 1 PGS.TS Huỳnh Thái Hoàng (Chủ tịch)

2 TS Nguyễn Trọng Tài (Thư kí) 3 TS Võ Công Phương (Phản biện 1) 4 TS Ngô Mạnh Dũng (Phản biện 2) 5 TS Nguyễn Thiện Thành (Ủy viên) Xác nhận của Chủ tịch Hội đồng đánh giá LV và Trưởng Khoa quản lý chuyên ngành sau khi luận văn đã được sửa chữa (nếu có)

PGS.TS Huỳnh Thái Hoàng TS Đỗ Hồng Tuấn

NHIỆM VỤ LUẬN VĂN THẠC SĨ

Họ tên học viên: ĐẶNG ANH TÙNG MSHV: 13153068 Ngày, tháng, năm sinh: 10/01/1990 Nơi sinh: Quảng Ngãi Chuyên ngành: Kỹ Thuật Điều Khiển Và Tự Động Hóa Mã số: 60520216

I TÊN ĐỀ TÀI:

HỆ THỐNG ĐỊNH VỊ TÍCH HỢP DÙNG THỊ GIÁC LẬP THỂ VÀ QUÁN TÍNH

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

- Xây dựng giải thuật tích hợp hệ thống định vị kết hợp INS và stereo-camera - Xây dựng phần mềm tích hợp chạy thời gian thực

- Xây dựng phần mềm đánh giá, cân chỉnh hệ thống dùng GPS-RTK

III NGÀY GIAO NHIỆM VỤ: 07/07/2014 IV NGÀY HOÀN THÀNH NHIỆM VỤ: 20/06/2015 V CÁN BỘ HƯỚNG DẪN: TS Nguyễn Vĩnh Hảo

TS Đỗ Hồng Tuấn

dẫn, tạo điều kiện thuận lợi cũng như cho tôi cơ hội tham gia nhóm nghiên cứu về hệ thống định vị của thầy Cảm ơn các anh, các bạn, các em trong nhóm nghiên cứu đã giúp đỡ tôi trong quá trình làm luận văn

Tôi xin cảm ơn thầy cô trong Khoa Điện-Điện tử, đặc biệt các thầy cô trong Bộ môn Tự động đã hướng dẫn và tạo môi trường làm việc linh động giúp tôi có thời gian tập trung hoàn thành luận văn

Cuối cùng tôi xin cám ơn cha mẹ và các anh chị em trong gia đình đã động viê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 06/2015

ĐẶNG ANH TÙNG

chính của học viên là xây dựng được phần mềm định vị thời gian thực dùng công nghệ xử lí ảnh stereo kết hợp hệ thống dẫn đường quán tính INS, tần số cập nhật ngõ ra là 100Hz bao gồm vị trí, vận tốc, ba góc Euler Phần mềm định vị phải đạt sai số vị trí 4%, sai số góc quay tĩnh 1°, sai số góc quay động 2° Bên cạnh đó học viên xây dựng chương trình cân chỉnh cho hệ thống INS/Stereo-camera với GPS-RTK để đánh giá chất lượng giải thuật Phần mềm sau khi hoàn thành có thể dùng cho các robot di động tự hành trong nhà hay ngoài trời

ABSTRACT

Localization system plays a fundamental role in the field of mobile robot research This thesis aims to develop a real-time localization algorithm using stereo-camera and INS Output data must be updated at 100Hz included position, velocity, and three Euler angles The algorithm precision achieves position error less than 4%, static angle accuracy at 1°, and dynamic angle accuracy at 2° In addition, I developed a software to calibrate and assess the INS/Strereo-camera system using GPS-RTK system The proposed algorithm may be applied for mobile robot both indoor and outdoor application

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

ĐẶNG ANH TÙNG

CHƯƠNG 1 TỔNG QUAN VỀ ĐỀ TÀI 1

1.1 Tổng quan về các hệ thống định vị cho robot di động 1

1.2 Hệ thống định vị tích hợp dùng Stereo-Vision và INS 9

1.3 Bộ lọc Kalman mở rộng EKF 9

1.4 Mục tiêu và các nội dung cần giải quyết của đề tài 12

CHƯƠNG 2 HỆ THỐNG DẪN ĐƯỜNG QUÁN TÍNH INS 13

2.1 Tổng quan các giải thuật của hệ INS 14

2.2 Giải thuật EKF thích nghi hệ INS 16

Xác định mô hình đối tượng và mô hình đo lường cho bộ lọc Kalman 2.2.1.theo phương pháp DCM 16

Tính toán các giá trị nhiễu nền 26

2.2.2.2.3 Kết quả thực nghiệm giải thuật định vị dùng INS 28

CHƯƠNG 3 HỆ THỐNG ĐỊNH VỊ DÙNG STEREO-CAMERA 31

3.1 Khái niệm hệ thống định vị dùng stereo-camera 32

3.2 Giải thuật định vị dùng stereo-camera 34

3.3 Kết quả thực nghiệm giải thuật định vị dùng stereo-camera 36

4.2.2.4.3 Hệ thống định vị tích hợp bất đồng bộ INS/Stereo-camera 52

5.1 Mô hình phần cứng 56

5.2 Mô hình phần mềm 60

Phần mềm phân tích và mô phỏng dùng MATLAB 60

5.2.1.Chạy thời gian thực và thu thập dữ liệu dùng C++ trên board PC 60

5.2.2.5.3 Kết quả thực nghiệm hệ thống định vị tích hợp INS/Stereo-camera 63

CHƯƠNG 6 KẾT LUẬN & HƯỚNG PHÁT TRIỂN 70

6.1 Kết luận 70

6.2 Hướng phát triển 70

DANH MỤC CÁC CÔNG TRÌNH KHOA HỌC 71

TÀI LIỆU THAM KHẢO 72

LÝ LỊCH TRÍCH NGANG 76

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

Hình 1.2 Robot Pioneer 3-DX là robot có hai bánh vi sai, có thể định vị robot dựa vào

Hình 1.5 Sơ đồ tổng quát hệ thống INS 4

Hình 1.6 Chất lượng định vị tăng lên rõ rệt sau khi vừa tắt chức năng SA trên vệ tinh Sai số cả kinh độ và vĩ độ đều nhỏ hơn 20m 5

Hình 1.7 Từ trái qua phải là: cảm biến siêu âm, laser scanner giá rẻ SICK, laser scanner cao cấp Velodyle Lidar, và Microsoft Kinect 6

Hình 1.8 Ảnh cho độ sâu từng pixel từ Kinect & Hình ảnh không gian 3D tái tạo từ bộ quét laser 7

Hình 1.9 Định vị cho quadrotor dùng camera đơn [5] 7

Hình 1.10 Xe thu thập dữ liệu của Đại học KIT, Đức [22] và kết quả chạy giải thuật của Hernan [19] Hai đường định vị dùng stereo-camera và đường chuẩn đo từ GPS gần như trùng nhau 8

Hình 2.1 Sơ đồ khối bộ ước lượng EKF thích nghi hai tầng 16

Hình 2.2 Mô hình tinh chỉnh độ lớn ma trận covariance 26

Hình 2.3 Mô hình đóng-ngắt bộ khử nhiễu nền 26

Hình 2.4 Giá trị nhiễu nền của vận tốc góc theo thời gian 27

Hình 2.5 Độ lệch của t m từ trường tr c , trước và sau khi c n chỉnh 28

Hình 2.6 Trường hợp quay cả ba tr c x, y, z của IMU Ba đường lần lượt là các góc Roll, pitch, yaw trên ba tr c 30

Hình 2.7 Đặt IMU đứng yên Cho tác động từ trường ngoài 30

Hình 3.1 Mô hình hình học tính toán tọa độ 3D của điểm dùng hai ảnh trái và phải 32Hình 3.2 Mô hình ước lượng chuyển động của Stereo camera 34

Hình 3.3 Bộ lọc các đặc trưng blob & corner dùng ma trận 5x5 34

Hình 3.5 Thử nghiệm chạy thẳng Sai số tối đa 0.7m/20m (3.5%) 36

Hình 4.1 Bố trí thiết bị để thực hiện thí nghiệm cân chỉnh góc lệch 39

Hình 4.2 Góc lệch giữa camera và IMU và nguyên lý cân chỉnh 39

Hình 4.3 Hình chiếu của đoạn thẳng lên hình cầu đơn vị 40

Hình 4.4 Vector thẳng đứng được xác định bằng camera sử d ng các điểm vô cực của ảnh 41

Hình 4.5 Vị trí của bàn cờ đối với hệ tr c camera trong các thí nghiệm 43

Hình 4.6 Sai số của từng thí nghiệm 44

Hình 4.7 Hình ban đầu của hai hệ GPS và INS/Stereo-camera 49

Hình 4.8 Hình hai đường di chuyển sau cân chỉnh 50

Hình 4.9 Sai số sau khi cân chỉnh của hai hệ 50

Hình 4.10 Hình ban đầu của hai hệ GPS và INS/Stereo-camera 51

Hình 4.11 Hình hai đường di chuyển sau cân chỉnh 51

Hình 4.12 Sai số sau khi cân chỉnh của hai hệ 52

Hình 4.13 Mô hình tích hợp hệ thống định vị 52

Hình 5.1 Cảm biến IMU 9DOFs ADIS16405 của Analog Devices 56

Hình 5.2 Stereo-camera thương mại Bumblebee của Point Grey 56

Hình 5.3 GPS-RTK chính xác cấp cm của South 56

Hình 5.4 Board máy tính dùng trong phần cứng –Mặt trên 57

Hình 5.5 Board máy tính dùng trong phần cứng –Mặt dưới 57

Hình 5.6 Bàn xoay kiểm tra chất lượng IMU 57

Hình 5.7 Mô hình phần cứng board mạch xử lí INS 58

Hình 5.8 Mô hình phần cứng bàn xoay đánh giá chất lượng IMU 58

Hình 5.9 Mô hình phần cứng hệ thống tích hợp GPS/INS/Stereo-camera 58

Hình 5.10 Lấy dữ liệu trên xe kéo trong s n trường 59

Hình 5.11 Lấy dữ liệu trên xe hơi trên đường 59

Hình 5.12 Quy trình kết hợp MATLAB Embedded Coder với KeilC cho VĐK STM32F407 60

Hình 5.15 Mô hình giải thuật cho Vision thread 63

Hình 5.16 Ảnh trái phải thu được từ camera khi đặt trên thanh trượt 64

Hình 5.17 Hình trên là kết quả quãng đường ước lượng so sánh từ các hai phương pháp Dữ liệu chuẩn lấy từ encoder 65

Hình 5.18 Đồ thị kết quả từ tập dữ liệu số 2 65

Hình 5.19 Ảnh trái khi đặt camera nằm ngang,ảnh phải là camera sau khi nghiêng xuống 30° đã mất phần bầu trời trong ảnh 66

Hình 5.20 Hình trái phải ngoài thực tế chói sáng rất khó dò đặc trưng 66

Hình 5.21 [BK2] Đường di chuyển khoảng 120m 67

Hình 5.22 [BK2] Sai số ngõ ra tăng theo thời gian 67

Hình 5.23 [TB4] Đường di chuyển thẳng khoảng 160m 68

Hình 5.24 [TB4] Sai số tăng mạnh theo thời gian 68

Bảng 2.1 Giá trị covariance của các đại lương đo từ cảm biến 22Bảng 2.2 Giá trị nhiễu nền của vận tốc góc 3 tr c 27Bảng 2.3 Kết quả giải thuật INS 29

CHƯƠNG 1 TỔNG QUAN VỀ ĐỀ TÀI

Tổng quan về các hệ thống định vị cho robot di động

1.1.Một trong ba câu hỏi cơ bản của các nghiên cứu về robot di động là vị trí hiện tại của robot “Where am I?” Để có thể thực hiện hành động kế tiếp, robot cần biết vị trí của mình trong không gian đang di chuyển Thông tin về vị trí của robot nếu đối với không gian di chuyển rút gọn 2D thì sẽ bao gồm: góc heading, và tọa độ trên mặt Oxy Nếu robot di chuyển trong không gian đầy đủ 3D thì phải bao gồm ba góc quay theo ba trục roll, pitch, yaw và vị trí trên hệ trục Oxyz Để tìm ra các thông tin trên robot sẽ tiếp nhận và xử lí dữ liệu thô thu thập từ các cảm biến Các cảm biến thường dùng trong bài toán định vị là: Encoder, IMU, GPS, Range sensor, và Vision Ta sẽ đi sơ lượt qua cách định vị của từng loại cảm biến trên và rút ra nhận xét về ưu và nhược điểm của từng loại để có cái nhìn tổng quan về bài toán định vị cho robot, từ đó có thể đưa ra so sánh giữa các phương pháp

Hình 1.1 Minh họa ba câu hỏi cơ bản trong lĩnh vực robot di động

Encoder: phương pháp định vị dùng encoder thường được gọi là Wheel Odometry

Phương pháp 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 di chuyển của từng bánh xe Kết hợp với kết cấu cơ khí của robot ta suy ra được

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 rút gọn 2D Người ta chia các nguyên nhân dẫn đến sai số của phương pháp này làm hai loại:

Hình 1.2 Robot Pioneer 3-DX là robot có hai bánh vi sai, có thể định vị robot

dựa vào encoder trên từng bánh

Các sai số hệ thống có thể loại trừ nhờ vào các phương pháp cân chỉnh, còn các sai số ngoài hệ thống muốn loại bỏ phải kết hợp thêm các cảm biến khác như trường hợp robot tự hành thám hiểm sao Hỏa Curiosity dùng thêm hệ thống Stereo-Vision/INS (gọi tắt là Stereo/INS) dò sự trượt bánh xe của robot để bù lại và cung cấp thêm thông tin để đạt được mô hình định vị không gian đầy đủ 3D [1],[2],[3],[4]

Hình 1.3 Robot tự hành thám hiểm sao Hỏa Curiosity dùng kết hợp Wheel/Stereo/INS để định vị với độ chính xác cao 2-3%

IMU: phương pháp định vị dùng cảm biến IMU được gọi là INS Phương pháp này

ban đầu chỉ được dùng trong lĩnh vực hàng không và các tên lửa quân sự vì rất đắt tiền Tuy nhiên sau khi công nghệ MEMS ra đời thì ứng dụng của 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

Hình 1.4 Cảm biến IMU của hãng Xsens với sai số động nhỏ hơn 2° là loại thường dùng trong các nghiên cứu trên thế giới về hệ thống INS cho robot di

o Giải thuật ước lượng IMU o Robot di chuyển chậm

Hình 1.5 Sơ đồ tổng quát hệ thống INS

Sai số hệ thống sẽ được cải thiện nhờ vào quá trình cân chỉnh, và tăng chất lượng của cảm biến Còn sai số ngoài hệ thống có thể được cải thiện nhờ vào các bộ lọc như Kalman filter Ngoài ra có thể tích hợp thêm cảm biến Magnetometer để tăng độ chính xác cho bộ IMU 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 Thông thường có thể dùng GPS cho môi trường ngoài trời và Vision đối với trong nhà

GPS: [27] phương pháp định vị dùng GPS là phương pháp định vị toàn cục tức là

robot không cần biết vị trí ban đầu Hệ thống GPS được Mỹ xây dựng có tổng cộng hơn 24 vệ tinh bay xung quanh trái đất trên quỹ đạo 20,350 km Mỗi vệ tinh bay vòng quanh Trái đất hai lần một ngày trong một hoặc sáu quỹ đạo để cung cấp thông tin liên tục và bao phủ khắp bề mặt Trái đất Mỗi một vệ tinh sẽ broadcast tín hiệu radio thông tin về vị trí, tình trạng, và thời gian chính xác {t1} với tốc độ ánh sáng {C} khoảng 299,792,458 m/s Các bộ thu GPS sẽ nhận được tín hiệu radio của một vệ tinh vào một thời gian {t2} và dùng nó để tính khoảng cách tới vệ tinh này là tích của {C} và ({t2} - {t1}) Có được ít nhất khoảng cách này từ 4 vệ tinh, bộ thu sẽ tính được vị trí của nó trên Trái đất

Từ phương pháp tính trên ta đánh giá sai số của vị trí sẽ dựa vào độ chính xác của các yếu tố:

 Vận tốc của sóng radio không chính xác như vận tốc ánh sáng vì bị trễ: lan truyền môi trường như khí quyển Trái đất, điều khiện thời tiết, bị phản xạ nhiều lần…

 Độ chính xác về thời gian của clock dùng trong bộ thu Từ tháng 5/2000, chính phủ Mỹ đã quyết định bỏ chức năng “Selective Availability” trên vệ tinh cho phép làm suy giảm chất lượng tín hiệu GPS dân sự (để tăng sai số) Từ 2007 khi bắt đầu xây dựng các thế hệ mới của các vệ tinh GPS, chính phủ Mỹ đã bỏ hoàn toàn chức năng này đảm bảo chất lượng tín hiệu GPS là như nhau ở mọi nơi trên thế giới (< 20m) Thực chất mỗi một vệ tinh duy trì hai hai dịch vụ: SPS cho dân sự ( Standard Positioning Service) và PPS cho quân sự (Precision Positioning Service) cả hai đều có độ chính xác như nhau tuy nhiên thay vì một tần số broadcast thì PPS cung cấp hai tần số để giảm sai số do tín hiệu radio bị suy giảm khi vào bầu khí quyển Trái đất (ionospheric correction) và đảm bảo tính bảo mật, cũng như chống nhiễu

Hình 1.6 Chất lượng định vị tăng lên rõ rệt sau khi vừa tắt chức năng SA trên vệ

tinh Sai số cả kinh độ và vĩ độ đều nhỏ hơn 20m

Với nhu cầu tăng độ chính xác cho các hệ định vị dùng GPS nhằm áp dụng vào các tác vụ như đo đạc bản đồ, điều khiển… giải pháp phổ dụng là dùng các trạm base cố định trên mặt đất với vị trí đã biết trước sẽ nhận tín hiệu từ GPS và tính toán sai lệch sau đó gửi giá trị hiệu chỉnh này đến các bộ thu Sai số có thể đạt được tầm cm tùy vào hệ thống khác nhau Có nhiều hệ thống đã được triển khai (gọi chung là các hệ thống mở rộng Augmentation Systems - AS) như:

 DGPS: dùng hệ thống các trạm base cố định để broadcast giá trị sai lệch hiện tại của GPS so với vị trí cố định của nó

 WAAS: mạng lưới các vệ tinh nhận giá trị hiệu chỉnh từ các trạm mặt đất và broadcast giá trị này cho các bộ thu Chức năng này chủ yếu dùng cho hàng không

Tóm lại đối với ứng dụng theo dõi hoặc giám sát thì là hoàn toàn khả thi khi sai số cho các bộ thu thông thường nhỏ hơn 20m và là định vị toàn cục nên không cần biết vị trí đầu Tuy nhiên đối với ứng dụng điều khiển cần độ chính xác cao cấp met hay decimet nếu muốn dùng GPS thì bắt buộc phải dùng thêm các công nghệ mở rộng AS như đã nêu trên

Range sensor: phương pháp định vị dùng các cảm biến khoảng cách sẽ ước lượng

vị trí 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 Một số cảm biến như 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 số khác dùng structure-light như Microsoft Kinect: 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, Kinect sẽ tính ra được độ sâu tương ứng với từng pixel trên ảnh

Hình 1.7 Từ trái qua phải là: cảm biến siêu âm, laser scanner giá rẻ SICK, laser

scanner cao cấp Velodyle Lidar, và Microsoft Kinect

Sai số của phương pháp này phụ thuộc nhiều vào chất lượng cảm biến và quá trình calib Trong trường hợp Microsoft Kinect phiên bản cho Window rất phù hợp cho các nghiên cứu về định vị, cảm ứng chuyển động người cho các tương tác người-

máy và giá thành khá rẻ Tuy nhiên nó bị một hạn chế là tầm nhìn của cảm biến bị giới hạn (44° VFOV, 57° HFOV [28]), và độ sâu cho giá trị tốt trong tầm 3m Đối với laser scanner có tầm hoạt động rộng và ngày nay đang được ứng dụng rộng rãi vào lĩnh vực xe tự hành tuy nhiên giá thành khá đắt Do đó việc sử dụng công nghệ định vị này là phải đánh đổi giữa chất lượng và giá cả

Hình 1.8 Ảnh cho độ sâu từng pixel từ Kinect & Hình ảnh không gian 3D tái tạo

từ bộ quét laser

Vision: phương pháp định vị dùng công nghệ xử lí ảnh từ camera gọi là visual

odometry Phương pháp này đang được dùng rộng rãi vì giá thành hợp lí đồng thời khả năng mở rộng các tính năng khác ngoài định vị của camera là rất lớn Ví dụ như định vị kết hợp lập bản đồ SLAM, tránh vật cản, dò biển báo giao thông … Chi tiết về giải thuật định vị dùng camera sẽ được trình bày kỹ hơn ở phần sau, nên phần này chỉ nêu tóm tắt một số ưu & nhược điểm

Hình 1.9 Định vị cho quadrotor dùng camera đơn [5]

Hình 1.10 Xe thu thập dữ liệu của Đại học KIT, Đức [22] và kết quả chạy giải thuật của Hernan [19] Hai đường định vị dùng stereo-camera và đường chuẩn

đo từ GPS gần như trùng nhau

Hệ thống định vị dùng công nghệ xử lí ảnh có nhiều ưu điểm vượt trội so với các công nghệ khác:

 Sử dụng được trong môi trường trong nhà lẫn ngoài trời

 Độ chính xác phụ thuộc nhiều vào giải thuật phần mềm hơn là phần cứng

 Không bị ảnh hưởng bởi hiện tượng trượt của robot

 Có thể tích hợp mở rộng thêm nhiều chức năng khác: dò để tránh vật cản, dò và bám, xây dựng lại bản đồ 3D quãng đường đã di chuyển (3D SLAM)… Tất nhiên bên cạnh đó hệ thống định vị dùng công nghệ xử lí ảnh vẫn có các nhược điểm đồng thời cũng là các thách thức đặt ra cho việc nghiên cứu như:

 Hệ thống định vị tương đối: nên phụ thuộc rất nhiều vào các thiết lập ban đầu như vị trí, góc nghiêng, các thông số nội của camera Hơn nữa sai số hệ thống sẽ tích lũy theo thời gian nên đặt ra yêu cầu phải có thêm phần hiệu chỉnh lại vị trí sau một thời gian dài di chuyển

 Nhạy với nhiễu từ các nguồn sáng: là vấn đề mà tất cả các hệ thống xử lí ảnh phải đối mặt

 Phụ thuộc vào mức độ giàu có của môi trường về thông tin: môi trường càng nhiều đặc trưng như cây cối, đồ vật… thì càng phù hợp sử dụng công nghệ xử lí ảnh

Hệ thống định vị tích hợp dùng Stereo-Vision và INS

1.2.Ở phần 1, học viên đã liệt kê ra nhiều kỹ thuật định vị khác nhau Mỗi một phương pháp có các ưu-nhược điểm và vùng hoạt động hiệu dụng riêng, không có phương pháp nào là tuyệt đối cho tất cả các trường hợp Do đó ta có thể đưa ra nhận xét là: một hệ thống định vị để đạt kết quả tốt hơn thì nên tích hợp nhiều phương pháp Đặc biệt hướng nghiên cứu định vị dùng xử lí ảnh từ stereo-camera đang là một hướng rất nóng vì tiềm năng ứng dụng lớn của nó vào các robot di động Đồng thời hệ INS với khả năng cho tần số cập nhật cao, cung cấp ma trận xoay chính xác và gia tốc ngoài trong trường hợp robot di chuyển tốc độ cao sẽ là giải pháp bổ trợ tốt cho camera khi mà hầu hết các giải thuật xử lí ảnh Stereo cho tần số cập nhật thấp (50-60ms) không phù hợp với các bài toán điều khiển Do đó trong đề tài luận văn này học viên chọn hệ thống định vị tích hợp INS/Stereo-camera và rộng hơn nữa là xây dựng thuật toán có thể tích hợp nhiều loại cảm biến khác nhau vào chung một hệ thống định vị

Bộ lọc Kalman mở rộng EKF

1.3.Trong hầu hết các bài báo về tích hợp hệ thống định vị bộ lọc Kalman mở rộng được sử dụng nhiều vì khả năng khử nhiễu trắng từ cảm biến, cũng như ước lượng được các biến trạng thái của hệ thống thông thường đóng vai trò quan trọng là đầu vào cho bộ điều khiển Ở đây học viên mô tả mô hình lý thuyết bộ lọc Kalman mở rộng sẽ được áp dụng xuyên suốt trong luận văn này

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

f( , )h( )

(1.1)

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:

 Trạng thái trước ước lượng:

1ˆ ( ) f(ˆ ( ), ( ), ) for k, k with ˆ ( )k ˆk

fF( ( ), ( ), )

x t u t t

ˆ( ) ( ( ), )

()()( ( ), )

Mục tiêu và các nội dung cần giải quyết của đề tài

1.4.Mục tiêu chính của đề tài là tìm hiểu giải thuật để tích hợp hệ thống định vị INS/Stereo-camera Bên cạnh đó học viên sẽ xây dựng một phần mềm định vị chạy thời gian thực cho tần số cập nhật 100Hz với đầy đủ ngõ ra bao gồm vị trí, và góc xoay của hệ Ngoài ra phần mềm còn có thể cung cấp các biến ước lượng khác khi cần thiết gồm vận tốc tịnh tiến, vận tốc góc quay, gia tốc dịch chuyển… Mục tiêu sai số vị trí đạt cho hệ INS/Stereo-camera nằm ở khoảng 4% trong môi trường lý tưởng có nhiều đặc trưng phục vụ cho bộ xử lí ảnh và ít vật thể chuyển động trong vùng hoạt động của camera Sở dĩ có điều kiện ràng buộc này vì giải thuật xử lí ảnh chỉ hiệu quả khi có nhiều điểm đặc trưng trong vùng hoạt động của camera

Tóm tắt các nội dung cần giải quyết trong đề tài: - Xây dựng giải thuật tích hợp hệ thống định vị kết hợp INS và stereo-camera - Xây dựng phần mềm tích hợp chạy thời gian thực

- Xây dựng phần mềm đánh giá, cân chỉnh hệ thống dùng GPS-RTK

CHƯƠNG 2 HỆ THỐNG DẪN ĐƯỜNG QUÁN TÍNH INS

Nguyên lý định vị của hệ INS đã được trình bày tóm tắt ở Chương 1 Cốt lõi của bộ định vị INS là giải thuật ước lượng hình chiếu vector trọng trường trên cảm biến IMU từ đó sẽ tính được giá trị gia tốc ngoài và dùng phép tích phân ta sẽ suy ra vị trí Thực chất là ta sẽ phải tìm ma trận xoay từ hệ trục IMU sang hệ trục Trái đất Hệ INS cho tần số cập nhật cao có thể lên tới trên vài KHz thỏa mãn hầu hết tất cả các mô hình điều khiển từ đơn giản tới phức tạp

Tuy nhiên chất lượng hệ INS phụ thuộc nhiều vào chất lượng của cảm biến Các ứng dụng cần độ chính xác cao như vũ trụ, hay trong quân sự dùng các cảm biến giá thành rất cao không phù hợp cho các ứng dụng dân sự Vì vậy sau khi công nghệ MEMS ra đời mặc dù giá thành các cảm biến này giảm xuống nhưng đồng nghĩa chất lượng cảm biến cũng xuống theo Nói chung ngõ ra quan trọng nhất của các bộ INS dùng công nghệ MEMS ứng dụng trong robot di động là ba góc xoay Euler roll, pitch, yaw hay là ma trận xoay chuyển hệ trục cảm biến sang hệ trục Trái đất Còn giá trị gia tốc ngoài ước lượng được thường đi kèm nhiễu nên việc tích phân ra vị trí sẽ mau chóng tạo ra sai số lớn không khả thi để sử dụng riêng lẻ

Thực tế INS dùng công nghệ MEMS cho ra kết quả 3 góc xoay Euler rất tốt nhưng luôn phải kết hợp với các loại cảm biến khác để cải thiện giá trị vị trí ví dụ như GPS, camera, RADAR…

Trong chương này ở phần 1 học viên trình bày tổng quan các giải thuật hiện nay về hệ INS, cách ước lượng các góc Euler Trong phần hai học viên đề xuất mô hình giải thuật INS dùng EKF thích nghi (đã được học viên xuất bản ở [25]) Kết quả thực ngiệm ở phần 3 cho thấy mô hình này đạt kết quả tốt đáp ứng được yêu cầu của luận văn

Tổng quan các giải thuật của hệ INS

2.1.Hiện nay có 3 phương pháp để biểu diễn góc quay và chuyển động tịnh tiến của một IMU trong không gian Phương pháp được sử dụng rộng rãi và tương đối đơn giản là sử dụng góc Euler theo 3 trục x, y, z Tuy nhiên, biểu diễn góc theo phương pháp góc Euler có nhược điểm là IMU không thể bám theo đối tượng trong tất cả các hướng do sự suy biến của góc xoay quanh trục y khi nó tiến tới 90o Phương pháp ma trận xoay Direction Cosine Matrix (DCM) sẽ khắc phục được trường hợp suy biến của phương pháp góc Euler Phương pháp thứ ba cũng được sử dụng rất nhiều đó là phương pháp toán tử xoay Quaternion, phương pháp này cũng khắc phục được sự suy biến của phương pháp Euler đồng thời cải thiện được khả năng tính toán Trong ba phương pháp trên, thì hai phương pháp DCM và Quaternion được sử dụng chủ yếu, là cơ sở để thiết kế các bộ ước lượng nói chung, đặc biệt là bộ lọc Kalman Trong hầu hết các bài báo quốc tế liên quan tới IMU, thuật toán ước lượng dựa trên Bộ ước lượng Kalman mở rộng được áp dụng rộng rãi trong việc ước lượng 7 biến trạng thái đầu vào gồm 3 thành phần vận tốc góc, 4 thành phần quaternion và 9 giá trị đo lường ngõ ra gồm 3 thành phần vận tốc góc, 3 thành phần gia tốc góc và 3 thành phần từ trường [30][31][32] Bộ ước lượng thiết kế đơn giản nhưng đòi hỏi tính toán nhiều, đặc biệt là ngõ ra phi tuyến Thuật toán Guass-Newton được kết hợp với bộ ước lượng Kalman (QUEST) để loại bỏ tính phi tuyến của ngõ ra của bộ ước lượng, giảm được khối lượng tính toán đáng kể Thuật toán cải tiến giảm bậc cho bộ ước lượng Kalman để giảm thời gian tính toán ma trận nghịch đảo, và một số kinh nghiệm trong việc đánh giá nhiễu quá trình và nhiễu đo lường cho bộ ước lượng Kalman mở rộng cũng đã được đề cập ở [33][34] Nhóm tác giả mô phỏng offline trên MATLAB để so sánh chất lượng của thuật toán QUEST với bộ ước lượng Kalman mở rộng, sau đó thực hiện kiểm tra kết quả real-time với ứng dụng viết bằng Java Độ chính xác với hệ thống tĩnh là 2o và hệ thống động là 4o

Tác giả Angelo M Sabatini [37] đã xây dựng mô hình đo lường cảm biến thực trong đó xem x t toàn diện các thành phần bias và nhiễu Bộ ước lượng Kalman mở rộng

dựa trên toán tử quaternion, ước lượng thêm 6 biến trạng thái gồm 3 thành phần bias accelerometer, 3 thành phần bias của magnetometer Bài báo đã đưa ra các biểu thức để cập nhật ma trận hiệp phương sai nhiễu quá trình Q , đồng thời cập nhật ma trận k

hiệp phương sai nhiễu đo lường R bằng các kiểm tra trước các dữ liệu đo lường k

accelerometer và magnetometer Tác giả đã kiểm tra kết quả dựa trên tập dữ liệu từ cảm biến InertiaCube2 (của hãng InterSense Inc.) với nhiều phương pháp khác nhau Mặc dù phương pháp này đòi hỏi phải tính toán rất nhiều, cộng với việc bù được ảnh hưởng nhiễu của từ trường môi trường xung quanh, đòi hỏi độ chính xác của cảm biến, nhưng kết quả đạt được là rất tốt, sai số RMSE nhỏ nhất đạt được là 2.57o

Vấn đề bù nhiễu từ trường [35][36] được nhóm tác giả thực hiện bằng bộ ước lượng Kalman bù, sai số vận tốc, và sai số nhiễu từ trường được ước lượng vào trong các phương trình trạng thái Kết quả đạt được với môi trường nhiễu từ trường nội rất tốt, sai số tĩnh (1.4o), sai số động là 2.6o (RMSE)

Bộ ước lượng Kalman dựa trên ma trận DCM cũng được thực hiện và so sánh với các bộ ước lượng sử dụng phương pháp Euler và Quaternion [38][39] Nhóm tác giả thiết kế 2 bộ ước lượng Kalman riêng biệt cho phương pháp DCM, một để ước lượng góc quay trục x, y và một cho các góc quay trục z Bằng cách này, việc tính toán sẽ nhanh hơn do tránh được ph p tính ma trận nghịch đảo có kích thước lớn Ngoài ra, phương pháp DCM cũng tránh được sai số khi xấp xỉ bậc nhất của bộ ước lượng Kalman mở rộng, và không bị ảnh hưởng bởi vector từ trường tham chiếu Đặc biệt việc bổ sung các biến gia tốc ngoài và nhiễu từ trường sẽ giúp mô hình thích nghi tốt hơn trong các điều kiện khác nhau Việc kiểm tra, so sánh kết quả được thực hiện trên tiện ích Matlab Aerospace Blockset Nhóm tác giả cũng đã mô phỏng hiện tượng suy biến của phương pháp góc Euler mà phương pháp DCM và Quaternion không gặp phải

Ở đây trong luận văn này học viên đề xuất một bộ ước lượng DCM bao gồm 2 bộ lọc thích nghi mở rộng Kalman nối tiếp nhau được phát triển dựa trên [38] để ước lượng các giá trị của ma trận xoay, các giá trị gia tốc ngoài và nhiễu từ trường

Giải thuật EKF thích nghi hệ INS

2.2.Bộ ước lượng DCM bao gồm 2 bộ lọc thích nghi mở rộng Kalman nối tiếp nhau Bộ AEKF1 sẽ ước lượng ba giá trị hàng thứ ba trong ma trận DCM, và gia tốc ngoài trên 3 trục Bộ AEKF2 sẽ ước lượng ba giá trị hàng thứ nhất trong ma trận DCM và nhiễu từ trường trên ba trục Hàng còn lại sẽ được suy ra dựa vào tính chất trực giao trong ma trận DCM

BRimdB

a

flagd

a

flagd

m

ˆ

EBR

1

2

3ˆ

ˆ

EB

EB

Hình 2.1 Sơ đồ khối bộ ước lượng EKF thích nghi hai tầng

Xác định mô hình đối tượng và mô hình đo lường cho bộ lọc Kalman 2.2.1.

theo phương pháp DCM

Ước lượng trạng thái góc nghiêng liên quan đến việc xác định góc roll và góc pitch bằng các sử dụng cảm biến vận tốc góc và gia tốc Dữ liệu từ cảm biến vận tốc góc xem như mô hình đối tượng, dữ liệu từ cảm biến gia tốc và từ trường là mô hình đo lường

a : Giá trị gia tốc ngoài tác động lên IMU đƣợc mô hình hóa thành bộ lọc thông

m : Giá trị nhiễu từ trường khi cảm biến bị ảnh hưởng bởi môi trường điện từ xung

quanh được mô hình thành bộ lọc thông thấp bậc một:

w : Giá trị nhiễu trắng của cảm biến

Do đó trong mô hình quá trình ta sẽ mở rộng thêm các biến trạng thái gia tốc ngoài và nhiễu từ trường Trong mô hình đo lường thứ nhất sử dụng vector gia tốc được xây dựng:

3132330

sin ossin sin sin os os

os sin sin os sin

EBEBEB

Mô hình rời rạc của hệ

Sử dụng các phương trình trạng thái ở biểu thức (8) đến (20), bộ ước lượng góc quay dựa trên phương pháp DCM được thực hiện thông qua 2 khối EAKF1 và EAKF2 như Hình 2.1 Sơ đồ khối bộ ước lượng EKF thích nghi hai tầng, khối thứ nhất ước lượng trạng thái góc 2 góc quay trục x, y (roll, pitch) và khối thứ 2 ước lượng góc quay trục z (yaw) Bằng cách này, khối lượng tính toán sẽ nhỏ hơn là ước lượng cùng lúc cả 3 góc quay

Mô hình đối tượng và mô hình đo lường của bộ lọc Kalman thứ 1:

3 31

13 3

0

Tk

Txk

ii

xx

aT

os sin sin os sin

2

3[ 0 ]

3 32

23 3

0

Tk

Txk

ii

dxx

k

R R R

IT

Covariance của nhiễu đo lường

Đối với nhiễu đo lường thông thường ta có thể tính dựa vào các thông số kĩ thuật trong tài liệu được cung cấp từ nhà sản xuất cảm biến Tuy nhiên giá trị này chỉ mang tính chất tham khảo cho mỗi dòng cảm biến, do đó đối với từng cảm biến cụ thể ta cần có phương pháp để tính các giá trị này Với giả thiết rằng giá trị đo được

trên ba trục của mỗi cảm biến là không tương quan nhau thì khi đó giá trị covariance cần tính trên mỗi trục cũng chính là variance của đại lượng đó Phương pháp thường được sử dụng để tính variance là đặt cảm biến đứng yên trong khoảng thời gian dài (khoảng một giờ đồng hồ) sau đó tính variance từ các giá trị thu được

Dưới đây là các giá trị covariance tính được cho 3 loại cảm biến: gia tốc, vận tốc góc và từ trường

Bảng 2.1 Giá trị covariance của các đại lương đo từ cảm biến

1

BxB

Bz

aT

a

222

r : giá trị variance của ba số hạng C C21, 2 2,C Vì ta không thể tính trực tiếp 23

như trường hợp ba giá trị đo gia tốc ở trên nên hằng số này phải được tính dựa vào phương pháp thử sai

 1, 2: là hai hệ số tỉ lệ dùng trong trường hợp muốn tinh chỉnh độ lớn của ma trận covariance nhiễu đo lường

Covariance của nhiễu quá trình

Khác với mô hình đo lường, ma trận covariance của nhiễu quá trình sẽ được tính dựa vào đặc tính cảm biến và phương trình lan truyền của mô hình toán đối tượng Ở

phần trước ta đã có các biến trạng thái của đối tượng là:

3 31

13 3

0

Tk

Txk

ii

xx

3 32

23 3

0

Tk

Txk

ii

dxx

k

R R R

IT

q

Trong đó:

 R : ma trận covariance của ba giá trị đo vận tốc góc tương ứng ma trận

đường chéo chứa ba giá trị variance của cảm cảm biến vận tốc góc tra trên bảng (1)

Ngoài ra, các biến trạng thái mở rộng gồm gia tốc ngoài và nhiễu từ trường trong mô hình cũng gây chậm đáp ứng của hệ thống Trong trường hợp tĩnh hoặc không có nhiễu từ ngoài thì giá trị các biến này phải xấp xỉ 0 để đảm bảo mô hình đo lường là chính xác

Giải pháp được học viên đưa ra là trong thuật toán sẽ có thêm một bộ thích nghi cho IMU trong các điều kiện hoạt động khác nhau Trong phần tiếp theo học viên sẽ trình bày các loại thuật toán thích nghi trong IMU bao gồm tinh chỉnh độ lớn ma trận covariance và “đóng-ngắt” bộ khử nhiễu nền trong các điều kiện khác nhau

Trong thực tế có hai thuật toán thích nghi để IMU có thể hoạt động tốt với các điều kiện khác nhau:

 Thuật toán hệ số tỉ lệ: với mỗi ma trận covariance ta nhân thêm một hệ số tỉ lệ Từ đó khi phát hiện có gia tốc ngoài hoặc từ trường ngoài ta sẽ tinh chỉnh hệ số tỉ lệ của nhiễu đo lường tăng lên và hệ số tỉ lệ của nhiễu quá trình nhỏ xuống

 Thuật toán khử nhiễu nền: vẫn giữ nguyên các ma trận covariance như trạng thái tĩnh tuy nhiên thêm vào mô hình trạng thái các biến nhiễu nền gồm gia tốc ngoài và từ trường ngoài Mô hình phương pháp này đã được phát triển ở phần trước Ở điều kiện thường (gia tốc ngoài nhỏ hoặc không có nhiễu từ trường ngoài) ta sẽ “ngắt” các giá trị này ra khỏi mô hình (bằng cách thiết lập giá trị các biến là 0); khi có điều kiện cần ta sẽ “đóng” các biến này vào mô hình

Yêu cầu đầu tiên của các phương pháp này là phải có điều kiện cần để “kích hoạt“ bộ thích nghi Thông thường ta sẽ dùng giá trị độ lớn là hiệu số tổng gia tốc đo được so với tổng gia tốc khi đứng yên 2

9.8 /

so với tổng từ cố định của Trái Đất tại vị trí đang x t Nếu giá trị hiệu này vượt một ngưỡng cho trước thì sẽ “kích hoạt“ bộ thích nghi Giá trị ngưỡng nếu quá lớn sẽ dẫn đến hệ thống chậm thích nghi với điều kiện ngoài, còn nếu quá nhỏ sẽ dẫn đến hệ thống nhạy với nhiễu Dựa vào một số kết quả đã được công bố ở [40],[41] cùng với thực nghiệm thì nhóm đề xuất giá trị ngưỡng cho gia tốc ngoài là

20.2( / )

THRES

am s và cho từ trường ngoài là mTHRES 0.1Gauss

Như vậy giá trị covariance thay đổi theo điều kiện như sau:

Hình 2.2 Mô hình tinh chỉnh độ lớn ma trận covariance

Ở đây khi có điều kiện cần độ lớn ma trận covariance nhiễu đo lường R sẽ được k

tăng lên và độ lớn ma trận covariance nhiễu quá trình Q sẽ giảm xuống k

tăng dần theo thời gian Do đó để khắc phục điều này ta cần tính giá trị bias ban đầu của cảm biến khi khởi động Trường hợp nếu không có nhiễu bias ban đầu thì các giá trị đo trung bình khi cảm biến đứng yên sẽ xấp xỉ 0 (rad/s) tuy nhiên do bị bias nên trị trung bình sẽ khác không Dùng tập dữ liệu khi cảm biến đứng yên trên phần 1 ta tính lần lượt các giá trị trung bình sẽ thu được giá trị bias trên ba trục của cảm biến vận tốc góc Kết quả tính toán nhiễu nền vận tốc góc theo 3 trục được trình bày như ở Bảng 2.2 và đồ thị minh họa ở Hình 2.4 Ta thấy rằng giá trị trung bình nhiễu nền của vận tốc góc ở mức -10mrad s đến 10mrad s Do đó, trong bộ ước lượng góc ta sẽ cộng thêm giá trị nhiễu nền để tăng độ chính xác của góc ng ra

Hình 2.4 Giá trị nhiễu nền của vận tốc góc theo thời gian Bảng 2.2 Giá trị nhiễu nền của vận tốc góc 3 tr c

Trục Trục y Trục

Giá trị nhiễu nền (mrad s) 12.1720 4.6173 -4.6138 Đối với cảm biến từ trường quá trình cân chỉnh được thực hiện bằng cách xoay IMU xung quanh các trục x, y, z Nếu không tồn tại nhiễu nền của từ trường ngoài thì giá trị đo của 2 trục bất kỳ sẽ vạch nên 1 vòng tròn có tâm tại gốc tọa độ Nếu tồn tại nhiễu nền, thì vòng tròn sẽ có tâm lệch khỏi góc tọa độ Nếu tồn tại nhiễu nền, công thức tính góc yaw sẽ không chính xác Do đó, để cân chỉnh cho chính xác ta phải bù 1 lượng bằng với độ lớn của tâm lệch