7. Bố cục chung của luận án
3.1 Giới thiệu chương
Trong chương này, đề xuất hệ thống INS đặt trên bàn chân nhằm nâng cao độ chính xác cho việc ước lượng chuyển động cũng như thông số bước đi. Hệ thống INS đề xuất này gồm 01 cảm biến IMU kết hợp với 01 cảm biến khoảng cách đặt trên bằn chân. Việc gắn hệ thống INS trên bàn chân với mục đích sử dụng cập nhật ZUPT trong khi cảm biến khoảng cách hướng xuống mặt đất trong quá trình bước đi là để tham chiếu độ cao và cập nhật chuyển động cho bàn chân.
Một số vấn đề cần giải quyết trong chương này gồm:
- Ước lượng chính xác mối quan hệ gồm vị trí và hướng của cảm biến khoảng cách trong hệ toạ độ của IMU.
- Xây dựng mơ hình bộ lọc Kalman kiểu MEKF cho hệ thống INS mới
dựa mơ hình bộ lọc Kalman cho hệ thống INS cơ bản đã được xây dựng
ở Chương 2.
- Xây dựng phương trình cập nhật ZUPT và các phương trình cập nhật sử dụng dữ liệu từ cảm biến khoảng cách.
- Trích xuất thơng số bước đi từ quỹ đạo chuyển động của cảm biến IMU đặt trên bàn chân.
- Đánh giá hiệu quả hệ thống đề xuất.
Nội dung chương này đã được chính tác giả và cộng sự công bố trong bài báo
[50] trên tạp chí Sensors (SCIE, Q1) năm 2015 và bài báo [99] trong kỷ yếu
hội nghị
VCCA năm 2019. Trong bài báo [50], các tác giả đã gắn 2 cảm biến khoảng cách vào hệ thống INS đặt trên bàn chân và sử dụng bộ lọc Kalman 21 trạng thái để ước lượng chuyển động của bàn chân. Trong bài báo [99], các tác giả chỉ sử dụng 1 cảm biến khoảng cách và bộ lọc Kalman 15 trạng thái nhằm giảm khối lượng tính tốn nhưng
3.2 Đề xuất hệ thớng INS đặt trên bàn chân
Bàn chân là điểm thích hợp để đặt cảm biến IMU do chuyển động bước chân có tính chu kỳ lặp đi lặp lại cũng như luôn tồn tại một khoảng thời gian bàn chân chạm đất ở đầu và cuối mỗi bước đi. Tại thời điểm bàn chân chạm đất thì vận tốc có thể được xem là bằng khơng (nên khoảng thời gian này gọi là ZVI), độ cao bàn chân cũng có thể được xem là bằng khơng trong điều kiện bước đi trên mặt phẳng. Các thông tin vận tốc và độ cao bàn chân tại khoảng thời gian ZVI được sử dụng phổ biến như là một tín hiệu đo để loại bỏ sai số và cập nhật vận tốc, vị trí và hướng bàn chân trong bộ lọc Kalman (thường gọi là ZUPT). Ngoài ra, do cảm biến IMU được đặt trên bàn chân nên chuyển động của cảm biến IMU trong quá trình bước đi phản ánh q trình chuyển động của bàn chân do đó các thơng số bước đi dễ dàng trích xuất được từ quỹ đạo chuyển động của cảm biến IMU.
Trong chương này, luận án sử dụng cảm biến khoảng cách hỗ trợ hệ thống INS gắn trên bàn chân để nâng cao độ chính xác trong ước lượng vị trí và hướng bàn chân. Cảm biến khoảng cách được bố trí hướng xuống mặt đất trong q trình di chuyển nhằm nâng cao độ chính xác của hệ thống INS. Mơ hình bộ lọc MEKF trong Chương
2cũng được điều chỉnh để ước lượng vị trí và hướng của cảm biến khoảng cách trong
hệ toạ độ BCS. Từ vị trí và hướng của cảm biến khoảng cách, thơng tin về độ cao của
bàn chân được xác định từ giá trị đo của cảm biến khoảng cách. Điều này cho phép xây dựng phương trình cập nhật của bộ lọc MEKF để cập nhật quỹ đạo chuyển động cho bàn chân trong quá trình di chuyển nhằm nâng cao độ chính xác trong ước lượng các thông số bước đi.
Hệ thống đề xuất được thể hiện trong Hình 3.1 bao gồm 01 IMU có tần số lấy mẫu được cấu hình là 100 và 01 cảm biến khoảng cách được gắn cố định trên một chiếc giày. Các thông số được quan tâm của cảm biến khoảng cách là giới hạn đo và tần số hoạt động. Trong đó, cảm biến khoảng cách dùng để đo độ cao của bàn chân trong quá trình bước đi, độ cao này thường nhỏ hơn 20 . Thời gian di chuyển của bàn chân trong một bước đi thường lớn hơn 30 . Trong khoảng thời gian này cần phải có ít nhất một dữ liệu từ cảm biến khoảng cách để cập nhật. Do vậy tần số của
cảm biến khoảng cách yêu cầu phải lớn hơn 33 . Trong luận án này chọn cảm biến khoảng cách VL6180 với khoảng cách đo lên đến 20 mm, sai số khoảng 2 mm và tần số lên đến 50 . Việc đồng bộ các tín hiệu từ các cảm biến được thực hiện bằng cách đọc xen kẽ 01 dữ liệu từ cảm biến khoảng cách với 03 dữ liệu từ cảm biến IMU. Do vậy, cảm biến khoảng cách được cấu hình hoạt động ở tần số 33,33 .
IMU rD b Cảm biến khoảng cách n
Hình 3.1 Tổng quan về INS đặt trên bàn chân (nguồn [50])
Thuật toán của hệ thống INS sử dụng bộ lọc MEKF trong Chương 2 được sử dụng để ước lượng hướng , vận tốc và vị trí theo thời gian của bàn chân (được đại diện bởi hệ toạ độ BCS). Tuy nhiên, mơ hình của bộ lọc MEKF có sự thay đổi bằng việc đưa vị trí và hướng của cảm biến khoảng cách trong BCS vào bộ lọc để ước lượng (do khó có thể đo chính xác bằng thước), đồng thời các thành phần nhiễu chậm thay đổi , trong Hình 2.12 được loại khỏi bộ lọc để giảm khối lượng tính tốn. Lúc này, được tính sơ bộ bằng giá trị thu thập tại thời điểm cảm
biến IMU đứng yên ban đầu và = 03×1 trong thuật tốn INA ở Hình 2.12.
Ngồi ra, các phương trình cập nhật của bộ lọc Kalman cần được xây dựng để sử dụng các tín hiệu đo từ cảm biến khoảng cách cũng như thông tin bàn chân chạm đất để cập nhật sai số của bộ lọc. Do vậy, trong chương này chỉ đề cập đến việc xây dựng mơ hình bộ lọc và các phương trình cập nhật cho bộ lọc có kế thừa kết quả từ Chương
2. Các thơng số bước đi sẽ được trích xuất từ vị trí kết hợp với các thời
điểm bàn chân chạm đất. Trong chương này sử dụng hệ trục toạ độ vật thể BCS và tồn cục WCS đã được trình bày trong Mục 2.2.2.1.
3.3 Xây dựng mơ hình bộ lọc MEKF cho hệ thớng
Gọi [ ] và [ ] là vị trí và hướng của cảm biến khoảng cách trong hệ tọa độ BCS. Lúc này ta có:
trong đó:
-
-
cảm biến khoảng cách có 2 vị trí đặt với thơng số như sau
Việc đo sơ bộ và
số của chúng ( ̅, ̅ ) được đưa vào bộ lọc Kalman để ước lượng nhằm nâng cao độ
chính xác.
Như vậy, các sai số ̅, ̅, , ̅ ̅ và ̅ được đưa vào bộ lọc Kalman để ước
lượng nên các biến trạng thái được sử dụng trong bộ lọc Kalman như sau:
̅
̅
= ̅∈ 15
̅
[ ̅ ]
Mơ hình cho bộ lọc Kalman có dạng như trong (2-19). Trong đó, thành phần và được bỏ qua khi tính ̅ ̇, ̅ ̇và ̅ ̇.Tổng hợp kết quả ta có:
1 ̅ ̇= [− ×] ̅ − 2 ̅ ̇= ̅ ̅ ̇≈ −2 ( ) ( ) ̅ − ( ) (3-3) ̅ ̇ = 0 ̅ ̇ = 0 trong đó, ̅ = ⃗ ̇ số nê n ̅ = 0 và
Như vậy các ma trận trong mơ hình hệ thống đề xuất như sau:
=
Ma trận hiệp phương sai của nhiễu mơ hình được tính tương tự như trong (2-
22), trong đó: - (1: 3,1: 3) = { (1: 3) (1: 3)} = 1 4 - (4: 6,4: 6) = { (4: 6) (4: 6)} = 0 - (7: 9,7: 9) = { (7: 9) (7: 9)} = ( ) ( ) - (10: 12,10: 12) = { (10: 12) (10: 12)} = 0 - (13: 15,13: 15) = { (13: 15) (13: 15)} = 0 Lúc này ta có: 1 4 = 0
trong đó và được tính trong (2-20).
Tương tự như đã trình bày ở Chương 2, mơ hình của bộ lọc ở dạng rời rạc như sau:
+1 = + trong đó:
=
Trong khoảng thời gian lấy mẫu , các thành phần trong ma trận
Do vậy ta có: trong đó: = [0 3×3 là hằng số. (3-6) Như vậy 3 ≈ [ Các ma trận
3.4 Xây dựng các phương trình cập nhật cho bộ lọc Kalman
3.4.1 Cập nhật vận tốc ZUPT
Trong q trình bàn chân bước đi, ln tồn tại các khoảng thời gian ZVI sau mỗi bước đi. Lúc này vận tốc của bàn chân có thể xem bằng khơng và độ cao của bàn chân cũng được xem là bằng không.
Trong [100], các ZVI này có thể được phát hiện trực tiếp bởi cảm biến vận tốc Doppler. Tuy nhiên, chúng ta có thể phát hiện các ZVI gián tiếp bằng cách sử dụng thuật toán phát hiện ZVI [101], [102].
Trong luận án này, một thuật toán phát hiện ZVI đơn giản được sử dụng. Nếu những điều kiện dưới đây được thỏa mãn thì thời điểm rời rạc phải thuộc ZVI [93], [50]:
‖
‖
, , −1
trong đó:
- , là các giá trị ngưỡng đặt trước cho độ lớn được xem là chuyển động của giá trị vận tốc góc và độ thay đổi của gia tốc
- và là một số nguyên đại diện cho khoảng thời gian đủ lớn để xem là đứng yên. Trong luận án này, chọn = = 30 tương ứng với khoảng thời gian là 0,3 giây, = 0,2 / và = 0,4 /
2 .
Những giá trị này được lấy từ thực nghiệm thơng qua việc phân tích dữ liệu liên quan trong quá trình bước đi.
Tại các thời khoảng thời gian ZVI này, ta có:
trong đó = [
Từ = + ̅và = + ̅kết hợp với (3-9), ta có:
̅= −
̅= −
Vậy phương trình cập nhật cho bộ lọc Kalman như sau:
= + trong đó: - - - - (3-10) (3-11)
- = (0, ) và = (0, ) là nhiễu trắng thể hiện việc vận tốc và độ cao của bàn chân khơng hồn tồn bằng khơng. Trong đó, tốc độ rung
động của bàn chân khi đứng trên mặt đất khoảng 0,01 / và độ dao
động về độ cao bàn chân khoảng 0,01 . Trong luận án chọn =
3.4.2 Xây dựng phương trình cập nhật sử dụng cảm biến khoảng cách
Giá trị đo của cảm biến khoảng cách có thể được mơ hình hóa như sau: = +
trong đó:
- là giá trị khoảng cách từ cảm biến khoảng cách đến điểm mà cảm biến chỉ vào trên mặt đất,
- = (0, ) là nhiễu trắng của cảm biến khoảng cách. Trong quá trình thực nghiệm xác nhận độ phân giải của cảm biến là 1 và
sai số
của cảm biến là ±2 nên luận án chọn = 0,000004 2.
Chú ý rằng đây không phải là độ cao của cảm biến vì cảm biến khơng hướng vng góc với mặt đất trong q trình bước đi. Độ cao này có thể tính được dựa vào phương của cảm biến khoảng cách.
Do mặt đất bằng phẳng và gốc hệ trục tọa độ WCS nằm trên mặt đất lúc này độ cao của bàn chân được tính như sau:
[0 0 1]=−[0 0 1] ()[
trong đó, [ + ] chính là vectơ được xác định từ cảm biến IMU đến điểm mà cảm biến khoảng cách chỉ trên mặt đất xét trong hệ toạ độ BCS, vectơ này được biểu diễn trong hệ toạ độ WCS bằng cách nhân với ma trận quay từ BCS sang WCS như sau ′( )[ Sử dụng tính chất( ) = ( )( trình (3-13), ta có: Thay (3-1) và (3-12) vào (3-14), ta có: −[0 0 1] ( )( + 2[ ̅ ×])
giá trị nhỏ, do vậy chúng ta có thể bỏ qua các thành phần tích của chúng với nhau, như vậy ta có: [ ( + ̅+ Sử dụng tính chất [ ×] = −[ ×] , ta có: = [0 = ( )
Như vậy phương trình cập nhật của bộ lọc Kalman sử dụng tín hiệu đo của cảm biến khoảng cách như sau:
trong đó:
= [0 0 1][2
[0 0 1] ( )
dụng tín hiệu khoảng cách từ cảm biến. Hiệp phương sai của nhiễu này được tính như sau:
= ([0
ℎ
Một phương trình cập nhật khác của bộ lọc Kalman được xây dựng dựa trên điều kiện vectơ hướng phải là vectơ đơn vị. Chúng ta có thể triển khai điều kiện này như sau:
‖ ‖=1
Triển khai phương trình trên, ta có:
‖
Khai triển bình phương 2 vế phương trình trên:
Từ điều kiện ‖ ‖ = ≈ 1 và ̅ ̅ ≈ 0, có thể xấp xỉ như sau: ̄ = 0
(3-20) (3-21) (3-22)
(3-23)
Như vậy, phương trình cập nhật cho bộ lọc Kalman xây dựng từ vectơ hướng của cảm biến khoảng cách như sau:
0 =+
trong đó:
-
- = (0, ) là nhiễu trắng đại diện cho sai số của việc xấp xỉ trong (3-23). Trong đó, độ lớn của vectơ sai số ‖ ̅ ‖ ≈ 1%‖ ‖ nên luận án
chọn = (10−2)4 3 = 10−8 3.
Tổng hợp các phương trình cập nhật (3-18) và (3-24), ta có phương trình cập nhật cho bộ lọc Kalman sử dụng cảm biến khoảng cách như sau:
Ma trận hiệp phương sai của nhiễu đo như sau:
3.5 Thực thi bộ lọc Kalman cho hệ thớng
Quy trình thực thi bộ lọc Kalman cho hệ thống đã được miêu tả một cách chi tiết trong Hình 3.2 . Trong đó:
- Tương tự như trong (2-36), việc khởi tạo giá trị ban đầu gồm có khởi tạo biến trạng thái ban đầu 0− = 015×1 và hiệp phương sai P0−=015×15.
- Tại các khoảng thời gian ZVI thì tiến hành cập nhật cho bộ lọc Kalman với việc tính các ma trận và theo (3-11). Theo miêu tả hệ thống thì cứ 3 dữ liệu của IMU thì có một dữ liệu của cảm biến khoảng cách. Tại các thời điểm có tín hiệu của cảm biến khoảng cách nhưng khơng thuộc khoảng thời gian ZVI thì tiến hành cập nhật cho bộ lọc Kalman với việc tính ma trận theo (3-25), theo (3-26).
- Việc tính hệ số Kalman , cập nhật giá trị ước lượng sử dụng giá trị đo và cập nhật hiệp phương sai được thực hiện theo các công thức (2-
33), (2-34) và (2-35).
- Việc dự đốn theo mơ hình được thực hiện bằng cách tính các ma trận theo công thức (3-7), theo
công thức (2-29), dự đốn biến trạng thái +1− theo cơng thức (2-30) và dự đốn hiệp phương sai
+1− theo cơng thức (2-31).
Bắt đầu Đ: Đúng S: Sai ℎở ạ − = 0 0 15×1 − = 0 0 15×15 S Cập nhật Cập nhật í ℎ S Tính Tính(2 − 29) Dự đốn − Dự đốn − Kết thúc
Hình 3.2 Lưu đồ thuật tốn thực thi bộ lọc Kalman cho hệ thống INS trên bàn chân
3.6 Trích xuất thơng sớ bước đi từ quỹ đạo bàn chân
Thuật toán của hệ thống INS sử dụng bộ lọc Kalman đã trình bày ở trên đã ước lượng được vị trí chuyển động của bàn chân theo thời gian. Các thông số bước đi của người dùng như độ dài bước chân/sải chân, thời gian bước, tốc độ bước,… có thể dễ dàng xác định được dựa vào các khoảng thời gian ZVI. Do cảm biến IMU chỉ đặt trên một bàn chân nên chu kỳ sải chân là khoảng thời gian tính từ thời điểm bắt đầu của một khoảng thời gian ZVI , đến thời điểm bắt đầu của khoảng thời gian ZVI tiếp theo +1, . Các khoảng thời gian ZVI này đã được xác định từ thuật tốn tìm khoảng thời gian ZVI đã thể hiện trong (3-8). Lúc này, chu kỳ sải chân sẽ bằng 2 lần chu kỳ bước chân và độ dài sải chân sẽ bằng 2 lần độ dài bước chân.
Lúc này thời gian sải chân thứ được tính như sau:
, = | , − +1, |
Trong đó, = 0,01 là chu kỳ lấy mẫu của cảm biến IMU.
Độ dài sải chân thứ là khoảng cách giữa hai vị trí tương ứng với thời điểm bắt đầu và kết thúc chu kỳ bước chân và được tính như sau:
, =
Tốc độ bước thứ là tỉ số giữa độ dài bước và thời gian bước: =
,
Ngồi ra ta có thể tính được độ dài bước, thời gian bước là một nửa của độ dài sải chân và thời gian sải chân tương ứng. Hơn nữa quá trình đi gồm nhiều sải chân, nên các thơng số trên có thể tính trung bình để đem lại độ tin cậy lớn hơn:
=
=
=
với là tổng số sải chân đi được.
Thời gian của giai đoạn chạm đất chính là khoảng thời gian của ZVI và thời gian của giai đoạn quay là thời gian của khoảng chuyển động tương ứng. Tần số bước là tỉ lệ số bước trong tổng thời gian bước với thời gian bước. Ngoài ra cịn nhiều thơng số khác có thể dễ dàng tính ra từ quỹ đạo chuyển động của bàn chân như độ cao bàn chân (dựa vào vị trí ), hướng bàn chân (dựa vào hướng ), độ ổn định bước (dựa vào gia tốc ), góc bước (dựa vào hướng ),…
3.7 Thí nghiệm kiểm chứng hệ thớng và phân tích kết quả
Hệ thống thí nghiệm để minh chứng cho độ chính xác của thuật tốn được