7. Bố cục chung của luận án
2.2 Triển khai hệ thống định vị quán tính
2.2.2 Triển khai thuật toán của hệ thống SINS
2.2.2.1 Các hệ trục toạ độ
Trong định vị quán tính, các hệ trục toạ độ được sử dụng phổ biến là hệ toạ độ quán tính, trái đất, địa lí, vật thể [20]. Các hệ trục toạ độ này thích hợp để định vị các vật thể có phạm vi hoạt động lớn như xe, máy bay, tên lửa, tàu vũ trụ,…Tuy nhiên, trong luận án này, ứng dụng định vị quán tính trong phạm vi rất hẹp nhằm thực hiện các bài kiểm tra thông số bước đi nên chỉ sử dụng 2 hệ trục toạ độ là hệ toạ độ vật thể BCS (��������) và hệ trục toạ độ quốc tế WCS (��������) làm tham chiếu bên
ngoài để xác định quỹ đạo chuyển động của vật thể. Hệ toạ độ BCS đại diện cho vị trí và hướng của vật thể chuyển động nên được chọn gắn liền với vật thể chuyển động. Do cảm biến IMU gắn với vật thể chuyển động nên hệ toạ độ BCS thường được chọn trùng với hệ trục toạ độ vật lý của cảm biến IMU để tiện cho q trình tính tốn. Thực chất, hệ toạ độ WCS chỉ được dùng với vai trò là hệ toạ độ cục bộ với tâm trùng với tâm của hệ toạ độ BCS tại thời điểm ban đầu, có trục �� hướng lên theo phương thẳng
đứng trong khi trục �� nằm ngang và trong cùng mặt phẳng đứng với trục �� của hệ
toạ độ BCS. Các chỉ số dưới trong ký hiệu [�]� hoặc [�]� được dùng để phân biệt một vectơ � ∈ �3 đang được sử dụng trong hệ toạ độ WCS hoặc BCS khi cần thiết. Trong Chương 4 có sử dụng thêm hệ toạ độ ICS và kí hiệu [�]�
được sử dụng để chỉ vectơ � biểu diễn trong hệ ICS.
2.2.2.2 Nguyên lý hoạt động của hệ SINS
Nguyên lý hoạt động của hệ SINS là nhận các tín hiệu gia tốc và vận tốc góc đo được từ cảm biến IMU, sau đó tích phân các tín hiệu đo được để tìm ra hướng, vận tốc và vị trí của vật thể trong hệ toạ độ WCS. Các tín hiệu vận tốc góc và gia tốc đo được của vật thể là trong hệ toạ độ BCS. Hướng của vật thể được xác định bằng cách tích phân giá trị vận tốc góc đo kết hợp với hướng ban đầu của vật thể.
Do vật thể chuyển động theo nhiều hướng khác nhau, nên hướng của vectơ gia tốc của vật thể
Gia tốcVận tốc ban đầu chuyển
động Vận tốc
Vị trí ban đầu Vị trí
Hướng
Hướng ban đầu
CB vận tốc góc
IMU
CB
gia tốc Loại trừ gia tốc trọng trường Chuyển sang WCS
cũng thay đổi liên tục. Hơn nữa, giá trị gia tốc đo được là gia tốc tổng bao gồm gia tốc chuyển động của vật thể và gia tốc trọng trường. Điều này gây khó khăn cho việc xác định chính xác vị trí và vận tốc chuyển động của vật thể. Tuy nhiên, bằng việc sử dụng hướng của vật thể đã được được xác định hồn tồn có thể loại bỏ thành phần gia tốc trọng trường và xác định được gia tốc chuyển động của vật thể trong hệ WCS. Vận tốc chuyển động của vật thể trong hệ toạ độ WCS được xác định bằng cách tích phân của gia tốc chuyển động của vật thể trong hệ toạ độ WCS kết hợp với vận tốc ban đầu của vật thể. Tương tự, vị trí chuyển động của vật thể trong hệ toạ độ WCS được xác định bằng cách tích phân của vận tốc đã được xác định kết hợp với vị trí ban đầu của vật thể. Nguyên lý xác định hướng, vận tốc vị trí vật thể trong thuật tốn hệ thống SINS được thể hiện trên Hình 2.10 [20].
Hình 2.10 Nguyên lý của hệ SINS (nguồn: [11])
2.2.2.3 Chuyển hệ toạ độ dùng quaternion
Do cảm biến gia tốc trong hệ SINS gắn liền và chuyển động cùng vật thể, nên vectơ gia tốc đo được có phương và chiều thay đổi liên tục. Hơn nữa trong thành phần gia tốc đo được bao gồm gia tốc chuyển động của vật thể và gia tốc trọng trường, dẫn tới khó khăn trong việc xác định chính xác vận tốc, vị trí của vật thể. Đồng thời, do hệ toạ độ được dùng để xác định vị trí và hướng của vật thể là WCS nên cần thiết phải chuyển vectơ gia tốc đo được trong hệ BCS sang hệ WCS.
Về mặt toán học, việc chuyển một vectơ từ hệ tọa độ này sang một hệ tọa độ khác chính là nhân vectơ đó với một ma trận chuyển nên việc chuyển vectơ gia tốc đo được � từ hệ toạ độ BCS sang WCS có thể được viết dưới dạng ma trận như sau
[�]� = ��[�]�
� (2-3)
Vì ma trận �� ∈ �3×3 thực hiện chuyển vectơ gia tốc từ hệ toạ độ BCS sang WCS, nên chính �� cũng chứa đựng thông tin về sự định hướng của hệ toạ độ BCS trong khơng gian tạo bởi hệ WCS [11].
Việc tìm ma trận chuyển có thể sử dụng 3 phương pháp [20]:
- Phương pháp ma trận cosin hướng DCM là ma trận có kích thước 3 × 3 biểu diễn hướng của ba vector đơn vị của hệ trục toạ độ này trong một hệ toạ độ khác.
- Phương pháp Euler: Nguyên lý của phương pháp này dựa trên nguyên tắc khi xoay một trong hai hệ trục tọa độ có gốc trùng nhau quanh các trục của nó một góc thích hợp, thì sẽ được một hệ trục tọa độ mới, có các trục trùng với hệ tọa độ còn lại. Việc này bao gồm 3 bước quay theo thứ tự quanh trục �, trục � và trục �.
- Phương pháp quaternion: Nguyên lý của phương pháp này dựa trên nguyên tắc khi xoay một trong hai hệ trục tọa độ có gốc trùng nhau quanh một vectơ với một góc thích hợp thì sẽ được một hệ trục tọa độ mới, có các trục trùng với hệ tọa độ cịn lại.
So với phương pháp DCM và Euler thì phương pháp quaternion có nhiều ưu điểm hơn [11] nên ma trận chuyển trong trong luận án này sử dụng phương pháp quaternion. Hơn nữa, việc chuyển biểu diễn một vectơ có thể thực hiện trực tiếp bằng phép nhân quaternion mà không cần sử dụng đến ma trận chuyển.
Quaterrnion được định nghĩa như là một số phức có ba thành phần ảo [39]:
� = �� + ��� +
��� + ��� (2-4)
trong đó �� là phần thực; các thành phần ảo 𝑖, 𝑗, � có thể được coi như ba vectơ đơn vị của một hệ trục toạ độ.
� � � �
Do 𝑖, 𝑗, � là ba thành phần ảo nên có tính chất sau: 𝑖2 = 𝑗2 = �2 = 𝑖𝑗� = −1 𝑖𝑗 = −𝑗𝑖 = � 𝑗� = −�𝑗 = 𝑖 �𝑖 = −𝑖� = 𝑗 (2-5)
Như vậy, khi tính tốn liên quan đến quaternion người ta chỉ quan tâm đến các tham số ��, ��, �� và �� nên quaternion thường được biểu diễn dưới dạng:
� = [�� �� ��
��]� ∈ �4×1 (2-6)
Khi chỉ quan tâm đến hướng của hệ toạ độ BCS và WCS thì chúng được xem như có cùng gốc. Lúc này, ta có thể quay các trục của hệ toạ độ WCS về trùng với các trục của hệ toạ độ BCS bằng phép quay quanh một trục có vectơ đơn vị � =
[�� �� ��] một góc � thích hợp. Lúc này, quaternion biểu diễn cho phép quay đó
cịn được ký hiệu là �� và có dạng như sau [88]:
� � � � �
�� = [cos sin � sin � sin � ]
� 2 2 � 2 � 2 �
(2-7) Lúc này, một vectơ [�]� trong hệ toạ độ WCS được biểu diễn (chuyển hệ trục toạ độ) trong hệ toạ độ BCS [�]� và ngược lại như sau:
[ ] ⊗ �0 ] = � ⊗ [ ∗ [�][�]� 0 � [ 0 0 ] = �∗ ⊗ [ ] ⊗ � [�][�]� � (2-8) trong đó:- �∗ = [��−�� −�� −��]�
là quaternion liên hợp của
quaternion � = [�� �� �� ��]�
,
- ⊗ là phép nhân quaternion. Cho 2
quaternion � =
� �
[ � � � � �� �� ��]� và � = [�� �� �� ��]� thì phép
nhân quaternion được định nghĩa ở dạng ma trận như sau: �� −�� −�� −�� ���� −�� −�� −�� �� � ⊗ � = [�� �� −�� �� ] [�� ] = [�� �� �� −�� ] [�� ] �� �� �� −�� ���� −�� �� �� �� �� −�� �� �� �� �� �� −�� �� �� (2-9)
Ngoài ra, việc chuyển đổi giữa các hệ trục toạ độ WCS và BCS trong (2-8) có thể thực hiện sử dụng ma trận chuyển như sau
[�]� = ��[�]
� �
[�]� �= ��[�]� (2-10)
Do ma trận chuyển là ma trận trực giao nên �� = �′�.
� �
Bằng cách triển khai (2-8) ta có ma trận chuyển �� để chuyển trục toạ độ từ WCS sang BCS tương ứng với quaternion � thường được ký hiệu là �(�) (tương tự, ma trận chuyển trục toạ độ từ BCS sang WCS là ma trận chuyển vị ��(�)) và được tính như sau [88]:
2(�2 + �2) − 1 2(� � + � � ) 2(� � − � � ) � � � � � � � � � � �(�) = [2(���� − ����)2(�2 + �2) − 1 2(���� + ����)] � � 2(���� + ����) 2(���� − ����) 2(�2 + �2� �) − 1 (2-11)
Ngược lại, việc chuyển từ một ma trận quay �(�) sang quaternion � được thể hiện ở thuật tốn Shepperd [89].
2.2.2.4 Khai triển tích phân để xác định hướng, vận tốc và vị trí
Việc triển khai tích phân để xác định hướng � ∈ �4, vận tốc � ∈ �3 và vị trí
� ∈ �3 của vật thể trong hệ WCS xuất phát từ phương trình vi phân của các đại lượng hướng, vị trí và vận tốc như sau [90]:
1 �� = Ω 2 �� = ��(�)[�]� � � = � (2-12) trong đó: 0 −�� −�� −�� - Ω = [�� 0 �� −�� ], �� �� − � � � � � 0 − � � � � 0
- ω = [ωxωy ωz] là vận tốc góc của vật thể trong hệ toạ độ BCS,
� �
- [�]� là gia tốc của vật thể trong hệ toạ độ BCS. Vận tốc góc � và gia tốc [�]� của vật thể được tính từ tín hiệu đo của cảm biến IMU như sau:
� = �� − �� [�]� = �� − �� −
�(�)[�̃]�
(2-13)
trong đó:
- �� ∈ �3 và �� ∈ �3 là các tín hiệu đo của cảm biến vận tốc góc và cảm biến gia tốc,
- [�̃]� = [0 0 �]� là vectơ gia tốc trọng trường trong hệ toạ độ
WCS, � thường được chọn là 9,81 𝑚/𝑠2,
- Các thành phần �� ∈ �3 và �� ∈ �3 là nhiễu chậm thay đổi của cảm biến gia tốc và cảm biến vận tốc góc. Trong đó, �� có thể được xác định sơ bộ bằng cách thu thập dữ liệu cảm biến lúc cảm biến không chuyển động. Ngoài ra, �� và �� cịn có thể được đưa vào bộ lọc để ước lượng một cách chính xác hơn.
Việc triển khai các khâu tích phân cho (2-12) được thực hiện bằng khai
triển Taylor bậc 3 đối với quaternion �, bậc 1 đối với vận tốc � và vị trí � nhằm xác định
��+1, ��+1 và ��+1 tại thời điểm (� + 1)� thông qua giá trị của
chúng tại thời điểm �� là ��, �� và ��. Lúc này ta có: �̈��2 � ′′′�3 � = � + �� � +� + �+1 � � 2 6 1 ��+1 = �� + 2 ���� 1 ��+1 = �� + 2 ��� � (2-14) Khai triển (2-14) sử dụng (2-12), ta có [91]:
3 1 1 1 1 ��+1 = (�‖�4 + Ωk� − Ω�−1� − �‖2�2�4 − Ω�Ω�−1�2 − ‖��‖2Ω��3) �� 4 4 6 24 48 1 ��+1 ≈ �� +[�̃]� � 2+ �′(��(�)�� )� −�+1′(��+1)�� � 1 ��+1 ≈ �� + 2 (��+1 + ��)� (2-15)
trong đó có sử dụng các dữ kiện và tính chất sau:
- � là thời gian lấy mẫu của cảm biến IMU. Trong luận án, sử dụng IMU có tần số 100 �� nên � = 0,01 𝑠.
- Ω� = k Ω� − Ω�−1 �
- Ω2 = −‖�‖2�3
- Ω3 = −‖�‖2Ω
Theo công thức (2-1 5 ), trước tiên cần tính giá trị quaternion ��+1 dựa vào vận tốc góc �� , ��+1 và giá trị quaternion �� đã biết trước đó. Sau đó, các giá trị quaternion ��, ��+1 được sử dụng để tính vận tốc
��+1 dựa vào giá trị của cảm biến gia tốc ��� , �� �+1 và vận tốc
�� đã biết trước đó. Cuối cùng, vị trí ��+1 được tính dựa vào giá trị vận tốc
��, ��+1 và vị trí �� đã biết trước đó.
Các giá trị ban đầu �1, �1, �1 để đưa vào công thức ( 2 - 1 5 ) được xác
định tại thời điểm đầu của q trình chuyển động. Trong thuật tốn, một q trình chuyển động ln bắt đầu và kết thúc bằng thời điểm đứng yên. Lúc này vận tốc chuyển động
bằng khơng �1 = 03×1, gốc của hệ toạ độ WCS được chọn trùng với góc của hệ toạ độ BCS tại thời điểm ban đầu nên vị trí của gốc hệ toạ độ BCS trong hệ toạ độ WCS bằng không �1 = 03×1. Phép quay giữa BCS và WCS ban đầu được tính dựa trên cơ
sở trục �� = [0 0 1]� và �� = [1 0 0]� của hệ toạ độ WCS. Trong đó, trục
�� thẳng đứng hướng lên trên trùng với phương gia tốc trọng trường. Tại thời điểm ban đầu, IMU không chuyển động nên gia tốc chuyển động bằng không và do đó cảm biến gia tốc đo được gia tốc trọng trường. Trục �� được chọn nằm trên cùng mặt
phẳng đứng với ��. Trong luận án, sử dụng thuật toán TRIAD [92] để xác định ma trận quay �(�1) giữa hai hệ trục toạ độ BCS và WCS theo điều kiện như sau
�(� ) = ���� 𝐷 ([ , � �1 ] , [ , � ]) 1 � ‖� �1 ‖ � �
Việc tính �1 từ �(�1) đã được thực hiện bằng thuật toán Shepperd [89]. Như vậy, từ nguyên lý hoạt động của hệ SINS trình bày tại Mục 2.2.2.2 cũng như cách triển khai của thuật tốn INA ở Mục 2.2.2.4, có thể thấy chất lượng của hệ SINS phụ thuộc trực tiếp vào độ chính xác của cảm biến IMU. Tuy nhiên các giá trị
đo được từ cảm biến IMU luôn luôn tồn tại nhiễu, đặc biệt là IMU dùng trong dân dụng. Hơn nữa, việc sử dụng xấp xỉ Taylor trong các khâu tích phân cũng gây ra sai số tích luỹ cho hệ thống. Các sai số này tích lũy theo thời gian do việc tích phân của thuật tốn INA. Do vậy, hệ thống SINS chỉ chính xác trong một khoảng thời gian
ngắn. Trong khi đó, các vật thể trong luận án chuyển động trong thời gian dài, vì vậy cần thiết phải áp dụng nhiều giải pháp khác nhau để nâng cao chất lượng hệ SINS [20].
Hệ thống SINS cần kết hợp với các hệ thống đo hoặc cảm biến khác để hiệu chỉnh lại các kết quả ước lượng sử dụng các bộ lọc. Hiện nay có 5 bộ lọc được sử dụng phổ biến cho hệ SINS là Euler-KF, MEKF, USQUE, MRP-UKF và DoEuler- UKF. Mỗi bộ lọc đều có ưu và nhược điểm riêng, trong đó bộ lọc MEKF vừa đảm bảo độ chính xác và thời gian tính tốn nhanh [87]. Trong luận án này, sử dụng bộ lọc Kalman kiểu MEKF cho hệ thống INS.