Xây dựng bộ ước lượng hướng và góc nghiêng trong không gian ba chiều

Đại Học Quốc Gia Tp Hồ Chí Minh TRƯỜNG ĐẠI HỌC BÁCH KHOA LÊ MẠNH THẮNG XÂY DỰNG BỘ ƯỚC LƯNG HƯỚNG VÀ GÓC NGHIÊNG TRONG KHÔNG GIAN CHIỀU Chun ngành Mã ngành : Tự động hóa : 605260 LUẬN VĂN THẠC SĨ TP HỒ CHÍ MINH, tháng 01 năm 2011 CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA ĐẠI HỌC QUỐC GIA TP HỒ CHÍ MINH Cán hướng dẫn khoa học: TS Nguyễn Vĩnh Hảo Cán chấm nhận xét 1: TS Huỳnh Thái Hoàng Cán chấm nhận xét 2: TS Nguyễn Thiện Thành Luận văn Thạc sĩ bảo vệ Trường Đại học Bách khoa, ĐHQG Tp HCM ngày 04 tháng 01 năm 2011 Thành phần hội đồng đánh giá luận văn Thạc sĩ gồm: TS Nguyễn Đức Thành – chủ tịch TS Huỳnh Thái Hoàng – phản biện PGS.TS Dương Hồi Nghĩa TS Hồng Minh Trí TS Võ Hoàng Duy TS Nguyễn Thiện Thành – phản biện Xác nhận chủ tịch hội đồng đánh giá luận văn Bộ môn quản lý chuyên ngành sau luận văn chỉnh sửa (nếu có) Chủ tịch hội đồng đánh giá LV Bộ mơn quản lý chuyên ngành Trang ii TRƯỜNG ĐẠI HỌC BÁCH KHOA PHÒNG ĐÀO TẠO SĐH Số: / BKSĐH CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc TpHCM, ngày 18 tháng 01 năm 2011 NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên: LÊ MẠNH THẮNG Phái: Nam Ngày, tháng, năm sinh: 21 tháng 06 năm 1985 Nơi sinh: Đồng Nai Chuyên ngành: Tự động hóa MSHV: 01508348 I Tên đề tài: XÂY DỰNG BỘ ƯỚC LƯỢNG HƯỚNG VÀ GÓC NGHIÊNG TRONG KHÔNG GIAN CHIỀU II Nhiệm vụ nội dung: _ III Ngày giao nhiệm vụ luận án: IV Ngày hoàn thành nhiệm vụ: _ V Cán hướng dẫn (ghi rõ học hàm, học vị, họ, tên): Nội dung yêu cầu LVTS thông qua Bộ mơn CÁN BỘ HƯỚNG DẪN CN BỘ MƠN (Học hàm, học vị, họ tên chữ ký) QL CHUYÊN NGÀNH Trang iii Lời Cảm Ơn! Lời đầu tiên, em xin trân trọng gửi lời cám ơn đến thầy cô Trường Đại học Bách khoa Thành phố Hồ Chí Minh, đặc biệt thầy cô Bộ môn Tự động hóa, Khoa Điện – Điện tử, người tận tình dìu dắt, truyền đạt kiến thức, kinh nghiệm quý báu cho em suốt thời gian học tập rèn luyện trường Em xin chân thành cảm ơn thầy – TS Nguyễn Vĩnh Hảo Sự hướng dẫn, giúp đỡ tận tình thầy động lực giúp cho em hoàn thành tốt luận văn Con xin cảm ơn ba mẹ, người thân gia đình nuôi nấng, động viên tảng chỗ dựa vững cho đời Tôi xin gửi lời cảm ơn đến bạn tôi, người giúp đỡ tôi, bên tơi lúc tơi khó khăn, giúp đỡ tơi q trình học tập, cho tơi kinh nghiệm quý báu sống rèn luyện thân Tp Hồ Chí Minh 08/12/2010 Lê Mạnh Thắng Trang iv TÓM TẮT LUẬN VĂN Luận văn xây dựng ước lượng hướng góc nghiêng từ liệu cảm biến vận tốc góc, cảm biến gia tốc cảm biến từ trường Các ước lượng hướng góc nghiêng dựa kiến thức phương pháp biểu diễn hướng góc nghiêng đối tượng không gian chiều Các phương pháp phổ biến bao gồm phương pháp góc Euler, phương pháp ma trận xoay DCM phương pháp toán tử xoay Quaternion Bộ ước lượng dựa phương pháp cần phải có khả lọc nhiễu ước lượng thông số tốt Bộ lọc Kalman lựa chọn tối ưu để giải vấn đề Trong đề tài trình bày ước lượng với lọc Kalman dựa phương pháp trình bày để có nhìn tổng quát, so sánh đánh giá sai số phương pháp thuật toán để thiết kế lọc Luận văn xây dựng ước lượng DCM PI, Kalman DCM, Kalman Gauss-Newton dựa toán tử quaternion, Kalman mở rộng (EKF) dựa tốn tử quaternion Cả ước lượng có ưu khuyết điểm khác thực tập liệu cảm biến Xsens giá thành cao, cảm biến board IMU 9DOF Rozar giá rẻ Kết ước lượng đạt với độ xác cao với tập liệu mẫu Xsens; nhiên, với board IMU 9DOF Rozar cịn bị sai số lớn (có khoảng di chuyển sai số lên tới 10 độ), điều chất lượng cảm biến giá rẻ board IMU Trong ước lượng xây dựng lọc Kalman DCM cho kết ước lượng tốt với độ ổn định cao, không bị ảnh hưởng vector từ trường tham chiếu nên áp dụng vào ứng dụng thực tế mobile robot, quadrotor ứng dụng khác Trang v DANH MỤC CÁC TỪ VIẾT TẮT AHRS Attitude and Heading Reference Systems DCM Direction Cosine Matrix EKF Extended Kalman Filter IMU Inertial Measurement Unit MARG Magnetic, Angular rate, and Gravity MEMS Microelectromechanical Systems MSE Mean Square Error RMSE Root Mean Square Error QUEST Quaternion Estimator WMM World Magnetic Model Trang vi Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo Mục Lục Tóm tắt luận văn iii Danh mục từ viết tắt iv Mục lục Chương 1.1 Tổng quan đề tài 1.1.1 Giới thiệu đề tài 1.1.2 Tình hình nghiên cứu đề tài 1.1.3 Giới thiệu số IMU thị trường ứng dụng 1.2 Nhiệm vụ luận văn: 1.3 Nội dung luận văn Chương 10 2.1 Phép biến đổi ma trận – Ma trận xoay 11 2.1.1 Hệ tọa độ 11 2.1.2 Ma trận xoay 11 2.1.3 Ma trận xoay quanh trục cố định 12 2.2 Phương pháp góc Euler 14 2.2.1 Phép xoay đối tượng với góc Euler 14 2.2.2 Phương trình động học góc Euler 15 2.2.3 Vấn đề phương pháp góc Euler 18 2.3 Phương pháp DCM (Direction Cosine Matrix) 18 2.3.1 Ma trận DCM 18 2.3.2 Phương trình vi phân ma trận DCM 18 2.4 Phương pháp toán tử xoay Quaternion 20 2.4.1 Ý tưởng thực toán tử quaternion 20 2.4.2 Đại số quaternion 20 2.4.3 Mối quan hệ quaternion vector 23 2.4.4 Quaternion biểu diễn phép xoay 25 2.4.5 Ma trận xoay Quaternions 25 2.4.6 Phương trình vi phân toán tử quaternion 28 2.5 Chuyển đổi góc Euler, ma trận DCM ma trận xoay Quaternion 29 2.5.1 Chuyển đổi ma trận DCM góc Euler 29 2.5.2 Chuyển đổi tốn tử quaternion góc Euler 30 2.5.3 Chuyển đổi ma trận DCM toán tử quaternion 31 Chương 32 3.1 Bộ lọc Kalman 33 3.2 Bộ lọc Kalman tuyến tính rời rạc 35 3.2.1 Hàm ước lượng dạng tuyến tính 36 3.2.2 Vấn đề tối ưu, phương trình độ lợi Kalman 36 3.2.3 Phương trình cập nhật đo lường 36 3.3 Vấn đề phi tuyến 39 3.3.1 Ước lượng phi tuyến 39 3.3.2 Tuyến tính hóa 39 3.3.3 Tuyến tính hóa quanh giá trị ước lượng 41 HVTH: Lê Mạnh Thắng - 01508348 Trang Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo 3.3.4 Thực lọc rời rạc tuyến tính hóa mở rộng 42 3.4 Bộ lọc Kalman mở rộng – Extended Kalman filter 42 3.4.1 Bộ lọc Kalman tuyến tính rời rạc 42 3.4.2 Bộ lọc Kalman mở rộng rời rạc 43 Chương 45 4.1 DCM cho hệ thống tham chiếu hướng góc nghiêng 46 4.1.1 Hệ thống tham chiếu hướng góc nghiêng AHRS 46 4.1.2 Thiết kế thuật toán DCM cho AHRS 49 4.2 Thiết kế lọc Kalman dựa phương pháp DCM 52 4.2.1 Xác định mơ hình đối tượng mơ hình đo lường 52 4.2.2 Thiết kế lọc Kalman tuyến tính rời rạc 54 4.3 Thiết kế lọc Kalman dựa thuật toán Gauss-Newton 56 4.3.1 Phương pháp bình phương cực tiểu hàm tuyến tính 56 4.3.2 Phương pháp Gauss-Newton 57 4.3.3 Ứng dụng phương pháp Gauss-Newton 58 4.3.4 Giảm bậc cho phương pháp Gauss-Newton 59 4.3.5 Thiết kế lọc Kalman 61 4.4 Thiết kế lọc Kalman mở rộng dựa phương pháp Quaternion 62 4.4.1 Xác định mơ hình đối tượng mơ hình đo lường 62 4.4.2 Thiết kế lọc 63 Chương 67 5.1 Giới thiệu board mạch phần cứng 68 5.1.1 Mô tả chung board 68 5.1.2 Cảm biến gia tốc ADXL345 68 5.1.3 Cảm biến vận tốc góc - Gyro LPR530AL LY530ALH 69 5.1.4 Cảm biến từ trường HMC5843 70 5.2 Thực nghiệm lấy mẫu liệu công cụ đánh giá 70 5.3 Thực kiểm tra ước lượng tập liệu chuẩn 72 5.3.1 So sánh lọc Kalman DCM lọc Kalman Gauss-Newton 72 5.3.2 So sánh ước lượng DCM PI lọc Kalman DCM 75 5.3.3 So sánh ước lượng Kalman Gauss-Newton EKF quaternion 77 5.3.4 So sánh ước lượng DCM PI Kalman DCM với EKF quaternion79 5.4 Thực kiểm tra ước lượng board IMU 9DOF 82 5.4.1 Bộ ước lượng DCM PI lọc EKF 82 5.4.2 Bộ lọc Kalman DCM & thuật toán Gauss-Newton 86 5.4.3 Bộ lọc Gauss-Newton & EKF 87 Chương 89 1.1 Kết luận 90 1.1.1 Các vấn đề làm 90 1.1.2 Những vấn đề chưa làm 90 1.2 Hướng phát triển 91 Tài liệu tham khảo 92 HVTH: Lê Mạnh Thắng - 01508348 Trang Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo Chương Giới Thiệu HVTH: Lê Mạnh Thắng - 01508348 Trang Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo 1.1 Tổng quan đề tài 1.1.1 Giới thiệu đề tài Vấn đề xác định hướng góc nghiêng đối tượng chuyển động (orientation tracking) thời gian thực ứng dụng rội rãi nhiều lĩnh vực, giới người ta sử dụng ứng dụng thực tế ảo, kỷ xảo điện ảnh, trị chơi giải trí tích hợp thiết bị công nghệ cao, hệ thống tự định vị hướng xe, robot thao tác di chuyển bám đuổi mục tiêu, điều khiển máy bay nhiều ứng dụng khác Các đề tài vấn đề có nghiên cứu với kết định Tuy nhiên, nước ta vấn đề chưa nghiên cứu nhiều Hệ thống IMU (Inertial measurement unit) ước lượng hướng góc nghiêng, kết hợp cảm biến từ trường (magnetometer), cảm biến vận tốc góc (gyroscope) cảm biến gia tốc (accelerometer), gọi chung MARG (Magnetic, Angular rate, and Gravity) Đây cảm biến hệ vi điện tử (MEMS – Microelectromechanical systems) Hình 1.1 mơ tả tổng qt q trình này, Hình 1.1 Sơ đồ khối tổng quát xây dựng ước lượng Công việc kết hợp liệu từ cảm biến MEMS để có uớc lượng hướng vật thể tốt sử dụng riêng lẻ hay cảm biến đơn Cảm biến IMU tích hợp chứa 03 thành phần gồm 03 vận tốc góc trực giao, 03 gia tốc dài trực giao, 03 giá trị từ trường trực giao Hướng góc nghiêng đối tượng có cách lấy tích phân liệu ngõ từ cảm biến vận tốc gốc Khó khăn sử dụng cảm biến MARG hữu độ khơng xác điều khiển liệu đọc từ cảm biến, ảnh hưởng tác động nhiễu khơng mong muốn bên ngồi yếu tố làm cho độ tin cậy liệu cảm biến giảm đáng kể Sai số tích lũy dễ tăng dần theo thời gian lượng tử hóa, khâu tích phân, sai số cảm biến Do đó, việc kết hợp cảm biến lại để giám sát bù trừ cho cần thiết HVTH: Lê Mạnh Thắng - 01508348 Trang Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo Hai phương pháp DCM thích ứng tốt nhiễu mơi trường Trong ước lượng ước lượng Kalman DCM cho kết sai số tốt Từ việc mô phỏng, so sánh đánh giá ước lượng với nhau, ta tiến hành thực nghiệm ước lượng real-time board IMU 9DOF 5.4 Thực kiểm tra ước lượng board IMU 9DOF Board IMU gửi mẫu liệu (3 gyros, accelerometer, magnetometer) đọc từ cảm biến với thời gian lấy mẫu Ts = 0.02 sec lên máy tính qua cổng COM với tốc độ baud 38400 Máy tính thực ước lượng on-line, đồng thời đọc liệu từ bàn xoay từ cổng song song để kiểm tra so sánh  Vector gia tốc trọng trường chọn g  [0, 0, 9.81] , vector từ trường Trái  Đất khu vực thực nghiệm chọn m0  [0.8800,  0.0046, 0.1154] Khảo sát phương sai nhiễu đo lường board 9DOF Rozar IMU, ta có Bảng 5.10 Phương sai nhiễu đo lường cảm biến IMU 9DOF Cảm biến Trục x Trục y Trục z Gyro (rad s ) 104 10 4 104 Accelerometer ( m s ) 106 106 1.5 105 Magnetometer (Gauss) 8.5 106 7.5 10 6 8.5 106 Thực nghiệm kiểm tra với thuật toán DCM PI, EKF quaternion xoay quanh trục riêng lẻ, xem mức độ ảnh hưởng xoay trục đến trục khác Kiểm tra với bàn xoay với kết xoay trục z, y, x lần lượt, 5.4.1 Bộ ước lượng DCM PI lọc EKF Thực với ước lượng DCM PI, HVTH: Lê Mạnh Thắng - 01508348 Trang 82 Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo 1 200  true true 0.5  est 0.5 true est est 150 0 100 -0.5 -0.5 -1 50 -1.5 -1 -2 -1.5 -50 -2.5 -2 10 20 30 40 50 -3 10 20 30 40 50 -100 10 20 30 40 50 (a) Chỉ xoay quanh trục z 180 100  true 170 -20 true 80  est  true est 160 60 150 40 140 20 130 120 -20 110 -40 100 -60 90 -80  est -40 -60 -80 -100 -120 80 10 20 30 40 50 -100 10 20 30 40 50 -140 10 20 30 40 50 (b) Chỉ xoay quanh trục y 100 20 -70  true true  est 50 true est 10 0 -90 -50 -10 -100 -100 -20 -110 -150 -30 -120 -200 20 40 60 -40 20 40 est -80 60 -130 20 40 60 (c) Chỉ xoay quanh trục x Hình 5.11 Xoay board IMU quanh trục Sau thực kiểm tra trục, tiến hành kiểm tra board quay tự trục roll, pitch, yaw 80 60 150  true 70 true 40  est true 100 est est 60 50 40 20 50 0 30 20 10 -20 -50 -40 -100 -60 -150 -10 -20 10 20 30 40 50 HVTH: Lê Mạnh Thắng - 01508348 -80 10 20 30 40 50 -200 10 20 30 40 50 Trang 83 Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo (a) Quay tự trục x, y, z 40 30  200  25  150 30 20 100 15 20 50 10 10 -50 0 -100 -5 -10 10 20 30 40 50 -10 10 20 30 40 50 -150 10 20 30 40 50 (b) Sai số góc phi, theta, psi Hình 5.12 Xoay board IMU tự với lọc DCM PI Thực với lọc Kalam mở rộng, 3.5 150  true true  est  true est  est 100 2.5 -1 50 1.5 -2 -3 0.5 -4 -50 -5 -0.5 -1 20 40 60 80 -6 20 40 60 80 -100 20 40 60 80 (a) Chỉ xoay quanh trục z 15 80  true  est 10 130 true 60  true est  est 120 40 110 20 0 100 -5 -20 90 -40 -10 80 -60 -15 -20 70 -80 20 40 60 80 -100 20 40 60 80 60 20 40 60 80 (b) Chỉ xoay quanh trục y 100 -0.4  true 80 102 true -0.6  est 60 true 101.5 est -0.8 40 est 101 -1 100.5 20 -1.2 100 -1.4 -20 99.5 -1.6 -40 98.5 -2 -80 -100 99 -1.8 -60 20 40 60 -2.2 20 40 60 98 20 40 60 (c) Chỉ xoay quanh trục x Hình 5.13 Xoay board IMU quanh trục với lọc EKF HVTH: Lê Mạnh Thắng - 01508348 Trang 84 Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo Bảng 5.11 So sánh ước lượng DCM PI EKF quaternion MSE Roll (o) Thí nghiệm MSE Pitch (o) MSE Yaw (o) DCM PI EKF DCM PI EKF DCM PI EKF Xoay quanh trục z 0.98 7.88 2.98 2.33 42.51 5.61 Xoay quanh trục y 41.03 56.47 2.17 4.54 41.54 15.33 Xoay quanh trục x 4.31 0.72 91.1 0.1 110.54 179 Xoay tự 80.95 146 25.12 9.2 300.91 372 Test với board quay đồng thời trục, 180 40  true 160 140 true 30  est 140  true est  est 120 20 100 120 10 100 80 -10 60 80 60 -20 40 40 20 -30 -40 -20 -50 10 20 30 40 50 20 10 20 30 40 50 0 10 20 30 40 50 (a) Quay tự trục x, y, z 60 15 10   40  10 20 -5 -10 -15 -20 -60 -20 -5 -40 -25 10 20 30 40 50 -10 10 20 30 40 50 -30 10 20 30 40 50 (b) Sai số góc phi, theta, psi Hình 5.14 Xoay board IMU tự với lọc EKF Từ bước thực nghiệm ban đầu trên, ta rút bảng so sánh sau,  Nhận xét Bộ ước lượng DCM PI thuật tốn đơn giản, khối lượng tính tốn nên nhúng vào vi điều khiển AVR 8bit Kiểm tra sai số tĩnh cho kết tốt, nhiên, với trạng thái động có vài thời điểm, thuật tốn khơng thể bám theo quỹ đạo HVTH: Lê Mạnh Thắng - 01508348 Trang 85 Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo Kết đánh giá sai số MSE (minimum square error) với DCM PI EKF cho thấy góc roll pitch cho kết ước lượng tốt góc yaw Từ bảng số liệu ta thấy liệu từ cảm biến mức độ ổn định chưa tốt 5.4.2 Bộ lọc Kalman DCM & thuật toán Gauss-Newton 120 60 110 80  true true  est est 40 100 true 60 es t 40 90 20 20 80 70 -20 60 -20 50 -40 -60 40 -40 -80 30 20 20 40 60 80 -60 20 40 60 -100 80 20 40 60 80 (a) Quay tự trục x, y, z 30  20 20   10 10 0 -10 -2 -20 -4 -30 -6 -40 -8 -50 20 40 60 80 -10 -10 -20 20 40 60 -30 80 20 40 60 80 (b) Sai số góc phi, theta, psi Hình 5.15 Kết ước lượng lọc Kalman DCM Bộ lọc Kalman dựa thuật toán Gauss-Newton 120 60 110 80  true true  est est 40  true 60 100  es t 40 90 20 20 80 0 70 -20 60 -20 -40 50 -60 -40 40 30 -80 20 40 60 80 -60 20 40 60 80 -100 20 40 60 80 (a) Quỹ đạo ước lượng quỹ đạo thật với Gauss-Newton HVTH: Lê Mạnh Thắng - 01508348 Trang 86 Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo 30 40 20   20  10 30 10 20 -10 10 -10 -20 -20 -30 -10 -40 -20 20 40 60 80 -30 -40 20 40 60 -50 80 20 40 60 80 (b) Sai số góc psi, theta, phi với thuật tốn Gauss-Newton Hình 5.16 Kết lọc Kalman Gauss-Newton Bảng 5.12 So sánh ước lượng DCM Gauss-Newton Quaternion Phương pháp Ma trận mơ hình MSE Roll (o) MSE Pitch (o) MSE Yaw (o) Kalman DCM 2.(3  3) 0.6 3.11 4.60 Gauss Quaternion 4 61.33 36.02 14.50 5.4.3 Bộ lọc Gauss-Newton & EKF Bộ lọc Kalman dựa thuật toán Gauss-Newton 160 50  true 140 140 true 40  es t  true 120 est  est 30 120 100 20 100 10 80 80 60 60 -10 40 -20 20 40 -30 20 -40 -20 -50 10 20 30 40 50 10 20 30 40 50 0 10 20 30 40 50 (a) Quay tự trục x, y, z 40 15 30    10 20 10 20 -20 0 -5 -10 -10 -20 -15 -30 -40 -60 10 20 30 40 50 -20 10 20 30 40 50 -40 10 20 30 40 50 (b) Sai số góc phi, theta, psi Hình 5.17 Kết ước lượng lọc Kalman DCM HVTH: Lê Mạnh Thắng - 01508348 Trang 87 Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo Bộ lọc Kalman mở rộng EKF 180 40  true 160 140 true 30  est 140  true 120 es t  est 20 100 120 10 100 80 -10 60 80 60 -20 40 40 -30 20 -40 -20 -50 10 20 30 40 50 20 10 20 30 40 50 0 10 20 30 40 50 (a) Quỹ đạo ước lượng quỹ đạo 60 15 10   40  10 20 -5 -10 -15 -20 -20 -5 -40 -60 -25 10 20 30 40 50 -10 10 20 30 40 50 -30 10 20 30 40 50 (b) Sai số lọc EKF Hình 5.18 Bảng 5.13 So sánh Bộ lọc Kalman mở rộng Kalman Gauss-Newton Phương pháp Ma trận mơ hình MSE Roll MSE Pitch MSE Yaw EKF 4 146 9.2 372 Gauss Quaternion 4 162 33.69 27.21  Nhận xét Thuật toán DCM PI nhúng xuống vi điều khiển AVR 8bit Các lọc thiết kế kiểm tra, nhiên chất lượng hạn chế HVTH: Lê Mạnh Thắng - 01508348 Trang 88 Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo Chương Kết Luận & Hướng Phát Triển HVTH: Lê Mạnh Thắng - 01508348 Trang 89 Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo 1.1 Kết luận 1.1.1 Các vấn đề làm Thực nghiên cứu phương pháp biểu diễn hướng góc nghiêng khơng gian chiều Dẫn phương trình vi phân động học cho phương pháp, để từ xây dựng mơ hình điều khiển, ước lượng hướng cho đối tượng Áp dụng lý thuyết lọc Kalman vào giải tốn ước lượng thơng số Đề tài sử dụng lọc Kalman để tổng hợp liệu từ cảm biến khác để có ước lượng tối ưu Bộ lọc Kalman dựa phương pháp DCM, thuật toán tối ưu quaternion Guass-Newton, Bộ lọc Kalman mở rộng dựa quaternion thực Áp dụng thuật toán DCM PI nhúng vào vi điều khiển AVR bit, cho kết ước lượng tốt Đề tài thực kiểm tra đánh giá phương pháp thuật toán dựa liệu đọc từ IMU 9DOF giá thành thấp Sai số đạt trạng thái tĩnh tốt, dao động khoảng  2 , đối tượng chuyển động với tần số thấp board có khả đáp ứng tốt yêu cầu  5 Một số thuật toán hoạt động tốt với nhiễu gia tốc bên tác động vào, DCM PI, Kalman DCM Kalman Guass-Newton Với cảm biến giá thành thấp, đề tài ứng dụng vào số lĩnh vực mà khơng địi hỏi khắc khe mức độ xác Song song với việc kiểm tra ước lượng board IMU 9DOF, đề tài thực mô với tập liệu mẫu cảm biến hãng Xsen, để đánh giá so sánh chất lượng sai số, từ rút kinh nghiệm trường hợp sai số chất lượng cảm biến, trường hợp thuật toán ước lượng 1.1.2 Những vấn đề chưa làm Khi đối tượng di chuyển với tần số cao khả đáp ứng ước lượng board IMU 9DOF hạn chế HVTH: Lê Mạnh Thắng - 01508348 Trang 90 Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo Vấn đề nhiễu từ trường vấn đề quan tâm, môi trường xung quanh tồn nhiều nhiễu từ trường Khi thực mô với tập liệu mẫu Xsen bị ảnh hưởng nhiễu từ trường với lọc Kalman mở rộng, ước lượng thêm biến trạng thái bias accelerometer magnetometer, kết đạt với góc roll pitch tốt; nhiên, góc yaw chưa ước lượng Chất lượng góc yaw chịu ảnh hưởng trực tiếp từ cảm biến từ trường, bị ảnh hưởng nhiều từ nhiễu từ trường xung quanh 1.2 Hướng phát triển Phát triển hệ thống nhúng với vi điều khiển tốc độ cao, hỗ trợ tính tốn số thực, tính tốn ma trận cho thuật toán Kalman DCM, Kalman Guass-Newton, Kalman mở rộng Sử dụng cảm biến có độ xác cao hãng Xsen, MicroStrain, Mink Hollow để áp dụng cho lĩnh vực địi hỏi mức độ xác cao HVTH: Lê Mạnh Thắng - 01508348 Trang 91 Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo Tài liệu tham khảo [1] Robot Grover Brown, Patric Y.C Hwang: “Introduction to Random Signals and Applied Kalman Filtering”, rd Edition, © 1997 by John Wiley & Sons, Inc, New York [2] Mohinder S Grewal, Angus P Andrews, “Kalman filtering: Theory and practice using Matlab”, 2nd Editon, © 2001 by John Wiley & Sons, Inc, New York [3] Mohinder S Grewal, Angus P Andrews, “Kalman filtering: Theory and practice using Matlab”, 3rd Editon, © 2008 by John Wiley & Sons, Inc, New York [4] Kuipers, J.B., “Quaternions and Rotation Sequences”, Princeton University Press, Princeton, New Jersey 1999 [5] J.E Dennis, Jr and Robert B Schnabel: “Numerical Methods for Unconstrained Optimization and Nonlinear Equations”, Prentice Hall, Englewood, NJ 1983 [6] Homer F Walker, “Numerical Methods for Nonlinear Equations”, Mathematic Sciences Department Worcester Polytechnic Institue, Massachusetts, February 2002 [7] Esmat Bekir “Introduction to Model Navigation System” © 2007 World Scientific Publishing Co Pte Ltd [8] Mohider S Grewal, Lawrence R Weill, Angus P Andrews, “Global positioning systems, inertial navigation, and integration”, nd Editon, © 2007 by John Wiley & Sons, Inc New Jersey [9] Ildeniz Duman, “Design, implementation, and testing of a real-time software system for a quaternion-based attitude estimation filter”, Master’s thesis, Naval postgraduate school Monterey, California, March 1999 [10] Jỗo Ls Marins, “An Extended Kalman filter for quaternion-based attitude estimation”, Master’s thesis, Naval postgraduate school Monterey, California, September 2000 [11] Rudy Negenborn: “Robot Localization and Kalman filters, on finding your position in a noisy world”, Master of science, Institute of Information and Computing Science, Utrecht University, 2003 HVTH: Lê Mạnh Thắng - 01508348 Trang 92 Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo [12] Warren S Flenniken, IV: “Modeling inertial measurement units and analyzing the effect of their error in navigation applications”, Master’s thesis, Graduate Faculty of Auburn University, Alabana, December 2005 [13] Joshy Madathiparambil Jose: “Performance comparison of Extended and Unscented Kalman filter implementation in INS-GPS integration”, Master’s thesis, Continuation Courses Space Science and Technology Department of Space Science, Kiruna, Lulea University of Technology, 2009 [14] E R Bachmann, I Duman, U Y Usta: “Orientation tracking for humans and robots using inertial sensors”, Accepted by the 1999 International Symposium on Computational Intelligence in Robotics & Automation (CIRA99), Massachusetts, October 26-28, 1999 [15] Jỗo Ls Marins, Xiaoping Yun, Eric R Bachmann, Robert B McGhee, and Michael J Zyda: “An Extended Kalman Filter for Quaternion-Based Orientation Estimation Using MARG Sensors”, Proceedings of 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems, November 2001 [16] Eric R Bachmann, Xiaoping Yun, Doug McKinney, Robert B McGhee, and Michael J Zyda “Design and Implementation of MARG Sensors for 3-DOF Orientation Measurement of Rigid Bodies”, Proceedings of the 2003 IEEE, International Conference on Robotics & Automation, Taiwan, Sep 14 2003 [17] Xiaoping Yun, Mariano Lizarraga, Eric R Bachmann, and Robert B McGhee: “An improved quaternion-based kalman filter for real-time tracking”, Proceedings of 2003 IEEE/RSJ Intl Conference on intelligent Robots and Systems, Las Vegas, Nevada, October 2003 [18] Henrik Rehbinder, Xiaoming Hu, “Drift-free attitude estimation for accelerated rigid bodie” Science direct, November 2003 [19] Henk J Luinge and Peter H Veltink “Inclination Measurement of Human Movement Using a 3-D Accelerometer With Autocalibration”, IEEE transactions on neural systems and rehabilitation engineering, vol 12, No 1, March 2004 [20] Mathieu St-Pierre, Denis Gingras Dr Ing, “Comparison between the unscented Kalman filter and the extended Kalman filter for the position estimation module of an HVTH: Lê Mạnh Thắng - 01508348 Trang 93 Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo integrated navigation information system”, IEEE intelligent Vehicles Symposium, June 2004 [21] Xiaoping Yun, Conrado Aparicio, Eric R Bachmann, and Robert B McGhee: “Implementation and Experimental Results of a Quaternion-Based Kalman Filter for Human Body Motion Tracking”, Proceedings of the 2005 IEEE International Conference on Robotics and Automation Barcelona, Spain, April 2005 [22] D Roetenberg, Henk J.Luinge, Chris T.M Baten and Peter H Veltink “Compensation of Magnetic Disturbances Improves Inertial and Magnetic Sensing of Human Body Segment Orientation”, IEEE transactions on neural systems and rehabilitation engineering, vol 13, No 3, September 2005 [23] Suh, J.S., Park, S.Y., Kang, H.J., Ro, Y.S “Attitude Estimation Adaptively Compensating External Acceleration”, JSME International Journal, Series C 49, 2006 [24] Xiaoping Yun, and Eric R Bachmann: “Design, Implementation, and Experimental Results of a Quaternion-Based Kalman Filter for Human Body Motion Tracking”, IEEE transactions on robotics, vol 22, No 6, December 2006 [25] Angelo M Sabatini, Senior Member, IEEE , “Quaternion-Based Extended Kalman Filter for Determining Orientation by Inertial and Magnetic Sensing”, IEEE transactions on biomedical engineering, vol 53, No 7, July 2006 [26] N.H.Q Phuong, Hee-Jun Kang, Young-Soo Suh, Young-Sik Ro, “A DCM Based Orientation Estimation Algorithm with an Inertial Measurement Unit and a Magnetic Compass”, Journal of Universal Computer Science, vol 15, no 4, 2009 [27] Young Soo Suh, Member, IEEE, “Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of external acceleration”, IEEE transactions on Instrumentation and Measurement, vol 59, May 2010 [28] D Choukroun, H Weiss, I.Y Bar-ITZhack, Y Oshman “Direction Cosine Matrix Estimation from vector Observations using a Matrix Kalman filter”, IEEE transactions on Aerospace and Electronic Systems, vol 46, No 1, January 2010 [29] H.J Luinge, P.H Veltink, and C.T.M Baten: “Estimating orientation with gyroscopes and accelerometers”, Technical and Health Care (1999) 455-459 HVTH: Lê Mạnh Thắng - 01508348 Trang 94 Luận văn cao học GVHD: TS Nguyễn Vĩnh Hảo [30] Anthony Kim, M.F Golnaraghi “A quaternion-based orientation estimation algorithm using an inertial measurement unit”, Waterloo, ON N2L3FI Canada [31] http://www.sparkfun.com HVTH: Lê Mạnh Thắng - 01508348 Trang 95 LÝ LỊCH TRÍCH NGANG Họ tên: LÊ MẠNH THẮNG Ngày tháng năm sinh: 21 – 06 – 1985 Nơi sinh: Đồng Nai Địa liên lạc: Khoa Điện – Điện tử Viễn thông, Trường Đại học Giao thông Vận tải TpHCM, số đường D3, Văn Thánh Bắc, P 25, Q Bình Thạnh, Tp HCM Email: lemanhthangbk@gmail.com lmthang@hcmutrans.edu.vn Quá trình đào tạo: 2003 – 2008: học đại học chuyên ngành Tự động hóa, Khoa Điện – Điện tử Trường Đại học Bách khoa Tp HCM 2008 – 2011: học cao học chuyên ngành Tự động hóa Trường Đại học Bách khoa Tp HCM Quá trình cơng tác: Tháng 04/2008 đến nay: Giảng viên Bộ mơn Tự động hóa, Khoa Điện – Điện tử Viễn thông, Trường Đại học Giao thông Vận tải Tp HCM Trang vii ... Luận văn xây dựng ước lượng hướng góc nghiêng từ liệu cảm biến vận tốc góc, cảm biến gia tốc cảm biến từ trường Các ước lượng hướng góc nghiêng dựa kiến thức phương pháp biểu diễn hướng góc nghiêng. .. pháp ước lượng hướng góc nghiêng khơng gian 3D  Nghiên cứu lý thuyết lọc Kalman, thiết kế lọc Kalman, thuật toán ước lượng hướng góc nghiêng  Phát triển phần mềm chương trình ước lượng hướng góc. .. sinh: Đồng Nai Chuyên ngành: Tự động hóa MSHV: 01508348 I Tên đề tài: XÂY DỰNG BỘ ƯỚC LƯỢNG HƯỚNG VÀ GÓC NGHIÊNG TRONG KHÔNG GIAN CHIỀU II Nhiệm vụ nội dung: