Tải bản đầy đủ - 0 (trang)
6 Thuật toán dẫn đường quán tính.

6 Thuật toán dẫn đường quán tính.

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

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



hN1: thời gian lấy tính phân.

hN3: bước thời cập nhật của hệ thống INS.

 0  1i   2 j  3 k

m * m0  m1i  m2 j  m3 k



C bN



là ma trận chuyển từ hệ tọa độ gắn liền vật thể sang hệ tọa độ định vị.



B NE



là ma trận chuyển từ hệ tọa độ định vị sang hệ tọa độ cố định tâm trái đất



 , , là 3 góc quay Roll, Pitch, và Yaw.

 ,  ,  là vĩ độ, kinh độ và góc phương vị.



Vx,y,y là các vận tốc trong hệ tọa độ cố định tâm trái đất.



V N ,VE ,V D ,là các vận tốc trong hệ tọa độ định vị.



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



- 25 -



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



IMU

 xb



 yb



a yb



a xb



 zb



a xb



Bù nhiễu của cảm biến vận

tốc góc theo mơ hình (2.18)



Bù nhiễu của cảm biến

gia tốc theo mơ hình

(2.18)



Tính độ tăng về góc



Tính độ tăng về vận tốc



hN1



hN1



W xb, yb, zb



 xb, yb , zb



Chỉnh lỗi góc (Coning)



Bù lỗi vận tốc Sculling



hN



hN

3



 xb



 yb



 zb



W xb



Tính Quaternion -1



W yb



W zb



3



Tính lại độ tăng vận tốc

trong hệ tọa độ định vị



q ifm



Wx, y



Liên kết



Liên kết



C ij

q0p q1p



q2p q3p



Wx W yWz



Hình 2.3 Thuật tốn dẫn đường qn tính



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



- 26 -



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



W x W y W z



q0p q1p q2p q3p

Tính Quaternion -2



bij  0 



Tính vận tốc Vx,y



 x , y , z Tính ma trận

hN3

f

0



q



f

1



q



f

2



q



bij



f

2



q



Vx , y



Tính tọa độ



p

0



q



Chuẩn hố các tham số

quaternion

Tính ma trận



C



ij tư thế

Tính tốn







hN3











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







- 27 -











hN3

VN ,VE



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



Hinh2.4: Thuật tốn dẩn đường qn tính.



2.7. Các loại nhiễu ảnh hưởng đến khối IMU.

Khối IMU chịu ảnh hưởng của hai loại nhiễu là nhiễu tất định và nhiễu thống kê.



2.7.1. Nhiễu tất định

Các loại nhiễu tất định của khối IMU gồm có độ lệch vận tốc góc, độ lệch gia tốc, lỗi

tỉ lệ vận tốc góc, lỗi tỉ lệ gia tốc, lỗi vị trí.



2.7.2 Nhiễu thống kê

Có thể liệt kê các loại nhiễu thống kê chính tác động đến hệ thống dẫn đường quán

tính như sau:

Ồn lượng tử: sinh ra khi chuyển tín hiệu tương tự sang dạng số. Nguyên nhân của của

nó là sự sai khác giữa mức lượng tử số và biên độ thật của tín hiệu tương tự. Có thể hạn

chế ồn lượng tử bằng các phương pháp mã hóa, thay đổi tần số lấy mẫu hoặc tăng thêm

mức lượng tử.

Nhiễu trắng : là nguyên nhân chính gây ra lỗi thống kê của các khơi IMU và có cơng

suất khơng đổi trên tồn dải tần. Bản chất của các lỗi bước góc ngẫu nhiên (đối với cảm

biến vận tốc góc) và bước vận tốc ngẫu nghiên (đối với cảm biến gia tốc) đều xuất phát từ

nhiễu trắng.

Bước ngẫu nhiên: Đây là loại nhiễu khơng có nguồn gốc rõ ràng, và có thể thẫy loại

nhiễu này tăng theo hàm mũ với khoảng tương quan thời gian dài. Đối với cảm biến vận

tốc góc có bước vận tốc góc ngẫu nhiên, đối với cảm biến gia tốc có bước gia tốc ngẫu

nhiên.

Nhiễu rung :Nhiễu rung là loại nhiễu ở tần số thấp, gây nên sự mất ổn định của độ

lệch, nguồn gốc của loại nhiễu này là do các linh kiện điện tử nhạy với rung ngẫu nhiên.



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



- 28 -



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



Chương 3: Áp dụng bộ lọc Kalman (hai bộ lọc Kalman) vào

bài toán dẫn đường

3.1. Bộ lọc Kalman.

Năm 1960 R.E Kalman đã xuất bản một bài báo với tiêu đề “A New Approach to

Linear Filtering and Predication Problems”. Nghiên cứu của Kalman đã khắc phục

hạn chế của bộ lọc Weiner-Hopf trong việc giải quyết bài toán thống kê tự nhiên. Kể

từ đó, danh từ bộ lọc Kalman đã ra đời. Bộ lọc này ước lượng trạng thái x   n của

quá trình thời gian rời rạc theo phương trình sai phân tuyến tính:



x k  Ax k  1  Bu k  1  wk  1



(3.1)



Với việc đo z   n



z k  Hx k  v k



(3.2)



Biến ngẫu nhiên wk và vk biểu diễn nhiễu đo và nhiễu q trình. Trong thuật tốn lọc

Kalman, đặc tính thống kê của hai biến này phải được biết trước. Chúng ta giả sử các

biến này độc lập có phổ trắng và phân bố Gauss.

P(W))N(0,Q)



(3.3)



P(R)N(0,R)

Trong thực tế, ma trận hiệp phương sai nhiễu quá trình Q và ma trận hiệp phương sai

nhiễu đo R phải thay đổi theo từng thời điểm, tuy nhiên chúng ta có thể giả sử là hằng số.

Ma trận A(nxn) trong phương trình sai phân (3.1) là ma trận chuyển trạng thái từ thời

điểm trước (k-1) sang thời điểm hiện tại (k). Chú ý rằng, trong thực tế A có thể thay đổi

theo từng thời điểm. Nhưng chúng ta cũng có thể giả sử nó là hằng số. Ma trận B (nx1) là

ma trận điều khiển có lối vào u   n . Ma trận H(mxn) trong phương trình (3.2) là ma

trận đo lường. Trong thực tế H có thể thay đổi theo từng thời điểm, ở đây chúng ta giả sử

là hằng số.



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



- 29 -



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 đố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 đố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 quá 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 -



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

6 Thuật toán dẫn đường quán tính.

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

×