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 quá 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ể
Trang 49
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 toán hệ thống SINS được thể hiện trên Hình 2.10 [20].
Hướng Vận tốc Vị trí CB gia tốc CB vận tốc góc IMU Hướng ban đầu Vận tốc ban đầu Chuyển sang WCS Loại trừ gia tốc trọng trường Gia tốc chuyển động Vị trí ban đầu
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 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.
Trang 50
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ạ độ.
Trang 51
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𝜃2 sin𝜃2𝑢𝑥 sin𝜃
2𝑢𝑦 sin 𝜃 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)
Trang 52
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 toá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]:
𝑞̇ = 12Ω𝑞 𝑣̇ = 𝐶𝑇(𝑞)[𝑎]𝑏 𝑟̇ = 𝑣 (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:
Trang 53 𝜔 = 𝑦𝑔− 𝑏𝑔 [𝑎]𝑏 = 𝑦𝑎 − 𝑏𝑎 − 𝐶(𝑞)[𝑔̃]𝑤 (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. Ngồ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ó: 𝑞𝑘+1= 𝑞𝑘+ 𝑞̇𝑘𝑇 +𝑞̈𝑘𝑇2 +2 𝑞𝑘′′′6𝑇3 𝑣𝑘+1 = 𝑣𝑘+12 𝑣̇𝑘𝑇 𝑟𝑘+1= 𝑟𝑘+12 𝑟̇𝑘𝑇 (2-14) Khai triển (2-14) sử dụng (2-12), ta có [91]: 𝑞𝑘+1 = (𝐼4+34 Ωk𝑇 −14 Ω𝑘−1𝑇 −16‖𝜔𝑘‖2𝑇2𝐼4−24 Ω1 𝑘Ω𝑘−1𝑇2−481 ‖𝜔𝑘‖2Ω𝑘𝑇3) 𝑞𝑘 𝑣𝑘+1≈ 𝑣𝑘+12 (𝐶′(𝑞𝑘+1)𝑦𝑎𝑘+1+ 𝐶′(𝑞𝑘)𝑦𝑎𝑘)𝑇 − [𝑔̃]𝑤𝑇 𝑟𝑘+1≈ 𝑟𝑘+12(𝑣𝑘+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 𝑇
Trang 54
- Ω2= −‖𝜔‖2𝐼3
- Ω3= −‖𝜔‖2Ω
Theo cơng thức (2-15), 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-15) đượ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 toá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
Trang 55
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.