7. Bố cục chung của luận án
2.3 Triển khai bộ lọc Kalman kiểu MEKF cho hệ SINS
Bộ lọc Kalman kiểu MEKF cho hệ SINS được trình bày trong [90]. Trong phần này, chỉ trình bày sơ lượt kết quả và vận dụng bộ lọc vào trường hợp của luận án.
Từ phương trình (2-12) và (2-13) ta có phương trình trạng thái của hệ thống như sau: 1 − − − 1 ̇= 2 [ ̇= ( )( − ( )[ ̃] ) ̇=
Như có thể thấy trong (2-16), thì mơ hình hệ thống là phi tuyến do ̇trong phương trình thứ 2 phụ thuộc bậc cao vào các thành phần của trạng thái như trong cơng thức tính ( ) (2-11). Hơn nữa, các thông số thay đổi theo tín hiệu và của cảm biến IMU. Do vậy bộ lọc kiểu MEKF khơng sử dụng trực tiếp mơ hình trên mà sử dụng mơ hình sai số qn tính, đây là mơ hình tuyến tính.
Lúc này, các giá trị hướng, vị trí và vận tốc tính theo thuật tốn INA như trong Mục 2.2.2.4 được gọi là các giá trị sơ bộ và được ký hiệu là , và .Gọi ̅ ∈ 3, ̅∈ 3 và ̅∈ 3 là giá trị sai số của hướng, vận tốc và vị trí sơ bộ.
Trang 55
Các giá trị ước lượng ( , và ), giá trị sơ bộ ( , ( ̅, ̅và ̅) liên hệ như sau [93]:
và ) và các giá trị sai số
IMU đôi khi cũng được đưa vào bộ lọc để ước lượng. Sau khi ước lượng được sai số ̅, ̅và ̅ thì hướng, vận tốc và vị trí của cảm biến IMU được hiệu chỉnh như sau:
1
≈ ⊗[ ]
̅ (2-18)
= + ̅ 1
trong đó, quaternion được hiệu chỉnh sử dụng
phép nhân quaternion ≈ ⊗ [ ̅]
nên bộ lọc được gọi là MEKF (Multiplicative Extended Kalman Filter). Mơ hình sai số qn tính có dạng [95]
- = ̅∈ 15
- =
- =
- [ ×]≜[
- ∈ 3 = (0, ) và ∈ 3 = (0, ) là thành phần nhiễu trắng của cảm biến vận tốc góc và cảm biến gia tốc, có phân phối chuẩn với giá
trị
trung bình bằng 0 và hiệp phương sai là = { } và = { } với ( ) là giá trị kỳ vọng của . Hiệp phương sai , có thể được tính bằng tích của bình phương mật độ nhiễu (noise density) và băng thông (bandwidth) trong Bảng 2.1 và Bảng 2.2 như sau [97]:
o Đối với dòng MTi-1
o Đối với dòng MTi-100
- Thành phần ̇và ̇được đại diện bởi các nhiễu trắng = (0, ) và = (0, ). Trong luận án chọn = = 10−8 3.
Ma trận hiệp phương sai của nhiễu mơ hình được tính = { }. Do các nhiễu thành phần của hệ thống không tương quan nhau nên các thành phần không nằm trên đường chéo bằng 0. Các thành phần trên đường chéo được tính như sau:
- (1: 3,1: 3) = { (1: 3) (1: 3)} = 1 4 - (4: 6,4: 6) = { (4: 6) (4: 6)} = - (7: 9,7: 9) = { (7: 9) (7: 9)} = 0 - (10: 12,10: 12) = { (10: 12) (10: 12)} = ( ) ( ) - (13: 15,13: 15) = { (13: 15) (13: 15)} =
Như vậy ma trận hiệp phương sai của nhiễu mơ hình Q được như sau (kết quả này cũng được thể hiện trong [96]):
1 4
=
0
Như vậy, trong mơ hình của bộ lọc khơng có tín hiệu điều khiển, các thành phần trong ma trận , thay đổi theo các giá trị gia tốc và vận tốc góc đo được của cảm biến và phụ thuộc vào hướng sơ bộ tính từ thuật tốn INA. Do các tín hiệu từ cảm biến IMU là rời rạc nên các ma trận , là không liên tục. Hơn nữa, để thực thi bộ lọc trên các thiết bị gọn nhẹ trong thực tế, mơ hình của hệ thống biểu diễn trong (2-19)
được chuyển thể chuyển về dạng rời. Mơ hình sai số qn tính ở dạng rời rạc như sau [95] (trang 200-203):
+1 =+
trong đó:
=
Trong khoảng thời gian lấy mẫu thì các thành phần trong ma trận là khơng đổi, do vậyđược tính như sau:
≈ 15+ . + trong đó:
= 2 = 1 Như vậy, từ (2-24) ta có: 3 ≈ + [
Ma trận hiệp phương sai tương ứng với trong phương trình (2-23) được tính như sau [98]:
= ( )
Khi sử dụng xấp xỉ Taylor bậc 1 cho (2-26), ta có:
= (+ )(+ )
Khai triển tích phân cho (2-28), ta có:
=+(+)
Q trình dự đốn theo mơ hình của bộ lọc Kalman gồm các bước như sau:
- Tính các ma trận:theo (2-25),theo (2-28)
- Dự đốn trạng thái mới:
− =
+1
- Dự đoán hiệp phương sai cho bộ lọc Kalman:
− +1
Phương trình cập nhật tổng quát cho bộ lọc Kalman có dạng:
trong đó:
- là ma trận phép đo,
- là ma trận quan sát vàđại diện cho nhiễu trắng của phép đo với ma trận hiệp phương là tại thời điểm .
Tùy vào đối tượng, đặc trưng chuyển động và tình huống cụ thể mà xây dựng các phương trình cập nhật phù hợp để nâng cao độ chính xác trong ước lượng chuyển động. Đây là phần đóng góp chính của luận án sẽ được trình bày chi tiết trong Chương
3 và Chương 4.
- Tính các ma trậnvà
- 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 hiệp phương sai
− +1 +1− = CB gia tốc IMU CB vận tốc góc
Hình 2.12 Thuật toán của hệ thống INS sử dụng bộ lọc Kalman MEKF
Do bộ lọc MEKF là bộ lọc gián tiếp ước lượng các sai số ̅, ̅, ̅nên sai số tại thời điểm ban đầu bằng không và khởi tạo giá trị ban đầu cho biến trạng thái là 0 = 015×1. Việc khởi tạo này là hồn tồn chính xác nên hiệp phương sai tương ứng của chúng bằng 0, riêng thành phần và thì thành phần hiệp phương sai của chúng
ban đầu bằng với ma trận hiệp phương sai và . Do vậy, khởi tạo giá trị ban đầu của hiệp phương sai 0 như sau:
Tóm lại, hoạt động của bộ lọc Kalman được tóm tắt như trong Hình 2.11 và hoạt động chung của thuật toán hệ thống INS sử dụng bộ lọc Kalman kiểu MEKF được minh hoạ chi tiết trong Hình 2.12.
2.4 Kết luận chương
Trong chương này đã trình bày về cấu tạo, nguyên lý của cảm biến IMU và việc triển khai thuật tốn hệ thống INS. Trong đó, thuật toán hệ thống INS sử dụng bộ lọc Kalman kiểu MEKF với các vấn đề chính như sau:
- Triển khai thuật tốn INA bằng cách kết hợp tích phân của các tín hiệu vận tốc góc và gia tốc từ cảm biến để ước lượng sơ bộ hướng, vận tốc và vị trí của cảm biến IMU trong q trình chuyển động.
- Triển khai bộ lọc Kalman kiểu MEKF sử dụng mơ hình sai số qn
tính để ước lượng sai số của các giá trị hướng, vận tốc và vị trí sơ bộ từ thuật tốn INA, sau đó hiệu chỉnh lại giá trị của chúng. Các dữ liệu gia tốc và vận tốc góc của cảm biến IMU và các giá trị ước lượng sơ bộ của thuật toán INA được đưa vào bộ lọc để xây dựng tham số của bộ lọc Kalman.
- Các giá trị đo từ các cảm biến khác hoặc từ một số giá trị suy ra từ tính chất chuyển động được dùng để xây dựng các phương trình cập nhật cho bộ lọc Kalman.
Thuật toán hệ thống INS sử dụng bộ lọc Kalman trong chương này sẽ được nghiên cứu ứng dụng vào hệ thống INS đặt trên bàn chân ở Chương 3 và hệ thống INS đặt trên khung tập đi ở Chương 4. Trong đó, mơ hình của bộ lọc sẽ được điều chỉnh, các phương trình cập nhật sẽ được xây dựng phù hợp với từng hệ thống nhằm nâng cao độ chính xác của việc ước lượng chuyển động.
Chương 3. NGHIÊN CỨU XÂY DỰNG HỆ THỐNG ĐỊNH VỊ QUÁN TÍNH ĐẶT TRÊN BÀN CHÂN