Xây dựng thuật toán định vị quán tính để ước lượng chuyển động cho khung tập đi có hai bánh trước

Chia sẻ: _ _ | Ngày: | Loại File: PDF | Số trang:6

Thêm vào BST

Báo xấu

17
lượt xem 2
download

Download Vui lòng tải xuống để xem tài liệu đầy đủ

Bài viết Xây dựng thuật toán định vị quán tính để ước lượng chuyển động cho khung tập đi có hai bánh trước đề xuất phương pháp xây dựng thuật toán định vị quán tính (INA) để ước lượng chuyển động cho khung tập đi có 2 bánh trước. Trên khung tập đi này có gắn 1 cảm biến quán tính (IMU) tại vị trí bất kỳ và 2 encoder gắn với trục của 2 bánh.

Chủ đề:

Bình luận(0) Đăng nhập để gửi bình luận!

Lưu

Nội dung Text: Xây dựng thuật toán định vị quán tính để ước lượng chuyển động cho khung tập đi có hai bánh trước

24 Phạm Duy Dưởng, Đoàn Quang Vinh, Phan Thị Hoài XÂY DỰNG THUẬT TOÁN ĐỊNH VỊ QUÁN TÍNH ĐỂ ƯỚC LƯỢNG CHUYỂN ĐỘNG CHO KHUNG TẬP ĐI CÓ HAI BÁNH TRƯỚC APPLYING AN INERTIAL NAVIGATION ALGORITHM TO MOVEMENT ESTIMATION OF TWO FRONT-WHEEL WALKERS Phạm Duy Dưởng1, Đoàn Quang Vinh2, Phan Thị Hoài3 1 Trường Đại học Sư phạm Kỹ thuật - Đại học Đà Nẵng; duyduongd2@gmail.com 2 Trường Đại học Bách khoa - Đại học Đà Nẵng; dqvinh@dut.udn.vn 3 Học viện An ninh Nhân dân; hoaihvan@gmail.com Tóm tắt - Việc ước lượng quỹ đạo chuyển động của khung tập đi Abstract - Walker’s movement estimation is very important in (walker) là rất cần thiết trong việc ước lượng các thông số bước đi cũng walking parameter estimation and health evaluation for walker như đánh giá tình trạng sức khỏe người sử dụng khung tập đi. Bài báo users. This paper proposes a method to apply an Inertial này đề xuất phương pháp xây dựng thuật toán định vị quán tính (INA) Navigation Algorithm (INA) to the movement estimation of a two để ước lượng chuyển động cho khung tập đi có 2 bánh trước. front-wheel walker to estimate the walker’s trajector. An IMU is Trên khung tập đi này có gắn 1 cảm biến quán tính (IMU) tại vị trí bất kỳ fixed with the frame of walker and two encoders are used to và 2 encoder gắn với trục của 2 bánh. Chúng tôi sử dụng các thông tin measure the rotation of walker’s wheels. The IMU’s data is used từ IMU để xây dựng mô hình cho bộ lọc Kalman dùng trong định vị quán in the model of the Kalman fillter used in the INA. The encoders’ tính. Thông tin từ các encoder được sử dụng như giá trị đo nhằm xây data is used as a measured value to derive measurement dựng các phương trình cập nhật cho bộ lọc Kalman. Chúng tôi đã tiến equations of the Kalman fillter. We have conducted an hành các thí nghiệm để đánh giá độ chính xác của thuật toán đề xuất. experiment to evalue the accuary of the proposed algorithm. The Các kết quả phân tích về định lượng thông qua thí nghiệm thực tế cho result of the experiment shows that the proposed algorithm is thấy thuật toán hoạt động đạt độ chính xác cao (>98%). good and gets high accuracy (> 98%). Từ khóa - Cảm biến quán tính; IMU; định vị quán tính; khung tập Key words - Inertial sensor; IMU; Inertial Navigation Algorithm; đi, bộ lọc Kalman walker; Kalman Fillter 1. Đặt vấn đề tập đi với 2 bánh trước có gắn một IMU và 2 encoder nhằm Khung tập đi là thiết bị rất phổ biến và cần thiết cho thực hiện các bài kiểm tra các thông số bước cho người dùng người bị hạn chế bởi khả năng đi lại như người già, người bị khung tập đi. Với hệ thống này, người bệnh có thể thực hiện thương ở chân... Trong đó, chuyển động của khung tập đi các bài tập và kiểm tra tại nhà mà không cần sự giám sát của phản ánh việc đi lại (thông số bước đi) cũng như tình trạng bác sĩ. Việc xác định các thông số bước đi được tác giả phân sức khỏe của người dùng. Do vậy, việc ước lượng chính xác tích từ quỹ đạo chuyển động của khung tập đi. chuyển động của khung tập đi trong quá trình sử dụng một Về nguyên tắc chúng ta có thể chỉ sử dụng một IMU để phần có thể giúp bác sĩ đánh giá được tình trạng sức khỏe ước lượng chuyển động trong 3D của khung tập đi thông cũng như tiến trình hồi phục chức năng của bệnh nhân [1, 2]. qua INA mà không cần đến sự hỗ trợ của encoder. Tuy Trên thế giới hiện nay có nhiều bài kiểm tra để đánh giá nhiên, nhược điểm lớn nhất của INA là sai số tích lũy theo các thông bước đi (ví dụ: 4-meters walk test [3], 50-foot thời gian do sử dụng nguyên lý tích phân. Do vậy, INA walk test [4], 30-seconds chair stand test [4] và the timed thường chỉ đạt độ chính xác cao cho các chuyển động ngắn up and go [5]). Trong đó, các thông số bước đi bao gồm độ hoặc phải chia các chuyển động dài thành các chuyển động dài bước, tốc độ bước, thời gian bước hoặc thời gian hoàn ngắn sử dụng các điểm có vận tốc bằng không hoặc phải thành một bài kiểm tra,… được sử dụng để đưa ra các gắn thêm các cảm biến khác để xây dựng các phương trình khuyến cáo cho bệnh nhân như khả năng bị té ngã đối với cập nhật quỹ đạo chuyển động cho INA. Trong trường hợp người bị rối loạn tiền đình, khả năng bị té ngã đối với người này, nhóm tác giả sử dụng các encoder để cập nhật quỹ đạo già [5]. Hơn nữa, ở tài liệu [3] gồm 54 trang nói về bài kiểm chuyển động cho khung tập đi trong trường hợp được đẩy tra 4 – Meters Walk Test, người ta đưa ra rất nhiều các tiêu đi liên tục trên mặt đất (thời gian chuyển động lớn) hoặc chí để khuyến cáo cho người dùng dựa vào tốc độ của bước được đẩy đi từng bước trên mặt đất (lúc này việc xác định đi. Cụ thể như sau: tốc độ bước bé hơn 0,6 m/s thì ở mức các điểm có vận tốc bằng không rất khó khăn). độ nguy hiểm, từ 0,6 m/s đến 1 m/s thì ở mức độ cảnh báo, Một nhược điểm của bài báo [6], tác giả đã xây dựng quỹ lớn hơn 1 m/s là bình thường; tốc độ bước bé hơn 0,67 m/s đạo của khung tập đi dựa trên việc kết hợp các khoảng thì phải cẩn thận và phải được chăm sóc, từ 0,67 m/s đến chuyển động riêng lẻ đó là các khoảng chuyển động liên 0,89 m/s thì có thể di chuyển và tham gia các hoạt động quan đến việc đẩy khung tập đi trên mặt đất (dùng các trong nhà, từ 0,89 m/s đến 1,11 m/s thì có thể bưng bê các encoder để tính quỹ đạo) và các khoảng chuyển động liên vật nhẹ và tham gia các công việc nhẹ trong sân vườn, từ quan đến việc nhấc khung tập đi lên khỏi mặt đất (dùng IMU 1,11 m/s đến 1,33 m/s thì có thể đi cầu thang,… Tuy nhiên, để ước lượng chuyển động). Mặt dù, kết quả xác định quỹ các bài kiểm tra trên thế giới hiện nay mới chỉ thực hiện đạo chuyển động khá chính xác nhưng đây không phải là cho người có khả năng đi lại được mà không cần công cụ cách thường dùng khi sử dụng bộ lọc Kalman trong định vị hỗ trợ dưới sự quan sát, đánh giá của bác sĩ. quán tính. Khi áp dụng bộ lọc Kalman trong định vị quán Trong bài báo [6], tác giả đã đề xuất một hệ thống khung tính, tín hiệu từ IMU được xem là mô hình của bộ lọc, các
TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ, ĐẠI HỌC ĐÀ NẴNG - VOL. 17, NO. 10.1, 2019 25 giá trị đo của thiết bị và cảm biến bên ngoài được sử dụng trường, trong khi phương x và y có thể được chọn ngẫu với vai trò giá trị đo của bộ lọc này và được sử dụng để xây nhiên sao cho 3 trục tọa độ vuông góc với nhau. Trên thực tế, dựng các phương trình cập nhật kết quả cho bộ lọc. Hơn nữa, để đơn giản trong tính toán, WCS được chọn trùng với BCS khi áp dụng cách này để ước lượng chuyển động chúng ta tại vị trí ban đầu của quá trình chuyển động. cần phải xây dựng thuật toán liên kết các chuyển động riêng Để phân biệt tọa độ đang xét trong hệ nào, bài báo này lẽ thành chuyển động tổng thể một cách phúc tạp. sử dụng chỉ số dưới cho BCS và ICS. Ví dụ: p (  p b hay Trong bài báo này, tác giả đề xuất xây dựng INA sử dụng bộ lọc Kalman để ước lượng quỹ đạo chuyển động của  p I ) được sử dụng để thể hiện vector p  R3 được biểu khung tập đi. Trong đó, IMU đảm nhiệm vai trò chính và diễn trong hệ tọa độ WCS (BCS hay ICS). xuyên suốt trong ước lượng quỹ đạo chuyển động cho cả chuyển động liên quan đến việc nhấc khung tập đi lên và đẩy Việc xác định mối quan hệ giữa IMU và khung tập đi khung tập đi trên mặt đất. Thông tin từ các encoder được sử (gồm vector tịnh tiến TbI  và ma trận quay QbI từ ICS  b dụng với vai trò là giá trị đo và dùng để cập nhật lại quỹ đạo cho bộ lọc Kalman trong trường hợp khung tập đi được đẩy sang BCS) được thể hiện trong [6]. đi trên mặt đất. Thuật toán đề xuất này cũng đạt được độ 2.2. Thuật toán định vị cho khung tập đi chính xác tương đương với thuật toán đã đề xuất trong [6] và Theo các tài liệu [7-9], các chuyển động của khung tập đi phù hợp với mục đích đo thông số bước đi của người dùng. trong quá trình sử dụng có thể được phân làm 2 loại chính là chuyển động liên quan đến việc đẩy khung tập đi trên mặt đất 2. Giải quyết vấn đề và chuyển động liên quan đến việc nhấc khung tập đi lên khỏi 2.1. Tổng quan về hệ thống mặt đất. Trong chuyển động liên quan đến việc đẩy khung tập IMU Board cảm biến đi trên mặt đất có thể phân làm 2 loại đó là đẩy khung đi liên quán tính TRIGER IN UART Dữ liệu tục và đẩy khung đi từng bước. Đối với chuyển động của Xung I/O UART khung liên quan đến việc nhấc khung tập đi lên có thể chia Dữ liệu MCU SPI MICRO SD CARD làm 2 loại, chính đó là chỉ nhấc 2 chân sau lên và nhấc hoàn INT 0 INT 1 encoder encoder toàn khung tập đi lên khỏi mặt đất. Thuật toán để phát hiện và trái phải phân loại chuyển động đã được đề cập trong bài báo [6]. Thuật toán định vị quán tính cơ bản Mô đun encoder sử dụng bộ lọc Kalman (Mục 2.3) Dây dữ liệu Encoder Đứng yên Chuyển động Đứng yên Nhất Đẩy khung Hình 1. Tổng quan hệ thống đề xuất khung lên trên mặt đất Hệ thống đề xuất được thể hiện trong Hình 1 bao gồm 01 IMU (Mti-1, hãng Xsens) và 02 encoder (loại 1024 xung/vòng) được gắn vào một một khung tập đi. Trong đó, Cập nhật vận Cập nhật vận tốc tại điểm Cập nhật vị trí tốc tại điểm IMU có thể được gắn tại 1 vị trí bất kỳ trên khung tập đi dừng (Mục 2.5) dừng trong khi mỗi encoder được gắn vào và đo độ dịch chuyển (Mục 2.4) (Mục 2.4) của mỗi bánh khung tập đi. IMU bao gồm cảm biến gia tốc và cảm biến vận tốc góc theo 3 trục với tần số lấy mẫu là Hình 2. Thuật toán định vị cho khung tập đi 100 Hz. Một bản mạch được thiết kế để thu thập và đồng Thuật toán định vị cho khung tập đi sử dụng INA cơ bản bộ dữ liệu của IMU và các encoder. Trong đó, Arduino Uno dùng bộ lọc Kalman (xem Mục 2.3) xuyên suốt trong quá R3 được sử dụng để đọc dữ liệu từ các cảm biến và lưu dữ trình sử dụng khung tập đi (xem Hình 2). Trong đó, thông liệu vào thẻ nhớ Micro SD. tin từ IMU gồm gia tốc tịnh tiến và vận tốc góc được sử dụng Trong bài báo này, nhóm tác giả sử dụng hệ trục tọa độ trong mô hình của bộ lọc Kalman. Nhiệm vụ còn lại là cần gắn liền với IMU (ICS – IMU Coordinate System), hệ trục tọa phải xây dựng các phương trình cập nhật các giá trị đo cho độ gắn liền với khung tập đi (BCS – Body Coordinate System) bộ lọc Kalman. Như có thể thấy trong Hình 3, tại các khoảng và hệ trục tọa độ toàn cầu (WCS – World Coordinate System). thời gian đứng yên nhóm tác giả sử dụng phương trình cập Trong đó, ICS gắn liền với hệ trục tọa độ vật lý của IMU. Gốc nhật vận tốc để cập nhật quỹ đạo cho khung tập đi (Mục 2.4). tọa độ của BCS là trung điểm của đoạn thẳng nối 2 điểm tiếp Tại các khoảng thời gian khung tập đi chuyển động, nếu xúc của 2 bánh khung tập đi với mặt đất. Phương x của BCS chuyển động này liên quan đến việc đẩy khung tập đi trên trùng với hướng thẳng tới trước của khung tập đi trong khi mặt đất thì thông tin từ các encoder được sử dụng để cập nhật phương y của BCS nằm trên đường nối 2 điểm tiếp xúc giữa vị trí (xem Mục 2.5) cho khung tập đi. 2 bánh với mặt đất và có phương hướng từ bánh phải sang 2.3. Thuật toán INA cơ bản sử dụng bộ lọc Kalman bánh trái. Phương z của BCS hướng thẳng lên trên khi khung Trong phần này, nhóm tác giả trình bày về INA và việc tập đi được đẩy đi trên mặt đất. WCS được chọn sao cho ứng dụng bộ lọc Kalman vào bài toán định vị quán tính để ước phương z hướng lên trên trùng với phương gia tốc trọng lượng quỹ đạo chuyển động của khung tập đi sử dụng IMU.
26 Phạm Duy Dưởng, Đoàn Quang Vinh, Phan Thị Hoài Đặt v R 3 và r R 3 là vận tốc và vị trí của IMU trong [14] như sau: trong WCS. Đặt Q ( q )  R 33 là ma trận quay từ WCS sang x (t ) = A (t ) x (t ) + w (t ) (5) ICS tương ứng với quaternion [10] q trong đó: R3 .   − y g  1  Quarernion q , vận tốc v và vị trí r của thiết bị liên    − I3 0 0 0 2 quan với nhau qua công thức [11]:    0 0 0 0 0  0 − x − y − z  A (t ) =    0 0 0 I3 0   1 x 0 z − y   −2QT ( qˆ )  ya  q=  0 0 0 0  x  q 2  y − z 0    0 0 0 0 0    z  y − x 0    1  v = QT ( q )  a I (1)  − 2 vg    r=v  wbg  w (t ) =   Trong đó,  =  x y  z  là vận tốc góc của ICS  0   −QT ( qˆ ) v  trong WCS và  a I  R 3 là gia tốc tịnh tiến trong ICS.  a   wba  Giá trị đầu ra của cảm biến vận tốc góc ( y g  R 3 ) và Trong đó,  a   R33 là ma trận đối xứng lệch tương ứng cảm biến gia tốc ( ya  R ) được cho bởi công thức 3 với vector a  R31 . Nhiễu wbg và wba đại diện cho sự thay y g =  + vg + bg (2) đổi nhỏ của thành phần nhiễu chậm thay đổi tương ứng. ya =  a I + Q ( q ) g + va + ba 2.4. Xây dựng phương trình cập nhật vận tốc tại các Trong đó, g  R3 là vector gia tốc trọng trường trong WCS. “điểm dừng” Trong quá trình sử dụng khung tập đi, có những khoảng bg  R 3 và ba  R3 là thành phần nhiễu thay đổi chậm của thời gian khung tập đi đứng yên trên mặt đất lúc này vận cảm biến vận tốc góc và cảm biến gia tốc. v g và va là thành tốc của khung tập đi có thể xem bằng 0 và độ cao của khung tập đi cũng được xem là bằng 0. Để tiện cho việc trình bày phần nhiễu trắng của cảm biến vận tốc góc và cảm biến gia tốc. trong bài báo, khoảng thời gian này gọi tắc là “điểm dừng”. Thuật toán tích phân để lấy tích phân công thức (1) Trong [15], các “điểm dừng” này có thể được phát hiện (thay  a I bằng ya − Q ( q ) g và thay  bằng y g ) được trực tiếp bởi cảm biến vận tốc Doppler. Tuy nhiên, chúng thể hiện trong [12]. Đặt qˆ , rˆ và vˆ là giá trị tích phân của ta có thể phát hiện các “điểm dừng” gián tiếp bằng cách sử quaternion, vị trí và vận tốc của IMU. dụng thuật toán phát hiện vận tốc bằng 0 [16, 17]. Do các cảm biến luôn có thành phần nhiễu, nên các giá Trong bài báo này, nhóm tác giả sử dụng một thuật toán phát hiện “điểm dừng” đơn giản. Nếu những điều kiện dưới trị tích phân đó chưa phải là giá trị đúng. Gọi q  R3 , đây được thỏa mãn thì thời điểm gián đoạn m phải thuộc r  R3 và v  R 3 là sai số của quaternion, vị trí và vận tốc “điểm dừng” của IMU. Lúc này ta có: Ng Ng y g ,i  Bg , m− i  m+ q = [031 I 3 ] qˆ*  q ( ) 2 Na 2 Na (6) r = r − rˆ (3) ya ,i − ya,i −1  Ba , m− i  m+ 2 2 v = v − vˆ Trong đó, N g và N a là các số nguyên. Bg và Ba là các giá Trong đó,  là phép nhân quaternion và q* là quaternion liên hợp của q . Phương trình (3) biểu diễn 3 thành phần của sai trị ngưỡng đặt trước. N g và N a liên quan đến khoảng thời số quaternion q  R3 (thay vì dùng 4 thành phần) theo [13]. gian tối thiểu mà khung tập đi được xem là đứng yên lúc này tín hiệu các cảm biến gia tốc và vận tốc góc gần như Các biến trạng thái được sử dụng trong bộ lọc Kalman không thay đổi giá trị đo được. Khoảng thời gian này được như sau: tác giả nhận định là phải lớn hơn 0,4 giây. Do vậy, tác giả q  chọn N g = N a = 40 khi chu kỳ lấy mẫu của cảm biến là b   g 0,01 giây. Bg và Ba liên quan đến giá trị của vận tốc góc x =  r   R15 (4) và độ thay đổi của gia tốc của khung tập đi lúc đứng yên.   v  Trên thực tế, những giá trị này thường rất nhỏ và gần như b  bằng không. Trong bài báo này, tác giả chọn Bg = 0,1  a Phương trình trạng thái cho bộ lọc Kalman được dẫn (tương ứng với vận tốc góc 0,1 rad/s) và Ba = 0,1 (tương
TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ, ĐẠI HỌC ĐÀ NẴNG - VOL. 17, NO. 10.1, 2019 27 ứng với độ thay đổi gia tốc là 0,1 m/s ). 2 này vector tịnh tiến chuyển BCS tại thời điểm i − 1 sang Tại “điểm dừng” này, ta có: BCS tại i xét trong hệ tọa độ BCS tại thời điểm i − 1 là: v = 031 Tbb,i,i−1  = E 0 0 (11) (7)  b,i −1  t ,i r ( 3) = 0 Ma trận quay từ BCS tại i − 1 sang BCS tại thời điểm i là: Thay (3) vào (7) ta có phương trình cập nhật vận tốc và vị trí cho bộ lọc Kalman như sau 1 0 0   zv   H v   vv  Qbb,,ii−1 =  0 1 0  (12)  z  =  H  x + v  (8)  0 0 1   r  r  r Trong đó, Trong trường hợp nếu Et ,i  E p,i thì ta có thể xem như zv = 031 − vˆ  R 31 khung tập đi quay quanh một đường tròn tâm C nào đó tại zr = 0 − rˆ ( 3) thời điểm i (xem Hình 3). Điều này nghĩa là bánh trái, bánh phải và gốc tọa độ O của BCS đều chuyển động trên H v = 039 I 3 033  các cung tròn đồng tâm C nhưng khác bán kính. H r = 018 I1 016  yw Với, vv và vr là nhiễu thể hiện việc vận tốc và độ cao của xb,i xb,i −1  khung tập đi gần bằng không chứ không hoàn toàn bằng không. 2.5. Xây dựng phương trình cập nhật vị trí sử dụng thông xw y b ,i Oi tin từ encoder Ep,i Trong quá trình khung tập đi được đẩy đi trên mặt đất xb,i −1 (phẳng và nằm ngang), chúng ta hoàn toàn có thể tính Et,i C  yb,i −1 được góc quay quanh trục đứng của khung tập đi. Góc quay này được sử dụng để dẫn ra phương trình cập nhật Oi −1 cho bộ lọc Kalman. Bánh trái Bánh phải Trước tiên, chúng ta cần tính góc quay quanh trục zb Hình 3. Chuyển động quay của khung tập đi sử dụng thông tin từ các encoder. Đặt Et ,i và E p,i là quãng Do khoảng cách giữa 2 bánh xe ( D = 610 mm ) là rất đường di chuyển của bánh trái và bánh phải trong khoảng lớn so với khoảng cách di chuyển của 2 bánh xe trong một thời gian lấy mẫu [( i − 1) T , iT ]. Quãng đường này được thời gian lấy mẫu ( Et ,i , E p,i  5 mm ) nên góc quay của tính bằng các xung encoder như sau khung tập đi quanh trục zb có thể được tính như sau: Et ,i = t pt ,i Et ,i − E p,i (9) i = (13) E p ,i =  p p p ,i D Trong đó, pt ,i và p p,i là số xung encoder trái và phải trong Ma trận quay quanh trục zb từ BCS tại i − 1 sang BCS thời gian lấy mẫu tại thời điểm i . t và  p là các hệ số tại i được tính như sau: của encoder liên quan đến số xung encoder/vòng quay và cos  i − sin  i 0 bán kính bánh xe trái và phải. Mặc dù, các hệ số t và  p Qbb,,ii−1 =  sin  i cos  i 0  (14) có thể tính được nhưng sẽ rất khó để đạt kết quả chính xác  0 0 1  do còn liên quan đến độ nghiêng của bánh và việc đo bán Lúc này, vector tịnh tiến chuyển BCS tại thời điểm kính bánh xe cũng khó chính xác. i − 1 sang BCS tại thời điểm i xét trong hệ tọa độ BCS tại Trong bài báo này nhóm tác giả tiến hành một thí thời điểm i − 1 là: nghiệm đơn giản để hiệu chỉnh các hệ số pt ,i và p p,i bằng E p,i + Et ,i b,i Tbb,i,i−1  = Qb,i −1 (:,1) (15) cách đẩy khung tập đi đi thẳng về trước. Gọi L là quảng  b,i −1 2 đường đi của khung tập đi, ta có: Trong đó, ký hiệu a (:,i ) là ma trận cột thứ i của ma trận a . L t = Vector tịnh tiến khi xét trong hệ tọa độ WCS được xác  N p i =1 t ,i định như sau (10) E p,i + Et ,i p = L Tbb,i,i−1 = Qbw,i −1Qbb,,ii−1  (:,1) (16)    N 2 p p ,i Thay Qbw,i −1 = QT ( qˆi −1 ) QbI vào phương trình (16) ta có: i =1 với N là tổng số dữ liệu của quá trình thí nghiệm. E p,i + Et ,i Khi Et ,i = E p,i thì khung tập đi chuyển động thẳng, lúc Tbb,i,i−1 = QT ( qˆi −1 ) QbI Qbb,,ii−1  (:,1) (17) 2  
28 Phạm Duy Dưởng, Đoàn Quang Vinh, Phan Thị Hoài Như vậy vị trí của khung tập đi (tâm của BCS) tại thời Mỗi người thực hiện 20 lượt đi, trong đó có 5 lượt đi với điểm i xét trong WCS được tính thông qua vị trí của khung chuyển động đẩy khung đi liên tục (kết quả được thể hiện tập đi tại thời điểm i − 1 như sau: trong bảng Bảng 1), 5 lượt đi với chuyển động đẩy khung đi từng bước (kết quả được thể hiện trong bảng Bảng 2), 5 lượt rb,i = rb ,i −1 + Tbb,i,i−1 (18) đi với chuyển động nhấc 2 chân sau của khung lên và đẩy tới Chúng ta có thể tính vị trí của khung tập đi thông qua trước (kết quả được thể hiện trong bảng Bảng 3), 5 lượt đi vị trí của IMU và ngược lại như sau: cuối cùng với chuyển động nhấc hoàn toàn khung lên và đặt tới trước (kết quả được thể hiện trong bảng Bảng 4). rb,i = ri − Qbw,i TbI   b Mô đun cảm biến quán tính (19) ri = rb,i + Qbw,i TbI   b Từ phương trình (19) ta có: T ri = rb,i + Qbw,i −1 Qbb,,ii−1  TbI  (20)    b Do Qbw,i −1 = QT ( qˆi −1 ) QbI ta có: Mô đun encoder ( ) T ri = rb,i + Q T qˆi −1 QbI Qbb,,ii−1  TbI  (21)    b Thay phương trình (18) vào phương trình (21) ta có: ri = rb,i −1 + Tbb,i,i−1 + QT ( qˆi −1 ) QbI Qbb,,ii−1  TbI  T (22)    b Áp dụng rb,i = ri − Qbw,i TbI  ta có:   b Hình 4. Hệ thống khung tập đi dùng trong thí nghiệm rb,i −1 = ri −1 − Qbw,i −1 TbI  (23) Bảng 1. Độ chính xác việc ước lượng khoảng cách di chuyển  b với chuyển động đẩy đi liên tục Thay Qbw,i −1 = QT ( qˆi −1 ) QbI vào phương trình (23) ta có: Lần thực Độ chính xác (%) hiện thí Người Người Người Người Người rb,i −1 = ri −1 − QT ( qˆi −1 ) QbI TbI  (24) nghiệm dùng 1  b dùng 2 dùng 3 dùng 4 dùng 5 1 96,9 97,6 99,9 98,7 100,0 Như vậy phương trình (22) có thể viết lại như sau: 2 98,2 98,0 99,6 98,3 99,8 ri = ri −1 − QT ( qˆi −1 ) QbI TbI   b 3 98,0 98,0 99,6 100,0 97,8 (25) 4 97,6 98,1 97,8 98,7 97,8 + Tbb,i,i−1 + QT ( qˆi −1 ) QbI Qbb,,ii−1  TbI  T    b 5 98,3 98,1 98,3 99,5 98,6 Trung Trong đó, các thành phần bên vế phải đã tính được 98,5 bình trước thời điểm i . Bảng 2. Độ chính xác việc ước lượng khoảng cách di chuyển Cuối cùng ta được phương trình cập nhật vị trí cho bộ với chuyển động đẩy đi từng bước lọc Kalman sử dụng thông tin từ các encoder như sau: Lần thực Độ chính xác (%) zq = Hq x + vq (26) hiện thí Người Người Người Người Người nghiệm dùng 1 dùng 2 dùng 3 dùng 4 dùng 5 với zq = ri − rˆi 1 98,9 97,6 96,6 99,1 99,7 2 99,1 98,7 98,2 99,3 99,8 H q = 039 I3 033   R315 3 98,3 99,1 98,1 99,0 99,5 4 98,9 98,1 98,9 99,4 99,4 Trong đó, nhiễu vq  R3 đại diện cho sai số của việc tính 5 98,6 98,9 98,8 99,8 98,7 Trung vị trí từ các encoder. bình 98,8 3. Kết quả nghiên cứu và bình luận Bảng 3. Độ chính xác việc ước lượng khoảng cách di với chuyển động nhấc 2 chân sau khung tập đi Hệ thống khung tập đi thực nghiệm để kiểm chứng độ Lần thực Độ chính xác (%) chính xác cho thuật toán đề xuất được thể hiện như trong hiện thí Người Người Người Người Người Hình 4. IMU được sử dụng là của hãng Xsens (Mti-1). Tần nghiệm dùng 1 dùng 2 dùng 3 dùng 4 dùng 5 số hoạt động của Mti-1 là 100 Hz và độ phân giải của 1 98,2 97,7 99,5 97,6 99,2 encoder là 1024 xung/vòng. 2 98,9 96,5 99,5 97,7 99,8 Một thí nghiệm được tiến hành với 5 người dùng có khả 3 97,3 95,9 99,6 99,6 99,4 năng đi lại bình thường sử dụng khung tập đi và đi dọc một 4 97,2 96,7 99,9 100,0 99,9 hành lang (bằng phẳng và không có vật cản) dài 20 m. 5 99,4 95,3 99,9 99,2 98,6 Những người dùng này đã được hướng dẫn cách sử dụng Trung khung tập đi giống người già hoặc người cần hỗ trợ đi lại. 98,5 bình
TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ, ĐẠI HỌC ĐÀ NẴNG - VOL. 17, NO. 10.1, 2019 29 INA cho kết quả là quỹ đạo 3D của khung tập đi trong giả đã xây dựng phương trình cập nhật vị trí cho khung tập quá trình sử dụng. Tuy nhiên, điều này rất khó để minh đi cho bộ lọc Kalman sử dụng thông tin từ các encoder. chứng độ chính xác. Để làm được điều này cần có hệ thống Trong phần thí nghiệm để minh chứng cho thuật toán, camera có độ chính xác cao và tốc độ hình ảnh lớn (ví dụ nhóm tác giả tiến hành làm thí nghiệm với 5 người đi dọc như hệ thống Optitrack 6 Flex 13). Hệ thống này có giá thành hành lang 20 m trong các trường hợp chuyển động liên rất cao và chỉ thích hợp trong không gian hẹp. Hơn nữa, với quan đến việc đẩy khung tập đi trên mặt đất phẳng và nhấc mục đích là xác định thông số bước đi (độ dài bước, tốc độ khung lên. Kết quả thực nghiệm cho thấy, độ chính xác việc bước). Trong đó, tốc độ bước được tính từ độ dài bước và ước lượng khoảng cách di chuyển của thuật toán đề xuất thời gian bước. Do vậy trong bài báo này, độ chính xác của cho độ chính xác là trên 98%. Đây là độ chính xác cao đối thuật toán đề xuất được kiểm nghiệm thông qua tổng độ dài với ứng dụng ước lượng thông số bước đi. Cụ thể là độ dài các bước đi. Tổng độ dài các bước đi chính là khoảng cách bước đi được ước lượng sẽ có sai số từ 0,6 đến 1,6 cm tùy mà INA đã ước lượng được trong suốt quá trình đi dọc hành theo độ dài bước chân. lang này. Độ chính xác trong các Bảng 1-4 được tính dựa Lời cảm ơn: Bài báo này được tài trợ bởi Đề tài NCKH trên khoảng cách sˆ mà INA đã ước lượng được so với cấp bộ với mã số B2018.DNA.07 (KYTH-45). Chủ nhiệm khoảng cách thực tế người dùng di chuyển s = 20 m. đề tài: PGS. TS Đoàn Quang Vinh, Trường Đại học Bách ˆ−s Như vậy, độ chính xác của thuật toán được tính là s . khoa - Đại học Đà Nẵng. s Theo các bảng này, độ chính xác phần trăm của việc TÀI LIỆU THAM KHẢO ước lượng khoảng cách di chuyển so với khoảng cách thực [1] Peel NM., Kuys SS, and Klein K, “Gait speed as a measure in geriatric tế (20 m) trung bình là 98,8% cho chuyển động đẩy đi từng assessment in clinical settings: A systematic review”, The Journals of bước, 98,5% cho chuyển động đẩy đi liên tục, 98,5% cho Gerontology Series A: Biological Sciences and Medical Sciences, 2012. chuyển động nhấc 2 chân sau của khung tập đi và 98,3% [2] Barthuly AM, Bohannon RW, Gorack W “Gait speed is a responsive cho chuyển động nhấc hoàn toàn khung tập đi. Rõ ràng ta measure of physical performance for patients undergoing short-term rehabilitation”, Gait Posture, 2012. có thể thấy, khoảng cách di chuyển của khung tập đi chính [3] https://www.physiotherapyalberta.ca/course_materials/gait_speed_ là tổng số của độ dài các bước chân của người dùng. nov_2015.pdf Do vậy, độ chính xác ước lượng khoảng cách di chuyển [4] Jackson A B, “Outcome measures for gait and ambulation in the spinal cũng có thể suy ra được độ chính xác của độ dài các bước cord injury population” The journal of spinal cord medicine, 2008. [5] Podsiadlo D and Richardson S, “The timed up & go: a test of basic đi. Giả sử, mỗi bước đi của người dùng có thể dao động từ functional mobility for frail elderly persons” Journal of the 30 cm đến 80 cm thì sai số dao động từ 0,6 cm đến 1,6 cm American geriatrics Society, 1991. (đối với độ chính xác là 98%). Như vậy, sai số này là rất [6] Duong PD, Toan DH, Suh YS, “Walking Monitoring for Users of nhỏ trong ứng dụng ước lượng thông số bước đi. Standard and Front-Wheel Walkers”, IEEE Transactions on Instrumentation and Measurement (2017). Bảng 4. Độ chính xác việc ước lượng khoảng cách di chuyển [7] Using a walker, MedlinePlus, 2015. Link: với chuyển động nhấc hoàn toàn khung tập đi https://www.nlm.nih.gov/medlineplus/ency/patientinstructions/000342.ht Lần thực Độ chính xác (%) [8] W. R. Frontera, J. K. Silver, và T. D. R. Jr“Essentials of Physical hiện thí Người Medicine and Rehabilitation”, 2nd ed. Philadelphica, PA: Elsevier. 2008. Người Người Người Người nghiệm dùng 1 dùng 2 dùng 3 dùng 4 dùng 5 [9] How to use a walker safely, BJC HealthCare. Link:http://www.bjchomecare.org/Homemedicalequipment/Walkin 1 97,6 97,9 99,0 99,4 96,2 gaids/Howtouseawalkersafely.aspx 2 98,5 97,5 98,0 99,8 98,6 [10] Kuipers JB, “Quaternions and Rotation Sequences: A Primer with 3 99,9 98,5 97,6 99,0 97,3 Applications to Orbits Aerospace, and Virtual Reality” Princeton University Press: Princeton, NJ, USA, 1999. 4 99,1 97,0 99,2 97,4 97,2 [11] Nam CNK, Kang HJ, Suh YS, “Golf Swing Motion Tracking Using 5 99,1 99,4 98,7 96,6 99,1 Inertial Sensors and a Stereo Camera”. IEEE Trans. Instrum. Meas, 2014. Trung [12] Savage PG, “Strapdown Inertial Navigation Integration Algorithm 98,3 Design Part 1: Attitude Algorithms”. J. Guid. Control Dyn, 1998. bình [13] Markley FL, “Multiplicative vs. Additive Filtering for Spacecraft 4. Kết luận Attitude Determination” In Proceedings of the 6th Cranfield Conference on Dynamics and Control of Systems and Structures in Bài báo đã đề xuất xây dựng INA để ước lượng chuyển Space, Riomaggiore, Italy, 2004. động cho khung tập đi có 2 bánh trước. Trong đó nhóm tác [14] Suh YS, Park S. “Pedestrian Inertial Navigation with Gait Phase Detection Assisted Zero Velocity Updating”. In Proceedings of the giả đã xây dựng các phương trình cập nhật quỹ đạo chuyển 4th International Conference on Autonomous Robots and Agents, động của khung tập đi sử dụng thông tin từ các encoder. Wellington, New Zealand, 2009. Tại thời điểm khung tập đi đứng yên, vận tốc và độ cao [15] Hawkinson W, “Geospatial Location, Accountability, and Navigation System For Emergency Responders” In Proceedings of the Position của khung tập đi có thể xem như bằng 0. Nhóm tác giả sử Location and Navigation Symposium, Myrtle Beach, SC, 2012. dụng tính chất này để xây dựng phương trình cập nhật thứ [16] Park S.K, Suh YS, “A Zero Velocity Detection Algorithm Using nhất để cập nhật vận tốc cho bộ lọc Kalman Inertial Sensors for Pedestrian Navigation Systems”. Sensors, 2010 Trong quá trình đẩy khung tập đi đi trên mặt đất bằng [17] Skog I, Nilsson JO, Handel P, “Evaluation of zero velocity detectors for foot-mounted inertial navigation systems”. In Proceedings of the phẳng, chúng ta cũng có thể sử dụng các thông tin từ International Conference on Indoor Positioning and Indoor encoder để định vị trí của khung tập đi. Do vậy, nhóm tác Navigation, Zurich, Switzerland, 2010. (BBT nhận bài: 20/6/2019, hoàn tất thủ tục phản biện: 09/10/2019)