《機(jī)器人智能算法導(dǎo)論》

配套課件本節(jié)簡(jiǎn)介能力目標(biāo)：能推導(dǎo)EKF-SLAM預(yù)測(cè)階段的協(xié)方差矩陣更新公式。能設(shè)計(jì)包含新地標(biāo)插入的系統(tǒng)狀態(tài)擴(kuò)展方法。能分析EKF-SLAM算法在高維狀態(tài)空間中的可擴(kuò)展性問題。知識(shí)目標(biāo)：掌握EKF-SLAM的核心原理及預(yù)測(cè)-更新框架。理解協(xié)方差矩陣分塊形式及動(dòng)態(tài)變化機(jī)制。掌握機(jī)器人運(yùn)動(dòng)模型與觀測(cè)模型在SLAM中的數(shù)學(xué)表示。理解數(shù)據(jù)關(guān)聯(lián)問題的本質(zhì)與解決方法。本講綱要EKF-SLAM的基本問題EKF-SLAM的關(guān)鍵模塊EKF-SLAM的完整算法SLAM的定義同步定位與地圖構(gòu)建（SimultaneousLocalizationAndMapping，SLAM）是指機(jī)器人從未知地點(diǎn)出發(fā)探索未知環(huán)境，并在運(yùn)動(dòng)過程中通過觀測(cè)地圖特征來定位自身位置和姿態(tài)，再根據(jù)自身位置來構(gòu)建地圖，從而達(dá)到同時(shí)定位和地圖構(gòu)建的目標(biāo)。SLAM具有廣泛而重要的應(yīng)用，其能使機(jī)器人自主行走、探索未知世界。一個(gè)開創(chuàng)性SLAM技術(shù)是基于EKF的SLAM技術(shù)。EKF-SLAM??是解決SLAM問題的一種??特定方法??或??算法框架??，利用已知的機(jī)器人的動(dòng)作執(zhí)行序列（運(yùn)動(dòng)控制數(shù)據(jù)序列）和機(jī)器人傳感器的觀測(cè)序列（觀測(cè)數(shù)據(jù)序列）及EKF來估計(jì)機(jī)器人的軌跡和探索得到的地圖。EKF-SLAM的基本問題SLAM的關(guān)鍵數(shù)據(jù)EKF-SLAM的基本問題數(shù)據(jù)類型含義描述t時(shí)刻機(jī)器人在全局坐標(biāo)系中的位置與方向，是EKF-SLAM中用于定位的核心數(shù)據(jù)。機(jī)器人通過傳感器對(duì)環(huán)境地標(biāo)測(cè)得的數(shù)據(jù)，用于修正系統(tǒng)狀態(tài)，常見形式為“距離-方位”數(shù)據(jù)。環(huán)境中所有地標(biāo)的全局坐標(biāo)系位置集合，地標(biāo)數(shù)量隨觀測(cè)動(dòng)態(tài)增加，是EKF-SLAM構(gòu)建的核心輸出。在一個(gè)機(jī)器人系統(tǒng)中，通常包括3種關(guān)鍵數(shù)據(jù)：機(jī)器人姿態(tài)、傳感器觀測(cè)數(shù)據(jù)、地圖數(shù)據(jù)。SLAM關(guān)鍵數(shù)據(jù)的關(guān)系EKF-SLAM的基本問題這3種數(shù)據(jù)有緊密的聯(lián)系，如果已知其中兩種數(shù)據(jù)，那么就可以推斷出剩余的一種數(shù)據(jù)。機(jī)器人定位的更正步驟實(shí)質(zhì)上是一個(gè)利用地圖數(shù)據(jù)和觀測(cè)數(shù)據(jù)推斷機(jī)器人姿態(tài)的過程。在這個(gè)過程中，首先，將預(yù)測(cè)步驟獲得的機(jī)器人姿態(tài)作為已知姿態(tài)，結(jié)合地圖數(shù)據(jù)做如下推斷：如果機(jī)器人處于這個(gè)姿態(tài)，那么其觀測(cè)數(shù)據(jù)應(yīng)是什么樣的。其次，利用實(shí)際觀測(cè)數(shù)據(jù)和推導(dǎo)的數(shù)據(jù)做差，獲得新的知識(shí)（Novelty）。最后，利用卡爾曼增益將

這個(gè)差量（依然是觀測(cè)數(shù)據(jù)類型）變換為姿態(tài)數(shù)據(jù)類型。同理，如果已知機(jī)器人姿態(tài)和觀測(cè)數(shù)據(jù)，那么我們也可以較容易地推

斷出地圖數(shù)據(jù)。基于EKF的機(jī)器人定位與基于EKF的SLAM??的區(qū)別EKF-SLAM的基本問題對(duì)比維度基于EKF的機(jī)器人定位基于EKF的SLAM地圖地圖已知，且現(xiàn)實(shí)世界中的地標(biāo)已經(jīng)被提取并且抽象為地圖。對(duì)現(xiàn)實(shí)世界中的地標(biāo)一無所知。地標(biāo)地標(biāo)的個(gè)數(shù)固定。根據(jù)觀測(cè)數(shù)據(jù)來動(dòng)態(tài)添加新地標(biāo)。狀態(tài)向量包含姿態(tài)的固定長(zhǎng)度短向量。不僅包含姿態(tài)向量還包含地圖向量，且長(zhǎng)度是動(dòng)態(tài)變化的?；贓KF的機(jī)器人定位和SLAM在算法上有很大的重疊，但也有較大的不同之處。EKF-SLAM的系統(tǒng)狀態(tài)EKF-SLAM的基本問題

系統(tǒng)狀態(tài)的均值EKF-SLAM的基本問題

系統(tǒng)狀態(tài)的協(xié)方差EKF-SLAM的基本問題

核心邏輯卡爾曼濾波器的兩大核心模塊是預(yù)測(cè)模塊和更正模塊。同樣，EKF-SLAM也包含預(yù)測(cè)模塊和更正模塊兩大核心模塊。EKF-SLAM預(yù)測(cè)模塊和常規(guī)EKF的預(yù)測(cè)模塊相同，利用已知的控制信息和前一時(shí)刻的狀態(tài)信息，預(yù)測(cè)當(dāng)前時(shí)刻的狀態(tài)。同樣，EKF-SLAM更正模塊和常規(guī)EKF的更正模塊一樣，利用新觀測(cè)到的地標(biāo)來更新當(dāng)前時(shí)刻預(yù)測(cè)的狀態(tài)。其中，在更正模塊中，還包含數(shù)據(jù)關(guān)聯(lián)子模塊和新地標(biāo)插入子模塊。當(dāng)機(jī)器人利用傳感器觀測(cè)到一個(gè)地標(biāo)時(shí)，數(shù)據(jù)關(guān)聯(lián)子模塊可以判斷此地標(biāo)是否已經(jīng)在地圖中存在。如果存在，數(shù)據(jù)關(guān)聯(lián)子模塊將返回該地標(biāo)在地圖中的坐標(biāo)。如果不存在，那么此地標(biāo)為一個(gè)新地標(biāo)，進(jìn)而利用新地標(biāo)插入子模塊擴(kuò)充系統(tǒng)狀態(tài)向量和對(duì)應(yīng)的協(xié)方差矩陣。EKF-SLAM的關(guān)鍵模塊預(yù)測(cè)模塊核心概念與變量定義EKF-SLAM的關(guān)鍵模塊變量符號(hào)含義維度/類型預(yù)測(cè)前的系統(tǒng)狀態(tài)2N+3維隨機(jī)向量（N為地標(biāo)數(shù)）預(yù)測(cè)的系統(tǒng)狀態(tài)2N+3維隨機(jī)向量

t時(shí)刻對(duì)機(jī)器人施加的控制確定性向量（維度由運(yùn)動(dòng)模型定）在機(jī)器人運(yùn)動(dòng)過程中引入的過程噪聲2N+3維高斯向量（均值為0）(2N+3)×(2N+3)維矩陣(2N+3)×(2N+3)維矩陣預(yù)測(cè)模塊的目標(biāo)是利用已知的t?1時(shí)刻系統(tǒng)狀態(tài)、機(jī)器人t時(shí)刻的控制輸入和過程噪聲，來建模執(zhí)行控制輸入后的t時(shí)刻系統(tǒng)狀態(tài)。預(yù)測(cè)模塊狀態(tài)轉(zhuǎn)移函數(shù)

EKF-SLAM的關(guān)鍵模塊預(yù)測(cè)模塊狀態(tài)轉(zhuǎn)移函數(shù)

EKF-SLAM的關(guān)鍵模塊預(yù)測(cè)模塊均值與協(xié)方差矩陣

EKF-SLAM的關(guān)鍵模塊預(yù)測(cè)模塊均值與協(xié)方差矩陣

EKF-SLAM的關(guān)鍵模塊預(yù)測(cè)模塊均值與協(xié)方差矩陣

EKF-SLAM的關(guān)鍵模塊預(yù)測(cè)模塊均值與協(xié)方差矩陣

EKF-SLAM的關(guān)鍵模塊更正模塊核心概念與觀測(cè)模型

EKF-SLAM的關(guān)鍵模塊更正模塊核心概念與觀測(cè)模型

EKF-SLAM的關(guān)鍵模塊更正模塊核心概念與觀測(cè)模型

EKF-SLAM的關(guān)鍵模塊數(shù)據(jù)關(guān)聯(lián)及新地標(biāo)插入

EKF-SLAM的關(guān)鍵模塊數(shù)據(jù)關(guān)聯(lián)

EKF-SLAM的關(guān)鍵模塊新地標(biāo)插入

EKF-SLAM的關(guān)鍵模塊新地標(biāo)插入

EKF-SLAM的關(guān)鍵模塊理論基礎(chǔ)

EKF-SLAM的完整算法初始化

EKF-SLAM的完整算法預(yù)測(cè)步驟

EKF-SLAM的完整算法預(yù)測(cè)步