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

Kalman濾波及其在SLAM中應(yīng)用 康丹 Outline Kalman濾波簡單實例擴展kalman濾波SLAM問題 準備知識 狀態(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)橄嗷オ毩?正態(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)估計 得到k 1時刻狀態(tài)為 其中 稱為新息 為kalman增益 其計算公式為 式中 為觀測噪聲協(xié)方差 kalman濾波 為了令kalman不斷迭代下去 需要更新K時刻狀態(tài)的協(xié)方差 時間更新和測量更新不斷重復(fù)迭代 就是kalman最吸引人的特性 簡單實例 考慮房間內(nèi)溫度對象過程 即過程的狀態(tài)不隨時間變化 沒有控制輸入 包含噪聲的觀測值是狀態(tài)變量的直接體現(xiàn) 那么 kalman濾波的5個方程分別為 簡單實例 假設(shè)房間溫度不變 k時刻為23度 估計值 該值的協(xié)方差假設(shè)為5度 上一時刻更新的協(xié)方差為3度 噪聲為2度 房間內(nèi)有一溫度計 其值顯示25度 該值的偏差 噪聲 為2度 則kalman增益為 K 1時刻的真實估計值為 該時刻更新協(xié)方差為 即為上邊3度的計算式 擴展卡爾曼濾波 可以看出 卡爾曼濾波估計是一個線性隨機系統(tǒng)的狀態(tài) 然而實際中 很多系統(tǒng)是非線性的 處理這些系統(tǒng)時 用擴展卡爾曼濾波 EKF 它是將期望和方差線性化的卡爾曼濾波器 控制對象的狀態(tài)空間表述為 擴展卡爾曼濾波 擴展卡爾曼濾波的5個方程為 擴展卡爾曼濾波 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 問題陳述 自主移動機器人從未知環(huán)境中一個未知點開始運動 經(jīng)過一系列未知特征點 并得到這些點到機器人距離的一組測量數(shù)據(jù) 這組測量數(shù)據(jù)由機器人頭部的測距傳感器測得 SLAM處理的目標是根據(jù)這些測量數(shù)據(jù)得到環(huán)境的估計地圖 并用這張地圖實現(xiàn)機器人的定位 SLAM問題 在SLAM中 機器人運動模型已知 即運動角速度和線速度 可以估計機器人的位置 機器人與特征點之間的測量值可以測出 即知道測量值 在SLAM中 根據(jù)EKF 用每一狀態(tài)的測量值來更新機器人的運動位置 然后由機器人位置和測量值求出特征點的坐標 SLAM問題 K時刻 機器人狀態(tài)可表示為Xv k 表示機器人的狀態(tài) Xlm k 為路標的狀態(tài) 它不隨時間變化而變化 可以寫成Xlm 觀測值是描述機器人和特征點距離的函數(shù) 則其中 為特征點i與機器人之間的距離 為機器人與特征點位置的夾角 SLAM問題 EKF SLAM算法 1 初始化機器人的位置Xv 0 和協(xié)方差陣P 0 可以令其都為0 2 預(yù)測 3 觀測值路標特征值的預(yù)測值的新息為新息的誤差協(xié)方差矩陣為 SLAM問題 4 數(shù)據(jù)關(guān)聯(lián)檢驗k 1時觀測到的特征點與0 1 k時觀測到的特征點是否關(guān)聯(lián)上 根據(jù)下式證明時刻觀測到的一組特征點確實是觀測到的 其中G為常數(shù) 否則 舍棄觀測值 5 更新 SLAM問題 6 返回 2 遞歸執(zhí)行 2 5 該算法在m