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ỹ
Trang 1I 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
Trang 2về thời gian như sau:
dx(t) = Fx(t) + Lw(t)
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à Qc
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 Ak và Qk có dạng:
A = exp(FΔt )k k (2)
k
⎞
⎟
⎪⎭
k
-1
k
k
Δt
T
0
T K
(F(Δt - τ)) dτ
∫
Trong đó: Δt = tk k+1- t là bước tính
T C k
k T
k
LQ L
⎝ ⎠
⎝
k k
Q = C D
• Prediction:
- (5)
k k-1 k
m = A m
k k-1 k-1 k-1 k-1
P = A P A + Q
• Update:
v = y - H m
T
k k k k
S = H P H + Rk
k
k
k
- T -1
k k k
m = m + K v
k k k k
P = P - K S K 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
-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
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:
Trang 3k k-1 k (7)
x = f(x ,k -1) + q
y = h(x ,k) + r
⎧
⎨
⎩
-1
) k
k
k
k
Trong đó:
n
k
x ∈R : là trạng thái của hệ thống ở
bước k
m
k
y ∈R : 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
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
x k-1
F (m ,k -1) H (m ,k)x -k
-k
• Prediction:
-k k-1
m = f(m ,k -1)
k x k-1 k-1 x k-1 k-1
P = F (m ,k -1)P F (m ,k -1) + Q (8)
• Update:
v = y - h(m ,k)k k -k
- - T
S = H (m ,k)P H (m ,k) + R
K = P H (m ,k)Sk k- Tx -k -1k (9)
m = m + K v
k k k k
P = P - K S K
trận Jacobi của hàm f và h, với
x
F (m,k -1) H (m,k)x
x j, j' j
j' x=m
δf (x,k -1) [F (m, k -1)] =
δx (10)
x j, j' 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:
k 1: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
k+1 k k k k
P = A P A + Q T
k k k k +1
C = P A [P ]-1 (13)
k k k k+1 k+
m = m + C [m - m 1]
Trang 4s s
k k k k+1 k+1
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
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
• 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
-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
s T
m = mT
-1
-T k
k
)
s
T T
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
-k+1 k
m = f(m ,k)
Pk+1- = F (m ,k)P F (m ,k) + Qx k k xT k k
T - (14)
k k x k k+1
C = P F (m ,k)[P ]
k k k k+1 k+1
m = m + C [m - m ]
-k k k k+1 k-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
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
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:
( T (15)
dx(t)
0 1 0
0 0 0 x(t) +
0 0 0
0 0
1 0 w(t)
0 1
Trong đó:
Trang 5θ 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ó:
1 Δt 0
A = exp(FΔt )Þ x = 0 1 0 x + q
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
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 )
2
2
1
2
(18)
Với mô hình đo đạc ta có:
y = h(x ,k) + r = a sin(θ ) + rk k k k k k (19)
Đạ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:
H (m,k) = a cos(θ ) 0 sin(θ )x ( k k 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ỳ)
Trang 6Vớ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
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♦
