Một thí nghiệm đơn giản được tiến hành để hiệu chỉnh các hệ số và bằng cách đẩy khung tập đi thẳng về trước. Gọi là quãng đường đi của khung tập đi, ta có:
với là tổng số dữ liệu của q trình thí nghiệm.
Q trình chuyển động của khung tập đi trong thời gian lấy mẫu có thể được xem như quay quanh một đường trịn tâm nào đó tại thời điểm (xem Hình 4.6). Lúc này bánh trái, bánh phải và gốc của hệ toạ độ BCS đều chuyển động trên các cung trịn đồng tâm khác bán kính.
Do khoảng cách giữa 2 bánh xe ( = 610 ) là rất lớn so với quãng đường di chuyển của 2 bánh xe trong một thời gian lấy mẫu ( , , , ≈ 5 )
nên góc quay của khung tập đi quanh trục có thể được tính như sau:
≈
Ma trận quay quanh trục hệ toạ độ BCS tại thời điểm rời rạc
từ hệ toạ độ BCS tại thời điểm rời rạc − 1 sang được tính như sau:
,
, −1
Lúc này, vectơ tịnh tiến chuyển hệ toạ độ BCS tại thời điểm rời rạc − 1 sang hệ toạ độ BCS tại thời điểm rời rạc xét trong hệ tọa độ BCS tại thời điểm rời rạc − 1 là:
[ ,
, −1
Vectơ tịnh tiến này khi xét trong hệ tọa độ WCS được xác định như sau:
,
Thay
, −1
, , −1
Như vậy vị trí của khung tập đi (tâm của BCS) tại thời điểm rời tạc xét trong hệ toạ độ WCS được tính thơng qua vị trí của khung tập đi tại thời điểm rời rạc − 1 như sau:
Vị trí của khung tập đi được tính thơng qua vị trí của cảm biến IMU ( ) và
ngược lại như sau:
Có thể thấy được trục
([ (
của BCS tại thời điểm rời rạc − 1
Như vậy, ta có:
Thay (4-12) vào (4-25):
Viết lại phương trình (4-26):
Sử dụng tính chất ( + ) =+ cho phương trình (4-27):
Tiếp tục sử dụng tính chất ( )
Sử dụng (4-29):
[ (
Tiếp tục sử dụng tính chất [ ×] = −[ ×] cho phương trình (4-30):
−2 (
Như vậy phương trình cập nhật quaternion cho bộ lọc Kalman sử dụng góc quay quanh trục như sau:
trong đó:
-
2
-
2
- Nhiễu 2 = (0, 2) ∈ 3 là nhiễu trắng đại diện cho việc tính tốn góc quay quanh trục khơng hồn tồn chính xác.
4.4.3 Xây dựng phương trình cập nhật vị trí sử dụng thơng tin từ encoder
Trong phần này, trình bày việc xây dựng phương trình cập nhật vị trí sử dụng thơng tin từ encoder. Từ phương trình (4-24), ta có:
Do
, −1
Thay phương trình (4-33) vào phương trình (4-34) như sau:
= , −1 + , ,
−1 + ( −1) [ , , −1] [ ] Áp dụng , = − ,
[ ] , ta có:
, −1 = −1 − , −1[ ] Thay
Như vậy phương trình (4-35) được viết lại như sau:
−1 (4-33) (4-34) (4-35) (4-36) (4-37) (4-38) trong đó, các thành phần bên vế phải đã tính được trước thời điểm rời rạc .
Như vậy, phương trình cập nhật vị trí sử dụng thơng tin từ các encoder như sau:
3 = 3 + 3
trong đó:
- 3 = −
- 3 = (0, 3) ∈ 3 là nhiễu trắng đại diện cho sai số của việc xác định vị trí sử dụng encoder. Từ các phương trình cập nhật ở Mục 4.4.1, 4.4.2 và 4.4.3, ta có phương trình cập nhật tổng quát trong trường hợp khung tập đi được đẩy đi trên mặt đất như sau:
= +
trong đó:
1
=[ 2]; =[
3
Ma trận hiệp phương của phép đo như sau:
1
Do q trình tính tốn để đưa đến phương trình cập nhật rất phức tạp nên hiệp phương sai của phép đo rất khó xác định và được chọn dựa vào thực tế và kinh nghiệm. Trong đó, do việc sử dụng encoder có độ phân giải cao để xác định hướng và vị trí nên sai số cũng như hiệp phương sai nhỏ, luận án chọn 1 = 2 = 3 = 10−6 3.
4.4.4 Kết hợp quỹ đạo ước lượng bởi IMU và quỹ đạo ước lượng bởi encoder
Ở cách thứ hai của việc xác định chuyển động của khung tập đi, cần xác định
quỹ đạo chuyển động của khung tập đi sử dụng encoder khi khung tập đi được đẩy đi
trên mặt đất và kết hợp với quỹ đạo chuyển động của khung tập đi mỗi khi được nhấc lên được ước lượng bởi cảm biến IMU. Các khoảng chuyển động liên quan đến việc đẩy khung tập đi trên mặt đất hay nhấc khung tập đi lên đã được phát hiện và phân loại ở Mục 4.3.3.
Việc ước lượng quỹ đạo chuyển động của khung tập đi sử dụng IMU bao gồm cả việc cập nhật vận tốc và độ cao của khung tập đi tại các khoảng khung tập đi đứng yên trên mặt đất đã được trình bày ở đầu Mục 4.4 với ma trận quan sát trong (4-10). Việc ước lượng quỹ đạo chuyển động của khung tập đi sử dụng encoder được thể hiện trong Hình 4.6 và phương trình (4-23). Việc kết hợp quỹ đạo chuyển động được
ước lượng bởi cảm biến IMU và quỹ đạo chuyển động được ước lượng bởi encoder được thực hiện thơng qua phương trình (4-24).
4.5 Trích xuất thơng sớ bước đi
Một hệ trục toạ độ BCS mới (BCSN) được định nghĩa có gốc nằm ở trung điểm đoạn nối giữa hai đế chân sau khung tập đi trong khi hướng các trục trùng với các trục của hệ toạ độ BCS như trong Hình 4.7. Lúc này, sau mỗi bước đi thì gốc hệ toạ độ BCSN được xem là trùng với gót chân của người dùng. Như vậy việc xác định thông số bước đi của người dùng dựa trên vị trí của hệ toạ độ BCSN theo thời gian. Việc này đã được đề cập đến trong công bố [107] của tác giả.