Luận văn thạc sĩ VNU UET nâng cao chất lượng hệ thống tích hợp INS,GPS sử dụng bộ lọc kalman

GIỚI THIỆU HỆ THỐNG GPS

Hệ thống định vị toàn cầu (tiếng Anh: Global Positioning System - GPS) là hệ thống xác định vị trí dựa trên vị trí của các vệ tinh nhân tạo, do Bộ Quốc phòng Hoa Kỳ thiết kế, xây dựng, vận hành và quản lý Hệ thống định vị toàn cầu GPS thu thập các thông tin về tọa độ (vĩ độ và kinh độ), tốc độ và độ cao của vật thể Hệ thống GPS gồm 24 vệ tinh phóng lên quỹ đạo trái đất (xem Hình 1) [3]

Các hệ thống dẫn đường truyền thống hoạt động dựa trên các trạm phát tín hiệuvô tuyến điện Được biết đến nhiều nhất là các hệ thống sau: LORAN – (LOng RAnge

Navigation) – hoạt động ở giải tần 90-100 kHz chủ yếu dùng cho hàng hải, hay TACAN –

(TACtical Air Navigation) – dùng cho quân đội Mỹ và biến thể với độ chính xác thấp VOR/DME – VHF (Omnidirectional Range/Distance Measuring Equipment) – dùng cho hàng không dân dụng

Hình 1 Quỹ đạo bay của các vệ tinh GPS

Hệ thống định vị toàn cầu GPS thu thập các thông tin về tọa độ (vĩ độ và kinh độ), độ cao và tốc độ của các vật thể [4] Các vệ tinh GPS bay vòng quanh Trái Đất hai lần trong một ngày theo một quỹ đạo rất chính xác và phát tín hiệu có thông tin xuống Trái Đất Các máy thu GPS nhận thông tin này và bằng phép tính lượng giác tính được chính đi từ vệ tinh với thời gian nhận được chúng Sai lệch về thời gian cho biết máy thu GPS ở cách vệ tinh bao xa Với nhiều cách đo được tới vệ tinh máy thu có thể tính được vị trí của người dùng và hiển thị lên bản đồ điện tử của máy

Hiện tại, GPS sử dụng 2 tần số L1 = 1575,42MHz và L2 = 1227,60MHz [1,6] Các sóng mang L1, L2 được điều biến bởi 3 loại mã sau:

- Mã P: là mã chính xác, có tần số 10,23 MHz, độ dài toàn phần 267 ngày Tuy vậy người ta đã chia mã này thành các đoạn có độ dài 7 ngày và gắn cho mỗi vệ tinh trong hệ thống GPS một trong các đoạn mã như thế, cứ sau 1 tuần lại thay đổi nên khó bị giải mã để sử dụng nếu ko được phép Mã P điều biến cả 2 sóng mang L1 và L2

- Mã C/A có tần số 1,023MHz, nó chỉ điều biến sóng mang L1, mã C/A được sử dụng cho mục đích dân sự, mỗi vệ tinh được gán 1 mã C/A riêng biệt

- Mã D là mã dùng để truyền lịch vệ tinh mới nhất, thông số của lớp khí quyển sóng điện từ truyền qua, thời gian của hệ thống, sai số đồng hồ vệ tinh, phân bố của các vệ tinh trên quỹ đạo Nó điều biến cả 2 sóng mang L1 và L2

Khi hoạt động, tần số L1 bao gồm 2 tín hiệu số, được gọi là mã P và mã C/A Mã P nhằm bảo vệ thông tin khỏi những sự truy nhập trái phép Tuy nhiên, mục đích chính của các tín hiệu mã hóa là nhằm tính toán thời gian cần thiết để thông tin truyền từ vệ tinh tới một thiết bị thu nhận trên mặt đất Sau đó, khoảng cách giữa 2 bên được tính bằng cách nhân thời gian cần thiết để tín hiệu đến nơi với tốc độ của ánh sáng là 300.000 km/giây

Tuy nhiên, tín hiệu có thể bị sai đôi chút khi đi qua bầu khí quyển Vì vậy, kèm theo thông điệp gửi tới các thiết bị nhận, các vệ tinh thường gửi kèm luôn thông tin về quỹ đạo và thời gian Việc sử dụng đồng hồ nguyên tử sẽ đảm bảo chính xác về sự thống nhất thời gian giữa các thiết bị thu và phát Để biết vị trí chính xác của các vệ tinh, thiết bị thu GPS còn nhận thêm 2 loại dữ liệu mã hóa:

 Dữ liệu Almanac: được cập nhật định kỳ và cho biết vị trí gần đúng của các vệ tinh trên quỹ đạo Nó truyền đi liên tục và được lưu trữ trong bộ nhớ của thiết bị thu nhận khi các vệ tinh di chuyển quanh quỹ đạo

 Dữ liệu Ephemeris: phần lớn các vệ tinh có thể hơi di chuyển ra khỏi quỹ đạo chính của chúng Sự thay đổi này được ghi nhận bởi các trạm kiểm soát mặt đất

Việc sửa chữa những sai số này là rất quan trọng và được đảm nhiệm bởi trạm chủ trên mặt đất trước khi thông báo lại cho các vệ tinh biết vị trí mới của chúng

Thông tin được sửa chữa này được gọi là dữ liệu Ephemeris Kết hợp dữ liệu Almanac và Ephemeris, các thiết bị nhận GPS biết chính xác vị trí của mỗi vệ tinh

Nhược điểm cơ bản của các hệ thống định vị là khi bị che khuất tầm nhìn vệ tinh, tín hiệu GPS bị sai số hoặc bị mất tín hiệu Ngoài ra còn có một số nguyên nhân sau gây ra sai số của phương pháp định vị GPS [2].

 Giữ chậm của tầng đối lưu và tầng ion – Tín hiệu vệ tinh bị chậm đi khi xuyên qua tầng khí quyển

 Tín hiệu đi nhiều đường – Điều này xảy ra khi tín hiệu phản xạ từ nhà hay các đối tượng khác trước khi tới máy thu

 Lỗi đồng hồ máy thu – Đồng hồ có trong máy thu không chính xác như đồng hồ nguyên tử trên các vệ tinh GPS

 Lỗi quỹ đạo – Cũng được biết như lỗi thiên văn, do vệ tinh thông báo vị trí không chính xác

 Số lượng vệ tinh nhìn thấy – Càng nhiều vệ tinh được máy thu GPS nhìn thấy thì càng chính xác Nhà cao tầng, địa hình, nhiễu loạn điện tử hoặc đôi khi thậm chí tán lá dầy có thể chặn thu nhận tín hiệu, gây lỗi định vị hoặc không định vị được

Nói chung máy thu GPS không làm việc trong nhà, dưới nước hoặc dưới đất

 Che khuất về hình học – Điều này liên quan tới vị trí tương đối của các vệ tinh ở thời điểm bất kì Phân bố vệ tinh lí tưởng là khi các quả vệ tinh ở vị trí tạo các góc rộng với nhau Phân bố xấu xảy ra khi các quả vệ tinh ở trên một đường thẳng hoặc cụm thành nhóm

HỆ THỐNG INS

Nguyên lý hoạt động của INS

Nguyên lý hoạt động của INS là tổng hợp các tín hiệu đo được bởi một cơ cấu đo lường quán tính IMU (Inertial measurement Unit), để xác định trạng thái hiện tại của hệ thống Các tín hiệu đo được bởi IMU bao gồm vận tốc góc (gyros) và gia tốc (accelerometer) Cấu tạo IMU bao gồm 3 cảm biến gia tốc và 3 cảm biến vận tốc góc Ở đây một hệ thống INS bao gồm IMU và thuật toán tính toán được tích hợp cùng Thuật toán sử dụng trong luận văn này là thuật toán SINS của Salychev (Phụ lục 2) INS đưa ra các thông số cho người dùng như: vị trí (kinh độ, vĩ độ, cao độ), vận tốc, các góc tư thế (góc nghiêng, góc chúc, góc hướng) (xem Hình 3)

Hình 2 dưới đây mô tả một khối IMU [2] Với những IMU trước đây sử dụng cảm biến quán tính hoạt động theo nguyên tắc cơ khí, thường có kích thước lớn, hoạt động kém hiệu quả, giá thành cao và tiêu thụ nhiều năng lượng Ngày nay khi khoa học công nghệ phát triển, đặc biệt là công nghệ vật liệu mới và công nghệ vi chế tạo đã tạo ra các cảm biến vi cơ có kích thước rất nhỏ (cỡ centimet), hoạt động hiệu quả, tiêu thụ ít năng lượng và đặc biệt là giá thành hạ Điều này mở ra cơ hội cho việc ứng dụng các cảm biến vi cơ trong nhiều lĩnh vực đời sống

Hình 2 Cấu trúc của IMU

Hình 3 dưới đây mô tả nguyên lí hoạt động của hệ thống INS [8]

Gravity, Non-gravitational acceleration correction

Coordinate system Transformation integration integration

Hình 3 Hệ thống định vị quán tính

Ma trận chuyển hệ tọa độ

Trong thuật toán SINS, để đưa ra các thông số cuối cùng, chúng ta cần các phép tính toán chuyển đổi qua các hệ tọa độ với nhau Sau đây học viên đưa ra một số ma trận chuyển đổi chính trong khi sử dụng thuật toán SINS

Một vấn đề quan trọng khi xác định vật thể trong không gian 3 chiều đó là tọa độ của vật trong hệ tọa độ Đề Các hoặc là 3 góc Euler [5]

Các góc Euler (, , ) (Hình 4 ) thay thế cho ba chuyển động quay kết hợp, di chuyển hệ trục tham chiếu đến một hệ trục ta đang xét Các chuyển động thành phần là chuyển động quanh một trục, có thể là , hoặc Tương ứng với các chuyển động này, ta có ma trận xoay thành phần được định nghĩa như sau

 ] (2.3) Trong đó R(Z,), R(Y, θ) và R(X, ) lần lượt là các ma trận góc xoay quanh trục x, y, z theo các góc tương ứng , , 

Khi đó ta có ma trận xoay tổng quát trong không gian 3 chiều Euclide như sau: cos sin 0 cos 0 sin 1 0 0 sin cos 0 0 1 0 0 cos sin

Phương trình (2.5) được sử dụng phản hồi để hiệu chỉnh lại các giá trị Quaternion trong mô hình Simulink

Ngoài cách xác định ma trận chuyển đổi giữa các hệ tọa độ bằng góc Euler, ta có thể sử dụng tới kĩ thuật tính quaternion đơn giản hơn

Một quaternion đơn vị có thể được định nghĩa như sau:

| | Chúng ta có thể kết hợp quaternion với một chuyển động quay quanh một trục như sau:

Trong công thức (2.6) trên, là góc quay (giá trị tính bằng Radian) và cos ( ), cos ( ), cos ( ) là các cosin định hướng của trục Khi đó ma trận quay của chuyển động quay trong không gian 3 chiều Euclide được xác định theo (2.7) như sau :

2.2.3 Tính góc Euler từ ma trận xoay

Giả sử ta có một ma trận xoay biết trước là

Từ phương trình (2.5) ta được: tan = a 32 /a 33 , sin = -a 31 , tan = a 21 /a 11 Như vậy

2.2.4 Tính Quaternion từ ma trận xoay

Với các góc Euler biết trước, ta có thể tính được Quaternion được xác định theo công thức (2.13) [10]

Từ phương trình (2.7) ta thu được các phương trình sau:

Giả sử rằng là dương sign( ) = sign(1) sign( ) = sign( - ) sign( ) = sign( - ) sign( ) = sign( - ) 2.2.5 Tính góc Euler từ Quaternion

Từ phương trình (2.5), (2.7) ta tính được các góc Euler như sau:

Phương trình chuyển động

Tư thế của vật thể trong không gian được xác định bởi 3 góc Euler [4,6] Mối liên hệ giữa các vận tốc góc của Roll, Pitch, Yaw (p, q, r) và 3 góc Euler , ,  được tính theo công thức (2.15) sau:

         ) [ ] (2.15) Như vậy khi tích phân các phương trình trên ta thu được các góc Euler

Các gia tốc (a x , a y , a z ) dọc theo các trục của hệ tọa độ vật thể liên hệ với 3 vận tốc (U, V, W) trong hệ tọa độ trái đất theo phương trình sau : ̇  ̇   (2.16) ̇  

Sau khi tích phân phương trình (2.16), chúng ta thu được U, V, W Sử dụng ma trận Direct Cosine Matrix (DCM), chúng ta có thể chuyển từ hệ tọa độ trái đất sang hệ tọa độ định vị:

Trong đó : DCM phương trình (2.5)

Tích phân phương trình (2.17), ta thu được vị trí của vật Tiếp theo chúng ta có thể thu được kinh độ, vĩ độ và độ cao của vật thông qua các phương trình (2.18) sau : ̇ ̇

Mô hình lỗi INS

Có nhiều loại sai số trong các hệ thống INS và chủ yếu là do các cảm biến quán tính gây nên [1,6] Bảng 1 liệt kê một số lỗi gây ra bởi các cảm biến gia tốc và vận tốc góc

Loại Gây nên sai số

Lỗi vị trí khi lắp đặt cảm biến Góc nghiêng, góc chúc và góc hướng Độ lệch (offset) của cảm biến gia tốc

Lối ra cảm biến gia tốc sẽ bị lệch đi một giá trị không đổi Giá trị này lại thay đổi mỗi khi tắt /bật thiết bị

Hiện tượng lệch và trôi của cảm biến vận tốc góc (do tác động của nhiệt độ)

Vật thể không chuyển động nhưng vẫn có vận tốc góc không đổi

Nhiễu ngẫu nhiên Lỗi ngẫu nhiên trong đo lường

Bảng 1 Lỗi gây ra bởi cảm biến quán tính

Những lỗi trong đo gia tốc và vận tốc góc sẽ dẫn tới các lỗi tăng dần khi xác định vị trí, vận tốc, và góc tư thế của vật thể bay Các lỗi tăng dần này được gọi là lỗi dẫn đường Có thể nhận thấy chắc chắn rằng hệ thống dẫn đường quán tính không thể hoạt động tự trị được mà phải được kết hợp với một hệ thống khác

Các véc tơ trạng thái lỗi của INS bao gồm lỗi hệ tọa độ, lỗi vận tốc, và các lỗi tư thế Khi các lỗi thay đổi theo thời gian, chúng được mô tả thông qua các phương trình vi phân Đối với các góc lệch nhỏ  N ,  E ,  Up [7,9] là độ lệch giữa hệ tọa độ p-frame và ll- frame (xem Hình 5) [8]

Hình 5 Sai lệch giữa hệ tọa độ p-frame và N- frame

Ma trận chuyển giữa 2 hệ tọa độ này có thể được mô tả theo (2.19), được suy ra từ dạng ma trận DCM chuẩn, và giả sử các góc này nhỏ thỏa mãn cos Φ = 1 và sin Φ = Φ [7,9]

Theo phương trình Poisson, vận tốc góc trong hệ tọa độ p-frame có thể được biểu diễn theo vận tốc góc tuyệt đối trong hệ tọa độ ll-frame và các đạo hàm của các lỗi tư thế

Dựa theo sai số của cảm biến  E ,  N ,  Up thì :

Lỗi tính toán vận tốc góc có thể thu được bằng cách vi phân các biểu thức theo hình chiếu trên hệ tọa độ ll-frame

Trong đó là vận tốc và các lỗi tương ứng trên hệ tọa độ ll-frame

, : vĩ độ và lỗi vĩ độ

U : tốc độ quay của trái đất

Thay phương trình (2.21) và (2.22) vào (2.20) ta được :

 ̇ (2.25) Tương tự các gia tốc cũng được biểu diễn như sau :

Trong đó : E , N , up là các hệ số tỉ lệ gia tốc.

B N, B E , B Up là các độ lệch 0 của gia tốc

      Đưa thêm lực Coriolis vào trong tính toán lỗi ta thu được : ̇  

Sự khác nhau giữa vận tốc góc trong hệ tọa độ p-frame và ll-frane là nguyên nhân chính gây ra độ trôi gyro và các lỗi tính toán các vận tốc góc chuyển từ ll-frame sang E- frame Ở đây, lỗi vị trí có thể thu được từ các phương trình sau: ̇ ̇ Gỉa sử h = 0 thì

(2.29) Như vậy ta có thể đơn giản các phương trình lỗi của INS như sau : ̇   ̇  

HỆ THỐNG TÍCH HỢP INS/GPS

Lý Thuyết

3.1.1 Bộ lọc Kalman Để định vị đối tượng chuyển động ta thường sử dụng các công cụ tính toán dự báo đến điểm tiếp theo của đối tượng dựa trên cơ sở sử dụng bộ lọc Kalman Bộ lọc Kalman bao gồm các phương pháp tính toán truy hồi hiệu quả cho phép ước đoán trạng thái của một quá trình

Mô hình của bộ lọc Kalman (Hình 6) bao gồm 2 bước sau [2]:

 Bước dự đoán: trạng thái tiếp theo của hệ thống được dự báo bởi các giá trị đo trước

 Bước cập nhật: các trạng thái hiện tại của hệ thống được đánh giá bởi các số liệu đo được tại thời điểm đó

Hình 6 Thuật toán Kalman cổ điển

Giả sử bộ lọc Kalman tuyến tính như sau [6]:

Trong đó: x k là vector trạng thái tại thời điểm k

A k,k-1 là ma trận chuyển có kích thước (nxn);

G k,k-1 là ma trận đầu vào có kích thước (nxr); w k-1 là ma trận nhiều đầu vào có kích thước (rx1) Vector đo lường có dạng :

(3.2) Ở đây z k là vector đo lường, kích thước (mx1); H k là ma trận đo, kích thước (mxn) w và v có hàm phân bố theo hàm Gauss với trung bình bằng 0 và ma trận hiệp phương sai (covariance) lần lượt là Q và R w~N(0,Q); v~N(0,R);

Giả sử x k - và x k + lần lượt là tiên nghiệm và hậu nghiệm ước lượng của véc tơ trạng thái x tại thời điểm k và đã biết ma trận hiệp phương sai lỗi x k thì lỗi ước lượng được xác định như sau: e k = x k - x k - (3.3)

Ma trận hiệp phương sai sẽ là :

P k = E(e k *e k T ) (3.4) Giá trị cập nhật trạng thái x k được ước lượng : xk + = xk

Với K k độ khuếch đại Kalman và được tính theo công thức sau:

(3.6) Thay phương trình (3.2) vào (3.5) ta được :

P k = (I – K k H k ).P k - (3.7) Phương trình (3.1) cũng tương đương với phương trình vi phân sau :

Do hệ thống định vị làm việc ở chế độ thời gian thực nên chúng ta chuyển phương trình trên về dạng rời rạc theo thời gian:

Trong đó Φ k là ma trận chuyển, và có dạng như sau :

Và ma trận hiệp phương sai liên quan tới w k sẽ là :

[ ]    Dưới đây là lưu đồ thuật toán Kalman (Hình 7) [7]:

Hình 7 Lưu đồ tính toán với bộ lọc Kalman

3.1.2 Hệ thống tích hợp INS/GPS

Khi tích hợp 2 hệ thống INS với GPS, chúng ta có thể sử dụng 2 mô hình kết hợp: phương thức lỏng (Hình 8) và chặt (Hình 9) và được mô tả dưới đây [2]

Hình 8 Sơ đồ INS/GPS theo phương thức vòng hở

Hình 9 Sơ đồ INS/GPS theo phương thức vòng kín

Trong luận văn này, tác giả sử dụng mô hình vòng hở và dùng bộ lọc Kalman để ước lượng lỗi INS Lỗi INS sau khi ước lượng, có thể bù cho hệ thống INS theo 2 kiểu Feedback (phản hồi, kín) hay Feedforward (hở) (Hình 10) [3]

Tín hiệu lí tưởng + nhiễu INS

Tín hiệu lí tưởng + nhiễu GPS nhiễu INS

_ nhiễu GPS - nhiễu INS lối ra được hiệu chỉnh

Tín hiệu lí tưởng + nhiễu INS

Tín hiệu lí tưởng + nhiễu GPS nhiễu INS

_ nhiễu GPS - nhiễu INS lối ra được hiệu chỉnh

(b) Hình 10 Cấu trúc theo kiểu vòng hở (a) và vòng kín (b)

Đề xuất của học viên

Trong mô hình lỗi INS mục 2.4 đã trình bày trước đó, học viên sử dụng phương trình lỗi INS này làm mô hình hệ thống và sử dụng 2 bộ lọc Kalman song song, từ đó đưa ra lưu đồ tính toán cho hệ thống tích hợp INS/GPS được mô tả như Hình 11 Các dữ liệu thô từ IMU cung cấp, dữ liệu này được tính toán theo thuật toán SINS để đưa ra các thông số vận tốc, vị trí, các góc tư thế Lối vào của khối ước lượng là độ chênh lệch của vị trí và vận tốc giữa INS và GPS Lối ra của khối ước lượng bao gồm các thông số: lỗi vị trí, lỗi vận tốc INS đã được ước lượng, các góc  E ,  N , Up , và độ trôi của các con quay vi cơ (G bx , G by ) Các lỗi vị trí và vận tốc này bù cho hệ thống INS để đưa ra kết quả vị trí và vận tốc chính xác hơn

3.2.1 Lưu đồ thuật toán hệ thống tích hợp INS/GPS

Khối ước lượng Lọc Dự đoán GPS

Chính xác lại vị trí, vận tốc

V INS - V GPS dX INS dV INS

_ xb yb zb a xb a yb a zb

Hình 11 Lưu đồ thuật toán hệ thống tích hợp INS/GPS

Trong lưu đồ thuật toán (Hình 11), khối ước lượng bao gồm 2 bộ lọc Kalman song song với 2 mục đích: thứ nhất là để ước lượng các giá trị lỗi vị trí và vận tốc để bù cho hệ thống INS, thứ 2 là cập nhật quaternion để hiệu chỉnh các góc hướng Bộ lọc Kalman sẽ được đề cập trong phần sau đây

3.2.2 Bộ lọc Kalman cho hệ thống tích hợp INS/GPS

Như đề cập trong mục 2.4 trước đó, tác giả sử dụng các phương trình lỗi INS như mô hình hệ thống, và các thành phần đo lường đưa tới các lối vào của bộ lọc là sự sai lệch giữa vận tốc và vị trí của INS với GPS Khi tín hiệu GPS mất, bộ lọc Kalman làm việc theo chế độ dự đoán

K F 2 8 t rạ ng th ái K F 1 9 t rạ ng th ái H ệ th ốn g ra dVn dVe dE dN

Hình 12 Bộ lọc Kalman trong mô phỏng

Với bộ lọc Kalman thứ nhất KF1 (xem Hình 12), để ước lượng các giá trị lỗi vị trí và vận tốc để bù cho hệ thống INS, được thiết lập dựa vào mô hình lỗi INS

Phương trình cơ học: ̇ (3.12) ̇ Dựa các phương trình (3.11), (3.12) ta xây dựng véc tơ trạng thái như sau:

[    ] ; và ma trận chuyển sẽ là :

Véc tơ đo lường dựa trên sự sai khác vị trí và vận tốc giữa INS và GPS [7] z 1 = E INS - E GPS = E INS - E GPS z 2 = N INS – N GPS = N INS – N GPS z 3 = - = - ; (3.13) z 4 = - = - ;

Các thông số E GPS , N GPS , , là sai số đo lường của GPS Như vậy có thể viết (3.13) dưới dạng ma trận (3.14) như sau:

Như vậy ma trận đo lường sẽ là

Sau khi ước lượng sai số vị trí, vận tốc, các thành phần này được bù trừ cho các thông số vị trí, vận tốc trong hệ thống INS

= - ̂ ; Trong đó: E INS/GPS , N INS/GPS là vị trí theo các phương bắc và Tây ̂ ; ̂ lỗi vị trí của INS đã được ước lượng ̂ ; ̂ lỗi vận tốc của INS đã được ước lượng

Bộ lọc Kalman thứ hai KF2 AHRS (Hình 12) (Attitude & Heading Reference System) [1] Bộ lọc Kalman thứ hai KF2 có thể ước lượng 8 trạng thái của hệ thống bao gồm: các lỗi vận tốc trên hệ toạ độ dẫn đường (eVN, eVE, eVD), độ trôi của các con quay vi cơ (Gbx, Gby, Gbz) và các lỗi góc nghiêng (Tn, Te) Lỗi ước lượng của INS được sử dụng để hiệu chỉnh ma trận chuyển hệ toạ độ C b N và véctơ quaternion Các ước lượng về độ trôi của con quay vi cơ được phản hồi về khối dẫn đường quán tính SINS

Bộ lọc Kalman thứ hai gồm có 8 trạng thái:

Tn, Te: là các lỗi góc nghiêng trong hệ tọa độ định vị (rad)

V N , V E , V d là các lỗi vận tốc trong hệ tọa độ cấp độ địa phương

Gbx, Gby, Gbz: là các giá trị độ trôi gây bởi các con cảm biến vận tốc góc (rad/s)

Cùng với ma trận chuyển trạng thái

Với Dvd là độ tăng vận tốc theo hướng vào tâm trái đất; h N là khoảng thời gian lấy mẫu của IMU, trong trường trường hợp này  t= 1/64; là tham số của hàm tương quan;

C nb là giá trị ma trận chuyển từ hệ tọa độ gắn liền vật thể (b-Frame) sang hệ tọa độ cấp độ địa phương ( ll-Frame )

Và ma trận đo lường

] Ở đây mỗi bộ lọc Kalman đều có thể chạy ở chế độ dự đoán và cập nhật ở tần số 1

Hz khi có thông tin của GPS Trong trường hợp tín hiệu GPS bị mất các bộ lọc Kalman sẽ hoạt động ở chế độ dự đoán Mỗi khi có lại tín hiệu GPS thì các bộ lọc này lại quay lại chế độ dự đoán và cập nhật như bình thường.

Kết quả mô phỏng

Với mô hình tích hợp INS/GPS sử dụng bộ lọc Kalman mà tôi đưa ra, kết quả mô phỏng hệ thống INS/GPS bao gồm các thông tin vị trí của GPS và của hệ INS/GPS; tốc độ của hệ thống GPS và của hệ INS/GPS; góc hướng của GPS và các góc tư thế của hệ INS/GPS Với cách sử dụng 2 bộ lọc Kalman song song như mô hình trước đây nhưng số trạng thái trong KF1 tăng lên véc tơ 9 trạng thái, đồng thời véc tơ đo lường cũng tăng lên, do đó chế độ lọc Kalman chính xác lên, cụ thể

Hình 13 (a), (b) mô tả thông tin vị trí theo quãng đường đối tượng chuyển động

Hình 13.Quỹ đạo GPS (a), INS/GPS (b), khoảng cách d (c) Để đánh giá khả năng ước lượng chính xác của hệ thống INS/GPS so với GPS, tác giả so sánh vị trí tại từng thời điểm thông qua thông số khoảng cách giữa 2 điểm tại cùng 1 thời điểm √ Hình 13 (c) mô tả sai lệch tại từng thời điểm của 2 hệ thống GPS và INS/GPS Bảng 2 cho ta thấy, vị trí INS/GPS bám quanh giá trị GPS với phương sai nhỏ

Bảng 2 Trung vị, phương sai của khoảng cách Đối với vận tốc theo các phương, kết quả mô phỏng thu được như sau

Hình 14.Vận tốc Vn của hệ INS/GPS, Vn của GPS và sai số tuyêt đối của vận tốc

Sai số tuyệt đối của Vn 0 0.007

Bảng 3 Sai số tuyệt đối của vận tốc theo phương Đông

Hình 15 Vận tốc Ve của hệ INS/GPS, Ve của GPS, và sai số tuyệt đối

Sai số tuyệt đối của Ve 0 0.003

Bảng 4 Sai số tuyệt đối của vận tốc theo phương Bắc

So sánh vận tốc Vn, Ve của hệ INS/GPS tại lối ra của bộ lọc Kalman và vận tốc của GPS được mô tả trong Hình 14 và Hình 15 Kết quả so sánh sai số vận tốc tuyệt đối (Bảng 3, Bảng 4) giữa hệ INS/GPS và GPS là rất nhỏ, điều này cho thấy vận tốc vận tốc của bộ lọc Kalman luôn bám sát theo vận tốc đo được của đầu thu GPS

Góc hướng là một trong số thông tin quan trọng của phương tiện chuyển động

Thông tin góc hướng của INS/GPS (đường đỏ) luôn bám sát theo thông tin chỉ góc hướng của GPS (đường xanh) (xem Hình 16 )

Hình 16 So sánh góc hướng INS/GPS và GPS

Ngoài ra, với mô hình bộ lọc Kalman véc tơ 9 trạng thái đưa ra, ta còn có thể ước lượng được giá trị các góc lệch  E , N , Up , sai lệch giữa các trục của 2 hệ tọa độ p-frame và N-frame (xem Hình 17)

Hình 17 Các góc lệch  E , N , Up

Hình 18 Độ trôi của cảm biến vận tốc góc G bx , G by Đặc thù của góc hướng luôn có sự thay đổi đột ngột, do đó việc ước lượng độ trôi của góc hướng bằng bộ lọc Kalman tuyến tính thường cho kết quả không ổn định (Hình

Hình 19 là các góc ngiêng và góc chúc của hệ thống sau khi đã được tính toán và hiệu chỉnh nhờ bộ lọc Kalman

Hình 19 Các góc chúc ( Pitch) và nghiêng (Roll)

Bảng 5 Các trung vị và phương sai của góc chúc và góc nghiêng Ở đây tác giả sử dụng dữ liệu của IMU, được gắn trên ô tô Thực tế, khi xe chuyển động trên mặt đất thì góc nghiêng và góc chúc thay đổi trong khoảng nhỏ, dao động xung quanh vị trí 0 Ở mô phỏng ta cũng thu được kết quả như vậy (Bảng 5) Điều này phù hợp mô phỏng với thực tế.