Mô hình b瓜 l丑c Kalman tiêu chu育n:

Một phần của tài liệu Nghiên cứu tự hành robot dựa trên phương pháp kết hợp nhiều cảm biến (Trang 38 - 42)

Các b瓜 l丑c Kalman d詠a trên h羽 th嘘pi"8瓜ng l詠c h丑c tuy院n tính trong mi隠n th運i

ikcp0"Ej¿pi"8逢嬰c mô hình hóa trên m瓜t chu厩k"Octmqx"8逢嬰c xây d詠ng trên các toán t穎 tuy院n tính b鵜 xáo tr瓜n b荏i các error có th吋 bao g欝m nhi宇u Gaussian. Tr衣ng thái c栄a h羽 th嘘pi"8逢嬰c bi吋u di宇p"f逢噂i d衣ng vector các s嘘 th詠c. T衣i m厩i kho違ng t<pi"vj運i gian r運i r衣c, m瓜t toán t穎 tuy院p"v pj"8逢嬰c áp d映ng cho tr衣pi"vjƒk"8吋 t衣o ra tr衣ng thái m噂i, v噂i m瓜t s嘘 nhi宇w"8逢嬰c tr瓜n vào và tùy ch丑n m瓜t s嘘 thông tin t瑛eƒe"8k隠u khi吋n trên h羽 th嘘ng n院w"ej¿pi"8逢嬰c bi院v"8院p0"Ucw"8„."o瓜t toán t穎 tuy院p"v pj"mjƒe"8逢嬰c tr瓜n v噂i nhi隠u nhi宇w"j挨p"v衣o ra các k院t qu違8亥w"tc"swcp"uƒv"8逢嬰c t瑛 tr衣ng thái th詠c

*Ð育pÑ+0" D瓜 l丑c Kalman có th吋 8逢嬰e" eqk" n " v逢挨pi" v詠 v噂i mô hình Markov 育n, v噂i

8k吋m khác bi羽t chính là các bi院n tr衣ng thái 育n nh壱n giá tr鵜 trong m瓜t không gian liên t映c (trƒk"pi逢嬰c v噂i m瓜t không gian tr衣ng thái r運i r衣e"pj逢"vtqpi"o»"j·pj"Octmqx"育n). Có m瓜t s詠 8嘘i ng磯u m衣nh m胤 gi英c" eƒe" rj逢挨pi" vt·pj" e栄a B瓜 l丑c Kalman và các

rj逢挨pi"vt·pj"e栄a mô hình Markov 育p0"Aƒpj"ikƒ"x隠 mô hình này và các mô hình

mjƒe"8逢嬰e"8逢c"tc"vtqpi"Tqygku"và Ghahramani (1999), [32] và Hamilton (1994),

Ej逢挨pi"350"]27]

A吋 s穎 d映ng b瓜 l丑e"Mcnocp"8吋逢噂c tính tr衣ng thái bên trong c栄a m瓜t quá trình ch雨 8逢c"tc"o瓜t chu厩i các quan sát nhi宇w."pi逢運i ta ph違i l壱p mô hình quá trình phù h嬰p v噂i khuôn kh鰻 c栄a b瓜 l丑e"Mcnocp0"Ak隠w"p {"e„"pij c"n "zƒe"8鵜nh các ma tr壱n sau:

Fk là mô hình chuy吋p"8鰻i tr衣ng thái, Hk là mô hình quan sát, Qk là hi羽r"rj逢挨pi"uck"

c栄a nhi宇u quá trình, Rk là hi羽r"rj逢挨pi"uck"e栄a ti院ng nhi宇w"swcp"uƒv="x "8»k"mjk"n "Bk,

o»"j·pj"8亥w"x q"8k隠u khi吋n, cho m厩k"d逢噂c th運i gian, k."pj逢"8逢嬰c mô t違d‒p"f逢噂i.

Hình 4 Mô hình bên trong b lc Kalman

Mô hình b瓜 l丑c Kalman gi違 8鵜nh tr衣pi"vjƒk"8¿pi"v衣i th運k"8k吋o"m"8逢嬰c phát tri吋n t瑛

tr衣ng thái 荏 (k-1) theo:

Xk = Fk xk-1 + Bk uk + Wk (1)

V噂i:

̋"Fk là mô hình chuy吋n tr衣pi"vjƒk"8逢嬰c áp d映ng cho tr衣pi"vjƒk"vt逢噂e"8„"Xm宕3

̋"Wk là nhi宇w"swƒ"vt·pj"8逢嬰c gi違 8鵜nh rút ra t瑛 phân ph嘘i chu育p"8c"dk院n trung bình b茨ng 0 v噂i hi羽r"rj逢挨pi"uck"Qk.

Wk漢 N (0; Qk)

T衣i th運k"8k吋m k, m瓜t quan sát (ho員e"rjfir"8q+"zk c栄a tr衣ng thái th詠c Xk 8逢嬰c th詠c hi羽n theo:

zk = Ck Xk + vk (2)

vtqpi"8„"Ck là mô hình quan sát ánh x衣 không gian tr衣ng thái th詠c vào không gian

quan sát và vk là nhi宇w"swcp"uƒv"8逢嬰c gi違 8鵜nh là nhi宇u tr逸ng Gaussian trung bình b茨ng 0 v噂i hi羽r"rj逢挨pi"uck"Rk.

vk漢 N (0; Rk)

Tr衣pi"vjƒk"dcp"8亥w"x "eƒe"xgev挨"pjk宇u 荏 m厩k"d逢噂c {X0, w1."È."wk, v1."È."vkʼ"8隠u

8逢嬰c gi違8鵜pj"n "8瓜c l壱p l磯n nhau.

B瓜 l丑e"Mcnocp"8逢嬰c áp d映ng b茨ng cách th詠c hi羽p"eƒe"d逢噂c sau:

雲 d逢噂e"逢噂e"n逢嬰ng tiên nghi羽m tr衣ng thái c栄a h羽 th嘘ng 荏 th運k"8k吋o"m-3"8逢嬰c xác

8鵜pj"pj逢"ucw<"

薫賃袋怠 賃= 繋賃薫賃 賃+ 遇賃 掲賃 (3)

Ucw"8„."v pj"vqƒp"oc"vt壱n hi羽r"rj逢挨pi"uck"uck"u嘘 d詠8qƒp"R"ejq"vj運k"8k吋m k + 1: (adsbygoogle = window.adsbygoogle || []).push({});

隈賃袋怠 賃= 釧賃 隈賃 賃釧賃脹+ Q (4) Ma tr壱n h羽 s嘘 Kalman: 沓賃袋怠噺 察隈隈賃袋怠 賃察脹 賃袋怠 賃察脹髪 三 (5) C壱p nh壱t l衣i tr衣ng thái c栄a h羽 th嘘ng d詠a vào k院t qu違8q"n逢運ng: 薫賃袋怠 賃袋怠 噺 薫賃袋怠 賃髪 沓賃袋怠盤子賃袋怠伐 察薫賃袋怠 賃匪 (6) Ma tr壱n hi羽r"rj逢挨pi"uck"o噂k"8逢嬰c c壱p nh壱t l衣k"pj逢"ucw<

隈賃袋怠 賃袋怠 噺 岫薩 伐 沓賃袋怠察岻隈賃袋怠 賃岫薩 伐 沓賃袋怠察岻脹髪"沓賃袋怠"三沓賃袋怠参 (7) V噂i 薩 là ma tr壱p"8挨p"x鵜.

Ho衣v"8瓜ng c栄a gi違i thu壱t có th吋8逢嬰c trình bày trong Pseudocode sau:

Algorithm 1: B瓜 l丑c Kalman

Input: 掲谷, 子賃袋怠, 散賃, 隈谷

Output: 薫谷袋怠, 皿賃袋怠

Data: 擦賃,刷賃, C, R, Q

Initialization: X0, P0 for current time k do

D逢噂e"逢噂c tính KF: 薫賃袋怠 賃= 擦賃薫賃 賃+ 遇賃 掲賃 隈賃袋怠 賃= 釧賃隈賃 賃 釧賃脹+ Q D逢噂c hi羽u ch雨nh KF: 沓賃袋怠噺 察隈隈賃袋怠 賃察脹 賃袋怠 賃察脹髪 三 薫賃袋怠 賃袋怠 噺 薫賃袋怠 賃髪 沓賃袋怠盤渓賃袋怠伐 察薫賃袋怠 賃匪 隈賃袋怠 賃袋怠 噺 岫薩 伐 沓賃袋怠察岻隈賃袋怠 賃岫薩 伐 沓賃袋怠察岻脹髪"沓賃袋怠"三沓賃袋怠参 end

Ej逢挨pi"60 THI蔭T K蔭 GI謂I THU一T K蔭T H営P C謂M BI蔭N VÀ B浦 AK陰U KHI韻N:

Một phần của tài liệu Nghiên cứu tự hành robot dựa trên phương pháp kết hợp nhiều cảm biến (Trang 38 - 42)