Tài liệu ứ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

MỤC LỤC MỞ ĐẦU ......................................................................................................................... 1 1. Tính cấp thiết của đề tài ...................................................................................... 1 2. Tổng quan về hệ thống........................................................................................ 2 3. Mục tiêu và nhiệm vụ của luận văn .................................................................... 3 4. Đối tượng nghiên cứu và phạm vi nghiên cứu ................................................... 4 5. Phương pháp nghiên cứu .................................................................................... 4 6. Ý nghĩa khoa học và thực tiễn của đề tài ............................................................ 4 7. Bố cục luận văn................................................................................................... 4 CHƯƠNG 1. NGHIÊN CỨU TỔNG QUAN .............................................................. 6 1.1. Giới thiệu về cảm biến quán tính .............................................................................. 6 1.2. Giới thiệu về cảm biến chuyển động ........................................................................ 7 1.3. Tổng quan về tình hình nghiên cứu .......................................................................... 9 CHƯƠNG 2. THUẬT TOÁN ĐỊNH VỊ QUÁN TÍNH ............................................ 10 2.1. Hệ trục tọa độ và một số khái niệm cơ bản ............................................................ 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í và góc cơ bản .................................................................... 14 2.6. Phương trình góc quay cơ bản (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à 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à 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 và cảm biến gia tốc .................................................................................................................................. 22 CHƯƠNG 3. BỘ LỌC KALMAN ............................................................................. 24 3.1. Giới thiệu chung về bộ lọc Kalman ........................................................................ 24 3.1.1. Hệ thống và 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 các phương trình của bộ lọc Kalman ............................................ 30 3.2. Bộ lọc Kalman cho thuật toán định vị quán tính .................................................... 31 CHƯƠNG 4. THIẾT KẾ XÂY DỰNG PHẦN CỨNG ............................................ 33 4.1. Giới thiệu tổng quan về hệ thống ........................................................................... 33 4.2. Phần cứng và đồng hóa dữ liệu .............................................................................. 34 4.3. Thuật toán ước lượng giữa BSC và ICS ................................................................. 36 CHƯƠNG 5. 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 của walker ...................................................................... 38 5.2. Thuật toán phát hiện chuyển động.......................................................................... 39 5.3. Thuật toán phân loại chuyển động .......................................................................... 41 CHƯƠNG 6. KẾT QUẢ THÔNG SỐ BƯỚC ĐI CỦA NGƯỜI DÙNG ............... 42 6.1. Kết quả phần cứng .................................................................................................. 42 6.2. Thông số bước đi .................................................................................................... 43 6.2.1. Phân loại chuyển động......................................................................................... 43 6.2.2. Thông số bước đi ................................................................................................. 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Ị QUÁN 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 và tự động hóa Trường Đại học Bách Khoa Đà Nẵng - ĐHĐB Tóm tắt - Hiện nay vấn đề định vị và dẫn đường cho các hệ thống chuyển động rất thiết thực đối với cuộc sống hằng ngày. Trong thực tế, với một chiếc điện thoại smart phone hay chiếc đồng hồ đeo tay thông minh đã tích hợp các 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) hiện đang được sử dụng rất rộng rãi trong nhiều lĩnh vực của kỹ thuật và đời sống. Luận văn này đề xuất một thuật toán để phát hiện và phân loại chuyển động cho người dùng thiết bị hỗ trợ đi lại (walker) có 2 bánh trước bằng cách kết hợp việc phát hiện 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 và phát hiện chuyển động liên quan đến việc lăn walker trên mặt đất sử dụng encoder. Việc phát hiện và phân loại chuyển động là rất cần thiết trong việc ước lượng các thông số bước đi cũng như đánh giá tình trạng sức khỏe của người già, người cần hỗ trợ đi lại. Các kết quả phân tính về định tính và định lượng thông qua thí nghiệm thực tế cho thấy thuật toán hoạt động ổn định và đạt độ chính xác cho phép của 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ợ đi lại; encoder; ổn định; chí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ủ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 độ củ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 dữ liệu đóng gói Kết quả 3 người dùng walker đi thẳng 20m Trang 34 35 51 DANH MỤC CÁC HÌNH Số hiệu hình 1. 2. 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 quán tính Mti-1 Năm chế độ cảm biến chuyển động Chuyển động quay của một vật xung quanh trục Z Hệ trục tọa độ dẫn hướng và khối tâm Hoạt động của bộ lọc Kalman Mô hình đo lường ước lượng của bộ lọc Kalman Tín hiệu thu trước và sau khi lọc qua Kalman Mô hình không gian trạng thái Tổng quan về hệ thống đề xuất Các hệ trục tọa độ sử dụng Sơ đồ đóng khung và lưu trữ dữ liệu Định nghĩa các chuyển động của người dùng khi sử dụng walker Lưu đồ thuật toán phân loại chuyển động Hệ thống walker được dùng trong thử nghiệm Mô đun cảm biến quán tính Đồ thị module vận tốc góc yg(2;) của cảm biến IMU Đồ thị đạo hàm theo gia tốc của IMU Đồ thị từ encoder đọc về 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 bằng encoder Tín hiệu nhận dạng từ encoder và IMU Tín hiệu encoder sau khi xử lý Tín hiệu IMU sau xử lý Kết quả phân loại chuyển động của 1 người đi 20(m) theo 4 kiểu Đồ thị chuyển động của người dùng walker 20(m) Phát hiện và phân loại chuyển động Chuyển động có chu kỳ của bánh xe trong quá trình đẩy đi liên tục Kết quả ước lượng thông số bước đi 20(m) với 4 kiểu chuyển động 3 3 6 7 7 9 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 1 MỞ ĐẦU 1. Tính cấp thiết của đề tài Ước tính trên thế giới có hàng chục triệu người đang sử dụng các thiết bị hỗ trợ di chuyển bao gồm: nạn, khung tập đi (walker), gậy... Với sự gia tăng độ tuổi và bệnh mãn tính, tàn tật đã thúc đẩy sự ra đời các thiết bị hỗ trợ. Ngày nay việc áp dụng kỹ thuật công nghệ vào đời sống con người được nghiên cứu và thực hiện nhiều. Cùng với sự già hóa dân số thì nhu cầu về các thiết bị hỗ trợ người lớn tuổi càng tăng cao nhằm đáp ứng nhu cầu cuộc sống và giảm tải thời gian chăm sóc người cao tuổi. Vì vậy, có thể dự đoán trong tương lai gần việc nghiên cứu các thiết bị hỗ trợ con người là một lĩnh vực nghiên cứu đáng chú ý. Cảm biến quán tính là một cảm biến gia tốc và góc quay, sử dụng trong các thiết bị quân sự, giải trí, thể thao, y tế, … với chức năng ước lượng các thông số chuyển động của thiết bị phục vụ cho việc định vị và điều khiển chuyển động cũng như góp phần quan trọng cho việc phân tích chuyển động của đối tượng cần nghiên cứu. Với việc ước lượng các thông số (quỹ đạo, vận tốc, góc nghiên,…) của chuyển động, cảm biến quán tính có thể được sử dụng để định vị cho người đi bộ và phương tiện, dẫn đường cho tên lửa, phân tích dáng đi của con người trong chăm sóc sức khỏe cũng như mô phỏng chuyển động trong thể thao, phim ảnh,… Một cảm biến quán tính thường gồm một cảm biến gia tốc (đo giá trị gia tốc tịnh tiến theo 3 trục) và một cảm biến vận tốc góc quay (đo giá trị vận tốc góc quay theo 3 trục). Việc ước lượng chuyển động của cảm biến quán tính được thực hiện bằng cách tích phân các tín hiệu từ cảm biến. Với việc tích phân 2 lớp của gia tốc tịnh tiến cho ta quãng đường di chuyển và tích phân của vận tốc góc quay cho ta hướng di chuyển. Từ thông tin về quãng đường và hướng di chuyển, ta có thể ước lượng được quỹ đạo di chuyển của hệ thống. Đó là nguyên lý cơ bản của thuật toán định vị quán tính. Ngày nay, do cấu trúc nhỏ, gọn, giá thành tương đối thấp, cảm biến quán tính được sử dụng trong các thiết bị dân sự như điện thoại di động, các thiết bị đo lường trong y tế, các thiết bị đo lường, phân tích trong thể thao,… nơi mà các hệ thống giám sát chuyển động truyền thống như hệ thống camera hoặc GPS thường quá đắt hoặc bị hạn chế bởi không gian làm việc. Do vậy, việc nghiên cứu và ứng dụng cảm biến quán tính vào kỹ thuật và đời sống ngày càng trở nên phổ biến và cần thiết. Trong đó, luận văn đã tập trung nghiên cứu và ứng dụng cảm biến quán tính cũng như thuật toán định vị quán tính để xác định thông số bước đi của người dùng. Luận văn hướng đến việc ứng dụng cảm biến quán tính vào 2 thiết bị hỗ trợ đi lại nhằm thực hiện các bài kiểm tra để đánh giá các thông số bước đi cho bệnh nhân. Với các thông số này, các bác sĩ có thể chuẩn đoán được tình trạng sức khỏe cũng như tiến trình hồi phục chức năng của bệnh nhân. Ngoài ra, bệnh nhân có thể thực hiện các bài kiểm tra tại nhà thông qua việc sử dụng hệ thống đề xuất theo sự chỉ dẫn từ trước. Với những lý do trên, tôi đã quyết định thực hiện đề tài: “Ứ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” để nghiên cứu sâu hơn thuật toán định vị quán tính, bộ lọc kalman và ứng dụng của chúng vào đánh giá thông số bước đi của người dùng. Từ đó, hướng đến việc theo dõi, đánh giá người sử dụng walker. 2. Tổng quan về hệ thống Hệ thống đề xuất được thể hiện trong Hình 1 bao gồm 01 cảm biến quán tính (Mti1, công ty Xsens) và 02 encoder (loại 1024 xung/vòng) được gắn vào một walker. Trong đó cảm biến quán tính có thể được gắn tại 1 vị trí bất kỳ trên khung walker trong khi mỗi encoder được gắn vào và đo độ dịch chuyển của mỗi bánh walker. Cảm biến quán tính bao gồm cảm biến gia tốc và cảm biến vận tốc góc theo 3 trục với tần số lấy mẫu là 100 Hz. Một bản mạch được thiết kế để thu thập và đồng bộ dữ liệu của cảm biến quán tính và các encoder. Trong đó, Arduino Uno R3 được sử dụng để đọc dữ liệu từ các cảm biến và lưu dữ liệu vào thẻ nhớ Micro SD. Hệ thống sử dụng hệ trục tọa độ gắn liền với cảm biến quán tính (ICS – IMU Coordinate System), hệ trục tọa độ gắn liền với walker (BCS – Body Coordinate System) và hệ trục tọa độ toàn cầu (WCS – World Coordinate System) (xem Hình 2). Trong đó ICS gắn liền với hệ trục tọa độ vật lý của cảm biến quán tính. Gốc tọa độ của BCS là trung điểm của đoạn thẳng nối 2 điểm tiếp xúc của 2 bánh walker với mặt đất. Phương x của BCS trùng với hướng thẳng tới trước của walker trong khi phương y của BCS nằm trên đường nối 2 điểm tiếp xúc giữa 2 bánh với mặt đất và có phương hướng từ bánh phải sang bánh trái. Phương z của BCS hướng thẳng lên trên khi walker được đẩy đi trên mặt đất. WCS được chọn sao cho phương z hướng lên trên trùng với phương gia tốc trọng trường trong khi phương x và y có thể được chọn ngẫu nhiên sao cho 3 trục tọa độ vuông góc với nhau. Trên thực tế, để đơn giản trong tính toán, WCS được chọn trùng với BCS tại vị trí ban đầu của quá trình chuyển động. 3 Hình 1: Tổng quan hệ thống đề xuất Hình 2: Các hệ trục tọa độ sử dụng Việc xác định mối quan hệ giữa cảm biến quán tính và walker (gồm vector tịnh tiến [𝑇𝑏𝐼 ]𝑏 và ma trận quay 𝐶𝑏𝐼 từ ICS sang BCS). 3. Mục tiêu và nhiệm vụ của luận văn Mục tiêu của luận văn là ứng dụng một thuật toán vào một thiết bị công nghệ và hệ thống ứng dụng để ước lượng thông số bước đi của người dùng nhằm nâng cao hiệu quả, tiện ích và thân thiện với người dùng walker. Tương lai tạo kênh đánh giá khách 4 quan về chuẩn đoán tình trạng sức khỏe của bệnh nhân cho một số bệnh liên quan đến việc đi lại. Mục tiêu cụ thể: - Xây dựng được thuật toán định vị quán tính cải biên nhằm nâng cao độ chính xác trong ước lượng các đại lượng chuyển động. - Thiết kế và chế tạo được phần cứng phục vụ kiểm chứng thuật toán hỗ trợ quá trình phục hồi chức năng của người bệnh. 4. Đối tượng nghiên cứu và phạm vi nghiên cứu 4.1. Đối tượng nghiên cứu  Cảm biến encoder  Cảm biến quán tính IMU  Thuật toán định vị quán tính  Lý thuyết bộ lọc Kalman  Phần mềm Matlab và lập trình tính toán từ dữ liệu đầu vào 4.2. Phạm vi nghiên cứu Đề tài giới hạn phạm vi nghiên cứu là ứng dụng thuật toán định vị quán tính sử dụng bộ lọc Kalman để ước lượng thông số bước đi của người dùng walker. Walker (front wheeled walker) được gắn 2 cảm biến encoder ở 2 bánh xe trước và 1 cảm biến quán tính IMU, vi điều khiển arduino để lưu dữ liệu vào thẻ nhớ (SD card). 5. Phương pháp nghiên cứu  Tìm hiểu bộ lọc Kalman  Tham khảo các hệ thống, thiết bị, bài báo liên quan đến walker  Xây dựng thuật toán định vị quán tính sử dụng bộ lọc Kalman để ước lượng chuyển động cho hệ thống. Tìm hiểu các đặc trưng riêng trong chuyển động cho từng đối tượng để tìm phương pháp cập nhật, tăng độ chính xác cho ước lượng chuyển động. Cụ thể hóa những điều này vào thuật toán cập nhật giá trị đo cho bộ lọc Kalman. Từ đó ước lượng thông số bước đi của người dùng walker. 6. Ý nghĩa khoa học và thực tiễn của đề tài Đề tài hướng đến việc ứng dụng một thuật toán vào một thiết bị trong cuộc sống hằng ngày. Walker được gắn cảm biến quán tính và thuật toán định vị đem đến thông tin khách quan và chính xác cho người dùng. Xây dựng nền tảng để cải tiến walker có thể sản xuất và ứng dụng đại trà các sản phẩm trong việc chăm sóc đánh giá sức khỏe tại các trung tâm hồi phục chức năng. 7. Bố cục luận văn Ngoài phần mở bài và phần kết luận và kiến nghị, luận văn gồm 6 chương: Chương 1: Nghiên cứu tổng quan 5 Chương 2: Thuật toán định vị quán tính Chương 3: Bộ lọc Kalman Chương 4: Thiết kế phần cứng Chương 5: Xây dựng thuật toán phát hiện và phân loại chuyển động Chương 6: Kết quả thông số bước đi của người dùng 6 CHƯƠNG 1 NGHIÊN CỨU TỔNG QUAN 1.1. Giới thiệu về cảm biến quán tính Khối IMU thời kì đầu sử dụng những cảm biến quán tính hoạt động theo nguyên tắc cơ khí. Những cảm biến cơ khí này thường có kích thước lớn, hoạt động kém hiệu quả, giá thành cao và tiêu thụ nhiều năng lượng. Ngày nay, cùng với sự tiến bộ của khoa học công nghệ, đặc biệt là công nghệ vật liệu mới và công nghệ vi chế tạo đã tạo ra các cảm biến vi cơ có kích thước rất nhỏ (xem Hình 1.1), hoạt động hiệu quả, tiêu thụ ít năng lượng và đặc biệt là giá thành hạ, điều này mở ra một khả năng rộng lớn cho việc ứng dụng các cảm biến vi cơ trong nhiều lĩnh vực đời sống. Hình 1.1. Hình ảnh IMU Một khối vi cơ IMU được cấu tạo từ các cảm biến vi cơ, thường là 3 cảm biến gia tốc và 3 cảm biến vận tốc góc, hoặc cũng có thể là 1 cảm biến gia tốc 3 chiều kết hợp với 3 cảm biến vận tốc góc. Các cảm biến vi cơ kết cấu hỗ trợ với nhau theo cấu trúc gắn liền hoặc theo cấu trúc nổi, từ đó có thể xác định được 3 thành phần chuyển động quay và tịnh tiến của vật thể. Sự xuất hiện của các cảm biến gia tốc và con quay hồi chuyển (thiết bị đo góc hay gia tốc góc) dựa trên công nghệ MEMS (Microelectromechanical system), có giá thành rẻ và đáng tin cậy tạo điều kiện cho việc tích hợp khả năng nhận biết chuyển động vào ngày càng nhiều loại hình ứng dụng. Mặc dù cảm biến MEMS đã được dùng cho việc phóng túi khí và trong các cảm biến áp suất ở ô tô từ hai thập kỷ trước nhưng công chúng chỉ hiểu được tiềm năng của những thiết bị này khi họ tiếp xúc với các máy chơi trò chơi Wii của Nintendo và điện thoại iPhone của Apple. 7 Nếu ta xem xét các khả năng của từng loại cảm biến chuyển động – gia tốc (bao gồm cả thông tin về chuyển động tịnh tiến như vị trí và hướng), rung động, chấn động, độ nghiêng, và quay – thì tầm ứng dụng của chúng sẽ mở rộng ra thêm rất nhiều. Ví dụ như một cảm biến gia tốc có thể dùng để thực hiện các kỹ thuật quản lý điện năng bằng cách ra lệnh cho một thiết bị chuyển sang chế độ tiêu thụ năng lượng thấp nhất khi thiết bị này được cho là ở trạng thái nghỉ do cảm biến không nhận thấy có chuyển động hay rung động. Các bảng điều khiển với các nút điều khiển vật lý phức tạp cũng đang dần được thay thế bởi các giao diện nhận biết được cử động và được điều khiển bằng các cú chạm ngón tay. Trong các trường hợp sử dụng khác thì cảm biến giúp cho hoạt động của hệ thống trở nên chính xác hơn, ví dụ như một chiếc la bàn được hiệu chỉnh tùy theo độ nghiêng của nó khi người dùng cầm trên tay. Hình 1.2. Cảm biến quán tính Mti-1 1.2. Giới thiệu về cảm biến chuyển động Trong năm kiểu chuyển động – gia tốc, rung động, chấn động, nghiêng và quay thì ngoại trừ chuyển động quay, những chuyển động còn lại đều là kết quả của sự thay đổi của gia tốc theo thời gian (xem Hình 1.3). Tuy nhiên, chúng ta không liên hệ những chuyển động trên bằng trực giác, với sự thay đổi của gia tốc hay giảm tốc. Xem xét riêng từng chế độ cảm biến giúp chúng ta nhận ra các tiềm năng của chúng dễ dàng hơn. Hình 1.3. Năm chế độ cảm biến chuyển động 8 Gia tốc, bao gồm cả chuyển động tịnh tiến, đo sự biến đổi của vận tốc theo thời gian. Vận tốc, được biểu diễn bởi đơn vị m/s, bao gồm thông tin cả về tốc độ và hướng của chuyển động. Gia tốc được tính theo đơn vị m/s2. Khi gia tốc có giá trị âm, như khi một chiếc ô tô đi chậm lại do lái xe đạp phanh, thì được gọi là giảm tốc. Nếu xét gia tốc theo thời gian thì rung động có thể coi như là gia tốc và giảm tốc thay nhau xảy ra theo chu kỳ tuần hoàn. Tương tự như vậy, chấn động là gia tốc xảy ra tức thì nhưng khác với rung động, gia tốc trong chấn động là một quá trình không tuần hoàn và thường chỉ xảy ra một lần. Hãy xem xét trong một khoảng thời gian dài hơn. Khi một vật bị di chuyển làm thay đổi độ nghiêng của nó thì sẽ có sự thay đổi tương ứng về mặt lực hấp dẫn. Chuyển động này thường xảy ra chậm hơn so với rung và chấn động. Do cả bốn dạng cảm biến chuyển động này đều liên quan tới gia tốc nên chúng được đo bằng lực g, đơn vị của lực hấp dẫn tác dụng lên một vật trên Trái đất (1g = 9.8m/s2). Cảm biến MEMS phát hiện độ nghiêng bằng cách đo mức ảnh hưởng của lực hấp dẫn lên từng trục chuyển động. Ví dụ với cảm biến gia tốc ba chiều sẽ cho ra ba kết quả đo biểu thị gia tốc trên các trục chuyển động X, Y, và Z. Phần lớn các cảm biến có mặt trên thị trường hiện nay đo gia tốc bằng cách sử dụng các cặp tụ điện vi sai. Mức khác biệt về điện dung của các cặp tụ điện này tỉ lệ thuận với gia tốc của cảm biến. Mức khác biệt này sau đó được chuyển đổi sang dạng điện thế hoặc các tín hiệu nhị phân (đối với các cảm biến có đầu ra số), rồi thông tin này được chuyển tới bộ vi xử lý để thực thi các hoạt động tiếp theo. Những tiến bộ công nghệ gần đây cho phép sản xuất các cảm biến gia tốc MEMS nhỏ gọn trong dải gia tốc thấp (low-g) lẫn gia tốc cao (high-g) với băng thông rộng hơn trước đây rất nhiều, do đó càng gia tăng hơn nữa tiềm năng ứng dụng của chúng. Cảm biến trong dải gia tốc thấp (gia tốc dưới 20g) phù hợp để phát hiện các chuyển động do con người tạo ra. Cảm biến trong dải gia tốc cao thì hữu dụng trong các ứng dụng liên quan tới máy móc hay xe cộ – về bản chất là các chuyển động không thể tạo ra bởi con người. Chuyển động quay được định lượng bằng tốc độ góc. Điểm khác biệt của kiểu chuyển động này là nó có thể diễn ra mà không hề có sự thay đổi về gia tốc. Để hiểu rõ vấn đề này hãy thử tưởng tượng một cảm biến quán tính 3 chiều, trong đó chiều X và Y nằm song song với mặt đất còn trục Z thì hướng tới tâm trái đất. Ở vị trí này, trọng lực đo được trên ba trục X, Y, và Z lần lượt là 0, 0, và 1g. Tiếp đó, quay cảm biến xung quanh trục Z (xem Hình 1.4), trục X và Y sẽ quay theo, và vẫn chỉ đo được 0g trọng lực, trong khi trên trục Z vẫn là 1g. Để phát hiện được chuyển động quay như trên cần sử dụng cảm biến con quay hồi chuyển. Do một số sản phẩm cần phải đo được chuyển động quay cùng với các dạng chuyển động khác nên các cảm biến con quay hồi chuyển có thể được tích hợp với các cảm biến gia tốc trong bộ IMU (Inertial Measurement Unit – bộ đo quán tính) đa chiều. 9 Hình 1.4. Chuyển động quay của một vật xung quanh trục Z Các ứng dụng thực tế của công nghệ MEMS trở nên hữu ích hơn khi tính năng phát hiện chuyển động quay được kết hợp với tính năng phát hiện các chuyển động do quán tính khác. Trong thực tế thì việc kết hợp này cần tới cả cảm biến gia tốc và cảm biến con quay hồi chuyển. Một số thiết bị đo dùng quán tính có kết hợp cảm biến gia tốc đa chiều, cảm biến con quay hồi chuyển đa chiều, ngoài ra có thể thêm cảm biến từ tính để tăng mức độ chính xác về hướng. IMU cũng có thể cung cấp thông tin cảm biến với 6 bậc tự do, giúp đạt được độ chính xác rất cao đáp ứng được yêu cầu của các ứng dụng ở thiết bị ghi hình y tế, dụng cụ phẫu thuật, bộ phận cơ thể giả cao cấp, và dẫn đường tự động cho các phương tiện công nghiệp. Một ưu thế khác của việc sử dụng IMU là các tính năng của thiết bị có thể được nhà sản xuất kiểm thử và hiệu chỉnh trước khi xuất xưởng. 1.3. Tổng quan về tình hình nghiên cứu Thuật toán định vị quán tính được ứng dụng rộng rãi trong ước lượng và giám sát chuyển động trong phân tích dáng đi và định vị cho người đi bộ. Có một số hệ thống sử dụng cảm biến gắn trên walker đã được đề xuất phục vụ trong chăm sóc sức khỏe. Hệ thống walker thông minh (loại 4 bánh) dựa trên ra đa Doppler, cảm biến gia tốc và cảm biến lực, được đề xuất sử dụng trong chăm sóc sức khỏe. Những cảm biến này được phân bố trên walker nhằm phân tích thông tin dáng đi của người dùng. Trong đó, một ra đa Doppler chỉ vào chân và một ra đa khác phát hiện sự quay của bánh xe trong khi cảm biến lực được bố trí tại tay cầm để ghi lại dữ liệu lực tay tác động vào. Cảm biến gia tốc 3D được ứng dụng để ước lượng mức độ vận động của người dùng. Loại walker chuẩn và walker với 2 bánh trước được sử dụng chính trong việc hỗ trợ đi lại. Do đó, việc ước lượng thông số bước đi cho những loại walker này là rất quan trọng. Trong hướng nghiên cứu này, chúng tôi đề xuất một phương pháp để ước lượng thông số bước đi cho cả walker chuẩn và walker với 2 bánh trước. 10 CHƯƠNG 2 THUẬT TOÁN ĐỊNH VỊ QUÁN TÍNH 2.1. Hệ trục tọa độ và một số khái niệm cơ bản Vì chúng ta chỉ quan tâm đến một hệ thống dẫn đường quán tính rất ngắn, nên chỉ cần xem xét hai tọa độ: hệ trục dẫn hướng và hệ trục tọa độ người dùng. Hệ trục dẫn hướng: là một hệ trục địa lý cục bộ mà có trục hướng bắc (trục x), hướng tây (trục y), và trục đứng cục bộ (trục z). Hệ trục khối tâm: một hệ trục mà các trục trùng với 3 trục trong đơn vị dẫn đường quán tính. Đối với một vector xác định p chúng ta thường ta sử dụng chỉ số b (body) và n (navigation) để nhấn mạnh rằng một vec tơ được biểu diễn trong một hệ trục tọa độ cụ thể. Hình 2.1. Hệ trục tọa độ dẫn hướng và khối tâm  Ký hiệu sử dụng trong hệ thống quán tính - 𝑟𝑛 ∈ 𝑅3×1 : vị trí trong hệ tọa độ dẫn hướng - 𝑣𝑛 ∈ 𝑅3×1 : vận tốc trong hệ tọa độ dẫn hướng - 𝜔𝑏 ∈ 𝑅3×1 : góc quay khối tâm - 𝑎𝑏 ∈ 𝑅3×1 : gia tốc được sinh ra từ các lực khác và từ trường hấp dẫn - 𝑔 ∈ 𝑅3×1 : gia tốc trọng lực trong hệ tọa độ dẫn hướng - 𝑦𝑔 ∈ 𝑅3 : đầu ra vận tốc góc - 𝑏𝑔 ∈ 𝑅3 : hướng vận tốc góc - 𝑣𝑔 ∈ 𝑅3 : nhiễu đo lường vận tốc góc - 𝑦𝑎 ∈ 𝑅3 : đầu ra gia tốc - 𝑏𝑎 ∈ 𝑅3 : hướng gia tốc 11 - 𝑣𝑎 ∈ 𝑅3 : nhiễu đo lường gia tốc - 𝑞 ∈ 𝑅4 : quaternion đại diện góc quay từ trục tọa độ dẫn hướng đến trục tọa độ khối tâm - 𝑞̂ ∈ 𝑅4 : tích phân quaternion - 𝑞𝑒 ∈ 𝑅4 : quaternion lỗi trong q, 𝑞 = 𝑞̂ ⊗ 𝑞𝑒 - K(a) được định nghĩa bởi (∈ 𝑅3 ) 0 −𝑎3 𝑎2 0 −𝑎1 ] 𝐾 (𝑎) =≜ [ 𝑎3 −𝑎2 𝑎1 0  Sản phẩm chéo 𝑎 × 𝑏 = 𝐾 (𝑎)𝑏 = −𝑏 × 𝑎 = −𝐾 (𝑏)𝑎 Như đã biết, cảm biến quán tính gồm cảm biến gia tốc (theo 3 trục) và cảm biến vận tốc góc (theo 3 trục). Thuật toán INA dựa trên nguyên tắc kết hợp tích phân 2 lớp của gia tốc (cho khoảng cách di chuyển) và tích phân một lớp của vận tốc góc (cho hướng di chuyển). Từ sự kết hợp giữa khoảng cách và hướng di chuyển ta có thể ước lượng được quỹ đạo di chuyển của đối tượng cần nghiên cứu. Tuy nhiên, trong các tín hiệu đo của cảm biến quán tính luôn có thành phần nhiễu nên INA thường được sử dụng với các bộ lọc (trong đó phổ biến nhất là bộ lọc Kalman). Trong phần này, tác giả trình bày về INA cơ bản và việc ứng dụng bộ lọc Kalman vào INA này. 2.2. Quaternion Quaternion 𝑞𝑛𝑏 = [𝑞0 𝑞1 𝑞2 𝑞3 ] là vec tơ định nghĩa Để cho p và q là những quaternion. 𝑝 = [𝑝0 𝑝⃐], 𝑞 = [𝑞0 𝑞̅⃗ ] mà 𝑝̅ = [𝑝1 𝑝2 𝑝3 ] , 𝑞̅ = [𝑞1 𝑞2 𝑞3 ] Tích có hướng của 𝑝 ⊗ 𝑞: 𝑝 ⊗ 𝑞 = 𝑝0 𝑞0 − 𝑝̅ ∙ 𝑞̅ + 𝑝0 𝑞̅ + 𝑝̅ × 𝑞̅ Phương trình dưới dạng ma trận : 𝑟 = 𝑝𝑞 theo công thức (2.2) [4] 𝑟0 𝑝0 −𝑝1 −𝑝2 −𝑝3 𝑞0 𝑞0 −𝑞1 −𝑞2 −𝑞3 𝑝0 −𝑝3 𝑝2 𝑞1 𝑞3 −𝑞2 𝑝1 𝑟 𝑝 𝑝 𝑞1 𝑞0 [𝑟1 ] = [𝑝1 𝑝0 ] [ ] [ = 𝑝0 −𝑝1 𝑞2 𝑞2 −𝑞3 𝑞0 𝑞1 ] [𝑝2 ] 2 2 3 𝑟3 𝑞3 𝑝3 −𝑝2 𝑝1 𝑝0 𝑞3 𝑞2 −𝑞1 𝑞0 𝑝3 (2.1) (2.2) Quaternion biểu diễn một khối quay của góc 𝜙 về vec tơ đơn vị 𝑢̅ được cho bởi công thức (2.3) [4] 𝜙 𝑞 = [𝑐𝑜𝑠 ( ) 2 𝜙 𝑠𝑖𝑛 ( )𝑢̅] (2.3) 2 Quanternion biểu diễn dưới dạng ma trận theo công thức (2.4) [4] ̅ ̅ 𝑞0 𝑝0 𝑝 𝑝′ 𝑞 𝑞′ ] [ 𝑞̅ ] = [ 0 ] [ 𝑝̅ ] 𝑝𝑞 = [ 0 𝑝̅ 𝑝0 𝐼 + [𝑝̅ ×] 𝑞̅ 𝑞0 𝐼 + [𝑞̅ ×] (2.4) 12 2.3. Quaternion trung bình - Vấn đề cơ bản : Tìm 𝑞̅, quaternion trung bình của 𝑞𝑖 - Giả thiết 𝑞 = 𝑞𝑒,𝑖 ⊗ 𝑞𝑖 𝑅𝑖 = 𝐸{𝑞𝑒,𝑖 𝑞′𝑒,𝑖 } với ̅𝑖 𝑞 𝑞′ ]𝑞 𝑞̅𝑒,𝑖 = 𝑞 ⊗ 𝑞̅𝑖∗ = [ 𝑖,0 −𝑞̅𝑖 𝑞̅𝑖,0 𝐼 + [𝑞̅𝑖 ×] −𝑞̅𝑒,𝑖 = Ξ𝑞 Trong đó Ξ được định nghĩa bằng: Ξ(𝑞𝑖 ) = [−𝑞̅𝑖 𝑞𝑖,0 𝐼 + [𝑞̅𝑖 ×]] 𝑁 𝑞̅ = mà 𝑁 ′ min ∑ 𝑞𝑒,𝑖 𝑅𝑖−1 𝑞𝑒,𝑖 = min ∑ 𝑞′ Ξ(𝑞𝑖 )′ Ξ(𝑞𝑖 )𝑞 𝑞 𝑞 𝑖=1 𝑖=1 𝑁 ′ 𝑀 = ∑𝑖=1 Ξ (𝑞𝑖 ) 𝑅𝑖−1 Ξ(𝑞𝑖 ) 𝑁 = min 𝑞′ 𝑀𝑞 𝑞 𝑅̅−1 = Ξ(𝑞̅ ) (∑ Ξ(𝑞𝑖 )′ 𝑅𝑖−1 Ξ(𝑞𝑖 )) Ξ(𝑞̅)′ 𝑖=1 Công cụ ước lượng UISL: quaternion trung bình Lệnh sử dụng trong Matlab: [qbar, Rbar] = quaternionaveraging2(q,R) Nếu số lượng quaternion là N, thì kích thước của q và R là 4xN và 3x3xN 2.4. Biểu diễn góc sử dụng quaternion 𝑃𝑏 và 𝑝𝑛 liên quan với nhau như biểu thức sau:[4] 𝑝𝑏 = 𝐶𝑛𝑏 (𝑞)𝑝𝑛 = 𝑞 ∗ 𝑝𝑛 𝑞 Với 𝑞 ∗ = [𝑞0 −𝑞1 −𝑞2 −𝑞3 ] Quan hệ giữa q và ma trận quay tương ứng được cho bởi 2𝑞02 + 2𝑞12 − 1 2𝑞1 𝑞2 + 2𝑞0 𝑞3 2𝑞1 𝑞3 − 2𝑞0 𝑞2 𝐶𝑛𝑏 (𝑞) = [2𝑞1 𝑞2 − 2𝑞0 𝑞3 2𝑞02 + 2𝑞02 − 1 2𝑞2 𝑞3 + 2𝑞0 𝑞1 ] 2𝑞1 𝑞3 + 2𝑞0 𝑞2 2𝑞2 𝑞3 − 2𝑞0 𝑞1 2𝑞02 + 2𝑞32 − 1 TrC được định nghĩa: 𝑇𝑟𝐶 = 6𝑞02 + 2(𝑞12 + 𝑞22 + 𝑞32 ) − 3 = 6𝑞02 + 2(1 − 𝑞02 ) − 3 = 4𝑞02 − 1 Do đó q0 được cho bởi 𝑞0 = 0.5 ∗ √𝑇𝑟𝐶 + 1 - Tính toán 𝐶 − 𝐶 theo công thức (2.5) ′ 𝐶 ′ − 𝐶 = 4𝑞0 𝐾 (𝑞̅) ⇒ 𝐾 (𝑞̅ ) = 1 4𝑞0 (𝐶 ′ − 𝐶 ) - Chuyển DCM sang quaternion: thuật toán Shepperd’s + Tìm qj2 như sau: (2.5) 13 𝑞02 = 1 (1 + 𝑇𝑟𝐶 ) 4 1 (𝐶 − 𝐶22 − 𝐶33 + 1) 4 11 1 𝑞22 = (𝐶22 − 𝐶11 − 𝐶33 + 1) 4 1 𝑞32 = (𝐶33 − 𝐶11 − 𝐶22 + 1) 4 𝑞12 = - Chọn qj lớn nhất - Chúng ta có thể tính toán quaternion như sau: 𝐶12 + 𝐶21 = 4𝑞1 𝑞2 𝐶13 + 𝐶31 = 4𝑞1 𝑞3 𝐶23 + 𝐶32 = 4𝑞2 𝑞3 𝐶12 − 𝐶21 = 4𝑞0 𝑞3 𝐶13 − 𝐶31 = −4𝑞0 𝑞2 𝐶23 − 𝐶32 = 4𝑞0 𝑞1 * Sử dụng công tụ ước lượng UISL: Tính toán q từ C Lệnh sử dụng trong Matlab: q = dcm2quaternion(C)  Biểu diễn góc Euler :𝐶𝑛𝑏 = 𝐶𝑥′′ (∅)𝐶𝑦′ (𝜃)𝐶𝑧 (𝜓) theo công thức (2.6) 𝑐𝑜𝑠𝜃𝑐𝑜𝑠𝜓 𝑐𝑜𝑠𝜃𝑠𝑖𝑛𝜓 −𝑠𝑖𝑛𝜃 𝑏( ) 𝐶𝑛 𝑞 = [𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃𝑐𝑜𝑠𝜓 − 𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜓 𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃𝑠𝑖𝑛𝜓 + 𝑐𝑜𝑠𝜙𝑐𝑜𝑠𝜓 𝑠𝑖𝑛𝜙𝑐𝑜𝑠𝜃 ] 𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃𝑐𝑜𝑠𝜓 + 𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜓 𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃𝑠𝑖𝑛𝜓 − 𝑠𝑖𝑛𝜙𝑐𝑜𝑠𝜓 𝑐𝑜𝑠𝜙𝑐𝑜𝑠𝜃 (2.6) Mà 1 0 0 0 𝑐𝑜𝑠𝜙 𝑠𝑖𝑛𝜙 ( ) ] 𝐶𝑥 𝜙 = [ 0 −𝑠𝑖𝑛𝜙 𝑐𝑜𝑠𝜙 𝑐𝑜𝑠𝜃 0 −𝑠𝑖𝑛𝜃 ( ) 1 0 ] [ 𝐶𝑦 𝜃 = 0 𝑠𝑖𝑛𝜃 0 𝑐𝑜𝑠𝜙 𝑐𝑜𝑠𝜓 𝑠𝑖𝑛𝜓 0 𝐶𝑧 (𝜓) = [−𝑠𝑖𝑛𝜓 𝑐𝑜𝑠𝜓 0] 0 0 1 Các góc Euler với quaternion 𝜓 𝜃 𝜙 𝜓 𝜃 𝜙 𝑞0 = 𝑐𝑜𝑠 𝑐𝑜𝑠 𝑐𝑜𝑠 + 𝑠𝑖𝑛 𝑠𝑖𝑛 𝑠𝑖𝑛 2 2 2 2 2 2 𝜓 𝜃 𝜙 𝜓 𝜃 𝜙 𝑞1 = 𝑐𝑜𝑠 𝑐𝑜𝑠 𝑠𝑖𝑛 − 𝑠𝑖𝑛 𝑠𝑖𝑛 𝑐𝑜𝑠 2 2 2 2 2 2 𝜓 𝜃 𝜙 𝜓 𝜃 𝜙 𝑞2 = 𝑐𝑜𝑠 𝑠𝑖𝑛 𝑐𝑜𝑠 + 𝑠𝑖𝑛 𝑐𝑜𝑠 𝑠𝑖𝑛 2 2 2 2 2 2 𝜓 𝜃 𝜙 𝜓 𝜃 𝜙 𝑞3 = 𝑠𝑖𝑛 𝑐𝑜𝑠 𝑐𝑜𝑠 − 𝑐𝑜𝑠 𝑠𝑖𝑛 𝑠𝑖𝑛 2 2 2 2 2 2 14 Quaternion với các góc Euler theo công thức (2.7) 𝜓 = 𝑎𝑡𝑎𝑛2(2𝑞1 𝑞2 + 2𝑞0 𝑞3 , 2𝑞02 + 2𝑞12 − 1 𝜃 = −𝑠𝑖𝑛−1 (2𝑞1 𝑞3 − 2𝑞0 𝑞2 ) (2.7) 𝜙 = 𝑎𝑡𝑎𝑛2(2𝑞2 𝑞3 + 2𝑞0 𝑞1 , 2𝑞02 + 2𝑞32 − 1) Khi góc quay nhỏ  𝑞0 ≈ 1, 𝑞1 , 𝑞2 , 𝑞3 ≈ 0 góc Euler được theo công thức sau: 1 2𝑞3 −2𝑞2 1 2𝑞1 ] 𝐶 (𝑞) ≈ [−2𝑞3 (2.8) 2𝑞2 −2𝑞1 1 0 −𝑞3 𝑞2 0 −𝑞1 ] = 𝐼 − 2 [ 𝑞3 −𝑞2 𝑞1 0 2.5. Các phương trình vị trí và góc cơ bản Các phương trình cho định vị quán tính được cho theo công thức (2.9)[4] như sau: 1 𝑞̇ = Ω(𝜔𝑏 )𝑞 2 𝑣̇𝑛 = 𝑎𝑛 = 𝐶′(𝑞)𝑎𝑏 (2.9) 𝑟̇𝑛 = 𝑣𝑛 Mà 0 −𝜔𝑥 −𝜔𝑦 −𝜔𝑧 𝜔𝑧 −𝜔𝑦 𝜔 0 ] Ω(𝜔) ≜ [𝜔𝑥 −𝜔 𝜔𝑥 0 𝑧 𝑦 𝜔𝑧 𝜔𝑦 −𝜔𝑥 0 𝑏(𝑡) 𝑞𝑛𝑏 (𝑡 ) = 𝑞𝑛 do đó phương trình góc quay theo công thức: 𝑏(𝑡+𝑑𝑡) 𝑞𝑛𝑏 (𝑡 + 𝑑𝑡 ) = 𝑞𝑛𝑏(𝑡+𝑑𝑡) = 𝑞𝑛𝑏(𝑡) 𝑞𝑏(𝑡) (2.10) Trong khoảng thời gian dt, b(t) sẽ quay một góc bởi 𝜙 = ‖𝜔𝑏 ‖2 𝑑𝑡 Do đó từ phương trình (2.3), ta có: 𝜙 𝜙 𝜔𝑏 𝑏(𝑡+𝑑𝑡) ] 𝑞𝑏(𝑡) = [𝑐𝑜𝑠 ( ) 𝑠𝑖𝑛 ( ) 2 2 ‖𝜔𝑏 ‖2 Nếu dt nhỏ, vậy 𝜙 2 = ‖𝜔𝑏 ‖2 𝑑𝑡 2 . Do đó ta có thể tính gần đúng phương trình trên như sau (sử dụng gần đúng 𝑐𝑜𝑠𝜃 ≈ 1, 𝑠𝑖𝑛𝜃 ≈ 0 khi 𝜃 nhỏ): 𝑏(𝑡+𝑑𝑡) 𝑞𝑏(𝑡) ≈ [1 ‖𝜔𝑏 ‖2 𝑑𝑡 ‖𝜔𝑏 ‖2 ] = [1 𝜔𝑏 𝑑𝑡 2 ] Thế phương trình (2.11) vào phương trình (2.10), ta có 𝜔𝑏 ] = 𝑞𝑛𝑏 (𝑡 ) + 𝑞𝑛𝑏 (𝑡) [0 𝑞𝑛𝑏 (𝑡 + 𝑑𝑡 ) = 𝑞𝑛𝑏 (𝑡 ) [1 2 (2.11) 𝜔𝑏 ] 2