Trang 102
4.4.1 Xây dựng phương trình cập nhật quaternion dựa vào phương đứng củ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:
𝐶(𝑞𝑖)𝑧𝑤 = 𝑦𝑎,1
‖𝑦𝑎,1‖ (4-11)
trong đó, 𝑧𝑤 = [0 0 1]𝑇 là trục 𝑧𝑤 xét trong hệ toạ độ WCS. Theo tính chất của quaternion, ta có:
𝐶(𝑞𝑖) = 𝐶(𝑞̂𝑖) − 2[𝑞̄𝑖×]𝐶(𝑞̂𝑖) (4-12) Thay (4-12) vào (4-11), ta có
[𝐶(𝑞̂𝑖) − 2[𝑞̄𝑖 ×]𝐶(𝑞̂𝑖)]𝑧𝑤 = 𝑦𝑎,1
‖𝑦𝑎,1‖ (4-13)
Viết lại phương trình (4-13), ta có: −2[𝑞̄𝑖 ×]𝐶(𝑞̂𝑖)𝑧𝑤 = 𝑦𝑎,1
‖𝑦𝑎,1‖− 𝐶(𝑞̂𝑖)𝑧𝑤 (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:
𝑧𝑒1 = 𝐻𝑒1𝑥 + 𝑣𝑒1 (4-15)
trong đó:
- 𝑧𝑒1 = 𝑦𝑎,1
‖𝑦𝑎,1‖− 𝐶(𝑞̂𝑖)𝑧𝑤
- 𝐻𝑒1 = [2[𝐶(𝑞̂𝑖)𝑧𝑤×] 03×12] ∈ 𝑅3×15
- 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.
Trang 103
4.4.2 Xây dựng phương trình cập nhật quaternion sử dụng góc quay quanh trụ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:
𝐸𝑡,𝑖 = 𝜆𝑡𝑝𝑡,𝑖
𝐸𝑝,𝑖 = 𝜆𝑝𝑝𝑝,𝑖 (4-16)
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 Bánh phải
𝐸𝑡,𝑖
𝐸𝑝,𝑖