4.4.1 Xây dựng phương trình cập nhật quaternion dựa vào phương đứngcủa khung tập đi của khung tập đi
Trong trường hợp khung tập đi được đẩy đi trên mặt đất, trục của khung tập đi luôn hướng lên trên trùng với trục . Theo định nghĩa của hệ toạ độ WCS, trùng với phương của gia tốc trọng trường. Gia tốc trọng trường chính là gia tốc đo được từ cảm biến IMU tại thời điểm trước khi chuyển động. Do vậy, ta có phương trình:
( ) trong đó,= [0 0 1] là trục
Theo tính chất của quaternion, ta có:
(
Thay (4-12) vào (4-11), ta có
[ (
Viết lại phương trình (4-13), ta có:
−2[ ̄×] ( ) = ,1 − ( )
‖ ,1‖
(4-12)
(4-13)
(4-14)
Sử dụng tính chất [ ×] = −[ ×] với , ∈ 3 cho phương trình (4-14), ta có phương trình cập nhật quaternion của khung tập đi như sau:
trong đó:
-
1
-
1
- Nhiễu 1 = (0, 1) ∈ 3 là nhiễu trắng đại diện cho việc trục khơng hồn tồn trùng với trong quá trình di chuyển.
4.4.2 Xây dựng phương trình cập nhật quaternion sử dụng góc quay quanhtrục đứng trục đứng
Trong quá trình khung tập đi được đẩy đi trên mặt đất, góc quay quanh trục đứng của khung tập đi được tính dựa vào các encoder và được sử dụng để dẫn ra phương trình cập nhật cho bộ lọc Kalman.
Trước tiên, cần tính góc quay quanh trục sử dụng thơng tin từ các encoder. Đặt , và , là quãng đường di chuyển của bánh trái và bánh phải trong khoảng thời gian lấy mẫu [( − 1) , ]. Quãng đường này được tính bằng các xung encoder như sau:
, = ,
,
=
, trong đó, và là các hệ số của encoder liên quan đến số xung encoder trong vịng quay và bán kính bánh xe trái và phải. Các hệ số và khó có thể được tính chính xác do liên quan đến độ nghiêng của bánh và việc xác định bán kính bánh xe.
, ,
Bánh trái