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