7. Bố cục chung của luận án
1.1 Một số khái niệm về thông số bước đi
Như vậy, từ nguyên lý hoạt động của hệ SINS trình bày tại Mục 2.2.2.2 cũng như cách triển khai của thuật tốn INA ở Mục 2.2.2.4, có thể thấy chất lượng của hệ SINS phụ thuộc trực tiếp vào độ chính xác của cảm biến IMU. Tuy nhiên các giá trị đo được từ cảm biến IMU luôn luôn tồn tại nhiễu, đặc biệt là IMU dùng trong dân dụng. Hơn nữa, việc sử dụng xấp xỉ Taylor trong các khâu tích phân cũng gây ra sai số tích luỹ cho hệ thống. Các sai số này tích lũy theo thời gian do việc tích phân của thuật tốn INA. Do vậy, hệ thống SINS chỉ chính xác trong một khoảng thời gian
ngắn. Trong khi đó, các vật thể trong luận án chuyển động trong thời gian dài, vì vậy cần thiết phải áp dụng nhiều giải pháp khác nhau để nâng cao chất lượng hệ SINS [20].
Hệ thống SINS cần kết hợp với các hệ thống đo hoặc cảm biến khác để hiệu chỉnh lại các kết quả ước lượng sử dụng các bộ lọc. Hiện nay có 5 bộ lọc được sử dụng phổ biến cho hệ SINS là Euler-KF, MEKF, USQUE, MRP-UKF và DoEuler- UKF. Mỗi bộ lọc đều có ưu và nhược điểm riêng, trong đó bộ lọc MEKF vừa đảm bảo độ chính xác và thời gian tính tốn nhanh [87]. Trong luận án này, sử dụng bộ lọc Kalman kiểu MEKF cho hệ thống INS.
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ự đoá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 tố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 quá 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
3.1 Giới thiệu chương
Trong chương này, đề xuất hệ thống INS đặt trên bàn chân nhằm nâng cao độ chính xác cho việc ước lượng chuyển động cũng như thông số bước đi. Hệ thống INS đề xuất này gồm 01 cảm biến IMU kết hợp với 01 cảm biến khoảng cách đặt trên bằn chân. Việc gắn hệ thống INS trên bàn chân với mục đích sử dụng cập nhật ZUPT trong khi cảm biến khoảng cách hướng xuống mặt đất trong quá trình bước đi là để tham chiếu độ cao và cập nhật chuyển động cho bàn chân.
Một số vấn đề cần giải quyết trong chương này gồm:
- Ước lượng chính xác mối quan hệ gồm vị trí và hướng của cảm biến khoảng cách trong hệ toạ độ của IMU.
- Xây dựng mơ hình bộ lọc Kalman kiểu MEKF cho hệ thống INS mới
dựa mơ hình bộ lọc Kalman cho hệ thống INS cơ bản đã được xây dựng
ở Chương 2.
- Xây dựng phương trình cập nhật ZUPT và các phương trình cập nhật sử dụng dữ liệu từ cảm biến khoảng cách.
- Trích xuất thơng số bước đi từ quỹ đạo chuyển động của cảm biến IMU đặt trên bàn chân.
- Đánh giá hiệu quả hệ thống đề xuất.
Nội dung chương này đã được chính tác giả và cộng sự cơng bố trong bài báo
[50] trên tạp chí Sensors (SCIE, Q1) năm 2015 và bài báo [99] trong kỷ yếu
hội nghị
VCCA năm 2019. Trong bài báo [50], các tác giả đã gắn 2 cảm biến khoảng cách vào hệ thống INS đặt trên bàn chân và sử dụng bộ lọc Kalman 21 trạng thái để ước lượng chuyển động của bàn chân. Trong bài báo [99], các tác giả chỉ sử dụng 1 cảm biến khoảng cách và bộ lọc Kalman 15 trạng thái nhằm giảm khối lượng tính tốn nhưng
3.2 Đề xuất hệ thớng INS đặt trên bàn chân
Bàn chân là điểm thích hợp để đặt cảm biến IMU do chuyển động bước chân có tính chu kỳ lặp đi lặp lại cũng như luôn tồn tại một khoảng thời gian bàn chân chạm đất ở đầu và cuối mỗi bước đi. Tại thời điểm bàn chân chạm đất thì vận tốc có thể được xem là bằng không (nên khoảng thời gian này gọi là ZVI), độ cao bàn chân cũng có thể được xem là bằng khơng trong điều kiện bước đi trên mặt phẳng. Các thông tin vận tốc và độ cao bàn chân tại khoảng thời gian ZVI được sử dụng phổ biến như là một tín hiệu đo để loại bỏ sai số và cập nhật vận tốc, vị trí và hướng bàn chân trong bộ lọc Kalman (thường gọi là ZUPT). Ngoài ra, do cảm biến IMU được đặt trên bàn chân nên chuyển động của cảm biến IMU trong quá trình bước đi phản ánh q trình chuyển động của bàn chân do đó các thơng số bước đi dễ dàng trích xuất được từ quỹ đạo chuyển động của cảm biến IMU.
Trong chương này, luận án sử dụng cảm biến khoảng cách hỗ trợ hệ thống INS gắn trên bàn chân để nâng cao độ chính xác trong ước lượng vị trí và hướng bàn chân. Cảm biến khoảng cách được bố trí hướng xuống mặt đất trong q trình di chuyển nhằm nâng cao độ chính xác của hệ thống INS. Mơ hình bộ lọc MEKF trong Chương
2cũng được điều chỉnh để ước lượng vị trí và hướng của cảm biến khoảng cách trong
hệ toạ độ BCS. Từ vị trí và hướng của cảm biến khoảng cách, thơng tin về độ cao của
bàn chân được xác định từ giá trị đo của cảm biến khoảng cách. Điều này cho phép xây dựng phương trình cập nhật của bộ lọc MEKF để cập nhật quỹ đạo chuyển động cho bàn chân trong quá trình di chuyển nhằm nâng cao độ chính xác trong ước lượng các thơng số bước đi.
Hệ thống đề xuất được thể hiện trong Hình 3.1 bao gồm 01 IMU có tần số lấy mẫu được cấu hình là 100 và 01 cảm biến khoảng cách được gắn cố định trên một chiếc giày. Các thông số được quan tâm của cảm biến khoảng cách là giới hạn đo và tần số hoạt động. Trong đó, cảm biến khoảng cách dùng để đo độ cao của bàn chân trong quá trình bước đi, độ cao này thường nhỏ hơn 20 . Thời gian di chuyển của bàn chân trong một bước đi thường lớn hơn 30 . Trong khoảng thời gian này cần phải có ít nhất một dữ liệu từ cảm biến khoảng cách để cập nhật. Do vậy tần số của
cảm biến khoảng cách yêu cầu phải lớn hơn 33 . Trong luận án này chọn cảm biến khoảng cách VL6180 với khoảng cách đo lên đến 20 mm, sai số khoảng 2 mm và tần số lên đến 50 . Việc đồng bộ các tín hiệu từ các cảm biến được thực hiện bằng cách đọc xen kẽ 01 dữ liệu từ cảm biến khoảng cách với 03 dữ liệu từ cảm biến IMU. Do vậy, cảm biến khoảng cách được cấu hình hoạt động ở tần số 33,33 .
IMU rD b Cảm biến khoảng cách n
Hình 3.1 Tổng quan về INS đặt trên bàn chân (nguồn [50])
Thuật toán của hệ thống INS sử dụng bộ lọc MEKF trong Chương 2 được sử dụng để ước lượng hướng , vận tốc và vị trí theo thời gian của bàn chân (được đại diện bởi hệ toạ độ BCS). Tuy nhiên, mơ hình của bộ lọc MEKF có sự thay đổi bằng việc đưa vị trí và hướng của cảm biến khoảng cách trong BCS vào bộ lọc để ước lượng (do khó có thể đo chính xác bằng thước), đồng thời các thành phần nhiễu chậm thay đổi , trong Hình 2.12 được loại khỏi bộ lọc để giảm khối lượng tính tốn. Lúc này, được tính sơ bộ bằng giá trị thu thập tại thời điểm cảm
biến IMU đứng yên ban đầu và = 03×1 trong thuật tốn INA ở Hình 2.12.
Ngồi ra, các phương trình cập nhật của bộ lọc Kalman cần được xây dựng để sử dụng các tín hiệu đo từ cảm biến khoảng cách cũng như thông tin bàn chân chạm đất để cập nhật sai số của bộ lọc. Do vậy, trong chương này chỉ đề cập đến việc xây dựng mơ hình bộ lọc và các phương trình cập nhật cho bộ lọc có kế thừa kết quả từ Chương
2. Các thơng số bước đi sẽ được trích xuất từ vị trí kết hợp với các thời
điểm bàn chân chạm đất. Trong chương này sử dụng hệ trục toạ độ vật thể BCS và tồn cục WCS đã được trình bày trong Mục 2.2.2.1.
3.3 Xây dựng mơ hình bộ lọc MEKF cho hệ thớng
Gọi [ ] và [ ] là vị trí và hướng của cảm biến khoảng cách trong hệ tọa độ BCS. Lúc này ta có:
trong đó:
-
-
cảm biến khoảng cách có 2 vị trí đặt với thơng số như sau
Việc đo sơ bộ và
số của chúng ( ̅, ̅ ) được đưa vào bộ lọc Kalman để ước lượng nhằm nâng cao độ
chính xác.
Như vậy, các sai số ̅, ̅, , ̅ ̅ và ̅ được đưa vào bộ lọc Kalman để ước
lượng nên các biến trạng thái được sử dụng trong bộ lọc Kalman như sau:
̅
̅
= ̅∈ 15
̅
[ ̅ ]
Mơ hình cho bộ lọc Kalman có dạng như trong (2-19). Trong đó, thành phần và được bỏ qua khi tính ̅ ̇, ̅ ̇và ̅ ̇.Tổng hợp kết quả ta có:
1 ̅ ̇= [− ×] ̅ − 2 ̅ ̇= ̅ ̅ ̇≈ −2 ( ) ( ) ̅ − ( ) (3-3) ̅ ̇ = 0 ̅ ̇ = 0 trong đó, ̅ = ⃗ ̇ số nê n ̅ = 0 và
Như vậy các ma trận trong mơ hình hệ thống đề xuất như sau: