Tải bản đầy đủ - 0 (trang)
Chương 3: Áp dụng bộ lọc Kalman (hai bộ lọc Kalman) vào bài toán dẫn đường

Chương 3: Áp dụng bộ lọc Kalman (hai bộ lọc Kalman) vào bài toán dẫn đường

Tải bản đầy đủ - 0trang

Ket-noi.com diễn đàn cơng nghệ, giáo dục



3.1.1.Bản chất tính tốn của bộ lọc.

 



Chúng ta định nghĩa x







k



n

  n là trạng thái tiền ước lượng ở thời điểm thứ k, x k  



là trạng thái hậu ước lượng tại thời điểm thứ k và cho ra giá trị đo z k. Chúng ta có thể định

nghĩa các lỗi tiền ước lượng và lỗi hậu ước lượng như sau:

 



(3.4)



ek  x k  x k





(3.5)



ek  x k  x k

Ma trận hiệp phương sai lỗi tiền ước lượng:

P   E[e k e k



T



(3.6)



]



Ma trận hiệp phương sai lỗi hậu ước lượng:

T



Pk  E[e k e k ]



(3.7)



Xuất phát từ phương trình cho bộ lọc Kalman, chúng ta đi tìm ra một phương trình





tính tốn trạng thái hậu ước lượng x k như là một tổ hợp tuyến tính của trạng thái tiền ước













lượng x k và sự khác nhau giữa giá trị đo thực tế z k và giá trị tiên đoán H x k được chỉ

trong phương trình sau.

 







 



x k x k  K (zk  H x k )



(3.8)



 



Giá trị ( z  H x k ) trong công thức (3.8) được gọi là giá trị sai khác giữa giá trị

k

 



tiên đoán H x k và giá trị thực tế z k đo được. Giá trị này bằng 0 nghĩa là hai giá trị hoàn

toàn đồng nhất với nhau.

Ma trận K (mxn) trong phương trình (3.8) gọi là hệ số khuếch đại Kalman nhằm mục

đích tối thiểu hố hiệp phương sai lỗi hậu ước lượng (3.7). Độ khuếch đại Kalman có thể

được xác định bởi phương trình sau:



Pk H T

K k  P H ( HP H  R)   T

HPk H  R



k



T





k



Sinh Viên: Cao Ngọc Phát



T



1



- 30 -



(3.9)



Ket-noi.com diễn đàn cơng nghệ, giáo dục



Quan sát phương trình (3.9), chúng ta thấy rằng, khi ma trận hiệp phương sai lỗi đo

lượng R tiến tới 0 thì hệ số khuếch đại K được xác định như sau:

1

lim

k



H

k





pk  0





Trường hợp khác, khi hiệp phương sai lỗi tiền ước lượng Pk tiến tới 0 thì:



lim k k 0



p k  0



Khi hiệp phương sai lỗi đo lường R tiến đến 0 thì giá trị z k là chính xác hơn, trong

 



khi giá trị tiên đoán H x k lại kém chính xác. Trường hợp, khi giá trị hiệp phương sai lỗi



ước lượng trước Pk tiến tới 0, giá trị zk là kém chính xác trong khi đó giá trị tiên đốn

 



H x k lại đạt độ chính xác hơn.



3.1.2. Bản chất thống kê của bộ lọc.







Công thức (3.8) thể hiện bản chất thống kê của tiền ước lượng x

quy định trên tất

k

cả các giá trị đo trước zk (Quy tắc Bayer).







E[ x k ]  x k









E[( x k  x k )( x k  x k ) T ]  Pk

Trạng thái hậu ước lượng trong phương trình (3.8) phản ánh giá trị trung bình (mơmen

bậc 1) của phân bố trạng thái nếu các điều kiện (3.3) được thoả mãn. Hiệp phương sai lỗi

trạng thái hậu ước lượng trong công thức (3.6) phản ánh sự thay đổi của phân bố trạng

thái (mômen bậc 2).



P ( x k | z k )  N ( E[ x k ], E[( x k















 x k )( x k  x k ) T ])  N ( x k , Pk )



Nói tóm lại, Bộ lọc Kalman ước lượng một quá trình bằng việc sử dụng một dạng của

điều khiển phản hồi: bộ lọc ước lượng trạng thái q trình tại một vài thời điểm và sau đó

quan sát phản hồi trong dạng của nhiễu đo. Các phương trình trong bộ lọc Kalman chia

thành hai nhóm: các phương trình cập nhật thời gian và các phương trình cập nhật đo.



Sinh Viên: Cao Ngọc Phát



- 31 -



Ket-noi.com diễn đàn cơng nghệ, giáo dục



Các phương trình cập nhật thời gian có nhiệm vụ dự đốn trước trạng thái hiện tại và

hiệp phương sai lỗi ước lượng cho thời điểm tiếp theo. Các phương trình cập nhật đo có

nhiệm vụ phản hồi, ví dụ việc kết hợp chặt chẽ giá trị đo mới vào giá trị tiền ước lượng để

thu được những cải tiến trong giá trị hậu ước lượng.

Phương trình cập nhật thời gian có thể gọi là phương trình tiên đốn, trong khi phương

trình cập nhật đo có thể coi như phương trình của bộ sửa sai.



Cập nhật

thời gian



Cập nhật đo

(sửa sai)



(tiên đốn)



Hình 3.1: Thuật tốn lọc Kalman.

Các phương trình cập nhật thời gian của bộ lọc Kalman rời rạc

 







x k  A x k  1  Bu k  1



Pk  APk  1 AT  Q

Các phương trình cập nhật đo của bộ lọc Kalman rời rạc:



K k  Pk H T ( HPk H T  R )  1





 



 



x x k  K k (zk  H x k )



Pk ( I  K k H ) Pk



Sinh Viên: Cao Ngọc Phát



- 32 -



Ket-noi.com diễn đàn cơng nghệ, giáo dục



Khởi tạo



Tính hệ số

khuếch đại Kalman

Zk



Tiên đoán



Cập nhật ước lượng



Trạng thái

Cập nhật Hiệp phương

sai



ước lượng

được cập nhật xk



Hình 3.2: Sơ đồ thực hiện thuật toán Kalman.



3.2. Xây dựng bộ lọc Kalman cải tiến.

3.2.1. Hệ thống dẫn đường tích hợp GPS / INS.



Sinh Viên: Cao Ngọc Phát



- 33 -



Ket-noi.com diễn đàn công nghệ, giáo dục



Hệ thống dẫn đường qn tính INS có 2 ưu điểm nổi bật khi so sánh với các hệ thống

dẫn đường khác là khả năng hoạt động tự trị và độ chính xác cao trong những khoảng thời

gian ngắn. Lỗi nghiêm trọng nhất của hệ thống dẫn đường quán tính INS là do các cảm

biến quán tính gây ra. Chính vì thế trong những ứng dụng thời gian dài thì hệ thống dẫn

đường qn tính INS thường sử dụng với các hệ thống hỗ trợ khác như hệ thống dẫn

đường vô tuyến (Loran, Omega và Tacan), hệ thống dẫn đường vệ tinh (GPS, GLONASS

và Transit), JTIDS, DME…Các hệ thống này hoạt động ổn định theo thời gian và vì thế

cần tích hợp INS và các hệ thống hỗ trợ này. Sự kết hợp GPS và INS là lý tưởng nhất vì

hai hệ thống này có khả năng bù trừ nhau hiệu quả. Trái tim của hệ thống tích hợp này

chính là bộ lọc tối ưu Kalman.

Bộ lọc Kalman rất hiệu quả và linh hoạt trong việc kết hợp đầu ra bị nhiễu của cảm

biến quán tính để ước lượng trạng thái của hệ thống khơng ổn định. Tín hiệu bị nhiễu từ

các cảm biến quán tính và GPS bao gồm các thơng tin về vị trí, vận tốc, toạ độ của vật thể

bay. Những tác nhân làm hệ thống không ổn định là nhiễu do cảm biến, do người sử dụng

và nhiễu do môi trường. Bộ lọc Kalman dùng để ước lượng lỗi do các cảm biến quán tính

gây ra và ta xây dựng được vectơ trạng thái



xˆ k



của bộ lọc từ các lỗi này. Các giá trị thu



được từ GPS dùng để xây dựng vectơ đo lường z. Sau khi mơ hình hố các lỗi này thì

thực hiện chu trình Kalman với các ước lượng vectơ trạng thái và ma trận tương quan tại

thời điểm ban đầu. Cấu trúc này được gọi là cấu trúc GPS hỗ trợ INS và các lỗi được xử

lý theo kiểu vòng mở và vòng đóng như mơ tả trong hình 3.3 và 3.4.

Cấu trúc vòng mở cho phép thực thi dễ dàng hơn nhưng cấu trúc vòng kín lại cho kết

quả chính xác hơn.



Sinh Viên: Cao Ngọc Phát



- 34 -



Ket-noi.com diễn đàn cơng nghệ, giáo dục



Hình 3.3: Cấu trúc GPS/INS vòng mở.



Hình 3.4: Cấu trúc GPS/INS vòng kín.



3.2.2. Bộ loc kalman cải tiến.

Trong khoá luận này, cấu trúc lọc Kalman được phát triển bao gồm hai bộ lọc song

song được sử dụng rất linh hoạt trong hệ thống dẫn đường. Khi tín hiệu GPS thu được tốt

thì hệ thống INS/GPS hoạt động với cấu hình vòng kín. Khi tín hiệu GPS bị mất thì hệ

thống dễ dàng chuyển sang cấu hình vòng mở. Khi tín hiệu GPS lại được khơi phục thì hệ

thống lại trở về cấu hình vòng kín. Việc chuyển linh hoạt giữa hai cấu hình vòng kín và

vòng hở giúp nâng cao chất lượng của toàn bộ hệ thống và khắc phục được nhược điểm

của cả hai cấu hình riêng lẻ.



Sinh Viên: Cao Ngọc Phát



- 35 -



Ket-noi.com diễn đàn cơng nghệ, giáo dục



Hình 3.5: Bộ lọc kalman song song.

Bộ lọc Kalman thứ nhất (KF1) được thiết lập với véctơ 3 trạng thái là sai số vận tốc

(eVN, eVE, eVD), véctơ đo lường là hiệu giữa vận tốc của GPS và INS. Tốc độ cập nhật của

vận tốc GPS là 1 Hz và của INS là 64 Hz. Vì thế tốc độ cập nhật của bộ lọc Kalman có

thể lựa chọn là 1 Hz. Chúng ta có thể lựa chọn tốc độ cập nhật cao hơn cho đầu ra của bộ

lọc Kalman này (chẳng hạn là 2 s) với giả thiết rằng khoảng thời gian 0.5 s thì vận tốc của

vật thể chuyển động là không thay đổi. Ưu điểm của bộ lọc ít trạng thái KF1 này là tốc độ

hội tụ nhanh, tránh được các sai số như bộ lọc Kalman khơng hội tụ hoặc sai sót khi thiết

lập mơ hình trạng thái. Đầu ra của KF1 được sử dụng để bù trừ cho đầu ra của hệ thống.

Bộ lọc Kalman thứ hai KF2 có thể ước lượng tám 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 (e VN, 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ạ độ CbN 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 qn tính SINS.

Ta có Ak,k-1 là ma trận chuyển trạng thái và bởi vì khoảng thời gian này nhỏ (tức là tốc

độ cập nhật của INS là lớn - ở đây là 64 Hz), chúng ta có thể xấp xỉ A k,k-1 như sau:



Ak ,k  1 e ( Ft ) I  Ft



Ak , k  1



0

 0

 0

0



 0  Dvd



Dvd

0



 0

0



0

 0

 0

0



0

 0



(3.10)



0 0 0 h N C11

0 0 0

0 0 0

0 0 0

0 0 0

0 0 0

0 0 0

0 0 0



h N C12



h N C13 

h N C 21 h N C 22 h N C 23 

0

0

0 



0

0

0 

0

0

0 



 hN 

0

0 

0

 hN 

0 



0

0

 hN  



(3.11)



Ở đó Dvd là độ tăng vận tốc theo hướng tâm trái đất của hệ trục toạ độ dẫn đường, 

là tham số của hàm tương quan, hN=1/64 s, và ma trận hiệp phương sai ứng với wk là:



Qk E  wk wkT   Ak GQG T AkT t

Ở đó:

Sinh Viên: Cao Ngọc Phát



- 36 -



(3.12)



Ket-noi.com diễn đàn công nghệ, giáo dục







Q diag  ax2  ay2  az2  2x  2x  2x







Xét phương trình đo lường:



zk = Hk xk + vk



(3.13)



Ở đó:



 Vn INS  VnGPS



z k  Ve INS  Ve GPS

 Vd

INS  Vd GPS









,







H k  I 33 0 35 



















Rk E v k vkT diag  Vn2



 Ve2



2

 Vd







Là ma trận hiệp phương sai của vk.

Trong trường hợp tín hiệu GPS bị mất hoặc không đáng tin cậy, thông tin thu được

sẽ được lấy từ khối INS cho tới khi tín hiệu GPS được khơi phục. Trong khoảng thời gian

đó, các lỗi dẫn đường sẽ tăng nhanh chóng theo thời gian do nhược điểm của các cảm

biến vi cơ điện tử. Vì thế, hệ thống của chúng ta cần phải tận dụng được các phép đo ảo

dựa vào tính chất động học của vật thể chuyển động (ở đây tập trung vào các phương tiện

chuyển động trên mặt đất). Cụ thể, trong khoá luận có thể tận dụng những ràng buộc về

vận tốc và độ cao của vật thể. Trong nhiều tình huống thì có thể coi độ cao của phương

tiện chuyển động bằng 0, tuy nhiên không thể áp dụng điều kiện này trên toàn bộ tuyến

đường.

Ràng buộc của vận tốc:



V xb (t ) 0

V



b

z



(3.14)



(t ) 0



Phương trình 21 được chuyển sang hệ dẫn đương như sau:



 Vxb 

V N 

V  C n  V b 

b 

y 

 E

 Vzb 

V D 





Sinh Viên: Cao Ngọc Phát



(3.15)



- 37 -



Ket-noi.com diễn đàn công nghệ, giáo dục



Với các quãng đường dài, phương tiện chuyển động cần dừng lại để cập nhật lỗi.

Trong trường hợp này thì các vận tốc trở về 0. Lúc này thì các vận tốc đầu ra của INS

chính là các lỗi vận tốc. Việc xác định xem phương tiện đã hoàn toàn dừng lại hay chưa sẽ

dựa vào vận tốc của GPS hoặc đầu ra của cảm biến gia tốc. Tín hiệu gia tốc sẽ được xử lí

bằng phương pháp cửa sổ trượt với kích thước cửa sổ cỡ vài giây.Ngưỡng xác định xe

dừng hẳn là 0.05 m/s. Khi phương tiện dừng hồn tồn, vận tốc theo hướng Đơng và Bắc

sẽ đưa vào bộ lọc. Cách xử lí này khác với cách xử lí thơng thường là véctơ đo lường

được tính bằng hiệu số giữa vận tốc của INS và GPS. Để cho việc ước lượng lỗi INS

chính xác hơn thì véctơ đo lường cần được gia tăng so với khi phương tiện chuyển động

nhằm thu được độ khuếch đại Kalman lớn hơn.

Khi mất GPS, các góc tư thế của vật thể nhờ việc dự đoán véctơ trạng thái. Ngồi ra,

để phục vụ cho việc hiệu chỉnh góc tư thế của hệ thống thì chúng ta còn có thể sử dụng

thơng tin góc hướng từ GPS hoặc cảm biến từ. Với ứng dụng 2D thì góc hướng tính theo

GPS được sử dụng với điều kiện rằng vận tốc chuyển động lúc này phải đủ lớn. Góc

hướng của GPS được tính như sau:



H



GPS



tg



1



VEGPS

VNGPS



(3.16)



Có thể phân loại các sai số trong các khối dẫn đường quán tính thành các sai số tất

định và các sai số ngẫu nhiên. Các sai số tất định có thể loại trừ khá dễ dàng nhờ các

phương pháp căn chuẩn thiết bị. Tuy nhiên, loại bỏ các sai số ngẫu nhiên trong hệ thống

dẫn đương lại là một vấn đề phức tạp. Trước tiên, chúng ta phải đặc trưng được các nhiễu

ngẫu nhiên tác động lên hệ thống sử dụng phương pháp mật độ phổ công suất, phương

pháp phương sai Allan hoặc kết hợp cả hai phương pháp này. Phương pháp xác định các

thông số nhiễu đã được trình bày chi tiết trong báo cáo của đề tài QGTĐ 05-09, và được

nêu lại ở phần nhiễu cũa khối IMU. Sau khi đặc trưng được sai số của khối IMU thì các

thơng số này sẽ được sử dụng trong các khối lọc Kalman nhằm bù trừ sai số .



Sinh Viên: Cao Ngọc Phát



- 38 -



Ket-noi.com diễn đàn công nghệ, giáo dục



Chương 4: Các kết qủa thực nghiệm.

4.1 Thiết bị phần cứng.

4.1.1 Khối dẫn đường quán tính MICRO-ISU BP3010.

MICRO-ISU BP3010 là một khối IMU được tích hợp 3 cảm biến vận tốc góc

ADXR300, và 3 cảm biến gia tốc ADXL210E có bù nhiệt theo mơ hình g ắn liền. Sáu lối

ra của 6 cảm biến này được đưa qua một mạch tích phân sáu đường để thu được 3 độ tăng

về góc dThx, dThy, dThz và 3 độ tăng vận tốc là dVx, dVy, dVz. Các vi điều khiển trong

khối IMU này tổng hợp dữ liệu tính Coning và Sculling và giao tiếp với bên ngoài qua

giao diện RS232. Khung truyền dữ liệu ra ngoài được cập nhật với các tốc độ khác nhau

là 8Hz, 16Hz, 32Hz và 64Hz, nhưng tốt nhất vẫn là 64Hz. Thông tin chi tiết được trình

bày trên trang Web của nhà sản xuất BEC Navigation System .



Hình 4.1: Hệ INS/GPS được kết nối (QGTĐ 05-09).



4.1.2 Thiết bị thu GPS

Thiết bị thu GPS HI-204E (Hình 4.2) của hãng Haicom có các đặc điểm sau:

 Có 12 kênh dẫn đường

Sinh Viên: Cao Ngọc Phát



- 39 -



Ket-noi.com diễn đàn công nghệ, giáo dục



 Độ nhạy tối thiểu -165 dB.

 Truyền dữ liệu qua cổng USB .

 Chuẩn truyền NMEA-108 với tốc độ 4800 baud

 Tốc độ cập nhật khung dữ liệu 1Hz

 Sai số định vị khi khơng có SA là 25m.

Các thơng tin thu từ GPS theo chuẩn NMEA-108: GGA, GLL, GSA, GSV, RMC, và VTG

mang thông tin về thời gian chuẩn, kinh độ , vĩ độ, tư thế, vận tốc, và một số thông tin

khác.

Thơng tin chi tiết được trình bày trên trang Web của nhà sản xuất Haicom [8].



Hình 4.2: Thiết bị thu GPS HI-204E.



4.2. Kết quả thực nghiệm.

4.2.1. Xây dựng hệ tích hợp INS/GPS theo thời gian thực.

a. Thiết kế và mô hình hố hệ thống dẫn đường tích hợp GPS / INS: mức hệ thống

Công cụ SIMULINK rất thuận tiện và hiệu quả trong việc thiết kế và mô phỏng hệ

thống dẫn đường ở mức hệ thống và có thể hỗ trợ trực tiếp cho mức thực thi. Toàn bộ hệ

thống dẫn đường đã được mô phỏng và kiểm tra nhờ ngơn ngữ lập trình nhúng trong mơi

trường SIMULINK/MATLAB,đã tận dụng được ưu điểm của cả SIMULINK và lập trình

với các lệnh M-files. Điều này sẽ rất thuận tiện để phát triển chương trình thời gian thực.

Ngồi các tập dữ liệu mơ phỏng, chương trình này còn có thể xử lí các dữ liệu thực (đo

Sinh Viên: Cao Ngọc Phát



- 40 -



Tài liệu bạn tìm kiếm đã sẵn sàng tải về

Chương 3: Áp dụng bộ lọc Kalman (hai bộ lọc Kalman) vào bài toán dẫn đường

Tải bản đầy đủ ngay(0 tr)

×