7. Bố cục chung của luận án
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ư đã
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:
𝐻𝑍𝑉𝐼 = [0 03×9 𝐼3 03×3
1×8 𝐼1 03×3 03×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ự đoán 𝑥𝑘+1− (2-30) Dự đoá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
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
𝐸𝑡,𝑖
𝐸𝑝,𝑖
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ó:
Trang 104 𝜆𝑡 = 𝐿 ∑𝑁 𝑝𝑡,𝑖 𝑖=1 𝜆𝑝 = 𝐿 ∑𝑁 𝑝𝑝,𝑖 𝑖=1 (4-17)
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:
𝛾𝑖 ≈𝐸𝑡,𝑖 − 𝐸𝑝,𝑖
𝐷 (4-18)
Ma trận quay quanh trục 𝑧𝑏 từ 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 𝑖 được tính như sau:
𝐶 𝑏,𝑖−1 𝑏,𝑖 = [ 𝑐𝑜𝑠 𝛾𝑖 − 𝑠𝑖𝑛 𝛾𝑖 0 𝑠𝑖𝑛 𝛾𝑖 𝑐𝑜𝑠 𝛾𝑖 0 0 0 1 ] (4-19)
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𝑏,𝑖 ]
𝑏,𝑖−1 = 𝐶𝑏,𝑖−1 𝑏,𝑖 [𝐸𝑝,𝑖 + 𝐸𝑡,𝑖
2 0 0] (4-20)
Vectơ tịnh tiến này khi xét trong hệ tọa độ WCS được xác định như sau: 𝑇𝑏,𝑖−1𝑏,𝑖 = 𝐶𝑏,𝑖−1𝑤 𝐶
𝑏,𝑖−1
𝑏,𝑖 [𝐸𝑝,𝑖 + 𝐸𝑡,𝑖
2 0 0] (4-21)
Thay 𝐶𝑏,𝑖−1𝑤 = 𝐶𝑇(𝑞̂𝑖−1)𝐶𝑏𝐼 vào phương trình (4-21), ta có: 𝑇𝑏,𝑖−1𝑏,𝑖 = 𝐶𝑇(𝑞̂𝑖−1)𝐶𝑏𝐼𝐶𝑏,𝑖−1 𝑏,𝑖 [𝐸𝑝,𝑖 + 𝐸𝑡,𝑖
Trang 105
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:
𝑟𝑏,𝑖 = 𝑟𝑏,𝑖−1+ 𝑇𝑏,𝑖−1𝑏,𝑖 (4-23)
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:
𝑟𝑏,𝑖 = 𝑟𝑖− 𝐶𝑏,𝑖𝑤[𝑇𝑏𝐼]𝑏
𝑟𝑖 = 𝑟𝑏,𝑖+ 𝐶𝑏,𝑖𝑤[𝑇𝑏𝐼]𝑏 (4-24) Có thể thấy được trục 𝑥𝑏 của hệ toạ độ BCS tại thời điểm rời rạc 𝑖 ([𝐶𝐼𝑏𝐶(𝑞𝑖)]𝑇[1 0 0]𝑇) chính là phép quay quanh trục 𝑧𝑏 một góc 𝛾𝑖 của trục 𝑥𝑏 của BCS tại thời điểm rời rạc 𝑖 − 1 ( 𝑉𝑃 = 𝐶𝑏,𝑖−1 𝑏,𝑖 [𝐶𝐼𝑏𝐶(𝑞̂𝑖−1)]𝑇[1 0 0]𝑇)
Như vậy, ta có:
[𝐶𝐼𝑏𝐶(𝑞𝑖)]𝑇[1 0 0]𝑇 = 𝑉𝑃 (4-25) Thay (4-12) vào (4-25):
[𝐶𝐼𝑏(𝐶(𝑞̂𝑖) − 2[𝑞̄𝑖×]𝐶(𝑞̂𝑖))]𝑇[1 0 0]𝑇 = 𝑉𝑃 (4-26) Viết lại phương trình (4-26):
[𝐶𝐼𝑏𝐶(𝑞̂𝑖) − 2𝐶𝐼𝑏[𝑞̄𝑖×]𝐶(𝑞̂𝑖)]𝑇[1 0 0]𝑇 = 𝑉𝑃 (4-27) Sử dụng tính chất (𝑎 + 𝑏) = 𝑎𝑇 + 𝑏𝑇 cho phương trình (4-27):
([𝐶𝐼𝑏𝐶(𝑞̂𝑖)]𝑇− 2[𝐶𝐼𝑏[𝑞̄𝑖×]𝐶(𝑞̂𝑖)]𝑇)[1 0 0]𝑇 = 𝑉𝑃 (4-28) Tiếp tục sử dụng tính chất (𝑎𝑏)𝑇 = 𝑏𝑇𝑎𝑇 cho phương trình (4-28):
([𝐶𝐼𝑏𝐶(𝑞̂𝑖)]𝑇 − 2[[𝑞̄𝑖×]𝐶(𝑞̂𝑖)]𝑇[𝐶𝐼𝑏]𝑇) [1 0 0]𝑇 = 𝑉𝑃 (4-29) Sử dụng tính chất (𝑎𝑏)𝑇 = 𝑏𝑇𝑎𝑇 và [𝑎 ×]𝑇 = −[𝑎 ×] cho phương trình (4-29):
[𝐶𝐼𝑏𝐶(𝑞̂𝑖)]𝑇[1 0 0]𝑇 + 2𝐶𝑇(𝑞̂𝑖)[𝑞̄𝑖×][𝐶𝐼𝑏]𝑇[1 0 0]𝑇 = 𝑉𝑃 (4-30) Tiếp tục sử dụng tính chất [𝑎 ×]𝑏 = −[𝑏 ×]𝑎 cho phương trình (4-30):
−2𝐶𝑇(𝑞̂𝑖)[([𝐶𝐼𝑏]𝑇[1 0 0]𝑇) ×]𝑞̄𝑖 = 𝑉𝑃 − [𝐶𝐼𝑏𝐶(𝑞̂𝑖)]𝑇[1 0 0]𝑇 (4-31) 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:
Trang 106
𝑧𝑒2 = 𝐻𝑒2𝑥 + 𝑣𝑒2 (4-32)
trong đó:
- 𝑧𝑒2 = 𝑉𝑃 − [𝐶𝐼𝑏𝐶(𝑞̂𝑖)]𝑇[1 0 0]𝑇
- 𝐻𝑒2 = [−2𝐶𝑇(𝑞̂𝑖)[([𝐶𝐼𝑏]𝑇[1 0 0]𝑇) ×] 03×12] ∈ 𝑅3×15
- 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ó:
𝑟𝑖 = 𝑟𝑏,𝑖 + 𝐶𝑏,𝑖−1𝑤 [𝐶𝑏,𝑖−1𝑏,𝑖 ]𝑇[𝑇𝑏𝐼]𝑏 (4-33) Do 𝐶𝑏,𝑖−1𝑤 = 𝐶𝑇(𝑞̂𝑖−1)𝐶𝑏𝐼 nên:
𝑟𝑖 = 𝑟𝑏,𝑖 + 𝐶𝑇(𝑞̂𝑖−1)𝐶𝑏𝐼[𝐶𝑏,𝑖−1𝑏,𝑖 ]𝑇[𝑇𝑏𝐼]𝑏 (4-34) Thay phương trình (4-33) vào phương trình (4-34) như sau:
𝑟𝑖 = 𝑟𝑏,𝑖−1+ 𝑇𝑏,𝑖−1𝑏,𝑖 + 𝐶𝑇(𝑞̂𝑖−1)𝐶𝑏𝐼[𝐶𝑏,𝑖−1𝑏,𝑖 ]𝑇[𝑇𝑏𝐼]𝑏 (4-35) Áp dụng 𝑟𝑏,𝑖 = 𝑟𝑖 − 𝐶𝑏,𝑖𝑤[𝑇𝑏𝐼]𝑏, ta có:
𝑟𝑏,𝑖−1 = 𝑟𝑖−1− 𝐶𝑏,𝑖−1𝑤 [𝑇𝑏𝐼]𝑏 (4-36)
Thay 𝐶𝑏,𝑖−1𝑤 = 𝐶𝑇(𝑞̂𝑖−1)𝐶𝑏𝐼 vào phương trình (4-36), ta có:
𝑟𝑏,𝑖−1 = 𝑟𝑖−1 − 𝐶𝑇(𝑞̂𝑖−1)𝐶𝑏𝐼[𝑇𝑏𝐼]𝑏 (4-37) Như vậy phương trình (4-35) được viết lại như sau:
𝑟𝑖 = 𝑟𝑖−1− 𝐶𝑇(𝑞̂𝑖−1)𝐶𝑏𝐼[𝑇𝑏𝐼]𝑏+ 𝑇𝑏,𝑖−1𝑏,𝑖 + 𝐶𝑇(𝑞̂𝑖−1)𝐶𝑏𝐼[𝐶𝑏,𝑖−1𝑏,𝑖 ]𝑇[𝑇𝑏𝐼]𝑏 (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 (4-39)
trong đó:
- 𝑧𝑒3 = 𝑟𝑖 − 𝑟̂𝑖
Trang 107
- 𝑣𝑒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:
𝑧 = 𝐻𝑥 + 𝑣𝑒 (4-40) trong đó: 𝑧 = [ 𝑧𝑒1 𝑧𝑒2 𝑧𝑒3] ; 𝐻 = [ 𝐻𝑒1 𝐻𝑒2 𝐻𝑒3 ] ; 𝑣𝑒 = [ 𝑣𝑒1 𝑣𝑒2 𝑣𝑒3]. Ma trận hiệp phương của phép đo như sau:
𝑅 = [
𝑅𝑒1 03×3 03×3 03×3 𝑅𝑒2 03×3 03×3 03×3 𝑅𝑒3
] (4-41)
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
Trang 108
ướ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:
𝑟𝑏𝑁,𝑖 = 𝑟𝑏,𝑖 + 𝐶𝑏,𝑖𝑤[𝑇𝑏𝑏𝑁]𝑏 (4-42) trong đó, [𝑇𝑏𝑏𝑁]𝑏 = [−𝑑 0 0]′ với 𝑑 là khoảng cách giữa 2 chân trước và chân sau 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 hồ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.
Trang 109
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 bánh xe tăng lên và giảm xuống có chu kỳ theo bước chân người dùng. Để thấy rõ được điều này, một bộ lọc thông thấp được sử dụng cho tốc độ quay của bánh xe (xem Hình 4.8). Như vậy, các bước đi riêng lẻ được xác định từ tốc độ quay của bánh xe trong trường hợp được đẩy đi liên tục.
Hình 4.8 Chuyển động có chu kỳ của bánh xe trong q trình đẩy đi liên tục
Thuật toán phát hiện đỉnh [110] được sử dụng để phát hiện ra các điểm cực tiểu. Trong đó, chỉ những điểm cực tiểu rõ ràng tại thời điểm 𝑘𝑗 thỏa mãn điều kiện sau:
𝐿𝑙 ≤ ‖𝑟𝑏𝑁,𝑘𝑗− 𝑟𝑏𝑁,𝑘𝑗−1‖ ≤ 𝐿ℎ (4-43)
Trong đó:
- 𝐿𝑙 = 0,3 𝑚 và 𝐿ℎ = 0,8 𝑚 là ngưỡng trên và ngưỡng dưới của độ dài của một bước đi.
Trang 110
- 𝑘𝑗 và 𝑘𝑗−1 là chỉ số thời điểm rời rạc của điểm cực tiểu hiện tại và điểm cực tiểu trước đó. Các điểm tương ứng với các bước chân được đánh dấu bằng dấu ‘*’ màu đỏ trong Hình 4.8.
Hình 4.9 thể hiện thơng số bước đi được trích xuất trong q trình đẩy khung tập đi liên tục dọc theo hành lang dài 20 𝑚 gồm tốc độ bước (hình dưới) và độ dài bước (hình trên). Thơng số bước đi gồm khoảng cách, số bước, thời gian hoàn thành, độ dài một bước, chu kỳ bước và tốc độ bước của một người dùng sử dụng 4 cách đi khác nhau với khung tập đi được thể hiện trong Bảng 4.3.
Bảng 4.3 Kết quả thông số bước đi một người dùng khung tập đi đi thẳng 20 m Loại Loại chuyển động Lần thí nghiệm Khoảng cách (m) Sớ bước (bước) Thời gian (s) Độ dài bước (mm) Chu kỳ bước (s) Tốc độ bước (mm/s) Đẩy đi từng bước 1 19,95 42 146,54 476,71 3,49 136,92 2 19,94 41 148,9 489,08 3,63 135,01 3 19,89 46 146,84 435,41 3,19 136,7 4 19,9 48 152,76 417,77 3,18 131,09 5 19,99 48 168,38 418,16 3,51 119,28 Đẩy đi liên tục 1 19,78 32 22,76 620,72 0,71 933,58 2 19,85 35 27,17 575,15 0,78 776,97 3 19,81 34 26,06 589,86 0,77 797,96 4 19,63 33 26,54 587,94 0,8 798,23 5 19,85 33 25,19 614,45 0,76 837,86 Nhấc 2 chân sau 1 20,42 38 136,12 539,05 3,58 150,63 2 19,84 38 130,6 523,94 3,44 152,67 3 20,03 39 138,94 514,93 3,56 144,68 4 20,26 39 138,57 521,12 3,55 146,7 5 19,66 39 132,1 506,13 3,39 149,71 Nhấc hoàn toàn 1 20,32 35 136,78 582,06 3,91 150,19 2 20,26 35 135,58 581,12 3,87 151,33 3 20,04 34 138,31 591,25 4,07 146,12 4 20,23 33 135,1 615,19 4,09 151,16 5 20,62 34 140,76 608,57 4,14 147,67
Sai sớ trung bình 1%
Ngồi các thơng số bước đi đơn giản, cịn có thể truy xuất được quỹ đạo chuyển động và tư thế của khung tập đi trong quá trình sử dụng như trong Hình 4.10. Điều này khá hữu ích cho các bác sĩ hoặc chuyên gia đánh giá khả năng đi lại và tình trạng sức khỏe của người dùng.
Trang 111
Hình 4.9 Thơng số bước đi trong quá trình đẩy khung tập đi 20 m
Hình 4.10 Độ cao và tư thế của khung tập đi trong quá trình sử dụng 4.6 Thí nghiệm đánh giá hoạt động của thuật tốn 4.6 Thí nghiệm đánh giá hoạt động của thuật tốn
4.6.1 Hệ thống thí nghiệm
Hình 4.11 thể hiện hệ thống khung tập đi, có gắn mơ đun cảm biến IMU của hãng Xsens (MTi-1) và 2 encoder có độ phân giải là 1024 xung/vịng của hãng LS,
Trang 112
được chế tạo phục vụ thí nghiệm kiểm chứng kết quả của thuật tốn đề xuất. Trong