Trang 100
Thuật toán phân loại chuyển động được thể hiện trong Hình 4.4. Trong đó, các khoảng chuyển động được phát hiện dựa vào encoder và cảm biến IMU theo các công thức (4-5), (4-8) và (4-9).
Chuyển động đẩy đi liên tục được xác định bởi chuyển động do encoder phát hiện ra nhưng khơng có sự quay quanh trục 𝑦𝑏 và thời gian của chuyển động là đủ dài hơn thời gian chuyển động của 1 bước chân 𝑇𝑏 (chọn 𝑇𝑏 = 1,5 𝑠). Nếu thời gian chuyển động trong trường hợp này nhỏtương ứng với thời gian của 1 bước chân thì đó là chuyển động đẩy đi từng bước. Chuyển động nhấc 2 chân sau được xác định bằng việc phát hiện chuyển động của encoder và chuyển động quay quanh trục 𝑦𝑏. Chuyển động nhấc hoàn toàn khung tập đi được xác định bằng việc có chuyển động quanh trục 𝑦𝑏 nhưng khơng có chuyển động do encoder phát hiện ra và khơng có chuyển động quay quanh trục 𝑧𝑏. Chuyển động đổi hướng được xác định bằng việc phát hiện chuyển động quay quanh phương 𝑦𝑏 và phương 𝑧𝑏 nhưng khơng có chuyển động quay của encoder.
4.4 Ước lượng quỹđạo chuyển động của khung tập đi
Việc ước lượng quỹ đạo chuyển động của khung tập đi có thể được thực hiện bằng hai cách. Cách thứ nhất là ước lượng chuyển động của khung tập đi sử dụng cảm biến IMU cho toàn bộ quá trình chuyển động của khung tập đi. Lúc này các tín hiệu từ encoder được sử dụng để xây dựng các phương trình cập nhật hướng, vận tốc và vị trí cho khung tập đi cho các khoảng chuyển động liên quan đến việc đẩy khung tập đi trên mặt đất. Phần này đã được tác giả luận án và cộng sự cơng bố trong cơng trình [106]. Cách thứ hai là kết hợp việc ước lượng chuyển động của khung tập đi sử dụng cảm biến IMU cho các khoảng chuyển động liên quan đến việc nhấc khung tập đi lên và việc ước lượng chuyển động của khung tập đi sử dụng encoder cho các khoảng chuyển động liên quan đến việc đẩy khung tập đi trên mặt đất. Phần này đã được tác giả luận án và cộng sự cơng bố trong cơng trình [99].
Việc ước lượng chuyển động của khung tập đi sử dụng cảm biến IMU được thực hiện bằng thuật toán hệ thống INS sử dụng bộ lọc Kalman kiểu MEKF như đã
Trang 101
được trình bày ở Chương 2 với các biến trạng thái đưa vào bộ lọc Kalman 𝑥 = [𝑞̅ 𝑏𝑔 𝑟̅ 𝑣̅ 𝑏𝑎]𝑇.
Tại các khoảng thời gian ZVI, cập nhật ZUPT được sử dụng cập nhật vận tốc cho bộ lọc Kalman. Nếu khung tập đi được sử dụng trên mặt phẳng nằm ngang như sàn nhà thì độ cao của khung tập đi cũng được cập nhật trong bộ lọc Kalman. Thuật toán để xác định các khoảng thời gian khung tập đi đứng yên trên mặt đất cũng như phương trình cập nhật vận tốc và độ cao được xây dựng cho bộ lọc Kalman đã được thể hiện như trong Chương 3 (xem Mục 3.4.1) với ma trận quan sát 𝐻𝑍𝑉𝐼 như sau:
𝐻𝑍𝑉𝐼 = [01×803×9𝐼1 03×3𝐼3 003×33×3] (4-10) Quy trình thực thi bộ lọc Kalman cho hệ thống tương tựnhư đã trình bày trong Hình 4.5. Trong đó có một số điểm khác so với Hình 3.2 như sau:
- Tính ma trận 𝐴𝑘 theo công thức (2-25),
- Tại các khoảng ZVI, cập nhật ZUPT với ma trận 𝐻𝑘 tính theo (4-10),
- Tại các khoảng chuyển động đẩy đi liên tục và đẩy đi từng bước, cập nhật quaternion và vị trí sử dụng thơng tin encoder theo cơng thức (4-40).
𝐾ℎở𝑖𝑡ạ𝑜 𝑥0−= 015×1 𝑃0−= 015×15 Tính 𝐴𝑘 (3-7) Tính 𝑄𝑘(2−29) Dự đốn 𝑥𝑘−+1 (2-30) Dự đốn 𝑃𝑘−+1 (2-31) Tính 𝑅𝑘 (3-11) Tính 𝐻𝑘 (4-10) ZVI=1 𝑑𝐷> 0 Tính 𝐾𝑘 (2-33) Cập nhật 𝑥𝑘 (2-34) Cập nhật 𝑃𝑘 (2-35) Tính 𝑅𝑘 (4-40) Tính 𝐻𝑘 (4-40) Bắt đầu Kết thúc S Đ S Đ Đ: Đúng S: Sai