Tải bản đầy đủ - 0 (trang)
Các ký hiệu trong lưu đồ:

Các ký hiệu trong lưu đồ:

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)



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

Các ký hiệu trong lưu đồ:

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

×