7. Bố cục chung của luận á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 xây dựng như Hình 3.3. Trong đó, một cảm biến IMU (MTi-100, hãng Xsens, Hà Lan) và 2 cảm biến khoảng cách VL6180 [103] được gắn trên chiếc giày dùng trong thí nghiệm. Việc sử dụng cảm biến khoảng cách thứ 2 này dùng để đánh giá liệu rằng vị trí đặt của cảm biến có ảnh hưởng đến kết quả nghiên cứu hay khơng và việc dùng nhiều cảm biến có nâng cao đáng kể độ chính xác hay khơng.
Trên hệ thống này còn gắn các hạt phản quang cố định với cảm biến IMU để hệ thống giám sát chuyển động bằng camera (Optitrack Six Flex 13 [104]) ghi lại quỹ đạo chuyển động của bàn chân. Hệ thống Optitrack Six Flex 13 được quốc tế công nhận là hệ thống giám sát chuyển động chuẩn dùng làm tham chiếu cho các hệ thống
khác [105]. Như vậy kết quả quỹ đạo chuyển động ước lượng được bằng thuật toán đề xuất sẽ được so sánh với quỹ đạo chuẩn được ghi lại bởi hệ thống camera.
Hạt phản quang
Cảm biến qn tính
Hình 3.3 Hệ thống INS đặt trên bàn chân sử dụng cho thí nghiệm (nguồn [50])
Giá trị khởi tạo ban đầu và giá trị ước lượng thông số của cảm biến khoảng cách 1 và cảm biến khoảng cách 2 như trong Bảng 3.1.
Bảng 3.1 Giá trị khởi tạo ban đầu và ước lượng thông số của cảm biến khoảng
cách tại 2 vị trí
Vị trí
Vị trí 1
Vị trí 2
Như trong Chương 1 đã khảo sát, các bài kiểm tra thông số bước đi được thực hiện tại nơi bằng phẳng (nền nhà, hành lang,…) với khoảng cách di chuyển nhỏ. Để đánh giá định tính và định lượng hệ thống, 02 thí nghiệm được thực hiện gồm thí nghiệm đi 3 bước dưới sự giám sát của hệ thống Optitrack tại Khoa Điện – Trường Đại học Ulsan, Hàn Quốc và thí nghiệm đi dọc hành lang 30 . Việc thí nghiệm đi 3 bước dưới sự giám sát của hệ thống Optitrack giúp xác định sai số trong 3D theo từng bước đi trong khi niệc thí nghiệm đi dọc hành lang giúp xác định sai số trong quãng đường dài.
3.7.1 Thí nghiệm với hệ thống OptiTrack
Thí nghiệm được thực hiện với một người dùng (xem thông tin tại hàng 1 của
Bảng 3.4) đi 4 lần, mỗi lần thực hiện 3 sải chân trên sàn nhà phẳng dưới sự giám sát
của hệ thống OptiTrack như thể hiện trong Hình 3.4 . Đây là hệ thống Flex-13
Optitrack [104] gồm 6 camera 1,3MP được cấu hình tốc độ ghi hình là 100 Hz. Mục đích của thí nghiệm này nhằm đánh giá chi tiết quỹ đạo 3D trong từng bước chân và đánh giá vai trò của bộ lọc Kalman và từng cập nhật theo thời gian.
Camera
Hạt phản quang
Mơ đun IMU
Hình 3.4 Hệ thống OptiTrack được dùng trong thí nghiệm và minh hoạ việc sử dụng
hệ thống ghi lại chuyển động
Để tính các sai số ước lượng trên các trục , và thì quỹ đạo ước lượng và quỹ đạo thực ghi lại bởi hệ thống OptiTrack phải thể hiện trong cùng hệ trục toạ độ. Hệ trục toạ độ của hệ thống OptiTrack được cấu hình có trục hướng lên thẳng đứng, trục trùng với hướng di chuyển. Quỹ đạo ước lượng được thể hiện trong hệ toạ độ WCS có trục cũng hướng lên thẳng đứng, quỹ đạo này được quay một góc đúng bằng góc giữa trục của hệ toạ độ WCS và hướng di chuyển. Do gốc hệ toạ đô WCS được đặt tại điểm đầu của chuyển động nên quỹ đạo thực ghi lại bởi hệ thống OptiTrack sẽ trừ cho toạ độ đầu tiên của nó để đưa điểm đầu của quỹ đạo chuyển động thực về gốc toạ độ. Như vậy, quỹ đạo chuyển động ước lượng và quỹ đạo thực
ghi lại bởi hệ thống OptiTrack là cùng trong hệ toạ độ và được dùng để tính sai số trên các trục , , .
Hình 3.5 Tín hiệu IMU và việc phát hiện ZVI
Tín hiệu gia tốc và vận tốc góc của cảm biến IMU theo 3 trục được thể hiện trong 3 hình đầu của Hình 3.5. Việc xác định độ lớn của độ thay đổi gia tốc và độ lớn của vận tốc góc cũng như việc phát hiện các khoảng ZVI riêng lẻ theo công thức (3-8) được thể hiện trong 2 hình cuối của Hình 3.5. Như có thể thấy, việc xác định các khoảng ZVI dùng đơn lẻ cảm biến gia tốc hoặc cảm biến vận tốc góc đều cho kết quả
khơng chính xác. Do vậy cần kết hợp các khoảng ZVI do cảm biến gia tốc và vận tốc góc phát hiện như trong Hình 3.6. Trong đó, có 3 khoảng chuyển động và 4 khoảng ZVI tương ứng với việc thực hiện 3 sải chân ở thí nghiệm trên. Để tập trung chi tiết về dữ liệu và hoạt động của thuật tốn, Hình 3.5 và Hình 3.6 đã được cắt bỏ khoảng thời gian đầu và cuối tương ứng với việc bàn chân đứng n trước và sau q trình thí nghiệm.
Về mặt định tính, vị trí ước lượng và sai số của việc ước lượng vị trí theo các trục được thể hiện trong các Hình 3.7, Hình 3.8, Hình 3.9 và Hình 3.10. Trong đó,
hình bên trái thể hiện vị trí bàn chân theo các trục với đường nét liền màu xanh thể hiện vị trí ước lượng, đường nét đứt màu đỏ thể hiện vị trí thực ghi lại bằng hệ thống OptiTrack trong khi hình bên phải thể hiện sai số của vị trí ước lượng so với vị trí thực.
Hình 3.6 Phát hiện ZVI của chuyển động từ các ZVI của cảm biến gia tốc và vận
tốc góc
Về mặt định lượng, Bảng 3.2 thể hiện các tiêu chí đánh giá sai số gồm sai số tối đa theo các trục, sai số trung bình trên các trục, sai số vị trí, sai số khoảng cách tương đối trong các trường hợp: chỉ sử dụng thuật toán INA, sử dụng bộ lọc Kalman
dùng cập nhật ZUPT, sử dụng cập nhật ZUPT kết hợp độ cao tại các khoảng ZVI và hệ thống đề xuất sử dụng cảm biến khoảng cách.
Trong Hình 3.7 cho thấy việc ước lượng vị trí sơ bộ của bàn chân sử dụng thuật tốn INA khơng sử dụng bộ lọc Kalman thì sai số của việc ước lượng bị tích luỹ và tăng lên rất nhanh, chỉ sau 3 sải chân đi sai lệch vị trí lên đến trên 34,9
tương ứng với sai số 1216% (xem cột 2 của Bảng 3.2) . Lúc này, đường vị trí ước lượng sẽ bị phân kỳ so với đường vị trí thực. Do vậy các hệ thống INS phải sử dụng các bộ lọc để hiệu chỉnh kết quả ước lượng.
Hình 3.7 Vị trí bàn chân khơng sử dụng bộ lọc Kalman
Trong cập nhật cho bộ lọc Kalman gồm cập nhật vận tốc ZUPT, cập nhât độ cao tại khoảng thời gian ZVI và cập nhật sử dụng cảm biến khoảng cách. Trong
Hình 3.8 thể hiện kết quả việc cập nhật vận tốc ZUPT tại các khoảng thời gian ZVI.
Lúc này, đường vị trí ước lượng đã bám theo đường vị trí thực đặc biệt trên các trục
, . Đây là phương pháp cập nhật rất phổ biến trong định vị quán tính. Với việc cập
nhật ZUPT này, sai số vị trí giảm từ 24,9 xuống cịn 0,023 tương ứng với sai số tương đối về khoảng cách giảm từ 1216% xuống 0,51% so với việc ước lượng sơ bộ chỉ dùng thuật toán INA (xem cột 3 của Bảng 3.2).
Do việc chuyển động theo trục (độ cao bàn chân) rất nhỏ nên thành phần sai số theo phương đứng rất đáng kể so với quãng đường di chuyển theo phương đứng. Nếu việc di chuyển trên mặt phẳng như sàn nhà thì việc cập nhật độ cao bàn chân lúc chạm đất được sử dụng kèm với cập nhật ZUPT như trong Hình 3.9 để loại bỏ sai số độ cao bàn chân sau mỗi bước đi. Trong đó sai số tối đa trên phương đứng giảm từ khoảng 5 xuống khoảng 4 và sai số trung bình trên phương đứng giảm từ 1,48 xuống còn 1,01 (xem cột 4 của Bảng 3.2) so với trường hợp chỉ sử dụng cập nhật ZUPT.
Bảng 3.2 Sai số vị trí khi sử dụng các loại cập nhật (m)
Sai số (m)
Tối đa trục x Tối đa trục y Tối đa độ cao
TB trục x TB trục y TB độ cao Vị trí Khoảng cách