ỨNG DỤNG BỘ LỌC KALMAN ĐỊNH VỊ QUÁN TÍNH ĐỂ ƯỚC LƯỢNG THÔNG SỐ BƯỚC ĐI CHO NGƯỜI DÙNG WALKER LUAN VĂN THẠC SĨ

ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA PHẠM KHẮC TRUNG ỨNG DỤNG BỘ LỌC KALMAN ĐỊNH VỊ QUÁN TÍNH ĐỂ ƯỚC LƯỢNG THƠNG SỐ BƯỚC ĐI CHO NGƯỜI DÙNG WALKER LUẬN VĂN THẠC SĨ KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA Đà Nẵng - Năm 2018 ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA PHẠM KHẮC TRUNG ỨNG DỤNG BỘ LỌC KALMAN ĐỊNH VỊ QUÁN TÍNH ĐỂ ƯỚC LƯỢNG THÔNG SỐ BƯỚC ĐI CHO NGƯỜI DÙNG WALKER Chuyên ngành: Kỹ thuật điều khiển tự động hóa Mã sớ: 8520216 ḶN VĂN THẠC SĨ Người hướng dẫn khoa học: PGS.TS Đoàn Quang Vinh Đà Nẵng - Năm 2018 LỜI CAM ĐOAN Tôi cam đoan cơng trình nghiên cứu riêng tơi Các số liệu, kết nêu luận văn trung thực chưa công bố công trình khác Tác giả luận văn Phạm Khắc Trung LỜI CẢM ƠN Với tình cảm chân thành, tơi bày tỏ lòng biết ơn Đại học Đà Nẵng, Trường Đại học Bách Khoa Đà Nẵng, Thầy tham gia quản lý, giảng dạy giúp đỡ suốt q trình học tập nghiên cứu Tơi xin bày tỏ biết ơn đến Thầy PGS.TS Đoàn Quang Vinh ThS Nguyễn Duy Dưởng, người trực tiếp giúp đỡ tơi nhiều q trình thực luận văn thời gian qua Mặc dù có nhiều cố gắng suốt q trình thực đề tài, song mặt hạn chế thiếu sót Tơi mong nhận ý kiến đóng góp dẫn thầy cô giáo bạn đồng nghiệp để luận văn hoàn thiện Xin cảm ơn! MỤC LỤC MỞ ĐẦU 1 Tính cấp thiết đề tài Tổng quan hệ thống Mục tiêu nhiệm vụ luận văn Đối tượng nghiên cứu phạm vi nghiên cứu Phương pháp nghiên cứu Ý nghĩa khoa học thực tiễn đề tài Bố cục luận văn CHƯƠNG NGHIÊN CỨU TỔNG QUAN 1.1 Giới thiệu cảm biến quán tính 1.2 Giới thiệu cảm biến chuyển động 1.3 Tổng quan tình hình nghiên cứu CHƯƠNG TḤT TỐN ĐỊNH VỊ QN TÍNH 10 2.1 Hệ trục tọa độ số khái niệm 10 2.2 Quaternion 11 2.3 Quaternion trung bình 12 2.4 Biểu diễn góc sử dụng quaternion 12 2.5 Các phương trình vị trí góc 14 2.6 Phương trình góc quay (góc euler) 15 2.7 Xác định góc quay từ giá trị đo 15 2.8 Các phương trình cảm biến 16 2.9 Ước lượng thay đổi vị trí 16 2.9.1 Tính q 16 2.9.2 Phương trình qe 17 2.9.3 Vận tốc vị trí 17 2.10 AHRS 18 2.11 Xác định góc quay từ giá trị đo 20 2.11.1 Thuật toán TRIAD 20 2.11.2 Xác định góc quay từ cảm biến 21 2.11.3 Thuật toán tổng quát 21 2.11.4 Khởi tạo góc quay ban đầu sử dụng cảm biến từ trường cảm biến gia tốc 22 CHƯƠNG BỘ LỌC KALMAN 24 3.1 Giới thiệu chung lọc Kalman 24 3.1.1 Hệ thống mô hình quan sát 26 3.1.2 Giả thiết 27 3.1.3 Nguồn gốc 27 3.1.4 Điều kiện không chệch 28 3.1.5 Hiệp phương sai số 29 3.1.6 Độ lời Kalman 30 3.1.7 Tóm tắt phương trình lọc Kalman 30 3.2 Bộ lọc Kalman cho thuật toán định vị quán tính 31 CHƯƠNG THIẾT KẾ XÂY DỰNG PHẦN CỨNG 33 4.1 Giới thiệu tổng quan hệ thống 33 4.2 Phần cứng đồng hóa liệu 34 4.3 Thuật toán ước lượng BSC ICS 36 CHƯƠNG XÂY DỰNG THUẬT TOÁN PHÁT HIỆN VÀ PHÂN LOẠI CHUYỂN ĐỘNG 38 5.1 Định nghĩa chuyển động walker 38 5.2 Thuật toán phát chuyển động 39 5.3 Thuật toán phân loại chuyển động 41 CHƯƠNG KẾT QUẢ THÔNG SỐ BƯỚC ĐI CỦA NGƯỜI DÙNG 42 6.1 Kết phần cứng 42 6.2 Thông số bước 43 6.2.1 Phân loại chuyển động 43 6.2.2 Thông số bước 49 KẾT LUẬN 52 DANH MỤC TÀI LIỆU THAM KHẢO 53 QUYẾT ĐỊNH GIAO ĐỀ TÀI (Bản sao) ỨNG DỤNG BỘ LỌC KALMAN ĐỊNH VỊ QN TÍNH ĐỂ ƯỚC LƯỢNG THƠNG SỐ BƯỚC ĐI CHO NGƯỜI DÙNG WALKER Học viên: Phạm Khắc Trung Mã số: Khóa: Chuyên ngành: Kỹ thuật điều khiển tự động hóa Trường Đại học Bách Khoa Đà Nẵng - ĐHĐB Tóm tắt - Hiện vấn đề định vị dẫn đường cho hệ thống chuyển động thiết thực sống ngày Trong thực tế, với điện thoại smart phone hay đồng hồ đeo tay thơng minh tích hợp cảm biến quán tính (IMU) để đo đạt đem đến thơng số, tiện ích cho người dùng Cảm biến quán tính (Inertial Measurement Unit – IMU) sử dụng rộng rãi nhiều lĩnh vực kỹ thuật đời sống Luận văn đề xuất thuật toán để phát phân loại chuyển động cho người dùng thiết bị hỗ trợ lại (walker) có bánh trước cách kết hợp việc phát chuyển động liên quan đến việc nhấc walker lên sử dụng cảm biến quán tính phát chuyển động liên quan đến việc lăn walker mặt đất sử dụng encoder Việc phát phân loại chuyển động cần thiết việc ước lượng thông số bước đánh giá tình trạng sức khỏe người già, người cần hỗ trợ lại Các kết phân tính định tính định lượng thơng qua thí nghiệm thực tế cho thấy thuật toán hoạt động ổn định đạt độ xác cho phép hệ thống Từ khóa – định vị; đường dẫn; cảm biến quán tính (IMU); phân loại chuyển động; hỗ trợ lại; encoder; ổn định; xác THE APPLICATION OF THE INERTIAL NAVIGATION OF KALMAN FILTER IN WALKING PARAMETER ESTIMATION OF WALKER USERS Abstract - Nowadays the roles of the motion systems become more and more necessary and practical in everyday of our life, especially the function regarding the navigation and direction In reality, a smart phone or smart watch may be integrated the Inertial Measurement Unit – IMU inside in order to determine some essential parameters and then bring the users benefits from such information Inertial Measurement Unit – IMU is now being used widely in many different areas of technology and life And the Thesis is going to suggest the algorithm which helps recognizing and categorizing the types of motion of people who use the front wheel driver walkers (walkers) The method applied is that combining the recognition of both following motions: (i) the motions related to lifting up the walker with using the inertial sensors; and (ii) the motions related to rolling the walker on the ground with using encoder The activities involved in recognizing and categorizing the types of motion are very essential in estimating the steps parameters as well as examining the health status of the old and other people who need to be supported in moving As the result of a number of practical analysis and experiments, the algorithm, in some extent of the system, has worked out in a stable and exact manner Key words - navigation; direction; inertial measurement unit (IMU), categorizing the types of motion; supported in moving; encoder; stable; exact DANH MỤC CÁC CHỮ VIẾT TẮT Viết tắt Tiếng Anh Tiếng Việt Inertial Measurement Units Khối đo quán tính Kalman Filter Bộ lọc Kalman EKF Extended Kalman Filter Bộ lọc Kalman mở rộng ICS IMU coordinate system Hệ trục tọa độ cảm biến WSC World coordinate system Hệ trục tọa độ trái đất BSC Body coordinate system Hệ trục tọa độ hệ IMU KF DANH MỤC CÁC BẢNG Số hiệu bảng 4.1 4.2 6.1 Tên bảng Đấu nối cảm biến với arduino Khung liệu đóng gói Kết người dùng walker thẳng 20m Trang 34 35 51 DANH MỤC CÁC HÌNH Sớ hiệu hình 1.1 1.2 1.3 1.4 2.1 3.1 3.2 3.3 3.4 4.1 4.2 4.3 5.1 5.2 6.1 6.2 6.3 6.4 6.5 6.6 6.7 6.8 6.9 6.10 6.11 6.12 6.13 6.14 6.15 Tên hình Trang Tổng quan hệ thống đề xuất Các hệ trục tọa độ sử dụng Hình ảnh IMU Cảm biến qn tính Mti-1 Năm chế độ cảm biến chuyển động Chuyển động quay vật xung quanh trục Z Hệ trục tọa độ dẫn hướng khối tâm Hoạt động lọc Kalman Mơ hình đo lường ước lượng lọc Kalman Tín hiệu thu trước sau lọc qua Kalman Mơ hình khơng gian trạng thái Tổng quan hệ thống đề xuất Các hệ trục tọa độ sử dụng Sơ đồ đóng khung lưu trữ liệu Định nghĩa chuyển động người dùng sử dụng walker Lưu đồ thuật toán phân loại chuyển động Hệ thống walker dùng thử nghiệm Mơ đun cảm biến qn tính Đồ thị module vận tốc góc yg(2;) cảm biến IMU Đồ thị đạo hàm theo gia tốc IMU Đồ thị từ encoder đọc chuyển từ BCS sang WCS Tiền xử lý tín hiệu encoder Đồ thị ước lượng khoảng cách di chuyển encoder Tín hiệu nhận dạng từ encoder IMU Tín hiệu encoder sau xử lý Tín hiệu IMU sau xử lý Kết phân loại chuyển động người 20(m) theo kiểu Đồ thị chuyển động người dùng walker 20(m) Phát phân loại chuyển động Chuyển động có chu kỳ bánh xe trình đẩy liên tục Kết ước lượng thông số bước 20(m) với kiểu chuyển động 3 7 10 25 25 26 26 33 33 35 38 41 42 42 43 43 44 44 45 46 46 47 47 48 49 50 50 50 Trong trường hợp đẩy liên tục khơng có điểm dừng bước đi, việc xác định bước riêng lẽ phức tạp nhiều Khi phân tích kỹ chuyển động quay bánh xe, tơi thấy việc quay bánh xe có chu kỳ trình đẩy mặt sàn phụ thuộc tương ứng với chu kỳ bước Để thấy rõ điều này, sử dụng lọc thông thấp cho tốc độ quay bánh xe Như ta thấy bước xác định từ tốc độ quay bánh xe trường hợp đẩy liên tục Chúng sử dụng thuật toán phát đỉnh để phát đỉnh cực tiểu, điểm cực tiểu rõ ràng thời điểm k j thõa mãn điều kiện sau Ll ≤ |rkj − rkj−1 | ≤ Lh (6.6) Trong Ll Lh ngưỡng ngưỡng độ dài bước đi, k j k j−1 số điểm cực tiểu điểm cực tiểu trước Các điểm tương ứng với bước chân đánh dấu dấu ‘*” màu đỏ (xem Hình 6.14) Hình 6.14 Chuyển động có chu kỳ bánh xe q trình đẩy liên tục Từ tính toán số bước đi, thời gian bước cho đoạn đẩy liên tục Từ kết hợp với kiểu chuyển động cịn lại ta ước lượng thơng số bước cho người dụng: con_rolling_parameters=[norm(r_walker(:,length(r_walker)))/1000 20norm(r_walker(:,length(r_walker)))/1000 length(stepp) total_time mean(stepp) std(stepp) mean(step_cycle) std(step_cycle)] Hình 6.15 Kết ước lượng thơng số bước 20(m) với kiểu chuyển động 51 Trả kết theo thứ tự: [ khoảng đường ước lượng; sai số; số bước; thời gian bước; độ lệch chuẩn; phương sai; độ lệch chuẩn kiểu bước] Thí nghiệm với người dùng dọc hành lang 20m sử dụng walker Mỗi người dọc hành lang 20 lần (5 lần sử dụng cách đẩy liên tục, lần sử dụng cách đẩy bước, lần sử dụng cách bánh sau lên đẩy tới lần hoàn toàn walker lên đẩy tới) Kết thí nghiệm thể (xem Bảng 6.1) RMSE trường hợp đẩy mặt đất 0.1m RMSE trường hợp walker lên 0.3m Điều dễ dàng nhận việc ước lượng chuyển động sử dụng encoder xác so với việc ước lượng chuyển động sử dụng cảm biến quán tính Bảng 6.1 Kết người dùng walker thẳng 20m Thông số ước lượng Người dùng Tiêu chuẩn đánh giá Lăn liên tục Lăn bước Nhấc chân sau Nhấc hoàn toàn Tổng hợp M 5,021 4,953 4,892 4,991 19,875 STD 0,025 0,033 0,108 0,112 0,137 RMSE 0,031 0,054 0,145 0,102 0,238 M 5.012 4,988 5,126 5,098 20,224 STD 0,018 0,016 0,113 0,078 0,187 RMSE 0,046 0,019 0,162 0,121 0,223 M 4,982 5,023 4,966 5,109 20,08 STD 0,019 0,010 0,125 0,098 0,146 MSE 0,025 0,015 0,117 0,140 0,128 M: giá trị trung bình kết ước lượng, STD: Sai lệch chuẩn sai số, RMSE: trung bình bình phương sai số 52 KẾT LUẬN Đề tài ứng dụng lọc kalman thuật toán định vị qn tính mơ hình walker Từ xây dựng thông số bước người dùng Việc so sánh kết ước lượng lọc thực tế xác Thuật tốn ước lượng thơng số hoạt động xác với nhiều kiểu chuyển động người dùng Gía trị RMSE, STD tương đối nhỏ, đảm bảo tính xác khâu ước lượng khoảng cách Các thông số bước (độ dài bước, tốc độ bước, thời gian bước…) quan trọng việc đánh giá khả lại người dùng Ví dụ người dùng sử dụng walker cách đẩy liên tục có đôi chân khỏe người sử dụng walker cách đẩy bước thường có chân yếu (hoặc bị thương) Người dùng có bước dài thời gian bước ngắn thường có khả lại tốt Độ xác việc ước lượng độ dài bước thời gian bước minh chứng việc cộng tổng độ dài bước (thời gian bước) so với tổng quảng đường (thời gian thực tế) Từ hướng đến việc nghiên cứu, phát triển đề tài đưa mơ hình thực tế, giúp theo dõi đánh giá sức khỏe người dùng walker 53 DANH MỤC TÀI LIỆU THAM KHẢO Tiếng Việt [1] Lý thuyết điều khiển nâng cao, Nguyễn Doãn Phước NXB KH & KT 2007 [2] Matlab & Simulink dành cho kỹ sư điều khiển tự động, Nguyễn Phùng Quang, Nhà xuất khoa học kỹ thuật, Hà Nội-2006 [3] Hướng dẫn sử dụng vi điều khiển Arduino Tiếng Anh [4] Young Soo Suh, “Basic Inertial Navigation Alogorithm for Personal Navigation”, Ulsan University, Korea, 2010 [7] Kalman filter, http://en.wikipedia.org/wiki/Kalman_filter [8] Vinh Tran-Quang, Phat Nguyen Huu, Takumi Miyoshi, “A Collaborative TargetTracking Algorithm Considering Energy Constraint in WSNs” 15-17 Sept 2011 [9] MedlinePlus (2015) Using a Walker [Online] Available: https://www.nlm.gov/medilineplus/ency/patientinstruction/00342.htm [10] W.R.Frontera,J.K.Silver, and T.D.Rizzo, Jr., Essentials of Physical Medicine and Rehabilition [11] Walking Monitoring for User of Standard and Front Wheel Walkers , Duy Duong Phan, Huu Toan Duong, and Young Soo Suh, Member, [12] L Chen and H Hu, "IMU/GPS based pedestrian localization," in Computer Science and Electronic Engineering Conference (CEEC), 4th ed, 2012, pp 23-28 [13] S Yang and Q Li, “Inertial sensor-based methods in walking speed estimation: A systematic review,” Sensors, vol 12, no 5, pp 6102–6116, 2012 [14] N Enayati, E D Momi, and G Ferrigno, “A quaternion-based unscented Kalman filter for robust optical/inertial motion tracking in computer-assisted surgery,” IEEE Trans Instrum Meas., vol 64, no 8, pp 2291–2301, Aug 2015 [15] K Lee, C Jung, and W Chung, “Accurate calibration of kinematic parameters for two wheel differential mobile robots,” J Mech Sci Technol., vol 25, no 6, pp 1603–1611, 2011