Xây dựng bộ lọc kalman mở rộng cho thuật toán định vị quán tính

Bài viết Xây dựng bộ lọc kalman mở rộng cho thuật toán định vị quán tính đề xuất việc xây dựng INA sử dụng bộ lọc Kalman mở rộng để nâng cao độ chính xác của việc ước lượng quỹ đạo chuyển động của đối tượng.

ISSN 1859-1531 - TẠP CHÍ KHOA HỌC VÀ CƠNG NGHỆ ĐẠI HỌC ĐÀ NẴNG, VOL 17, NO 9, 2019 45 XÂY DỰNG BỘ LỌC KALMAN MỞ RỘNG CHO THUẬT TOÁN ĐỊNH VỊ QUÁN TÍNH IMPLEMENT AN EXTENDED KALMAN FILTER FOR INERTIAL NAVIGATION ALGORITHM Phạm Duy Dưởng1, Đoàn Quang Vinh2 Trường Đại học Sư phạm Kỹ thuật - Đại học Đà Nẵng; duyduongd2@gmail.com Trường Đại học Bách khoa - Đại học Đà Nẵng; dqvinh@dut.udn.vn Tóm tắt - Cảm biến quán tính (IMU) ngày ứng dụng rộng rãi ước lượng chuyển động sử dụng thuật toán định vị quán tính (INA) INA dựa nguyên tắc kết hợp tích phân hai lớp gia tốc tích phân vận tốc góc Tuy nhiên, việc sử dụng nguyên lý tích phân làm cho sai số ước lượng tích lũy nhanh theo thời gian nhiễu thành phần cảm biến Trong báo này, đề xuất việc xây dựng INA sử dụng lọc Kalman mở rộng để nâng cao độ xác việc ước lượng quỹ đạo chuyển động đối tượng Ngồi ra, báo cịn phân tích rõ hạn chế giới thiệu số phương pháp phổ biến để nâng cao độ xác INA sử dụng lọc Kalman mở rộng Abstract - Inertial Measurement Unit (IMU) is widely used in motion estimation based on Inertial Navigation Algorithm (INA) INA is based on the principle of combining double integration of acceleration and integration of angular velocity However, using the integral principle will make the estimated error of accumulation very fast over time due to noise of the components in the sensor In this paper we present the construction of INA using the extended Kalman filter to improve the accuracy of the object's motion trajectory estimation In addition, the article also analyzes the limitations and introduces some common methods to improve the accuracy of INA using extended Kalman filter Từ khóa - Cảm biến quán tín; IMU; định vị quán tính; lọc Kalman; Kalman mở rộng Key words - Inertial sensor; IMU; Inertial Navigation Algorithm; Kalman Filter; extend Kalman Filter Đặt vấn đề IMU sử dụng rộng rãi hệ dẫn đường quán tính, bao gồm cảm biến gia tốc theo trục cảm biến vận tốc góc theo trục Trước đây, IMU chủ yếu sử dụng quân hàng không vũ trụ Ngày nay, với phát triển công nghệ vi – – điện tử, IMU ngày chế tạo với kích thước nhỏ giá thành ngày rẻ ứng dụng vào việc ước lượng quỹ đạo chuyển động Nguyên lý việc ước lượng quỹ đạo chuyển động IMU kết hợp tích phân hai lớp gia tốc (cho giá trị quãng đường di chuyển) tích phân vận tốc góc (cho giá trị hướng di chuyển) Từ quảng đường di chuyển hướng di chuyển ta nội suy quỹ đạo di chuyển đối tượng chuyển động (cảm biến gắn lên đối tượng chuyển động) [1] Đây nguyên lý INA Tín hiệu gia tốc vận tốc góc lấy từ cảm biến gia tốc cảm biến vận tốc góc tích hợp IMU Hệ thống định vị quán tính ngày sử dụng rộng rãi ước lượng quỹ đạo chuyển động khắc phục nhược điểm hệ thống định vị dùng camera không bị giới hạn không gian làm việc khắc phục nhược điểm hệ thống định vị tồn cầu GPS làm việc nơi khơng có sóng GPS (hoặc sóng GPS yếu) [2] Tuy nhiên, IMU tồn thành phần nhiễu giá trị đo nên sử dụng nguyên lý tích phân để ước lượng quỹ đạo chuyển động sai số bị tích lũy theo thời gian, đặc biệt IMU dùng dân dụng có giá thành rẻ độ xác thấp Điều đặt yêu cầu phải có phương pháp để giảm sai số nâng cao độ xác ước lượng Trong đó, lọc Kalman ứng dụng để lọc nhiễu cập nhật giá trị đo cho việc ước lượng quỹ đạo chuyển động Một điều cần đặc biệt ý giá trị đo IMU giá trị đo lọc Kalman mà thơng số mơ hình lọc Thực vậy, giá trị ước lượng lọc quỹ đạo chuyển động bao gồm vị trí hướng, giá trị đo IMU gia tốc vận tốc góc Hơn nữa, mơ hình lọc sử dụng biến trạng thái giá trị ước lượng cuối INA (vị trí hướng chuyển động) phi tuyến Do vậy, đặt nhu cầu cần phải xây dựng lọc Kalman mở rộng để ước lượng quỹ đạo chuyển động Trong báo này, nhóm tác giả đề xuất xây dựng INA lọc Kalman mở rộng để ước lượng quỹ đạo chuyển động đối tượng có gắn IMU Ngồi ra, nhóm tác giả cịn xây dựng phương trình cập nhật cho lọc Kalman để nâng cao độ xác số trường hợp đơn giản minh chứng cho hoạt động độ xác thuật tốn xây dựng Xây dựng INA zb Twb , Cwb xb yb zw yw xw Hình Ước lượng chuyển động bàn chân sử dụng IMU Để giám sát chuyển động đối tượng, IMU gắn cố định lên đối tượng Lúc quỹ đạo chuyển động đối tượng xem trùng với quỹ đạo chuyển động IMU Hình thể ứng dụng định vị quán tính để phân tích chuyển động bàn chân Trong Phạm Duy Dưởng, Đồn Quang Vinh 46 đó, quỹ đạo chuyển động bàn chân vị trí hướng hệ trục tọa độ gắn với bàn chân (BCS – Body Coordinate System) hệ tọa độ định vị (WCS – Wolrd Coordinate System) Vị trí hướng vector tịnh tiến 𝑇𝑤𝑏 ma trận quay 𝐶𝑤𝑏 chuyển từ hệ trục WCS sang BCS BCS thường chọn trùng với hệ trục tọa độ vật lý IMU Gốc WCS thường chọn trùng với BCS thời điểm đầu chuyển động, trục 𝑧𝑤 hướng thẳng đứng lên Trong INA, nhóm tác giả đặt v ∈ R3 r ∈ R3 vận tốc vị trí IMU WCS Đặt C(q) ∈ R3×3 ma trận quay từ WCS sang BCS tương ứng với quaternion [3] q ∈ R4 Lúc 𝑟 ≡ 𝑇𝑤𝑏 𝐶(𝑞) ≡ 𝐶𝑤𝑏 Việc thực phép quay khơng gian xác định theo nhiều cách khác ma trận quay DCM, quaternion, phương pháp Euler Trong nghiên cứu này, nhóm tác giả xây dựng hướng BCS theo phương pháp quaternion 2.1 Xây dựng phương trình vi phân cho định vị quán tính Quaternion 𝑞 phép quanh vector đơn vị 𝑢 góc 𝜙 định nghĩa sau: 𝜙 𝜙 (1) 𝑞 = [cos sin 𝑢] ∈ 𝑅4 2 Do WCS cố định BCS thay đổi theo thời gian nên ta biểu diễn quaternion 𝑞 thời điểm 𝑡 𝑏(𝑡) 𝑏 (𝑡) 𝑞𝑤 = 𝑞𝑤 (2) Tương tự áp dụng (2) 𝑡 + 𝑑𝑡 ta có: 𝑏(𝑡+𝑑𝑡) 𝑏 (𝑡 𝑞𝑤 + 𝑑𝑡) = 𝑞𝑤 (3) Mặc khác 𝑏(𝑡+𝑑𝑡) 𝑏(𝑡) 𝑏(𝑡+𝑑𝑡) 𝑞𝑤 = 𝑞𝑤 𝑞𝑏(𝑡) (4) Kết hợp (3) (4) ta có: 𝑏(𝑡) 𝑏(𝑡+𝑑𝑡) 𝑏 (𝑡 𝑞𝑤 + 𝑑𝑡) = 𝑞𝑤 𝑞𝑏(𝑡) (5) Trong khoảng thời gian [𝑡, 𝑡 + 𝑑𝑡], giả sử vận tốc góc BCS 𝜔 (được đo cảm biến vận tốc góc) Lúc BCS quay góc 𝜙 = ‖𝜔‖2 𝑑𝑡 quanh trục quay 𝜔 𝑢 = ‖𝜔‖ Theo định nghĩa phép quay quaternion 𝑞 ta có: 𝜙 𝜙 𝜔 (6) sin ] 2 ‖𝜔‖2 Do 𝑑𝑡 nhỏ nên 𝜙 = ‖𝜔‖2 𝑑𝑡 nhỏ Như vậy, ta xấp xỉ phương trình (6) cách cho 𝜙 𝜙 𝜙 cos = sin = sau: 𝑏(𝑡+𝑑𝑡) 𝑞𝑤 = [cos 2 𝑏(𝑡+𝑑𝑡) 𝑞𝑤 = [1 𝜔𝑑𝑡 ] (7) Từ (5) (7) ta có: 𝑏(𝑡) 𝑏 (𝑡 𝑞𝑤 + 𝑑𝑡) = 𝑞𝑤 𝑏(𝑡) + 𝑞𝑤 [0 𝜔𝑑𝑡 ] Biến đổi phương trình (8) ta có: 𝑏 (𝑡 𝑏 𝑞𝑤 + 𝑑𝑡) − 𝑞𝑤 (𝑡) 𝜔 𝑏 = 𝑞𝑤 (𝑡) [0 ] 𝑑𝑡 𝑏 𝑏 Khi 𝑑𝑡 → ta có: 𝑞̇ 𝑤 (𝑡) = 𝑞𝑤 (𝑡)[0 𝜔] (8) (9) (10) 𝑏 (𝑡)[ Sử dụng tính chất phép nhân quanternion 𝑞𝑤 𝜔], phương trình (10) viết lại sau: (11) 𝑞̇ = Ω𝑞 với −𝜔𝑥 −𝜔𝑦 −𝜔𝑧 𝜔𝑧 −𝜔𝑦 𝜔𝑥 Ω = [𝜔 −𝜔 ] 𝜔𝑥 𝑧 𝑦 𝜔𝑧 𝜔𝑦 −𝜔𝑥 ω ω ω y z ] vận tốc góc BCS WCS ω = [ x Kết hợp phương trình vi phân quaternion (11) với tính chất đạo hàm vị trí vận tốc đạo hàm vận tốc gia ta có phương trình quan hệ 𝑟, 𝑣 𝑞 sau [4]: 𝑞̇ = Ω𝑞 (12) 𝑣̇ = 𝐶 𝑇 (𝑞)[𝑎]𝑏 𝑟̇ = 𝑣 Trong đó, [a]b ∈ R3 gia tốc tịnh tiến BCS BCS Giá trị đầu cảm biến vận tốc góc (yg ∈ R3 ) cảm biến gia tốc (ya ∈ R3 ) cho công thức: yg = ω + vg + bg (13) ya = [a]b + C(q)[g̃]w + va + ba Trong đó, [g̃]w ∈ R3 vector gia tốc trọng trường WCS bg ∈ R3 ba ∈ R3 thành phần nhiễu thay đổi chậm cảm biến vận tốc góc cảm biến gia tốc vg va thành phần nhiễu trắng cảm biến vận tốc góc cảm biến gia tốc Thuật tốn tích phân để lấy tích phân cơng thức (12) (thay [a]b ya − C(q)g̃ thay ω yg ) thể trong Mục 2.2 2.2 Thuật tốn tích phân định vị qn tính Thuật tốn tích phân nhằm xác định giá trị vị trí, vận tốc quaternion theo thời gian sử dụng triển khai Taylor vị trí, vận tốc quaternion Triển khai Taylor cho hàm 𝑞(𝑥) thời điểm 𝑥 = (𝑘 + 1)𝑇 𝑎 = 𝑘𝑇 Khi sử dụng xấp xỉ bậc ta có kết sau: 𝑞̈ 𝑘 𝑇 (14) 𝑞𝑘+1 = 𝑞𝑘 + 𝑞̇ 𝑘 𝑇 + Khai triển phương trình (14) sử dụng cơng thức phương trình (12) ta có: 1 (15) 𝑞𝑘+1 = (𝐼 + Ω𝑇 + Ω2 𝑇 + Ω̇𝑇 ) 𝑞𝑘 Trong đó, Ω có tính chất sau Ω2 = −‖𝜔‖22 𝐼 Ω3 = −‖𝜔‖22 Ω (16) Ω𝑘 − Ω𝑘−1 Ωk̇ = 𝑇 Triển khai (15) sử dụng tính chất (16) ta có: 1 (17) 𝑞𝑘+1 = (𝐼 + Ωk 𝑇 − Ω𝑘−1 𝑇 − ‖𝜔‖22 𝑇 ) 𝑞𝑘 4 Triển khai xấp xỉ Taylor bậc sử dụng kết xấp xỉ Taylor bậc (17) ta có: ISSN 1859-1531 - TẠP CHÍ KHOA HỌC VÀ CƠNG NGHỆ ĐẠI HỌC ĐÀ NẴNG, VOL 17, NO 9, 2019 1 𝑞𝑘+1 = (𝐼 + Ωk 𝑇 − Ω𝑘−1 𝑇 − ‖𝜔‖22 𝑇 4 (18) − Ω Ω 𝑇 24 𝑘 𝑘−1 ‖𝜔‖22 Ω𝑘 𝑇 ) 𝑞𝑘 − 48 Triển khai Taylor bậc để tính vận tốc 𝑣𝑘+1 sử dụng phương trình thứ (12) ta có: 𝑣𝑘+1 = 𝑣𝑘 + 𝐶 𝑇 (𝑞𝑘 )(𝑦𝑎𝑘 − 𝐶(𝑞𝑘 )[𝑔̃]𝑤 )𝑇 (19) Triển khai phương trình (19) ta có: 𝑣𝑘+1 = 𝑣𝑘 + 𝐶 𝑇 (𝑞𝑘 )𝑦𝑎𝑘 𝑇 − [𝑔̃]𝑤 𝑇 (20) Xấp xỉ (19) sử dụng tín hiệu đầu cảm biến ta có: 𝑣𝑘+1 =≈ 𝑣𝑘 + (𝐶 𝑇 (𝑞𝑘+1 )𝑦𝑎𝑘+1 (21) + 𝐶 𝑇 (𝑞𝑘 )𝑦𝑎𝑘 )𝑇 − [𝑔̃]𝑤 𝑇 Triển khai Taylor bậc để tính vị trí 𝑟𝑘+1 sử dụng phương trình thứ (12) ta có: (22) 𝑟𝑘+1 = 𝑟𝑘 + (𝑣𝑘+1 + 𝑣𝑘 )𝑇 Xây dựng lọc Kalman mở rộng cho INA Đặt q̂, r̂ v̂ giá trị tích phân quaternion, vị trí vận tốc IMU Gọi q̅ ∈ R3 , r̅ ∈ R3 v̅ ∈ R3 sai số quaternion, vị trí vận tốc IMU: q̅ = [03×1 I3 ] (q̂∗ ⊗ q) r̅ = r − r̂ v̅ = v − v̂ Trong đó, ⊗ phép nhân quaternion q∗ quaternion liên hợp q Phương trình biểu diễn thành phần sai số quaternion q̅ ∈ R3 (thay dùng thành phần) thời gian lấy mẫu 𝑑𝑡 → góc quay 𝜙 quanh trục 𝑢 nhỏ, lúc thành phần quaternion theo định 𝜙 nghĩa công thức (1) cos ≈ [5] 𝑞𝑒 ≈ [ ] 𝑞̅ Đạo hàm 𝑞 theo cơng thức (25) ta có: 𝑞̇ = 𝑞̂̇ ⊗ 𝑞𝑒 + 𝑞̂ ⊗ 𝑞𝑒̇ Triển khai phương trình (26) ta có: 1 0 𝑞 ⊗ [ ] = 𝑞̂ ⊗ [𝑦 ] ⊗ 𝑞𝑒 + 𝑞̂ ⊗ 𝑞𝑒̇ 𝜔𝑏 2 𝑔 47 (26) (27) Sử dụng quanternion liên hợp cho vế (27) ta có: 1 0 (28) 𝑞𝑒̇ = 𝑞𝑒 ⊗ [ ] − [𝑦 ] ⊗ 𝑞𝑒 𝜔 2 𝑔 𝑏 Sử dụng tính chất 𝑎 ⊗ 𝑏 = 𝑏⨂𝑎 + [ ] cho 𝑎̅ × 𝑏̅ phương trình (28) ta có: 0 (29) 𝑞̇ 𝑒 = 𝑞𝑒 ⊗ [𝜔 − 𝑦 ] + [𝑞̅ × 𝑦 ] 𝑏 𝑔 𝑔 Từ ta tính 𝑞̅ sau: 1 (30) 𝑞̅̇ = (𝜔𝑏 − 𝑦𝑔 ) + 𝑞̅ × (𝜔𝑏 − 𝑦𝑔 ) − 𝑦𝑔 × 𝑞̅ 2 Do 𝑞̅ 𝜔𝑏 − 𝑦𝑔 nhỏ nên bỏ qua cơng thức (30), ta có: 1 (31) 𝑞̅̇ = −𝑦𝑔 × 𝑞̅ + 𝑞̅ − (𝑣𝑔 + 𝑏𝑔 ) 2 Trong ý rằng, 𝑏𝑔 loại bỏ giai đoạn tiền xử lý, ta viết lại cơng thức (31) sau: 1 (32) 𝑞̅̇ = [−𝑦𝑔 ×]𝑞̅ + 𝑞̅ − 𝑣𝑔 2 Chúng ta sử dụng phương trình thứ (12) để xây dựng nên 𝑣̅ 𝑣̂̇ = 𝐶 𝑇 (𝑞̂)𝑦𝑎 − 𝑔̂ (33) 𝑣̇ = 𝐶 𝑇 (𝑞)[𝑎]𝑏 Do 𝐶 𝑇 (𝑞) ≠ 𝐶 𝑇 (𝑞̂)𝑦𝑎 − 𝑔̂ nên 𝑣 ≠ 𝑣̂ Đặt 𝑣̅ sai số vận tốc định nghĩa sau: (34) 𝑣̅ ≜ 𝑣 − 𝑣̂ Lấy đạo hàm vế (34) ta có: (35) 𝑣̅̇ = 𝑣̇ − 𝑣̂̇ Biến trạng thái đưa vào lọc Kalman sau: 𝑞̅ Thay 𝑣̇ 𝑣̂̇ từ (33) vào (35) ta có: 𝑥 = [ 𝑟̅ ] ∈ 𝑅9 𝑣̅ (36) 𝑣̅̇ = 𝐶 𝑇 (𝑞)𝑎𝑏 − 𝐶 𝑇 (𝑞̂)𝑦𝑎 + 𝑔̂ Phương trình trạng thái cho lọc Kalman có dạng: Thay 𝑎𝑏 = 𝑦𝑎 − 𝐶(𝑞)𝑔 − 𝑏𝑎 − 𝑣𝑎 vào (36) ta có: (23) 𝑥̇ (𝑡) = 𝐴(𝑡)𝑥(𝑡) + 𝑤(𝑡) 𝑣̅̇ = 𝐶 𝑇 (𝑞)(𝑦𝑎 − 𝐶(𝑞)𝑔 − 𝑏𝑎 − 𝑣𝑎 ) − 𝐶 𝑇 (𝑞̂)𝑦𝑎 + 𝑔̂ (37) Xác định ma trận 𝐴(𝑡) 𝑤(𝑡) xuất phát từ việc tính Biến đổi phương trình (37) ta có: đạo hàm biến trạng thái thành phần 𝑥 𝑣̅̇ = (𝐶 𝑇 (𝑞) − 𝐶 𝑇 (𝑞̂))𝑦𝑎 − 𝐶 𝑇 (𝑞)(𝑏𝑎 + 𝑣𝑎 ) + 𝑔̂ − 𝑔 (38) Quaternion 𝑞 tính theo công thức Sử dụng công thức xấp xỉ 𝐶(𝑞) ≈ 𝐶(𝑞̂) − 2𝐾(𝑞̅ )𝐶(𝑞̂) (12) Gọi 𝑞̂ giá trị tính từ cơng thức cho phương trình (38) ta có: (24) 𝑣̅̇ ≈ −(2𝐾(𝑞̅)𝐶(𝑞̂))𝑇 𝑦 − 𝐶 𝑇 (𝑞̂)(𝑏 + 𝑣 ) 𝑞̂̇ = Ω(yg )𝑞̂ 𝑎 𝑎 𝑎 (39) 𝑇 + (2𝐾(𝑞̅)𝐶(𝑞̂)) (𝑏𝑎 + 𝑣𝑎 ) + 𝑔̂ − 𝑔 Do 𝑦𝑔 chứa thành phần nhiễu trắng 𝑣𝑔 nhiễu chậm thay đổi 𝑏𝑔 nên 𝑦𝑔 ≠ 𝜔𝑏 Điều dẫn đến 𝑞̂ ≠ 𝑞 Sử dụng tính chất 𝐾(𝑎)𝑏 = −𝐾(𝑏)𝑎 ta có: Tồn 𝑞𝑒 sai số của 𝑞̂, (40) 𝑣̅̇ ≈ −2𝐶 𝑇 (𝑞̂)𝐾(𝑦𝑎 )𝑞̅ + 𝐶 𝑇 (𝑞)(−𝑏𝑎 − 𝑣𝑎 ) (25) 𝑞 = 𝑞̂ ⊗ 𝑞𝑒 Thành phần nhiễu chậm thay đổi thường khử Trong đó, 𝑞𝑒 có giá trị nhỏ xấp xỉ sau: trình tiền xử lý nên ta lượt bỏ phương trình (40) Phạm Duy Dưởng, Đồn Quang Vinh 48 Từ phương trình thứ (12) ta có: 𝑟̅̇ = 𝑟̇ − 𝑟̂̇ (41) Thay 𝑟̇ = 𝑣, 𝑟̂̇ = 𝑣̂ 𝑣̅ = 𝑣 − 𝑣̂ ta có: 𝑟̅̇ = 𝑣̅ (42) Các thành phần 𝑏𝑎 𝑏𝑔 nhiễu chậm thay đổi cảm biến gia tốc cảm biến vận tốc góc, thành phần đạo hàm 𝑏𝑎̇ 𝑏𝑔̇ đại diện nhiễu trắng 𝑤𝑏𝑎 𝑤𝑏𝑔 Vậy ma trận mơ hình lọc Kalman cho định vị qn tính (23) sau: [−𝑦𝑔 ×] − 𝐼 𝐴(𝑡) = [ ] 0 𝐼 −2𝐶 𝑇 (𝑞̂)[𝑦𝑎 ×] 0 − 𝑣𝑔 𝑤(𝑡) = [ ] 𝑇 (𝑞 −𝐶 ̂)𝑣𝑎 3×3 [a ×] ∈ R ma trận đối xứng lệch tương ứng với vector a ∈ R3×1 Xây dựng phương trình cập nhật cho lọc Kalman Do tồn thành phần nhiễu IMU việc sử dụng nghiên lý tích phân INA nên sai số việc ước lượng quỹ đạo chuyển động tích lũy tăng lên nhanh theo thời gian, đặc biệt cảm biến giá rẻ dùng dân dụng Do vậy, việc xây dựng phương trình cập nhật cho lọc Kalman cần thiết để đảm bảo độ xác việc ước lượng Việc xây dựng phương trình tùy thuộc vào đặc trưng riêng đối tượng tính chất chuyển động Trong báo này, nhóm tác giả xây dựng phương trình cập nhật tình cụ thể IMU đặt giày Hình để làm thí nghiệm minh chứng cho hoạt động INA hiệu phương trình cập nhật Trong q trình bước đi, có khoảng thời gian bàn chân chạm đất lúc vận tốc bàn chân xem khơng độ cao bàn chân xem không (nếu di chuyển mặt sàn phẳng nằm ngang) Khoảng thời gian thường gọi ZVI (Zero Velocity Interval) Trong [6], ZVI phát trực tiếp cảm biến vận tốc Doppler Tuy nhiên, phát ZVI gián tiếp cách sử dụng thuật toán phát vận tốc [7, 9] Trong báo này, nhóm tác giả sử dụng thuật toán phát ZVI đơn giản Nếu điều kiện thỏa mãn thời điểm gián đoạn m phải thuộc ZVI 𝑁𝑔 𝑁𝑔 𝑦𝑔,𝑖 ≤ 𝐵𝑔 , 𝑚− ≤𝑖 ≤𝑚+ 2 (43) 𝑁𝑎 𝑁𝑎 𝑦𝑎,𝑖 − 𝑦𝑎,𝑖−1 ≤ 𝐵𝑎 , 𝑚 − ≤𝑖≤𝑚+ 2 N g N a số nguyên Bg , Ba giá trị ngưỡng đặt trước Tại ZVI này, ta có: 𝑣 = 03×1 (44) 𝑟(3) = Do ta có phương trình cập nhật vận tốc độ cao cho lọc Kalman sau: 𝑧𝑣 𝑣𝑣 𝐇 (45) [ 𝑧 ] = [ 𝐯 ] 𝑥 + [𝑣 ] 𝐇𝐫 𝑟 𝑟 𝑧𝑟 = 03×1 − 𝑣̂ 𝑧𝑟 = − 𝑟̂ (3) 𝐻𝑣 = [03×3 03×3 𝐼3 ] 𝐻𝑟 = [01×5 𝐼1 03×3 ] với vv vr nhiễu thể việc vận tốc độ cao khung tập gần khơng khơng hồn tồn khơng Thí nghiệm kết Để tiến hành thí nghiệm minh chứng kết quả, nhóm tác giả chế tạo mơ đun thu thập liệu gồm IMU Mti-1 hãng Xsens thẻ nhớ Micro SD Hình Trong Mti-1 có tần tần số lấy mẫu 100 Hz, thẻ nhớ Micro SD dùng để lưu trữ liệu q trình thực thí nghiệm Hình Mơ đun thu thập liệu (trái) thiết kế (phải) Camera Hạt phản quang Mô đun IMU Hình Mơ đun thu thập liệu gắn bàn chân Để thực thí nghiệm đánh giá INA, nhóm tác giả tiến hành thí nghiệm ước lượng chuyển động bàn chân theo ý tưởng Hình Q trình thí nghiệm tiến hành Hình Trong đó, mơ đun thu thập liệu IMU đặt mu bàn chân Đây điểm thuận lợi để đặt cảm biến vị trí ổn định trình bàn chân ISSN 1859-1531 - TẠP CHÍ KHOA HỌC VÀ CƠNG NGHỆ ĐẠI HỌC ĐÀ NẴNG, VOL 17, NO 9, 2019 chạm đất Người dùng tiến hành bước thẳng trước giám sát hệ thống camera Optitrack Flex 13 Hệ thống camera ghi lại chuyển động hạt phản quang gắn mơ đun IMU với độ xác 0,2 mm Do vậy, quỹ đạo chuyển động hạt phản quang hệ thống camera ghi lại xem quỹ đạo thực bàn chân người dùng Hình Vị trí bàn chân sử dụng INA khơng cập nhật 49 Về mặt định tính, Hình 4, Hình Hình thể vị trí bàn chân theo trục 𝑥, 𝑦 𝑧 trình bước theo thời gian trường hợp sử dụng INA mà khơng có cập nhật (Hình 4), có cập nhật vận tốc ZVI (Hình 5) có cập nhật vận tốc kết hợp với cập nhật độ cao ZVI (Hình 6) Trong đó, trục 𝑧 độ cao bàn chân, đường nét liền màu xanh thể vị trí ước lượng đường màu đỏ nét đứt thể vị trí thực bàn chân ghi lại hệ thống camera Như thấy Hình 4, khơng có cập nhật sai số ước lượng tăng lên nhanh (tức đường ước lượng bị phân kỳ nhanh so với đường thực) Cụ thể, sau giây ước lượng sai số tăng lên đến gần 20 m trục 𝑥, gần 30 m trục 𝑦 khoảng m trục 𝑧 Đây nhược điểm lớn IMU, nhiệm vụ nghiên cứu xây dựng đề xuất giải pháp để tăng độ xác việc ước lượng Khi sử dụng phương trình cập nhật vận tốc (xem Hình 5) ta thấy kết hồn tồn khác so với Hình Lúc đường ước lượng bám theo đường thực Tuy nhiên, sai lệch đáng kể theo phương 𝑧 (độ cao bàn chân) Khi áp dụng tiếp phương trình cập nhật độ cao bàn chân ta có kết Hình Lúc này, sai lệch theo phương 𝑧 cập nhật sau bước Việc phân tích mặc định tính cho thấy rõ vai trị việc xây dựng phương trình cập nhật cho INA, đặc biệt phương trình cập nhật ZVI Bảng Sai số vị trí theo trục sử dụng cập nhật vận tốc ZVI (m) TT RMSE x RMSE y RMSE z Tổng 0,0183 0,0121 0,0112 0,0416 0,0252 0,0178 0,0207 0,0637 0,0153 0,0133 0,0104 0,0391 0,0276 0,0201 0,0151 0,0627 TB 0,0216 0,0158 0,0144 0,0518 TT: thứ tự thí nghiệm; TB: trung bình Bảng Sai số vị trí theo trục sử dụng cập nhật vận tốc cập nhật độ cao ZVI (m) Hình Vị trí bàn chân sử dụng INA cập nhật vận tốc điểm ZVI Hình Vị trí bàn chân sử dụng INA cập nhật vận tốc độ cao điêm ZVI TT RMSE x RMSE y RMSE z Tổng 0,0187 0,0121 0,0083 0,0391 0,0249 0,0167 0,0086 0,0502 0,0155 0,0130 0,0089 0,0374 0,0223 0,0190 0,0105 0,0519 TB 0,0204 0,0152 0,0091 0,0447 Về mặt định lượng, kết thí nghiệm sử dụng cập nhật vận tốc ZVI liệt kê Bảng 1; cập nhật vận tốc độ cao ZVI liệt kê Bảng Trong đó, tiêu chí bậc hai trung bình bình phương sai số (RMSE) sử dụng để đánh giá độ xác Lúc tổng sai số theo phương khoảng cm tổng số bước với độ dài m Như vậy, độ xác việc ước lượng vị trí sử dụng INA nâng cao lên nhiều sử dụng phương trình cập nhật điểm ZVI Nếu xét riêng cho bước sai số ước lượng khoảng 1,7 cm Đây sai số hoàn toàn chấp nhận việc ước lượng thông số bước người dùng So sánh hai Bảng ta thấy, sử dụng phương trình Phạm Duy Dưởng, Đoàn Quang Vinh 50 cập nhật độ cao tác dụng nâng cao độ xác việc ước lượng theo phương đứng Kết tương tự trường hợp nhóm tác giả thử nghiệm kết trường hợp sử dụng cập nhật độ cao Lời cảm ơn: Bài báo tài trợ Đề tài NCKH cấp với mã số B2018.DNA.07 (KYTH-45) Chủ nhiệm đề tài: PGS TS Đoàn Quang Vinh, Trường Đại học Bách khoa - Đại học Đà Nẵng Kết luận Bài báo xây dựng INA lọc Kalman mở rộng cho INA nhằm nâng cao độ xác việc ước lượng quỹ đạo chuyển động sử dụng IMU Đây phương pháp để ước lượng quỹ đạo chuyển động khắc phục số nhược điểm hệ thống giám sát chuyển động dùng camera GPS Ngồi ra, báo cịn xây dựng số phương trình cập nhật phổ biến cho INA gồm cập nhật vận tốc độ cao điểm ZVI Những phương trình cập nhật góp phần quan trọng trọng việc nâng cao độ xác giúp loại bỏ đáng kể sai số tích lũy theo thời gian INA Bài báo xây dựng mô đun thu thập liệu sử cho cảm biến IMU tiến hành thí nghiệm trường hợp cảm biến đặt bàn chân để phân tích hoạt động thuật tốn INA phương trình cập nhật Kết thí nghiệm mặt định tính định lượng cho thấy việc sử dụng phương trình cập nhật xuyên suốt trình chuyển động cần thiết góp phần nâng cao độ xác cho INA Riêng trường hợp cảm biến đặt bàn chân sai số việc ước lượng cho bước khoảng 1,7 cm Đây sai số hoàn toàn chấp nhận ứng dụng ước lượng thông số bước cho người dùng TÀI LIỆU THAM KHẢO [1] Pham, D.D.; Suh, Y.S., “Foot Pose Estimation Using an Inertial Sensor Unit and Two Distance Sensors” Sensors, 2015, 15 1588815902 [2] Pham, D.D.; Suh, Y.S., “Pedestrian Navigation Using Foot-Mounted Inertial Sensor and LIDAR” Sensors (Basel) 2016;16(1):120 [3] Kuipers, J.B “Quaternions and rotation sequences: a primer with applications to orbits, aerospace, and virtual reality”; Princeton University Press: New Jersey, 1999 [4] Nam, C.N.K.; Kang, H.J.; Suh, Y.S “Golf Swing Motion Tracking Using Inertial Sensors and a Stereo Camera” IEEE Transactions on Instrumentation and Measurement 2014, 63, 943–952 [5] Markley, F.L “Multiplicative vs Additive Filtering for Spacecraft Attitude Determination” Proc of 6th Cranfield Conference on Dynamics and Control of Systems and Structures in Space, 2004, pp 467–474 [6] Jackson A B, “Outcome measures for gait and ambulation in the spinal cord injury population” The journal of spinal cord medicine, 2008 [7] Hawkinson W, et al “Geospatial Location, Accountability, and Navigation System For Emergency Responders” In Proceedings of the Position Location and Navigation Symposium, Myrtle Beach, SC, 2012 [8] Park S.K, Suh YS, “Zero Velocity Detection Algorithm Using Inertial Sensors for Pedestrian Navigation Systems” Sensors, 2010 [9] Skog I, Nilsson JO, Handel P “Evaluation of zero velocity detectors for foot-mounted inertial navigation systems” In Proceedings of the International Conference on Indoor Positioning and Indoor Navigation, Zurich, Switzerland, 2010 (BBT nhận bài: 03/7/2019, hoàn tất thủ tục phản biện: 26/8/2019) ... cứu này, nhóm tác giả xây dựng hướng BCS theo phương pháp quaternion 2.1 Xây dựng phương trình vi phân cho định vị quán tính Quaternion