Ứ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Ĩ

Ứ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 Chuyên ngành: Kỹ thuật điều khiển và tự động hóa Tóm tắt - Hiện na

ĐẠI HỌC ĐÀ NẴNG

TRƯỜNG ĐẠI HỌC BÁCH KHOA

KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA

Đà Nẵng - Năm 2018

TRƯỜNG ĐẠI HỌC BÁCH KHOA

LUẬ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

Tôi cam đoan đây là công trình nghiên cứu của riêng tôi Các số liệu, kết quả nêu trong luận văn là trung thực và chưa từng được ai công bố trong bất kỳ công trình nào 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 với Đại học Đà Nẵng, Trường Đại học Bách Khoa Đà Nẵng, các Thầy đã tham gia quản lý, giảng dạy và giúp đỡ tôi trong suốt quá trình học tập và nghiên cứu

Tôi xin bày tỏ sự biết ơn đến Thầy PGS.TS Đoàn Quang Vinh và ThS Nguyễn Duy Dưởng, người đã trực tiếp giúp đỡ tôi rất nhiều trong quá trình thực hiện luận văn trong thời gian qua

Mặc dù đã có nhiều cố gắng trong suốt quá trình thực hiện đề tài, song vẫn còn mặt hạn chế và thiếu sót Tôi rất mong nhận được ý kiến đóng góp và sự chỉ dẫn của thầy cô giáo và các bạn đồng nghiệp để luận văn được hoàn thiện hơn

Xin cảm ơn!

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 Chuyên ngành: Kỹ thuật điều khiển và tự động hóa

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

IMU Inertial Measurement Units Khối đo quán tính

KF 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ệ

DANH MỤC CÁC BẢNG

Số hiệu

4.1 Đấu nối cảm biến với arduino 34

6.1 Kết quả 3 người dùng walker đi thẳng 20m 51

1.3 Năm chế độ cảm biến chuyển động 7 1.4 Chuyển động quay của một vật xung quanh trục Z 9 2.1 Hệ trục tọa độ dẫn hướng và khối tâm 10 3.1 Hoạt động của bộ lọc Kalman 25 3.2 Mô hình đo lường ước lượng của bộ lọc Kalman 25 3.3 Tín hiệu thu trước và sau khi lọc qua Kalman 26 3.4 Mô hình không gian trạng thái 26 4.1 Tổng quan về hệ thống đề xuất 33 4.2 Các hệ trục tọa độ sử dụng 33 4.3 Sơ đồ đóng khung và lưu trữ dữ liệu 35 5.1 Định nghĩa các chuyển động của người dùng khi sử dụng walker 38 5.2 Lưu đồ thuật toán phân loại chuyển động 41 6.1 Hệ thống walker được dùng trong thử nghiệm 42 6.2 Mô đun cảm biến quán tính 42 6.3 Đồ thị module vận tốc góc yg(2;) của cảm biến IMU 43 6.4 Đồ thị đạo hàm theo gia tốc của IMU 43 6.5 Đồ thị từ encoder đọc về chuyển từ BCS sang WCS 44 6.6 Tiền xử lý tín hiệu encoder 44 6.7 Đồ thị ước lượng khoảng cách di chuyển bằng encoder 45 6.8 Tín hiệu nhận dạng từ encoder và IMU 46 6.9 Tín hiệu encoder sau khi xử lý 46

6.11 Kết quả phân loại chuyển động của 1 người đi 20(m) theo 4 kiểu 47 6.12 Đồ thị chuyển động của người dùng walker 20(m) 48 6.13 Phát hiện và phân loại chuyển động 49 6.14 Chuyển động có chu kỳ của bánh xe trong quá trình đẩy đi liên tục 50 6.15 Kết quả ước lượng thông số bước đi 20(m) với 4 kiểu chuyển động 50

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

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

(Mti-1, 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

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

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

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

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

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

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

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

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

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

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 : 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𝑞0− 𝑝̅ ∙ 𝑞̅ + 𝑝0𝑞̅ + 𝑝̅ × 𝑞̅ (2.1) Phương trình dưới dạng ma trận : 𝑟 = 𝑝𝑞 theo công thức (2.2) [4]

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)𝑢̅] (2.3) 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)

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:

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𝑞22𝑞1𝑞2− 2𝑞0𝑞3 2𝑞02+ 2𝑞02− 1 2𝑞2𝑞3+ 2𝑞0𝑞12𝑞1𝑞3+ 2𝑞0𝑞2 2𝑞2𝑞3− 2𝑞0𝑞1 2𝑞02+ 2𝑞32− 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)

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

−2𝑞3 1 2𝑞12𝑞2 −2𝑞1 1 ] (2.8)

= 𝐼 − 2 [

0 −𝑞3 𝑞2

𝑞3 0 −𝑞1

−𝑞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) 𝑟̇𝑛 = 𝑣𝑛

‖𝜔 𝑏 ‖2 ] = [1 𝜔𝑏 𝑑𝑡

2 ] (2.11) Thế phương trình (2.11) vào phương trình (2.10), ta có

𝑞𝑛𝑏(𝑡 + 𝑑𝑡) = 𝑞𝑛𝑏(𝑡) [1 𝜔𝑏

2 ] = 𝑞𝑛𝑏(𝑡) + 𝑞𝑛𝑏(𝑡) [0 𝜔𝑏

2 ]

Có thể viết phương trình như sau:

2.6 Phương trình góc quay cơ bản (góc euler)

Phương trình trạng thái cơ bản sử dụng góc Euler theo công thức:[4]

] [

𝜔𝑥

𝜔𝑦

𝜔𝑧] (2.12)

2.7 Xác định góc quay từ giá trị đo

Đo gia tốc: 𝑦𝑎  hiệu chỉnh gia tốc 𝑦𝑎,𝑐

𝐴 = 𝑈Σ𝑉′

Tham số p có thể tìm thấy từ

𝑝 = 𝑐𝑉(: ,7)

Mà c được cho bởi

𝑐 (𝑣(, 7)2𝑣(1, 7)+

𝑣(5, 7)2

𝑣(2, 7) +

𝑣(6, 7)2

𝑣(3, 7) − 𝑣(7, 7)) = 𝑔2Công cụ ước lượng UISL (gần đúng ellipsoid) : Để cho 𝑥 ∈ 𝑅𝑁×1, 𝑦 ∈ 𝑅𝑁×1, 𝑧 ∈

Giả sử rằng y a là hằng số, y a dữ liệu trong suốt thời gian chuyển đổi (giữa 2

khoảng thời gian không đổi) bị bỏ qua

2.9 Ước lượng thay đổi và vị trí

Quaternion q tính toán từ phương trình (2.9) Cho 𝑞̂ được tính từ phương

trình:[4]

𝑞̂ =1

2Ω(𝑦̇ 𝑔)𝑞̂ (2.14)

Từ 𝑦𝑔 ≠ 𝜔𝑏 (y g bao gồm nhiễu cảm biến v g và dự kiến vận tốc góc b g), 𝑞̂ ≠ 𝑞

Từ đó q e được sử dụng để chỉ ra 1 lỗi nhỏ trong

𝑇

Từ 𝐶′(𝑞) ≠ 𝐶′(𝑞̂)𝑦𝑎 − 𝑔, chúng ta có 𝑣𝑛 ≠ 𝑣̂ và 𝑟̂ ≠ 𝑟 𝑣𝑒 và 𝑟𝑒 được sử dụng Cho lỗi vận tốc và lỗi vị trí, tương ứng:

≈ ∫ 𝑄(𝑟) + 𝐴𝑟𝑄(𝑟) + 𝑄(𝑟)𝐴′𝑟𝑑𝑟

𝑇 0

Mà

𝑚̃ = [

𝑐𝑜𝑠𝛼0

Cách giải quyết

𝑞𝑒 = (𝐵′𝐵)−1𝐵′𝑐 Gián tiếp với gần đúng tối ưu

min

𝑞 𝑒 = ‖𝐵𝑞𝑒 − 𝑐‖22+ ‖𝐷𝑖𝑎𝑔(𝛿1, 𝛿2, 𝛿3)𝑞𝑒‖22

𝑞𝑒 = (𝐵′𝐵 + 𝐷𝑖𝑎𝑔(𝛿12, 𝛿22, 𝛿32))−1𝐵′𝑐 + Bước thứ nhất: tìm 𝑞𝑒(1: 2)

2.11 Xác định góc quay từ giá trị đo

2.11.1 Thuật toán TRIAD

Giả sử véc tơ đơn vị a i và b i (i=1,2) được cho và chúng ta muốn tìm một ma trận quay đáp ứng C theo công thức (2.28) [4]

𝑎1 = 𝐶𝑏1, 𝑎2 = 𝐶𝑏2 (2.28)

Nếu a i và b i không là những véc tơ đơn vị, làm chúng thành những véc tơ đơn vị

Ba véc tơ trung gian

Trong thuật toán Triad, ma trận quay C được chọn sao cho 𝑎1 = 𝐶𝑏1 được thỏa

mãn Sau đó các thành phần không xác định trong C được xác định từ 𝑎2 = 𝐶𝑏2 Vì vậy phép quay từ thuật toán TRIAD tương tự như giải pháp của vấn đề tối ưu hóa với

𝜆 rất lớn:

min

𝐶 𝜆‖𝑎1 = 𝐶𝑏1‖22+ ‖𝑎2 = 𝐶𝑏2‖22 (2.29)

Công cụ ước lượng UISL: Tính toán C sử dụng thuật toán TRIAD

Lệnh sử dụng trong Matlab: [C] = triad(a,b)

% a = (3 x 2 )

% b = (3 x 2 )

] = 𝐶(𝑞) [10

0] Công cụ ước lượng UISL: Tính toán quaternion từ ngõ ra gia tốc

Lệnh sử dụng trong Matlab: q = quaternionya(ya)

% ya : 3x1 accelerometer output

% q : quaternion

2.11.3 Thuật toán tổng quát

Tìm ma trận quay trong vấn đề tối ưu sau đây theo công thức (2.30) [4]

Mà a i và b i là véc tơ đơn vị và 𝜆𝑖 là yếu tố trọng số Khó khăn của C đó là 𝐶 ∈

𝑆𝑂(3): 𝑖 𝑒., C phải là một ma trận trực giao với yếu tố quyết định 1

L(c) được định nghĩa bởi

𝑀 = ∑ 𝜆𝑖𝑎𝑖𝑏𝑖′

𝑛

𝑖=1

(∗ 1)𝑇𝑟(𝐴𝐵) = 𝑇𝑟(𝐵𝐴)

Lưu ý: min L(C) tương đương với 𝑚𝑎𝑥𝑇𝑟(𝐶𝑀′)

Phép luận 3: Xem xét vấn đề sau với 𝐶 ∈ 𝑆𝑂(3):

max

𝐶 𝑇𝑟𝐶𝑀′

Giải pháp tối ưu được cho như sau:

Công cụ ước lượng UISL: tính toán C từ a i và b i và 𝜆𝑖

Lệnh sử dụng trong Matlab: [C] = dcmfromab(a,b,lambda)

Mà 𝑦𝑚 được trung bình hóa (tức là ‖𝑦𝑚‖ = 1

- Góc nhúng α từ trường: ulsan (khoảng 50 độ)

𝑚𝑛,𝑧] = [

𝑐𝑜𝑠𝛼0

𝑎1 = 𝑦̅𝑎, 𝑏1 = 𝑔̃

‖𝑔̃‖ , 𝑎2 = 𝑦̅𝑚, 𝑏2 = 𝑚̃

+Sau đó ma trận quay được tính bởi

𝐶 = [−𝑦̅𝑎 × 𝑦̅𝑎 ×𝑦̅𝑚

‖𝑦̅ 𝑎 ×𝑦̅ 𝑚 ‖

𝑦̅𝑎×𝑦̅𝑚

‖𝑦̅ 𝑎 ×𝑦̅ 𝑚 ‖ 𝑦̅𝑎] (2.34)

- Công cụ ước lượng UISL: Tính toán C từ ya và ym sử dụng phương trình (2.34)

- Lệnh sử dụng trong Matlab: [C] = dcmfromyaym(ya,ym)

CHƯƠNG 3

BỘ LỌC KALMAN

3.1 Giới thiệu chung về bộ lọc Kalman

Bộ lọc Kalman được đề xuất từ năm 1960 bởi giáo sư Kalman để thu thập và kết hợp linh động các thông tin từ cảm biến thành phần Một khi phương trình định hướng

và mẫu thống kê nhiễu trên mỗi cảm biến được biết và xác định, bộ lọc Kalman sẽ cho ước lượng giá trị tối ưu (chính xác do đã được loại sai số, nhiễu) như là đang sử dụng một tín hiệu “tinh khiết” và có độ phân bổ không đổi Cùng với sự phát triển của tính toán kỹ thuật số, bộ lọc Kalman đã trở thành chủ đề nghiên cứu sôi nổi và ứng dụng trong nhiều ngành kỹ thuật công nghệ khác nhau: trong tự động hóa, trong định vị và một số lĩnh vực truyền thông khác

Một cách khái quát, bộ lọc Kalman là một tập hợp các phương pháp toán học mô

tả một phương pháp tính toán truy hồi hiệu quả cho phép ước đoán trạng thái của một quá trình sao cho trung bình phương sai của độ lệch (giữa giá trị thực và giá trị ước đoán) là nhỏ nhất Bộ lọc Kalman rất hiệu quả trong việc ước đoán các trạng thái quá khứ, hiện tại và tương lại thậm chí ngay cả khi tinh chính xác của hệ thống mô phỏng không được khẳng định

Bộ lọc Kalman đơn giản là thuật toán xử lý dữ liệu hồi quy tối ưu Có nhiều cách xác định tối ưu, phụ thuộc tiêu chuẩn lựa chọn trình thông số đánh giá Nó cho thấy rằng

bộ lọc Kalman tối ưu đối với chi tiết cụ thể trong bất kỳ tiêu chuẩn có nghĩa nào Một khía cạnh của sự tối ưu này là bộ lọc Kalman hợp nhất tất cả thông tin được cung cấp tới nó Nó xử lý tất cả giá trị sẵn có, ngoại trừ độ sai số, ước lượng giá trị hiện thời của những giá trị quan tâm, với cách sử dụng hiểu biết động học thiết bị giá trị và hệ thống,

mô tả số liệu thống kê của hệ thống nhiễu, gồm nhiễu ồn, nhiễu đo và sự không chắc chắn trong mô hình động học, và những thông tin bất kỳ về điều kiện ban đầu của giá trị quan tâm

Hoạt động của bộ lọc Kalman (xem Hình 3.1) có thể chia làm 2 giai đoạn chính: cập nhật trạng thái theo thời gian và cập nhật giá trị đo Giai đoạn cập nhật trạng thái theo thời gian (Time Update) hay còn gọi là dự đoán (Predict) là giai đoạn sử dụng mô hình của bài toán (phương trình trạng thái) và giá trị trước đó để dự đoán giá trị hiện tại Các giá trị đo được của cảm biến quán tính (gia tốc tịnh tiến và vận tốc góc) được sử dụng cho giai đoạn này để dự đoán giá trị cần ước lượng Giai đoạn cập nhật giá trị đo (Measurement Update) hay còn gọi là hiệu chỉnh (Correct) là giai đoạn sử dụng giá trị

đo để hiệu chỉnh lại giá trị dự đoán trước đó Thông tin vận tốc bằng 0 tại các “điểm dừng” chính là giá trị đo cho thông số vận tốc Điều này giúp thuật toán định vị quán tính hiệu chỉnh giá trị ước lượng và giảm rất đáng kể sai số trong quá trình ước lượng các thông số chuyển động

Hình 3.1 Hoạt động của bộ lọc Kalman

So sánh một số bộ lọc thường dùng trong đo lường, giảm thiểu sai số của hệ thống

 Omplementary filter: Đơn giản nhất và cũng khá hiệu quả

 Kalman filter: Phức tạp nhưng phổ biến nhất và rất hiệu quả kể trong các ứng dụng chuyên nghiệp

 Madgwick filter: Khá mới và khá hiệu quả

 Mahony filter: Khá hiệu quả như Madgwick

Hình 3.2 Mô hình đo lường ước lượng của bộ lọc Kalman

Dự đoán trạng thái mới

𝑥̂𝑘−= 𝐴𝑥̂𝑘−1+ 𝐵𝑢𝑘−1

Dự đoán hiệp phương sai

𝑃𝑘− = 𝐴𝑃𝑘−1𝐴 𝑇 + 𝑄

Cập nhật theo thời gian

(dự đoán theo mô hình)

Tính hệ số Kalman

𝐾𝑘 = 𝑃𝑘− 𝐻 𝑇 (𝐻𝑃𝑘− 𝐻 𝑇 + 𝑅) −1

Cập nhật giá trị ước lượng sử dụng giá

trị đo 𝑧𝑘𝑥̂𝑘 = 𝑥̂𝑘−+ 𝐾𝑘(𝑧𝑘− 𝐻𝑥̂𝑘−)

Cập nhật giá trị đo (hiệu chỉnh)

Khởi tạo giá trị 𝑥̂0− , 𝑃0

Hình 3.3 Tín hiệu thu trước và sau khi lọc qua Kalman

Hình trên mô hình hóa hoạt động của mạch lọc Kalman Chúng ta có tín hiệu đo được, chúng ta có mô hình của tín hiệu đo được (đòi hỏi tuyến tính) và sau đó là áp dụng vào trong hệ thống phương trình của mạch lọc để ước lượng trạng thái quan tâm Thực

ra tín hiệu đo là không khó, phương trình đã có sẵn, cái chung ta cần chính là mô hình hoá hệ thống Để có thể ứng dụng một cách hiểu quả mạch lọc Kalman thì chúng ta phải

mô hình hóa được một cách tuyến tính sự thay đổi của trạng thái cần ước lượng hoặc dự đoán

3.1.1 Hệ thống và mô hình quan sát

Chúng ta giả sử rằng có thể mô hình hóa bởi phương trình chuyển trạng thái sau:

𝑥𝑘+1 = 𝐹𝑘𝑥𝑘 + 𝐺𝑘𝑢𝑘+ 𝑤𝑘Trong đó xk là trạng thái tại thời điểm k, uk là vector điều khiển đầu vào, wk là nhiễu quá trình – thường là nhiễu Gaussian trắng cộng (AWGN), Gk là ma trận chuyển đổi đầu vào và Fk là ma trận chuyển trạng thái

Ngoài ra chúng ta giả sử rằng, khả năng quan sát trạng thái được thực hiện thông qua một hệ thống đo lường có thể được biểu diễn bởi một phương trình tuyến tính như sau:

𝑧𝑘 = 𝐻𝑘𝑥𝑘 + 𝑣𝑘Trong đó zk là thông tin quan sát hay đo lường thực hiện tại thời điểm k, xk là trạng thái tại thời điểm k, Hk là ma trận quan sát và vk là nhiễu cộng trong quá trình đo lường (xem Hình 3.4)

Hình 3.4 Mô hình không gian trạng thái

3.1.2 Giả thiết

Chúng ta giả thiết như sau:

- Nhiễu quá trình và nhiễu đo lường wk và vk là không tương quan, là nhiễu Gaussian trắng cộng (AWGN) có giá trị trung bình bằng không và ma trận hiệp

phương sai đã biết

Khi đó,

𝐸[𝑤𝑘𝑤𝑙𝑇] = {𝑄𝑘

0 𝑘 = 𝑙 𝑘 ≠ 𝑙𝐸[𝑣𝑘𝑣𝑙𝑇] = {𝑅𝑘

0 𝑘 = 𝑙 𝑘 ≠ 𝑙𝐸[𝑤𝑘𝑣𝑙𝑇] = 0 Cho tất cả k,l

Trong đó 𝑄𝑘 và 𝑅𝑘 là các ma trận đối xứng nửa xác định dương

- Trạng thái khởi tạo hệ thống x0 là một vector ngẫu nhiên không tương quan với

cả hệ thống và nhiễu đo lường

- Trạng thái khởi tạo hệ thống có giá trị trung bình và ma trận hiệp phương sai đã biết theo công thức (3.1):

𝑥̂0|0 = 𝐸[𝑥0] 𝑣à 𝑃0|0 = 𝐸[(𝑥̂0|0− 𝑥0)(𝑥̂0|0− 𝑥0)𝑇] (3.1) Đưa ra những giả định trên với mục đích để xác định, đưa ra tập giá trị quan sát z1,

…, zk+1, bộ lọc ước lượng ở thời điểm + 1 tạo ra một ước lượng tối ưu của trạng thái

xk+1 mà chúng ta ký hiệu bởi 𝑥̂𝑘+1, tối thiểu hóa kỳ vọng của hàm tổn thất bình phương lỗi

𝐸[‖𝑥𝑘+1−𝑥̂𝑘+1‖2] = 𝐸[(𝑥𝑘+1 − 𝑥̂𝑘+1)𝑇(𝑥𝑘+1− 𝑥̂𝑘+1)]

3.1.3 Nguồn gốc

Ký hiệu ước lượng dự đoán của trạng thái xk+1, dựa trên quan sát ở thời điểm k, z1,

… , zk là 𝑥̂𝑘+1|𝑘 Đó gọi là một bước trước dự đoán hay đơn giản là dự đoán Bây giờ,

giải pháp để tối thiểu hóa phương trình (3.1) là kỳ vọng của trạng thái ở thời điểm k+1

được ước định dựa trên quan sát ở thời điểm k Như vậy,