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

I. LỜI MỞ ĐẦU Với các phương pháp tính toán truy hồi, một số công cụ ứng dụng trong đánh giá chuyển động gọi là Làm trơn (Smoother) đã được phát triển mạnh mẽ nhằm xác định chính xác nhất quỹ đạo chuyển động khi đã có tọa độ đ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ự báo vị trí tiếp theo gần đúng nhất của đối tượng chuyển động để đưa đối tượng tới đích, làm trơn (Smoother) là quá trình tính ngược từ tọa độ đích về tọa độ xuất phát nhằm vẽ ra quỹ đạo thực, gần như là quỹ đạo trơn nhất 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 tượng. Lớp bài toán này thường áp dụng trong điều khiển hành trình theo quỹ đạo định trước của vật thể bay (trong quá trình chuyển động có thể thay đổi quỹ đạo), sử dụng trong đánh giá quỹ đạo bay của máy bay không người lái. 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. II. KHÔNG GIAN TRẠNG THÁI VÀ LỌC KALMAN Một mô hình tuyến tính biến đổi theo thời gian được diễn tả bằng công thức liên tục về thời gian như sau: dx(t) =Fx(t)+Lw(t) dt (1) Trong đó: Điều kiện quán tính là x(0)~N(m(0),P(0)) F và L ma trận hằng với đặc tính hoạt động của mô hình w(t) là ồn trắng với cường độ phổ là Q c Bộ lọc Kalman kinh điển được giới thiệu bởi Rudolph E. Kalman (1960), đưa ra một giải pháp đệ quy để đánh giá những hệ thống động rời rạc tuyến tính hóa thời gian. Lọc Kalman bao gồm 2 bước: bước Dự đoán (prediction), ở đó các 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 đó; và bước Cập nhật (Update), ở đó 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 đó. Theo Särkkä (2006), sử dụng lọc Kalman cho mô hình trên A k và Q k có dạng: (2) k A = exp(FΔt) k k ⎞ ⎟ ⎪ ⎭ k -1 k (3) k Δt T kKC 0 T K Q = exp(F(Δt-τ))LQ L exp (F(Δt-τ)) dτ ∫ Trong đó: là bước tính. kk+1 Δt=t -t (4) T C k k T k LQ L C F0 =exp Δt D I 0 -F ⎧⎫ ⎞ ⎛⎞ ⎛⎛ ⎪⎪ ⎟ ⎨⎬ ⎜⎟ ⎜⎜ ⎟ ⎝⎠ ⎝ ⎝⎠ ⎪ ⎠ ⎩ Với : -1 kk Q=CD • Prediction: (5) - kk-1k m=A m -T k k-1 k-1 k-1 k-1 P=A P A +Q • Update: - kkk v=y-Hm T kkk k S=HPH+R k k k k -T-1 kkk K=PHS (6) - kkk m=m+Kv -T kkkk P=P-KSK Trong đó: • và là giá trị dự báo trung bình và ma trận hiệp biến của trạng thái riêng biệt tại thời điểm k trước khi có giá trị đo đạc (Tiền nghiệm) - k m - k P • và là giá trị đánh giá trung bình và ma trận hiệp biến của trạng thái riêng biệt tại thời điểm k sau khi có giá trị đo đạc (Hậu nghiệm) k m k P k v là giá trị mới đưa vào (thặng dư đo đạc) tại thời điểm bước k k S là ma trận hiệp biến dự báo đo đạc tại bước k k K là hệ số khuếch đại lọc, chính là độ lợi cần tìm của mạch lọc Kalman trong mỗi ước đoán 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 Kalman truyền thống không được áp dụng được. Trong những trường hợp này ta sử dụng mô hình lọc Kalman mở rộng EKF (extended Kalman filter) như sau: (7) kk-1 k kkk x = f(x , k -1) + q y = h(x ,k) + r ⎧ ⎨ ⎩ -1 ) k k k k Trong đó: n k xR∈ : là trạng thái của hệ thống ở bước k m k yR∈ : là giá trị đo đạc tại thời điểm k k-1 k-1 q~N(0,Q : là nhiễu xử lý k r~N(0,R) : là nhiễu đo đạc f: là hàm (phi tuyến) động học của mô hình h: là hàm (phi tuyến) vector đo đạc của mô hình Bộ lọc Kalman mở rộng và ứng dụng (Jazwinski, 1970; Maybeck, 1982; Bar- Shalom, 2001; Grewal và Andrews, 2001; Särkkä, 2006), mở rộng phạm vi của bộ lọc Kalman để lọc tối ưu các vấn đề phi tuyến bằng cách thành lập một xấp xỉ Gaussian phân phối trạng thái x và các phép đo y dựa trên biến đổi sử dụng chuỗi Taylor. Sự khác nhau giữa hai bộ lọc KF và EKF là ma trận và trong KF được thay thế bởi ma trận Jacobi và trong EKF. Giá trị và cũng được tính toán khác trong EKF, dựa trên biến đổi xấp xỉ tuyến tính và dạng toàn phương. k A k H xk-1 F(m ,k-1) - xk H(m,k) - k m k v • Prediction: - kk-1 m = f(m ,k -1) -T k x k -1 k -1 x k -1 k -1 P=F(m ,k-1)P F(m ,k-1)+Q (8) • Update: - kk k v = y - h(m ,k) T- kxkkxk S=H(m,k)PH(m,k)+R (9) -T - -1 kkxk k K = P H (m ,k)S - kkk m=m+Kv -T kkkk P=P-KSK Trong đó: và là ma trận Jacobi của hàm f và h, với x F(m,k-1) x H(m,k) j xj,j' j' x=m δf (x,k -1) [F (m,k -1)] = δx (10) j xj,j' j' x=m δh (x,k) [H (m, k)] = δx (11) III. BỘ LÀM TRƠN SMOOTHER  Mô hình RTS (Rauch-Tung- Striebel smoother) Bộ làm trơn cho mô hình lọc Kalman rời rạc theo thời gian, còn được biết đến dưới tên gọi Rauch-Tung-Striebel-smoother RTS (Rauch, 1965; Gelb, 1974; Bar-Shalom, 2001), sử dụng trong quá trình tính toán làm trơn mô hình (7) được đưa ra theo phân tán P, theo công thức: ss k1:T k k k P(x y ) = N(x m ,P ) (12) Giá trị ước đoán và ma trận hiệp biến được tính toán trong các dạng làm trơn cho lọc Kalman, theo công thức sau: k m k P - k+1 k m=Am -T k+1 k k k k P=APA+Q T kkk+1 k C=PA[P ] -1 (13) ss kkkk+1+ k m=m+C[m -m ] 1 ss kk kk+1+1 k P=P+C[P -P ] Trong đó: • và là những ước lượng của bộ làm trơn (smoother) cho trạng thái ước đoán và ma trận hiệp biến bước k. s k m s k P • và là những ước lượng của bộ lọc (filter) cho trạng thái ước đoán và ma trận hiệp biến bước k. k m k P • và là giá trị ước đoán trạng thái ước đoán và ma trận hiệp biến bước k+1 - k+1 m - k+1 P • là độ lợi làm trơn tại thời điểm 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ụ thể. k C Sự khác biệt giữa các bộ lọc Kalman và 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 (backward smoother). Trong thuật toán làm trơn RTS tính toán đệ quy bắt đầu từ bước thời gian cuối cùng T, với và . s T m=m T -1 - T k k ) s TT P=P  Mô hình ETS (extended Forward- Backward smoother) Bộ làm trơn ETS xây dựng các hoạ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 dữ liệu chuyển tiếp đi từ giá trị đo lường đầu tiên hướng đến các giá trị mới hơn, và bộ lọc 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 được sinh ra bởi hai bộ lọc một cách phù hợp với một ước tính làm trơn cho trạng thái, trong đó độ 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 tắc Gelb, 1974). Với mô hình forward- backward, bộ làm trơn ETS có những lỗi tương 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 đoán và ma trận hiệp biến được tính toán trên cơ sở mô hình phi tuyến f như sau: k m k P - k+1 k m = f(m , k) -T k+1 x k k x k k P=F(m,k)PF(m,k)+Q (14) T- kkxk k+1 C = P F (m ,k)[P ] ss kkkk+1k+1 m=m+C[m -m ] ss- kk kk+1k-1 P=P+C[P -P ]C IV. MÔ PHỎNG Trong bài báo này, tác giả triển khai bộ 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 thái nhận được bởi quan sát , trộn nhiễu Gauss. Khi mô phỏng ta sử dụng hàm phát sine ngẫu nhiên thay cho các tín hiệu thực tế nhận được của quỹ đạo bay thực tế từ tín hiệu GPS và từ sensor INS, với các tham số hệ số biên độ, vận tốc góc và độ lớn góc thay đổi, được trộn với nhiễu ngẫu nhiên. Khi thay thế hàm phát sine 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 tượng. k x 1:k y kk h(x , k) = a sin(θ ) Theo mô hình vận tốc Wiener, vector trạng thái được biểu diễn như sau: (15) ( T kkkk x=θωa dx(t) dt = (16) 010 000x(t)+ 000 ⎛⎞ ⎜⎟ ⎜⎟ ⎜⎟ ⎝⎠ 00 10w(t) 01 ⎛⎞ ⎜⎟ ⎜⎟ ⎜⎟ ⎝⎠ 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 k a là hệ số biên độ tại bước tính thứ k Từ (2) có: kkk k-1 1 Δt0 A=exp(FΔt)Þ x = 0 1 0x +q 001 ⎛⎞ ⎜⎟ ⎜⎟ ⎜⎟ ⎝⎠ k-1 k-1 (17) Với là khoảng cách bước tính, trong mô phỏng chọn , 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 Δt = 0,01 Từ (3) có: k Δt T kKC 0 T K Q= exp(F(Δt-τ))LQ L exp (F(Δt-τ)) dτ ∫ Với nhiễu ngẫu nhiên Gauss ta tính được: k q ~ N(0,Q ) 32 11 2 k-1 1 1 2 11 Δtq Δtq 0 32 1 Q= Δtq Δtq 0 2 00Δtq ⎛⎞ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟ ⎝⎠ (18) Với mô hình đo đạc ta có: (19) kkkkk y = h(x ,k) + r = a sin(θ )+r k Đạo hàm vector đo đạc, thay vào công thức mô hình lọc và làm trơn, ta tính được ma trận Jacobi như sau: ( ) xkk H(m,k)=acos(θ )0sin(θ ) k (20) Kết quả mô phỏng Hình 1. Kalman Filter (KF) Hình 2. Rauch-Tung-Striebel (RTS Smoother) Hình 3. Extended Forward-Backward smoother (ETS) Nhận xét: Mô hình KF và RST chỉ thực hiện được 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 nhiên thời gian tính toán bị kéo dài hơn gấn gần 10 lần (t ~ 250 cho 4 chu kỳ). V. KẾT LUẬN Bộ làm trơn được sử dụng để nâng cao chất lượng dự đoán và vẽ ra quỹ đạo thực của chuyển động, bằng cách kết hợp dữ liệu trước và dữ liệu đã có trong các tính toán nhằm tăng tần số lấy mẫu, đáp ứng các biên độ sai lệch lớn, tái tạo quỹ đạo từ dữ liệu thực GPS/INS. Lớp bài toán này áp dụng trong điều khiển hành trình theo quỹ đạo định trước của vật thể bay, thường thấy trong các máy bay không người lái, đòi hỏi tốc độ tính toán rất cao. Vì vậy để xác định chính xác được quỹ đạo, ta 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ý. Tài liệu tham khảo [1]. Greg Welch, Gary Bishop (Updated July 24, 2006); An Introduction to the Kalman Filter; Chapel Hill, NC 27599-3175 [2]. Jim Ledin (2004); Embedded Control Systems in C/C++: An Introduction for Software Developers Using MATLAB; CMP Books, ISBN:1578201276. [3]. Bruno Otávio Soares Teixeira (2005); Flight path reconstruction using the unsented Kalman filter algorithm; Procedings of COBEM 2005, by ABCM [4]. Thanh Binh Ngo, Hung Lan Le, Thanh Hai Nguyen; Survey of Kalman Filters and Their Application in Signal Processing; Procedings of AICI 2009, by IEEE and Springer ♦ . có thể thay đổi quỹ đạo) , sử dụng trong đánh giá quỹ đạo bay của máy bay không người lái. 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 một số công cụ ứng dụng trong đánh giá chuyển động gọi là Làm trơn (Smoother) đã được phát triển mạnh mẽ nhằm xác định chính xác nhất quỹ đạo chuyển động khi đã có tọa độ điểm xuất phát và. tượng chuyển động để đưa đối tượng tới đích, làm trơn (Smoother) là quá trình tính ngược từ tọa độ đích về tọa độ xuất phát nhằm vẽ ra quỹ đạo thực, gần như là quỹ đạo trơn nhất của chuyển động.