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ơ ∈ 3 và 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ó:
[ ]
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 =
trong đó, [ ] = [ ] .
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
[ ] = ( , − ) ( , − ) ( , ) ( , ) ( , )[ ] trong đó: 1 -(,)=[0 0 cos - (,)=[ 0 −sin cos - ( , ) = [sin 0 - =√2+2 - cos = - sin = - cos = - sin = −
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].