Xem mẫu
- 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 33
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 R33 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 R31 . 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 = [031 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 = 031 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 = 031 − vˆ R 31 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 = 039 I 3 033
các cung tròn đồng tâm C nhưng khác bán kính.
H r = 018 I1 016
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 = 039 I3 033 R315 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)
nguon tai.lieu . vn
