Kalman濾波及其在SLAM中應(yīng)用-25頁P(yáng)PT文檔.ppt

Kalman濾波及其在SLAM中應(yīng)用 康丹 Outline Kalman濾波簡單實(shí)例擴(kuò)展kalman濾波SLAM問題 準(zhǔn)備知識 狀態(tài)空間模型 在控制領(lǐng)域中 用輸入向量 狀態(tài)向量和輸出向量描述系統(tǒng)的一種方法 叫狀態(tài)空間模型 如下式表示時刻的狀態(tài)向量 表示時刻的輸入向量 表示時刻的輸出向量 A表示狀態(tài)矩陣 B為控制矩陣或者輸入矩陣 C為觀測矩陣 D為輸入輸出矩陣 Kalman濾波 背景介紹 Kalman 匈牙利數(shù)學(xué)家 卡爾曼濾波器源于他的博士論文和1960年發(fā)表的論文 ANewApproachtoLinearFilteringandPredictionProblems 線性濾波與預(yù)測問題的新方法 kalman濾波 引入一個離散控制系統(tǒng)的控制模型 定義系統(tǒng)的觀測變量為z 得到測量方程為 其中 表示時刻的狀態(tài)向量 表示時刻的輸入向量 表示時刻的觀測向量 分別為過程激勵噪聲和觀測噪聲 它們?yōu)橄嗷オ?dú)立 正態(tài)分布的白噪聲 協(xié)方差分別是Q R 這里假設(shè)它們不隨系統(tǒng)狀態(tài)變化而變化 kalman濾波 kalman濾波第一步 用系統(tǒng)的過程模型來預(yù)測下一狀態(tài)的系統(tǒng) 假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是 可以預(yù)測下一時刻k 1出現(xiàn)的狀態(tài) 更新狀態(tài)的協(xié)方差矩陣 其中 為過程激勵噪聲協(xié)方差 kalman濾波 結(jié)合觀測值更新狀態(tài)估計(jì) 得到k 1時刻狀態(tài)為 其中 稱為新息 為kalman增益 其計(jì)算公式為 式中 為觀測噪聲協(xié)方差 kalman濾波 為了令kalman不斷迭代下去 需要更新K時刻狀態(tài)的協(xié)方差 時間更新和測量更新不斷重復(fù)迭代 就是kalman最吸引人的特性 簡單實(shí)例 考慮房間內(nèi)溫度對象過程 即過程的狀態(tài)不隨時間變化 沒有控制輸入 包含噪聲的觀測值是狀態(tài)變量的直接體現(xiàn) 那么 kalman濾波的5個方程分別為 簡單實(shí)例 假設(shè)房間溫度不變 k時刻為23度 估計(jì)值 該值的協(xié)方差假設(shè)為5度 上一時刻更新的協(xié)方差為3度 噪聲為2度 房間內(nèi)有一溫度計(jì) 其值顯示25度 該值的偏差 噪聲 為2度 則kalman增益為 K 1時刻的真實(shí)估計(jì)值為 該時刻更新協(xié)方差為 即為上邊3度的計(jì)算式 擴(kuò)展卡爾曼濾波 可以看出 卡爾曼濾波估計(jì)是一個線性隨機(jī)系統(tǒng)的狀態(tài) 然而實(shí)際中 很多系統(tǒng)是非線性的 處理這些系統(tǒng)時 用擴(kuò)展卡爾曼濾波 EKF 它是將期望和方差線性化的卡爾曼濾波器 控制對象的狀態(tài)空間表述為 擴(kuò)展卡爾曼濾波 擴(kuò)展卡爾曼濾波的5個方程為 擴(kuò)展卡爾曼濾波 A是f對x的偏導(dǎo)的雅可比矩陣 即W是f對w的偏導(dǎo)的雅可比矩陣 即H是h對x的偏導(dǎo)的雅可比矩陣 即V是h對v的偏導(dǎo)的雅可比矩陣 即 SLAM問題 SLAM Simultaneouslocalizationandmapping自主定位和地圖創(chuàng)建 有時也稱為CML concurrentmappingandlocalization 問題陳述 自主移動機(jī)器人從未知環(huán)境中一個未知點(diǎn)開始運(yùn)動 經(jīng)過一系列未知特征點(diǎn) 并得到這些點(diǎn)到機(jī)器人距離的一組測量數(shù)據(jù) 這組測量數(shù)據(jù)由機(jī)器人頭部的測距傳感器測得 SLAM處理的目標(biāo)是根據(jù)這些測量數(shù)據(jù)得到環(huán)境的估計(jì)地圖 并用這張地圖實(shí)現(xiàn)機(jī)器人的定位 SLAM問題 在SLAM中 機(jī)器人運(yùn)動模型已知 即運(yùn)動角速度和線速度 可以估計(jì)機(jī)器人的位置 機(jī)器人與特征點(diǎn)之間的測量值可以測出 即知道測量值 在SLAM中 根據(jù)EKF 用每一狀態(tài)的測量值來更新機(jī)器人的運(yùn)動位置 然后由機(jī)器人位置和測量值求出特征點(diǎn)的坐標(biāo) SLAM問題 K時刻 機(jī)器人狀態(tài)可表示為Xv k 表示機(jī)器人的狀態(tài) Xlm k 為路標(biāo)的狀態(tài) 它不隨時間變化而變化 可以寫成Xlm 觀測值是描述機(jī)器人和特征點(diǎn)距離的函數(shù) 則其中 為特征點(diǎn)i與機(jī)器人之間的距離 為機(jī)器人與特征點(diǎn)位置的夾角 SLAM問題 EKF SLAM算法 1 初始化機(jī)器人的位置Xv 0 和協(xié)方差陣P 0 可以令其都為0 2 預(yù)測 3 觀測值路標(biāo)特征值的預(yù)測值的新息為新息的誤差協(xié)方差矩陣為 SLAM問題 4 數(shù)據(jù)關(guān)聯(lián)檢驗(yàn)k 1時觀測到的特征點(diǎn)與0 1 k時觀測到的特征點(diǎn)是否關(guān)聯(lián)上 根據(jù)下式證明時刻觀測到的一組特征點(diǎn)確實(shí)是觀測到的 其中G為常數(shù) 否則 舍棄觀測值 5 更新 SLAM問題 6 返回 2 遞歸執(zhí)行 2 5 該算法在m