7. Bố cục chung của luận án
4.2 Đề xuất hệ thống INS đặt trên khung tập đi
4.2.3 Thuật toán ước lượng mối quan hệ giữa hệ toạ độ ICS và BCS
Đặt vectơ 𝑇𝑏𝐼 ∈ 𝑅3và ma trận 𝐶𝑏𝐼 ∈ 𝑅3×3 là vectơ tịnh tuyến và ma trận quay chuyển một vectơ biểu diễn trong hệ toạ độ BCS sang ICS. Lúc này, 𝑇𝑏𝐼 là vị trí của cảm biến IMU trong hệ toạ độ BCS, có thể đo được bằng thước. Do các vectơ cột trong ma trận 𝐶𝑏𝐼 chính là các trục của hệ toạ độ BCS trong ICS, nên 𝐶𝑏𝐼 = [[𝑥𝑏]𝐼 [yb]𝐼 [zb]𝐼]. Do cảm biến IMU có kích thước rất nhỏ nên khơng thể đo chính xác phương hướng các trục bằng thước nên cần đề xuất phương pháp ước lượng 𝐶𝑏𝐼.
Theo cách định nghĩa của hệ toạ độ BCS thì 𝑧𝑏 thẳng đứng và hướng lên trên trùng phương với gia tốc trọng trường trong trường hợp khung tập đi di chuyển trên mặt đất nằm ngang. Tại thời điểm ban đầu thì khung tập đi đang đứng yên nên giá trị gia tốc 𝑦𝑎 mà IMU đo được chính là gia tốc trọng trường và có hướng từ dưới lên trên. Như vậy ta có:
[𝑧𝑏]𝐼 = 𝑦a,1
‖𝑦a,1‖ (4-1)
Thí nghiệm để ước lượng [𝑥𝑏]𝐼 được thực hiện bằng cách đẩy khung tập đi theo đường thẳng hướng về trước trên sàn nhà một khoảng cách ngắn (khoảng 1 𝑚). Lúc này, hướng di chuyển của khung tập đi trong hệ toạ độ BCS là [𝑑]𝑏 được tính tốn sử dụng thơng tin từ 2 encoder như trong Hình 4.6 và cơng thức (4-23). Ngồi ra hướng di chuyển này trong hệ toạ độ ICS là [𝑑]𝐼 cũng được ước lượng sử dụng cảm biến IMU bằng thuật toán của hệ thống INS trong Chương 2.
Theo quy trình của thí nghiệm thì trục [𝑥𝑏]𝑏 là trùng hướng với [𝑑]𝑏, tuy nhiên trong q trình thực hiện thí nghiệm [𝑑]𝑏 khơng hồn tồn trùng với trục [𝑥𝑏]𝑏.Gọi 𝜃 là góc giữa [𝑑]𝑏 và [𝑥𝑏]𝑏 và được xác định như sau
cos 𝜃 =‖[𝑑]𝑏𝑑𝑥 ‖ sin 𝜃 =‖[𝑑]𝑑𝑦𝑏‖
(4-2)
Trang 94
Việc xác định [𝑥𝑏]𝐼 được xác định bằng cách quay [𝑑]𝐼 quanh trục [𝑧𝑏]𝐼 một góc đúng bằng góc 𝜃 giữa [𝑑]𝑏 và [𝑥𝑏]𝑏. Phép quay vectơ [𝑑]𝐼 quay trục [𝑧𝑏]𝐼 để tìm [𝑥𝑏]𝐼 được thực hiện thơng qua 5 bước liên tiếp bao gồm việc quay quanh trục 𝑥𝐼 để đưa [𝑑]𝐼 về mặt phẳng 𝑂𝐼𝑥𝐼𝑧𝐼, quay quanh trục 𝑦𝐼 để đưa [𝑑]𝐼 về trùng với 𝑧𝐼, quay quanh trục 𝑧𝐼 một góc 𝜃, quay ngược lại theo trục 𝑦𝐼, quay ngược lại theo trục 𝑥𝐼. Kết quả của việc này được tóm tắt như sau
[𝑥𝑏]𝐼 = 𝐶(𝑥𝐼, −𝛼)𝐶(𝑦𝐼, −𝛽)𝐶(𝑧𝐼, 𝜃)𝐶(𝑦𝐼, 𝛽)𝐶(𝑥𝐼, 𝛼)[𝑑]𝐼 (4-3) trong đó: - 𝐶(𝑥𝐼, 𝛼) = [10 cos 𝛼 − sin 𝛼0 0 0 sin 𝛼 cos 𝛼 ] - 𝐶(𝑦𝐼, 𝛽) = [ cos 𝛽 0 sin 𝛽0 1 0 −sin 𝛽 0 cos 𝛽]
- 𝐶(𝑧𝐼, 𝜃) = [cos 𝜃 −sin 𝜃 0sin 𝜃 cos 𝜃 0
0 0 1] - 𝑙 = √𝑧𝑏𝑦2 + 𝑧𝑏𝑧2 - cos 𝛼 =𝑧𝑏𝑧 𝑙 - sin 𝛼 = 𝑧𝑏𝑦 𝑑 - cos 𝛽 = 𝑙 - sin 𝛽 = −𝑧𝑏𝑥
Theo định nghĩa về ma trận quay thì 𝐶𝑏𝐼 được tính như sau
𝐶𝑏𝐼 = [[𝑥𝑏]𝐼 [𝑧𝑏]𝐼× [𝑥𝑏]𝐼 [𝑧𝑏]𝐼] (4-4) Một thuật toán để ước lượng nhanh mối quan hệ giữa cảm biến IMU và khung tập đi loại không bánh gồm ma trận quay và vectơ tịnh tiến cũng được tác giả luận án đề xuất trong công bố [107].
Trang 95