- Đổi hướng: đây là chuyển động thường được sử dụng khi người dùng đổi hướng chuyển động. Lúc này người dùng đứng trên 2 chân, sau đó nhấc khung tập đi lên và quay một góc trước khi đặt khung tập đi trở lại mặt đất (xem Hình 4.3e). Hai bánh của khung tập đi gần như khơng lăn trong q trình di chuyển.
Với khung tập đi khơng có bánh thì chuyển động đẩy đi liên tục và đẩy đi từng bước không tồn tại.
4.3.2 Thuật toán phát hiện chuyển động của khung tập đi
Về nguyên tắc cảm biến IMU có thể phát hiện được mọi chuyển động của vật thể dựa vào việc phát hiện gia tốc và vận tốc góc quay theo các hướng. Tuy nhiên trường hợp chuyển động đẩy đi trên mặt đất có các nguyên nhân gây khó khăn cho việc phát hiện chuyển động của cảm biến IMU như sau:
- Gia tốc thay đổi chủ yếu theo phương di chuyển và không rõ ràng do vậy việc sử dụng sự thay đổi gia tốc để phát hiện khoảng chuyển động là khơng chính xác.
- Vận tốc góc quay thường rất nhỏ do vậy khơng thể sử dụng vận tốc góc để phát hiện chuyển động.
Lúc này, các encoder được sử dụng phát hiện được chuyển động liên quan đến chuyển động đẩy đi trên mặt đất. Trong phần này, đề xuất giải pháp phát hiện và phân loại chuyển động sử dụng kết hợp thông tin từ encoder và cảm biến IMU.
Chuyển động của khung tập đi liên quan đến việc đẩy đi trên mặt đất được phát hiện thông qua việc lăn bánh xe hay encoder. Thời điểm được xem là thuộc khoảng thời gian chuyển động của bánh xe nếu một trong 2 điều kiện sau được thỏa mãn:
| | ≥ ,
,
| | ≥ ,
,
trong đó , ( , ) là số xung encoder ở bánh trái (phải) tại thời điểm rời rạc , là ngưỡng đặt của giá trị này và là một số nguyên đại diện cho khoảng thời gian đủ lớn để xem là một chuyển động. Trong luận án này, chọn = 30 tương ứng với khoảng thời gian là 0,3 giây. Trong q trình phân tích thực nghiệm luận án chọn
= = 2 xung.
Các chuyển động liên quan đến việc nhấc khung tập đi lên ảnh hưởng đến vận tốc góc quay quanh trục của hệ toạ độ BCS trong khi chuyển động liên quan đến đổi hướng vừa liên quan đến vận tốc góc quay quanh các trục và của hệ BCS. Như vậy, để xác định khoảng chuyển động sử dụng cảm biến IMU, các tín hiệu của cảm biến IMU theo các phương nêu trên được xét đến để việc phát hiện chuyển động chính xác và hiệu quả hơn.
Vận tốc góc quay theo phương tại thời điểm rời rạc được tính như sau:
Vận tốc góc quay theo phươngtại thời điểm của IMU được tính như sau:
Thời điểm được xem là thuộc khoảng thời gian chuyển động liên quan đến việc nhấc khung tập đi lên nếu điều kiện sau được thỏa mãn:.
trong đó:
- là ngưỡng của vận tốc góc quay theo phương mà khung tập đi bị nhấc lên,
- là số nguyên đại diện cho khoảng thời gian đủ lớn để xem là một chuyển động. Trong luận án này, chọn = 30 tương ứng với thời gian là 0,3 giây và = 0,2 tương ứng với sự thay đổi 0,2
/ .
Tương tự, thời điểm được xem là thuộc khoảng thời gian chuyển động liên quan đến việc quay khung tập đi lên nếu điều kiện sau được thỏa mãn:
, ≥ ,−
trong đó:
- là ngưỡng của vận tốc góc quay theo phương mà khung tập đi bị nhấc lên,
- là số nguyên đại diện cho khoảng thời gian đủ lớn để xem là một chuyển động. Trong luận án này, chọn = 30 tương ứng với thời gian là 0,3 giây và = 0,2 tương ứng với sự thay đổi 0,2
/ .
Như vậy, các khoảng chuyển động của khung tập đi là tổng hợp của các khoảng chuyển động do encoder và cảm biến IMU phát hiện ra.
4.3.3 Thuật toán phân loại chuyển động của khung tập đi
Bắt đầu
Tìm các khoảng chuyển động:
- Sử dụng encoder
- Quay quanh trục y dùng IMU
- Quay quanh trục z dùng IMU
Chuyển động quanh y
S
Đ S
Đẩy đi liên
tục Đẩy đi từngbước chân sauNhấc 2
Nhấc hoàn
toàn Quay đổihướng chuyển độngKhơng
Kết thúc
Hình 4.4 Thuật tốn phân loại chuyển động
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 tồn bộ q 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ư đã
đượ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:
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ậntheo cơng thức (2-25),
- Tại các khoảng ZVI, cập nhật ZUPT với ma trậntí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).
Bắt đầu Đ: Đúng S: Sai ℎở ạ − = 00 15×1 − = 0 0 15×15 S ZVI=1 Tính Tính Tính Cập nhật Cập nhật S Tính Tính Tính(2 − 29) Dự đốn − Dự đốn − Kết thúc
Hình 4.5 Lưu đồ thuật toán thực thi bộ lọc Kalman cho INS trên khung tập đi
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
Hình 4.6 Chuyển động quay của khung tập đi
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 qt 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ả.
Hình 4.7 Hệ trục toạ độ BCSN dùng trong xác định bước đi
Vị trí gốc hệ toạ độ BCSN được tính thơng qua vị trí gốc hệ toạ độ BCS như sau:
trong đó, []
của khung tập đi.
Trong các trường hợp chuyển động đẩy đi từng bước, nhấc 2 chân sau và nhấc lên hoàn tồn khung tập đi thì việc xác định từng bước riêng lẻ dựa trên nguyên tắc là mỗi bước đi cách nhau bởi một khoảng ZVI và tại khoảng ZVI này vị trí của gốc hệ toạ độ BCSN được xem là vị trí của bàn chân người dùng. Việc trích xuất thơng số bước đi dựa vào các khoảng ZVI đã được thể hiện trong Mục 3.6.
Trong trường hợp khung tập đi được đẩy đi liên tục thì khơng có các khoảng thời gian ZVI này giữa các bước đi, do vậy việc xác định từng bước đi riêng lẻ sẽ phức tạp hơn nhiều. Khi phân tích chuyển động của khung tập đi thơng qua thơng tin encoder thì tốc độ chuyển động của khung tập đi tương ứng với tốc độ quay của