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

配套課件本節(jié)簡(jiǎn)介能力目標(biāo)：能夠推導(dǎo)EKF算法中預(yù)測(cè)與更新步驟的數(shù)學(xué)公式。設(shè)計(jì)基于EKF的機(jī)器人定位系統(tǒng)框架并分析其性能。知識(shí)目標(biāo)：掌握機(jī)器人定位的基本概念與核心要素(狀態(tài)、地圖、聲型)。理解擴(kuò)展卡爾曼濾波器（EKF）在機(jī)器人定位中的數(shù)學(xué)原理及實(shí)現(xiàn)流程。分析運(yùn)動(dòng)學(xué)模型與傳感器觀測(cè)模型中的不確定性建模方法。本講綱要機(jī)器人定位的基本概念基于EF的機(jī)器人定位的基本框架機(jī)器人地圖機(jī)器人的運(yùn)動(dòng)學(xué)模型機(jī)器人定位的觀測(cè)模型基于EKF的機(jī)器人定位完整算法什么是機(jī)器人定位？定義：在特定環(huán)境中確定機(jī)器人自身的位置和方向（姿態(tài)）。作用：是自主機(jī)器人開展下一步自主決策與任務(wù)執(zhí)行的前提。三個(gè)關(guān)鍵概念：機(jī)器人姿態(tài)（Pose）：三維空間中的位置與方向（常用歐拉角/四元數(shù)等描述）。機(jī)器人狀態(tài)（State）：刻畫機(jī)器人自身與環(huán)境的相關(guān)變量；在機(jī)器人定位中，狀態(tài)就是姿態(tài)。機(jī)器人地圖（Map）：環(huán)境的抽象化數(shù)據(jù)表示；在機(jī)器人定位中，需要事先知道機(jī)器人所處環(huán)境的地圖。機(jī)器人定位的基本概念定位問題的本質(zhì)本質(zhì)：機(jī)器人定位＝機(jī)器人狀態(tài)（姿態(tài)）估計(jì)。由于存在運(yùn)動(dòng)與傳感器噪聲→因此必須建模帶噪的運(yùn)動(dòng)模型與傳感器觀測(cè)模型，并進(jìn)行求解。從數(shù)學(xué)幾何變換的角度來說，機(jī)器人定位本質(zhì)上是建立機(jī)器人自身局部坐標(biāo)系與全局坐標(biāo)系之間的變換關(guān)系。機(jī)器人定位的基本概念EKF的基本原理現(xiàn)實(shí)里運(yùn)動(dòng)學(xué)模型與傳感器測(cè)量模型都存在誤差，單靠任一模型都難以在真實(shí)環(huán)境中得到準(zhǔn)確位置；兩模型信息互補(bǔ)。EKF將狀態(tài)轉(zhuǎn)移（運(yùn)動(dòng)學(xué)）模型與觀測(cè)（傳感器）模型通過“預(yù)測(cè)—更正”兩步循環(huán)進(jìn)行融合，降低定位不確定性。輸入初始姿態(tài)分布（高斯），輸出每一步的姿態(tài)分布（仍為高斯）。基于EKF的機(jī)器人定位的基本框架EKF的基本步驟初始化與先驗(yàn)：EKF假設(shè)初始位置不是確定值，而是已知均值/方差的高斯分布（否則濾波無法啟動(dòng)）。預(yù)測(cè)：利用上一時(shí)刻后驗(yàn)姿態(tài)分布與運(yùn)動(dòng)學(xué)模型得到當(dāng)前時(shí)刻的先驗(yàn)（預(yù)測(cè)）姿態(tài)分布。更正：基于地圖和觀測(cè)模型，計(jì)算在預(yù)測(cè)姿態(tài)下的預(yù)測(cè)觀測(cè)，與傳感器真實(shí)觀測(cè)比對(duì)，得到卡爾曼增益并更新姿態(tài)。更新后的姿態(tài)成為下一時(shí)刻的先驗(yàn)，循環(huán)往復(fù)?；贓KF的機(jī)器人定位的基本框架框架圖

基于EKF的機(jī)器人定位的基本框架地圖為何重要？定位要與地圖配合使用，地圖決定了觀測(cè)匹配與數(shù)據(jù)關(guān)聯(lián)的效果。地圖類型：拓?fù)涞貓D，尺度地圖，混合地圖。機(jī)器人地圖地圖的種類機(jī)器人地圖拓?fù)涞貓D（TopologicalMap）定義：對(duì)現(xiàn)實(shí)環(huán)境抽象化、精簡(jiǎn)化的表示，僅保留可辨識(shí)元素（如Place/地標(biāo)），常用圖結(jié)構(gòu)的形式標(biāo)識(shí)（節(jié)點(diǎn)=地點(diǎn)/地標(biāo)；邊=動(dòng)作序列/代價(jià)/連通關(guān)系）。特征：無比例尺，兩點(diǎn)相對(duì)位置/距離不對(duì)應(yīng)真實(shí)幾何，僅表達(dá)連通關(guān)系。適用任務(wù)：大規(guī)模路徑規(guī)劃；不適合SLAM（幾何不一致）。地圖的種類機(jī)器人地圖尺度地圖（MetricMap）定義：與現(xiàn)實(shí)環(huán)境幾何一致、具有比例尺；元素間在全局坐標(biāo)下具精確位置關(guān)系。類型：柵格地圖、特征地圖、點(diǎn)云地圖；語義地圖是加了語義標(biāo)簽的尺度地圖。適用任務(wù)：Mapping/Localization/SLAM/小規(guī)模路徑規(guī)劃。地圖的種類機(jī)器人地圖混合地圖（HybridMap）由來：拓?fù)鋱D緊湊、可擴(kuò)展但不含真實(shí)幾何；尺度圖幾何精確但數(shù)據(jù)密集、耗資源。實(shí)際應(yīng)用中常融合優(yōu)點(diǎn)構(gòu)建混合地圖（取長(zhǎng)補(bǔ)短，適應(yīng)大范圍環(huán)境）。地圖的數(shù)據(jù)結(jié)構(gòu)機(jī)器人地圖雖然地圖粗分三類，但數(shù)據(jù)結(jié)構(gòu)差異很大；本章僅考慮適合機(jī)器人定位與SLAM的“特征地圖”（屬于尺度地圖中的一種）。特征地圖四個(gè)特點(diǎn)：①不保留全部環(huán)境元素，僅記錄便于辨識(shí)的地標(biāo)；②地標(biāo)以特征形式保存（非人類友好外觀）；③有比例尺，地標(biāo)在全局坐標(biāo)系下位置精確；④通過特征匹配完成地標(biāo)關(guān)聯(lián)。地圖的數(shù)據(jù)結(jié)構(gòu)機(jī)器人地圖

運(yùn)動(dòng)學(xué)模型定義機(jī)器人的運(yùn)動(dòng)學(xué)模型

機(jī)器人常用坐標(biāo)系機(jī)器人的運(yùn)動(dòng)學(xué)模型1.機(jī)器人自身坐標(biāo)系：幾何中心（或質(zhì)心）為原點(diǎn)，x軸沿行進(jìn)方向，y軸與x垂直、右手定則確定正向。2.傳感器坐標(biāo)系：定義在傳感器上；區(qū)分“內(nèi)/外傳感器坐標(biāo)系”；內(nèi)傳感器與自身坐標(biāo)系有固定姿態(tài)偏移。3.世界（全局）坐標(biāo)系：固定靜態(tài)坐標(biāo)系，用于統(tǒng)一姿態(tài)估計(jì)。平面運(yùn)動(dòng)姿態(tài)：(??,??,??)(x,y,θ)3個(gè)自由度；世界坐標(biāo)系采用右手定則（x→y圍繞z逆時(shí)針）。自身坐標(biāo)系刻畫“速度”（線速度與角速度），并非“點(diǎn)的物理位置坐標(biāo)”。全局坐標(biāo)系示意圖（右手定則）雙輪差速驅(qū)動(dòng)速度模型–1（基于速度輸入）機(jī)器人的運(yùn)動(dòng)學(xué)模型

雙輪差速驅(qū)動(dòng)速度模型–1（基于速度輸入）機(jī)器人的運(yùn)動(dòng)學(xué)模型

雙輪差速驅(qū)動(dòng)速度模型–1（基于速度輸入）機(jī)器人的運(yùn)動(dòng)學(xué)模型

雙輪差速驅(qū)動(dòng)速度模型–1（基于速度輸入）機(jī)器人的運(yùn)動(dòng)學(xué)模型

機(jī)器人的運(yùn)動(dòng)學(xué)模型幾何關(guān)系與參考點(diǎn)：參考點(diǎn)選后輪軸中心點(diǎn)M；M、ICR、L、R共線（四點(diǎn)共線關(guān)系）。四輪機(jī)器人的二維幾何形狀機(jī)器人四點(diǎn)之間的關(guān)系

機(jī)器人的運(yùn)動(dòng)學(xué)模型

機(jī)器人的運(yùn)動(dòng)學(xué)模型

機(jī)器人的運(yùn)動(dòng)學(xué)模型

雙輪差速驅(qū)動(dòng)里程計(jì)模型機(jī)器人的運(yùn)動(dòng)學(xué)模型

雙輪差速驅(qū)動(dòng)里程計(jì)模型機(jī)器人的運(yùn)動(dòng)學(xué)模型

雙輪差速驅(qū)動(dòng)里程計(jì)模型機(jī)器人的運(yùn)動(dòng)學(xué)模型

觀測(cè)模型要解決什么？

機(jī)器人定位的觀測(cè)模型狀態(tài)–觀測(cè)轉(zhuǎn)移函數(shù)

機(jī)器人定位的觀測(cè)模型狀態(tài)–觀測(cè)轉(zhuǎn)移函數(shù)機(jī)器人定位的觀測(cè)模型基于EKF的機(jī)器人狀態(tài)-觀測(cè)轉(zhuǎn)移示意圖距離–方位觀測(cè)模型

機(jī)器人定位的觀測(cè)模型可執(zhí)行EKF-Localization

基于EKF的機(jī)器人定位完整算法初始化

基于EKF的機(jī)器人定位完整算法預(yù)測(cè)步驟

基于EKF的機(jī)器人定位完整算法過程噪聲映射

基于EKF的機(jī)器人定位完整算法更正步驟

基于EKF的機(jī)器人定位完整算法更正步驟

基于EKF的機(jī)器人定位完整算法數(shù)據(jù)關(guān)聯(lián)

基于EKF的機(jī)器人定位完整算法基于EKF的機(jī)器人定位完整算法（偽代碼）基于EKF的機(jī)器人定位完整算法基于EKF的機(jī)器人定位完整算法（偽代碼）基于EKF的機(jī)器