PHÁT TRIỂN CÔNG CỤ LÀM TRƠN RTS (RAUCH-TUNG-STRIEBEL) TRONG XÁC ĐỊNH QUỸ ĐẠO CHUYỂN ĐỘNG

Chia sẻ: Le Thuy Duong | Ngày: | Loại File: PDF | Số trang:6

Thêm vào BST

Báo xấu

184
lượt xem 9
download

Download Vui lòng tải xuống để xem tài liệu đầy đủ

PHÁT TRIỂN CÔNG CỤ LÀM TRƠN RTS (RAUCH-TUNG-STRIEBEL) TRONG XÁC ĐỊNH QUỸ ĐẠO CHUYỂN ĐỘNG ThS. NGÔ THANH BÌNH Khoa Điện - Điện tử Trường Đại học Giao thông Vận tải KS. TRẦN QUỐC TOẢN Khoa Cơ điện Trường Đại học Kinh doanh & Công nghệ Tóm tắt: Để đị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 điểm đến tiếp theo của đối tượng trên cơ sở sử dụng bộ lọc Kalman. Tuy nhiên quỹ đạo vẽ được trên bản đồ không phải là đường quỹ đạo thật mà là một đường...

Chủ đề:

Bình luận(0) Đăng nhập để gửi bình luận!

Lưu

Nội dung Text: PHÁT TRIỂN CÔNG CỤ LÀM TRƠN RTS (RAUCH-TUNG-STRIEBEL) TRONG XÁC ĐỊNH QUỸ ĐẠO CHUYỂN ĐỘNG

PHÁT TRIỂN CÔNG CỤ LÀM TRƠN RTS (RAUCH-TUNG-STRIEBEL) TRONG XÁC ĐỊNH QUỸ ĐẠO CHUYỂN ĐỘNG ThS. NGÔ THANH BÌNH Khoa Điện - Điện tử Trường Đại học Giao thông Vận tải KS. TRẦN QUỐC TOẢN Khoa Cơ điện Trường Đại học Kinh doanh & Công nghệ Tóm tắt: Để đị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 điểm đến tiếp theo của đối tượng trên cơ sở sử dụng bộ lọc Kalman. Tuy nhiên quỹ đạo vẽ được trên bản đồ không phải là đường quỹ đạo thật mà là một đường zigzag của các điểm dự báo. Trong đánh giá chuyển động chúng ta cần quỹ đạo thực, vì vậy việc quan tâm tới quá trình làm trơn các trạng thái đo đạc, gọi là smoother, bao giờ cũng đi kèm theo bài toán định vị chuyển động. Bài báo này khảo sát và phát triển một số dạng làm trơn quỹ đạo, bao gồm RTS (Rauch-Tung-Striebel smoother), ETS (extended Forward-Backward smoother). Một số kết kết quả mô phỏng trên Matlab được đưa ra để minh họa cho các thuật toán này, từ đó tác giả đưa ra một số lưu ý khi khảo sát quỹ đạo chuyển động. Summary: To locate moving objects we often use calculator tools to predict the next point of the objects based on using Kalman filter. But drawing lines on the orbit map are not the actual trajectory instead of the zigzag lines of predicting points. In the evaluation we need the real orbit, so the interest in the process as smoothing measurement status, called smoother, always accompanied by positioning motion problems. This paper surveys and develops some forms of smoothing orbit, including the RTS (Rauch-Tung-Striebel smoother), ETS (extended Forward- Backward smoother). A number of the Matlab simulation results are given to illustrate these algorithm, from that the author gives some notices in surveying trajectory. quỹ đạo thực, gần như là quỹ đạo trơn nhất I. LỜI MỞ ĐẦU của chuyển động. Kết quả thu được là đường quỹ đạo nhẵn, gần với thực tế nhất của đối Với các phương pháp tính toán truy hồi, tượng. Lớp bài toán này thường áp dụng trong một số công cụ ứng dụng trong đánh giá điều khiển hành trình theo quỹ đạo định trước chuyển động gọi là Làm trơn (Smoother) đã của vật thể bay (trong quá trình chuyển động được phát triển mạnh mẽ nhằm xác định chính có thể thay đổi quỹ đạo), sử dụng trong đánh xác nhất quỹ đạo chuyển động khi đã có tọa giá quỹ đạo bay của máy bay không người lái. độ điểm xuất phát và kết thúc của hành trình. Ngược lại với quá trình lọc (Filter) nhằm dự II. KHÔNG GIAN TRẠNG THÁI VÀ báo vị trí tiếp theo gần đúng nhất của đối LỌC KALMAN tượng chuyển động để đưa đối tượng tới đích, Một mô hình tuyến tính biến đổi theo làm trơn (Smoother) là quá trình tính ngược từ thời gian được diễn tả bằng công thức liên tục tọa độ đích về tọa độ xuất phát nhằm vẽ ra
về thời gian như sau: • dx(t) Update: = Fx(t) + Lw(t) (1) dt v k = y k - H k m- k Trong đó: Sk = H k Pk H T + R k k Điều kiện quán tính là x(0)~N(m(0),P(0)) K k = Pk HTS-1 - (6) F và L ma trận hằng với đặc tính hoạt kk động của mô hình m k = m- + K k v k k w(t) là ồn trắng với cường độ phổ là Qc Pk = Pk - K k Sk K T - Bộ lọc Kalman kinh điển được giới thiệu k bởi Rudolph E. Kalman (1960), đưa ra một Trong đó: giải pháp đệ quy để đánh giá những hệ thống m- và Pk là giá trị dự báo trung - • động rời rạc tuyến tính hóa thời gian. Lọc k bình và ma trận hiệp biến của trạng thái riêng Kalman bao gồm 2 bước: bước Dự đoán biệt tại thời điểm k trước khi có giá trị đo đạc (prediction), ở đó các trạng thái tiếp theo của (Tiền nghiệm) hệ thống được dự báo bởi các giá trị đo trước đó; và bước Cập nhật (Update), ở đó các trạng • mk và Pk là giá trị đánh giá trung thái hiện tại của hệ thống được đánh giá bởi bình và ma trận hiệp biến của trạng thái riêng các số liệu đo được tại thời điểm đó. Theo biệt tại thời điểm k sau khi có giá trị đo đạc Särkkä (2006), sử dụng lọc Kalman cho mô (Hậu nghiệm) hình trên Ak và Qk có dạng: v k là giá trị mới đưa vào (thặng dư đo A k = exp(FΔt k ) (2) đạc) tại thời điểm bước k Δt k ∫ exp(F(Δt - τ))LQC LT exp Qk = Sk là ma trận hiệp biến dự báo đo đạc tại (3) K 0 bước k (F(Δt K - τ))T dτ K k là hệ số khuếch đại lọc, chính là độ Trong đó: Δt k = t k+1 - t k là bước tính. lợi cần tìm của mạch lọc Kalman trong mỗi ⎧⎛ F ⎫⎛ 0 ⎞ LQC L ⎞ ước đoán T ⎛ Ck ⎞ ⎪ ⎪ ⎟ Δt k ⎬ ⎜ ⎟ (4) ⎜ ⎟ = exp ⎨⎜ ⎟ Dk ⎠ ⎪⎝ I ⎠ ⎪⎝ 0 T ⎝ -F ⎠ Tuy nhiên trong thực tế tồn tại nhiều hệ ⎩ ⎭ thống phi tuyến, ở đó các tính toán của bộ lọc Với : Qk = Ck D-1 Kalman truyền thống không được áp dụng k được. Trong những trường hợp này ta sử dụng • Prediction: mô hình lọc Kalman mở rộng EKF (extended m- = A k-1mk -1 (5) Kalman filter) như sau: k Pk = A k-1Pk-1AT + Qk-1 - k-1
⎧ x k = f(x k-1 , k -1) + q k -1 Sk = H x (m- , k)Pk HT (m- ,k) + R k - k x k (7) ⎨ ⎩ y k = h(x k , k) + rk K k = Pk HT (m- ,k)S-1 - (9) x k k Trong đó: m k = m- + K k v k k n x k ∈ R : là trạng thái của hệ thống ở Pk = Pk - K k Sk K T - bước k k Trong đó: Fx (m,k -1) và H x (m, k) là ma yk ∈ R m : là giá trị đo đạc tại thời điểm k trận Jacobi của hàm f và h, với q k -1 ~ N(0,Qk-1 ) : là nhiễu xử lý δf j (x, k -1) (10) [Fx (m, k -1)] j, j' = rk ~ N(0, R k ) : là nhiễu đo đạc δx j' x=m f: là hàm (phi tuyến) động học của mô δh j (x, k) hình (11) [H x (m, k)] j, j' = δx j' h: là hàm (phi tuyến) vector đo đạc của x=m mô hình III. BỘ LÀM TRƠN SMOOTHER Bộ lọc Kalman mở rộng và ứng dụng Mô hình RTS (Rauch-Tung- (Jazwinski, 1970; Maybeck, 1982; Bar- Striebel smoother) Shalom, 2001; Grewal và Andrews, 2001; Bộ làm trơn cho mô hình lọc Kalman rời Särkkä, 2006), mở rộng phạm vi của bộ lọc rạc theo thời gian, còn được biết đến dưới tên Kalman để lọc tối ưu các vấn đề phi tuyến gọi Rauch-Tung-Striebel-smoother RTS bằng cách thành lập một xấp xỉ Gaussian phân (Rauch, 1965; Gelb, 1974; Bar-Shalom, phối trạng thái x và các phép đo y dựa trên 2001), sử dụng trong quá trình tính toán làm biến đổi sử dụng chuỗi Taylor. Sự khác nhau trơn mô hình (7) được đưa ra theo phân tán P, giữa hai bộ lọc KF và EKF là ma trận A k và theo công thức: H k trong KF được thay thế bởi ma trận P(x k y1:T ) = N(x k m s , Pk ) s Jacobi Fx (mk-1 , k -1) và H x (m- , k) trong (12) k k EKF. Giá trị m- và v k cũng được tính toán k Giá trị ước đoán mk và ma trận hiệp khác trong EKF, dựa trên biến đổi xấp xỉ biến Pk được tính toán trong các dạng làm tuyến tính và dạng toàn phương. trơn cho lọc Kalman, theo công thức sau: • Prediction: m- = A k m k+1 m- = f(mk-1 ,k -1) k Pk+1 = A k Pk AT + Qk - k - T Pk = Fx (mk-1 , k -1)Pk-1Fx (mk-1 , k -1) + Qk-1 (8) C k = Pk A T [Pk +1 ]-1 (13) k • Update: m s = m k + C k [m s +1 - m k +1 ] k k yk - h(m- , k) vk = k
tắc Gelb, 1974). Với mô hình forward- s s Pk = Pk + C k [Pk +1 - Pk +1 ] backward, bộ làm trơn ETS có những lỗi tương Trong đó: tự như các bộ làm trơn RTS, nhưng xử lý được trong trường hợp phi tuyến. Các giá trị ước ms và Pk là những ước lượng của s • k đoán mk và ma trận hiệp biến Pk được tính bộ làm trơn (smoother) cho trạng thái ước toán trên cơ sở mô hình phi tuyến f như sau: đoán và ma trận hiệp biến bước k. m- = f(mk ,k) • mk và Pk là những ước lượng của k+1 bộ lọc (filter) cho trạng thái ước đoán và ma - T Pk+1 = Fx (mk ,k)Pk Fx (mk , k) + Qk trận hiệp biến bước k. Ck = Pk Fx (mk , k)[Pk+1 ]-1 T - (14) m- - • k+1 và Pk+1 là giá trị ước đoán ms = mk + Ck [ms - m- ] trạng thái ước đoán và ma trận hiệp biến bước k k+1 k+1 k+1 Pk = Pk + Ck [Pk+1 - Pk-1 ]CT s s - k • Ck là độ lợi làm trơn tại thời điểm IV. MÔ PHỎNG k, chỉ ra với bao nhiêu giá trị làm trơn ước lượng được chính xác tại bước thời gian cụ Trong bài báo này, tác giả triển khai bộ thể. lọc Kalman và bộ làm trơn RST, phát triển bộ làm trơn ETS với xấp xỉ phân tán của trạng Sự khác biệt giữa các bộ lọc Kalman và thái x k nhận được bởi quan sát y1:k , trộn RTS là cách tính toán thuận (forward) trong lọc và tính truy hồi trong bộ làm trơn nhiễu Gauss. Khi mô phỏng ta sử dụng hàm (backward smoother). Trong thuật toán làm phát sine ngẫu nhiên h(x k , k) = a k sin(θ k ) thay trơn RTS tính toán đệ quy bắt đầu từ bước cho các tín hiệu thực tế nhận được của quỹ ms = mT và thời gian cuối cùng T, với đạo bay thực tế từ tín hiệu GPS và từ sensor T INS, với các tham số hệ số biên độ, vận tốc s PT = PT . góc và độ lớn góc thay đổi, được trộn với Mô hình ETS (extended Forward- nhiễu ngẫu nhiên. Khi thay thế hàm phát sine Backward smoother) này bằng tín hiệu thu được ngoài thực tế ta sẽ vẽ được quỹ đạo chuyển động thực của đối Bộ làm trơn ETS xây dựng các hoạt động tượng. làm trơn như là một sự kết hợp của hai bộ lọc, trong đó có bộ lọc đầu tiên quét Theo mô hình vận tốc Wiener, vector dữ liệu chuyển tiếp đi từ giá trị đo lường đầu trạng thái được biểu diễn như sau: tiên hướng đến các giá trị mới hơn, và bộ lọc x k = (θk ak ) T (15) ωk thứ hai quét ngược theo hướng ngược lại. Kết quả có thể được hiển thị, kết hợp các ước lượng ⎛0 1 0⎞ ⎛ 0 0⎞ được sinh ra bởi hai bộ lọc một cách phù hợp dx(t) ⎜ ⎟ ⎜ ⎟ = ⎜ 0 0 0 ⎟ x(t) + ⎜ 1 0 ⎟ w(t) (16) với một ước tính làm trơn cho trạng thái, trong dt ⎜0 0 0⎟ ⎜0 1⎟ ⎝ ⎠ ⎝ ⎠ đó độ lệch phương sai nhỏ hơn so với bất kỳ lỗi nào sinh ra trong hai bộ lọc đơn (theo nguyên Trong đó:
θk là tham số góc của hàm sin tại thời đ iể m k ωk là vận tốc góc trong bước tính thứ k a k là hệ số biên độ tại bước tính thứ k Từ (2) có: ⎛ 1 Δt 0 ⎞ ⎜ ⎟ A k = exp(FΔt k )Þ x k = ⎜ 0 1 0 ⎟ x k-1 + q k-1 ⎜0 0 1⎟ ⎝ ⎠ Hình 1. Kalman Filter (KF) (17) Với Δt là khoảng cách bước tính, trong mô phỏng chọn Δt = 0,01 , ngoài thực tế tùy thuộc tốc độ xử lý của chip ta sẽ tính toán bước tính này cho phù hợp. Từ (3) có: Δt k ∫ exp(F(Δt - τ))LQC LT exp Qk = K 0 (F(Δt K - τ))T dτ Với nhiễu ngẫu nhiên Gauss Hình 2. Rauch-Tung-Striebel (RTS Smoother) q k ~ N(0,Qk-1 ) ta tính được: ⎛1 3 ⎞ 12 ⎜ 3 Δt q1 Δt q1 0⎟ 2 (18) ⎜ ⎟ ⎜1 ⎟ Q k-1 = ⎜ Δt 2 q1 Δtq1 0⎟ 2 ⎜ ⎟ ⎜0 0 Δtq 2 ⎟ ⎜ ⎟ ⎝ ⎠ Với mô hình đo đạc ta có: yk = h(x k ,k) + rk = a k sin(θ k ) + rk (19) Đạo hàm vector đo đạc, thay vào công Hình 3. Extended Forward-Backward thức mô hình lọc và làm trơn, ta tính được ma smoother (ETS) trận Jacobi như sau: Nhận xét: H x (m, k) = ( a k cos(θ k ) 0 sin(θ k ) ) (20) Mô hình KF và RST chỉ thực hiện được Kết quả mô phỏng khi quỹ đạo phát với sai lệch có biên độ nhỏ (max ~ 1.5 lần), kết quả thu được gần với quỹ đạo thực, thời gian tính toán nhanh (t ~ 25 cho 4 chu kỳ).
Với ETS kết quả xử lý sẽ đáp ứng tốt hơn. Mô phỏng với nhiễu ngẫu nhiên có biên độ sai lệch lớn (max ~ 4 lần), ta vẫn thu được quỹ đạo gần đúng với quỹ đạo thực. Tuy Tài liệu tham khảo nhiên thời gian tính toán bị kéo dài hơn gấn [1]. Greg Welch, Gary Bishop (Updated July 24, gần 10 lần (t ~ 250 cho 4 chu kỳ). 2006); An Introduction to the Kalman Filter; Chapel Hill, NC 27599-3175 V. KẾT LUẬN [2]. Jim Ledin (2004); Embedded Control Bộ làm trơn được sử dụng để nâng cao Systems in C/C++: An Introduction for Software chất lượng dự đoán và vẽ ra quỹ đạo thực của Developers Using MATLAB; CMP Books, chuyển động, bằng cách kết hợp dữ liệu trước ISBN:1578201276. và dữ liệu đã có trong các tính toán nhằm tăng [3]. Bruno Otávio Soares Teixeira (2005); Flight tần số lấy mẫu, đáp ứng các biên độ sai lệch path reconstruction using the unsented Kalman lớn, tái tạo quỹ đạo từ dữ liệu thực GPS/INS. filter algorithm; Procedings of COBEM 2005, by Lớp bài toán này áp dụng trong điều khiển ABCM hành trình theo quỹ đạo định trước của vật thể [4]. Thanh Binh Ngo, Hung Lan Le, Thanh Hai bay, thường thấy trong các máy bay không Nguyen; Survey of Kalman Filters and Their người lái, đòi hỏi tốc độ tính toán rất cao. Vì Application in Signal Processing; Procedings of vậy để xác định chính xác được quỹ đạo, ta AICI 2009, by IEEE and Springer♦ cần lưu ý tới một số vấn đề sau: • Một số hệ thống với các thông số có thể thay đổi đột ngột có thể không tồn tại ma trận Jacobi (20). Như vậy quá trình tính toán có thể rất khó thực hiện được, hoặc tiêu tốn rất nhiều thời gian để vẽ ra được quỹ đạo. • Trong một số trường hợp việc tính toán ma trận Jacobi là rất khó khăn, có thể không đạt được cả trong tính toán đạo hàm mô phỏng và lập trình cho chip, kể cả với chip vi xử lý mạnh. Những lỗi này rất khó để debug, và rất khó khăn để phát hiện ra với các đánh giá từng phần rời rạc của hệ thống, đặc biệt là khi không biết trước quỹ đạo yêu cầu hoặc quỹ đạo bị thay đổi trong hành trình. • Sự hội tụ của các thuật toán làm trơn bị ảnh hưởng rất nhiều bởi quá trình ban đầu và ma trận hiệp phương sai đo lường, vì vậy cần lưu ý tới khả năng tồn tại của các ước tính số liệu ồn trắng trong quá trình xử lý.