7. Bố cục chung của luận án
4.3 Thuật toán phát hiện và phân loại chuyển động của khung tập đ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: �� �� |��,𝑖| ≥ ��, � − 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 q trình phân tích thực nghiệm luận án chọn
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,
Bắt đầu
Chuyển động sử dụng encoder Đ: ĐúngS: Sai ĐS
Chuyển động quanh y Chuyển động quay quanh y SĐS
T chuyển động
> T bước (Tb) Chuyển động quay quanh z Chuyển động quay quanh z
Đ S Đ S Đ Đ S
Kết thúc
Không chuyển động Quay đổi hướng
Nhấc hoàn toàn Nhấc 2 chân sau
Đẩy đi từng bước Đẩy đi liên tục
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
- �� 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.