Đầu
khung Gia tớc Vận tớc
góc Xung encoder 1 Xung encoder 2 Số thứ tự dữ liệu
Trang 93
4.2.3 Thuật toán ước lượng mối quan hệ giữa hệ toạđộ ICS và BCS
Đặt vectơ 𝑇𝑏𝐼 ∈ 𝑅3và ma trận 𝐶𝑏𝐼 ∈ 𝑅3×3 là vectơ tịnh tuyến và ma trận quay chuyển một vectơ biểu diễn trong hệ toạ độ BCS sang ICS. Lúc này, 𝑇𝑏𝐼 là vị trí của cảm biến IMU trong hệ toạ độ BCS, có thể đo được bằng thước. Do các vectơ cột trong ma trận 𝐶𝑏𝐼 chính là các trục của hệ toạ độ BCS trong ICS, nên 𝐶𝑏𝐼 = [[𝑥𝑏]𝐼 [yb]𝐼 [zb]𝐼]. Do cảm biến IMU có kích thước rất nhỏ nên khơng thể đo chính xác phương hướng các trục bằng thước nên cần đề xuất phương pháp ước lượng 𝐶𝑏𝐼.
Theo cách định nghĩa của hệ toạ độ BCS thì 𝑧𝑏 thẳng đứng và hướng lên trên trùng phương với gia tốc trọng trường trong trường hợp khung tập đi di chuyển trên mặt đất nằm ngang. Tại thời điểm ban đầu thì khung tập đi đang đứng yên nên giá trị gia tốc 𝑦𝑎 mà IMU đo được chính là gia tốc trọng trường và có hướng từ dưới lên trên. Như vậy ta có:
[𝑧𝑏]𝐼 = 𝑦a,1
‖𝑦a,1‖ (4-1)
Thí nghiệm để ước lượng [𝑥𝑏]𝐼 được thực hiện bằng cách đẩy khung tập đi theo đường thẳng hướng về trước trên sàn nhà một khoảng cách ngắn (khoảng 1 𝑚). Lúc này, hướng di chuyển của khung tập đi trong hệ toạ độ BCS là [𝑑]𝑏 được tính tốn sử dụng thơng tin từ 2 encoder như trong Hình 4.6 và cơng thức (4-23). Ngồi ra hướng di chuyển này trong hệ toạ độ ICS là [𝑑]𝐼 cũng được ước lượng sử dụng cảm biến IMU bằng thuật toán của hệ thống INS trong Chương 2.
Theo quy trình của thí nghiệm thì trục [𝑥𝑏]𝑏 là trùng hướng với [𝑑]𝑏, tuy nhiên trong quá trình thực hiện thí nghiệm [𝑑]𝑏 khơng hồn tồn trùng với trục [𝑥𝑏]𝑏.Gọi 𝜃 là góc giữa [𝑑]𝑏 và [𝑥𝑏]𝑏 và được xác định như sau
cos 𝜃 =‖[𝑑]𝑏𝑑𝑥 ‖ sin 𝜃 =‖[𝑑]𝑑𝑦𝑏‖
(4-2)
Trang 94
Việc xác định [𝑥𝑏]𝐼 được xác định bằng cách quay [𝑑]𝐼 quanh trục [𝑧𝑏]𝐼 một góc đúng bằng góc 𝜃 giữa [𝑑]𝑏 và [𝑥𝑏]𝑏. Phép quay vectơ [𝑑]𝐼 quay trục [𝑧𝑏]𝐼 để tìm [𝑥𝑏]𝐼 được thực hiện thơng qua 5 bước liên tiếp bao gồm việc quay quanh trục 𝑥𝐼 để đưa [𝑑]𝐼 về mặt phẳng 𝑂𝐼𝑥𝐼𝑧𝐼, quay quanh trục 𝑦𝐼 để đưa [𝑑]𝐼 về trùng với 𝑧𝐼, quay quanh trục 𝑧𝐼 một góc 𝜃, quay ngược lại theo trục 𝑦𝐼, quay ngược lại theo trục 𝑥𝐼. Kết quả của việc này được tóm tắt như sau
[𝑥𝑏]𝐼 = 𝐶(𝑥𝐼, −𝛼)𝐶(𝑦𝐼, −𝛽)𝐶(𝑧𝐼, 𝜃)𝐶(𝑦𝐼, 𝛽)𝐶(𝑥𝐼, 𝛼)[𝑑]𝐼 (4-3) trong đó: - 𝐶(𝑥𝐼, 𝛼) = [10 cos 𝛼 − sin 𝛼0 0 0 sin 𝛼 cos 𝛼 ] - 𝐶(𝑦𝐼, 𝛽) = [ cos 𝛽 0 sin 𝛽0 1 0 −sin 𝛽 0 cos 𝛽]
- 𝐶(𝑧𝐼, 𝜃) = [cos 𝜃 −sin 𝜃 0sin 𝜃 cos 𝜃 0
0 0 1] - 𝑙 = √𝑧𝑏𝑦2 + 𝑧𝑏𝑧2 - cos 𝛼 =𝑧𝑏𝑧 𝑙 - sin 𝛼 = 𝑧𝑏𝑦 𝑑 - cos 𝛽 = 𝑙 - sin 𝛽 = −𝑧𝑏𝑥
Theo định nghĩa về ma trận quay thì 𝐶𝑏𝐼 được tính như sau
𝐶𝑏𝐼 = [[𝑥𝑏]𝐼 [𝑧𝑏]𝐼× [𝑥𝑏]𝐼 [𝑧𝑏]𝐼] (4-4) Một thuật tốn để ước lượng nhanh mối quan hệ giữa cảm biến IMU và khung tập đi loại không bánh gồm ma trận quay và vectơ tịnh tiến cũng được tác giả luận án đề xuất trong cơng bố [107].
Trang 95
4.3 Thuật tốn phát hiện và phân loại chuyển động của khung tập đi
4.3.1 Định nghĩa chuyển động của khung tập đi
Một số hướng dẫn sử dụng khung tập đi được thể hiện trong [108], [109]. Trong đó, chuyển động của khung tập đi có bánh trước được chia thành 5 loại bao gồm:
- Đẩy đi liên tục: chuyển động này liên quan đến việc người dùng đẩy khung tập đi và bước liên tục về phía trước. Lúc này 2 bánh của khung tập đi sẽ lăn liên tục trên sàn nhà. Đây là chuyển động phù hợp với người có 2 chân khỏe mạnh (xem Hình 4.3a).
- Đẩy đi từng bước: đây là chuyển động phù hợp cho người có đơi chân khơng khỏe hoặc bị đau ở một chân. Lúc này người dùng đẩy khung tập đi từng bước về phía trước và dừng lại ở mỗi bước (xem Hình 4.3b). Theo đó, 2 bánh của khung tập đi lăn từng đoạn trên sàn nhà.
- Nhấc 2 chân sau: đây là chuyển động tương tự chuyển động đẩy đi từng bước, tuy nhiên trong trường hợp này người dùng nhấc 2 chân sau của khung lên và đẩy tới trước (xem Hình 4.3c). Lúc này 2 bánh của khung tập đi lăn từng từng đoạn trên sàn nhà.
- Nhấc hoàn toàn: đây là chuyển động tương tự chuyển động nhấc 2 chân sau, tuy nhiên trong trường hợp này khung tập đi được nhấc hoàn toàn lên khỏi mặt đất trong mỗi chuyển động và đặt xuống đất ở cuối chuyển động (xem Hình 4.3d). Lúc này, 2 bánh của khung tập đi gần như khơng lăn trong q trình di chuyển.
Trang 96
(A)
(E)
Hướng lực tác động Chân yếu/bị thương Chân khoẻ mạnh Mặt sàn Khung nhìn từ trên Khung nhìn từ mặt bên (1) (2) (3) (B) (1) (2) (3) (4) (5) (C) (1) (2) (3) (4) (5) (D) (1) (2) (3) (4) (5) (1) (2) (3) (4) (5)
Đẩy đi từng bước Đẩy đi liên tục
Nhấc 2 chân sau
Nhấc hồn tồn
Đổi hướng
Hình 4.3 Định nghĩa các chuyển động của người dùng khi sử dụng khung tập đi- Đổi hướng: đây là chuyển động thường được sử dụng khi người dùng đổi - Đổ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.
Trang 97
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 tố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:
|𝑝𝑡,𝑖| ≥ 𝐵𝑝, 𝑘 −𝑁2𝑝 ≤ 𝑖 ≤ 𝑘 +𝑁2𝑝 |𝑝𝑝,𝑖| ≥ 𝐵𝑝, 𝑘 −𝑁𝑝2 ≤ 𝑖 ≤ 𝑘 +𝑁𝑝2
(4-5)
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 quá trình phân tích thực nghiệm luận án chọn 𝐵𝑝 = 𝐵𝑡 = 2 xung.
Trang 98
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: 𝑦𝑔𝑦,𝑖 = [0 1 0]𝐶𝐼𝑏𝑦𝑔,𝑖 (4-6) Vận tốc góc quay theo phương 𝑧𝑏 tại thời điểm 𝑖 của IMU được tính như sau:
𝑦𝑔𝑧,𝑖 = [0 0 1]𝐶𝐼𝑏𝑦𝑔,𝑖 (4-7) 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:.
‖𝑦𝑔𝑦,𝑖‖ ≥ 𝐵𝑦, 𝑘 −𝑁𝑦2 ≤ 𝑖 ≤ 𝑘 +𝑁2𝑦 (4-8) 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:
𝑦𝑔𝑧,𝑖 ≥ 𝐵𝑧, 𝑘 −𝑁2𝑧 ≤ 𝑖 ≤ 𝑘 +𝑁2𝑧 (4-9) 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,
Trang 99
- 𝑁𝑧 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
Chuyển động sử dụng encoder Chuyển động quanh y Đ T chuyển động > T bước (Tb) S
Đẩy đi liên tục Đ Đẩy đi từng bước S Nhấc 2 chân sau Đ Chuyển động quay quanh y Chuyển động quay quanh z Nhấc hoàn
toàn Quay đổi
hướng Chuyển động quay quanh z S Đ S Đ S S Khơng chuyển động Đ 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
Đ: Đúng
S: Sai
Bắt đầu
Kết thúc
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
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 q 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ó: