移動(dòng)機(jī)器人學(xué) 課件第六章 移動(dòng)機(jī)器人定位與建圖

華南理工大學(xué)自動(dòng)化科學(xué)與工程學(xué)院《移動(dòng)機(jī)器人學(xué)》

華南理工大學(xué)自動(dòng)化科學(xué)與工程學(xué)院第六章

移動(dòng)機(jī)器人定位與建圖

6.移動(dòng)機(jī)器人定位與建圖地圖構(gòu)建和定位是移動(dòng)機(jī)器人自主導(dǎo)航的關(guān)鍵基礎(chǔ)問題。SLAM技術(shù)通過迭代優(yōu)化機(jī)器人位姿和環(huán)境地圖，解決了兩者相互影響的難題。SLAM方法可分為濾波SLAM和圖優(yōu)化SLAM，傳感器上分為激光SLAM和視覺SLAM。激光SLAM因其高精度和實(shí)時(shí)性廣泛應(yīng)用于自動(dòng)駕駛、AGV和掃地機(jī)器人等領(lǐng)域。本章研究SLAM問題的數(shù)學(xué)描述及基于改進(jìn)RBPF的SLAM方法的實(shí)現(xiàn)原理，并概述柵格地圖構(gòu)建。重點(diǎn)分析基于Gmapping算法的激光雷達(dá)建圖及ORB-SLAM3視覺算法。6.1SLAM理論基礎(chǔ)SLAM核心思想SLAM的核心思想是通過機(jī)器人或車輛的傳感器數(shù)據(jù)，實(shí)時(shí)完成自身位姿估計(jì)和環(huán)境地圖的構(gòu)建。在未知環(huán)境中，SLAM利用傳感器檢測(cè)運(yùn)動(dòng)信息和環(huán)境特征，通過特征匹配和數(shù)據(jù)關(guān)聯(lián)不斷更新全局地圖，從而實(shí)現(xiàn)自主定位與導(dǎo)航。這一技術(shù)在機(jī)器人自主導(dǎo)航、無人駕駛等領(lǐng)域具有重要意義。SLAM系統(tǒng)框圖6.1SLAM理論基礎(chǔ)SLAM概率模型SLAM的概率模型通過貝葉斯濾波和馬爾可夫假設(shè)描述機(jī)器人在未知環(huán)境中的定位和建圖不確定性。通過結(jié)合運(yùn)動(dòng)模型和觀測(cè)模型，貝葉斯濾波計(jì)算狀態(tài)和地圖的后驗(yàn)概率分布，不斷更新以減小不確定性。SLAM問題原理圖馬爾可夫假設(shè)簡(jiǎn)化了計(jì)算復(fù)雜度，使得SLAM可以實(shí)時(shí)運(yùn)行。此外，SLAM還使用因子圖和回環(huán)檢測(cè)來提高精度，并結(jié)合深度學(xué)習(xí)增強(qiáng)魯棒性。6.1SLAM理論基礎(chǔ)SLAM概率模型根據(jù)概率論原理，SLAM問題可以建模成P

(xt,

M

|Z0:t,

U0:t)機(jī)器人當(dāng)前時(shí)刻t下的位姿xt狀態(tài)轉(zhuǎn)移方程可以描述為xt

=

f

(xt?1,

ut)

+

Vt機(jī)器人t時(shí)刻的狀態(tài)轉(zhuǎn)移概率分布如下P

(xt,

M

|xt?1,

U0:t)

=

P

(xt,

M

|xt?1,

ut)

=

F

(xt?1,

ut)

+

Vt6.1SLAM理論基礎(chǔ)SLAM概率模型根據(jù)機(jī)器人傳感器測(cè)量數(shù)據(jù)，當(dāng)前時(shí)刻機(jī)器人對(duì)路標(biāo)位置的觀測(cè)方程可以描述為zt

=

h(xt,

M

)

+

Wt基于貝葉斯估計(jì)理論，結(jié)合機(jī)器人的觀測(cè)方程（6-4）得到如下的觀測(cè)信息的概率分布：P

(zt|xt,

M

)

=

H(xt,

M

)

+

Wt6.1SLAM理論基礎(chǔ)SLAM概率模型通過遞歸的方式得到SLAM問題的預(yù)測(cè)公式和更新公式，即機(jī)器人聯(lián)合后驗(yàn)概率密度分布，具體如下：P

(xt,

M

|Z0:t?1,

U0:t)

=

P

(xt,

M

|xt?1,

ut)P

(xt?1,

M

|Z0:t?1,

U0:t?1)dxt?1預(yù)測(cè)公式：更新公式：6.2基于改進(jìn)RBPF粒子濾波的SLAM算法Rao-Blackwellized粒子濾波算法Rao-Blackwellized粒子濾波（RBPF）算法結(jié)合了Rao-Blackwellized估計(jì)與粒子濾波，適用于高維非線性非高斯?fàn)顟B(tài)估計(jì)問題。通過條件獨(dú)立性假設(shè)將高維狀態(tài)空間分解為低維子空間，每個(gè)子空間使用粒子濾波獨(dú)立估計(jì)，從而降低計(jì)算復(fù)雜性并提高估計(jì)精度。RBPF通過迭代采樣和權(quán)重更新，逐步逼近真實(shí)狀態(tài)分布，避免粒子退化。相比傳統(tǒng)粒子濾波，RBPF算法在精度和效率上具有優(yōu)勢(shì)，適用于復(fù)雜系統(tǒng)，但需要根據(jù)具體問題調(diào)整算法參數(shù)以優(yōu)化性能。6.2基于改進(jìn)RBPF粒子濾波的SLAM算法Rao-Blackwellized粒子濾波算法基于RBPF的SLAM（RBPF-SLAM）問題實(shí)際上是計(jì)算機(jī)器人位姿和地圖的聯(lián)合概率分布的問題，通過RBPF分解為對(duì)機(jī)器人位姿的后驗(yàn)概率分布和地圖概率分布，最后通過兩者乘積獲得。聯(lián)合后驗(yàn)概率分布如下：6.2基于改進(jìn)RBPF粒子濾波的SLAM算法Rao-Blackwellized粒子濾波算法RBPF算法的5個(gè)步驟總結(jié)如下：粒子初始化：根據(jù)機(jī)器人的運(yùn)動(dòng)模型，通過運(yùn)動(dòng)狀態(tài)轉(zhuǎn)移概率估計(jì)生成N個(gè)獨(dú)立粒子，并為每個(gè)粒子分配初始權(quán)重。重要性采用：根據(jù)建議分布函數(shù)將上一時(shí)刻的粒子代入當(dāng)前時(shí)刻，生成新的粒子。重要性權(quán)重更新：更新每個(gè)粒子的權(quán)重，權(quán)重反映了粒子與真實(shí)后驗(yàn)分布的匹配程度，通過觀測(cè)數(shù)據(jù)（如激光雷達(dá)）計(jì)算得到。重采樣：為了避免粒子退化，重采樣步驟根據(jù)粒子權(quán)重選擇權(quán)重大的粒子，丟棄權(quán)重小的粒子，所有粒子權(quán)重重設(shè)為1/N。更新地圖：根據(jù)機(jī)器人的位姿和觀測(cè)數(shù)據(jù)，更新地圖中的路標(biāo)信息。6.2基于改進(jìn)RBPF粒子濾波的SLAM算法基于改進(jìn)RBPF的Gmapping算法傳統(tǒng)Gmapping算法：通過粒子濾波（ParticleFilter,PF）估計(jì)機(jī)器人的位姿，并結(jié)合激光雷達(dá)數(shù)據(jù)構(gòu)建環(huán)境地圖。粒子濾波：基于大量的粒子來近似機(jī)器人的狀態(tài)分布。局限性：需要大量粒子來保證精度，計(jì)算量大、實(shí)時(shí)性差。改進(jìn)RBPF的Gmapping算法：通過引入RBPF方法將狀態(tài)空間分解為可觀測(cè)部分和不可觀測(cè)部分?？捎^測(cè)部分：通過最大似然估計(jì)進(jìn)行精確計(jì)算。不可觀測(cè)部分：使用粒子濾波進(jìn)行概率估計(jì)。改進(jìn)點(diǎn)：精確計(jì)算與概率估計(jì)相結(jié)合，減少粒子數(shù)量，提升算法的實(shí)時(shí)性和計(jì)算效率。通過優(yōu)化粒子權(quán)重計(jì)算和引入自適應(yīng)重采樣，降低粒子退化問題。6.2基于改進(jìn)RBPF粒子濾波的SLAM算法基于改進(jìn)RBPF的Gmapping計(jì)算方法狀態(tài)空間分解RBPF方法：通過對(duì)機(jī)器人狀態(tài)空間進(jìn)行分解，減少粒子濾波的計(jì)算復(fù)雜度。可觀測(cè)狀態(tài)：通過最大似然估計(jì)精確計(jì)算。不可觀測(cè)狀態(tài)：使用粒子濾波估算。目標(biāo)：減少粒子數(shù)量，提高計(jì)算效率。2.粒子權(quán)重計(jì)算最大似然估計(jì)（MLE）：用于計(jì)算可觀測(cè)部分的最優(yōu)估計(jì)。粒子濾波：對(duì)不可觀測(cè)狀態(tài)進(jìn)行概率估計(jì)。權(quán)重更新公式：6.2基于改進(jìn)RBPF粒子濾波的SLAM算法基于改進(jìn)RBPF的Gmapping計(jì)算方法

6.3基于激光雷達(dá)的Gmapping算法室內(nèi)地圖構(gòu)建實(shí)現(xiàn)基于激光雷達(dá)的Gmapping算法原理激光雷達(dá)數(shù)據(jù)采集：激光雷達(dá)通過發(fā)射激光束并測(cè)量反射時(shí)間，獲取周圍物體的距離數(shù)據(jù)，形成點(diǎn)云，揭示環(huán)境的結(jié)構(gòu)與布局。點(diǎn)云數(shù)據(jù)處理與地圖對(duì)齊：Gmapping算法對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行濾波和降噪，并使用掃描匹配技術(shù)將當(dāng)前激光掃描與已有地圖對(duì)齊。機(jī)器人狀態(tài)估計(jì)與地圖更新：通過粒子濾波技術(shù)，Gmapping算法不斷估計(jì)機(jī)器人位置并更新地圖，使位置和地圖信息融合。6.3基于激光雷達(dá)的Gmapping算法室內(nèi)地圖構(gòu)建實(shí)現(xiàn)Gmapping算法的地圖構(gòu)建與應(yīng)用粒子濾波與地圖構(gòu)建：在Gmapping算法中，粒子濾波不僅用于估計(jì)機(jī)器人位置，還用于構(gòu)建環(huán)境的二維柵格地圖，每個(gè)粒子攜帶一個(gè)局部地圖，經(jīng)過多次迭代更新，最終生成全局一致的地圖。柵格地圖表示與環(huán)境適應(yīng)：該地圖中的柵格表示環(huán)境信息，顏色或數(shù)值反映占據(jù)概率或高度信息，算法能夠適應(yīng)動(dòng)態(tài)環(huán)境并處理噪聲?；丨h(huán)檢測(cè)與地圖優(yōu)化：Gmapping算法通過回環(huán)檢測(cè)糾正誤差，提升地圖的準(zhǔn)確性，實(shí)驗(yàn)驗(yàn)證表明，改進(jìn)RBPF的Gmapping算法能夠在復(fù)雜環(huán)境中有效實(shí)現(xiàn)機(jī)器人定位與建圖。6.3基于激光雷達(dá)的Gmapping算法室內(nèi)地圖構(gòu)建實(shí)現(xiàn)SLAM實(shí)驗(yàn)環(huán)境與Gmapping應(yīng)用

Gmapping實(shí)驗(yàn)設(shè)置與場(chǎng)景：實(shí)驗(yàn)環(huán)境:本節(jié)實(shí)驗(yàn)在6×8m2的室內(nèi)實(shí)驗(yàn)室進(jìn)行，實(shí)驗(yàn)場(chǎng)景如圖6-3所示。激光雷達(dá)的掃描角度為270°，并通過遠(yuǎn)程PC端監(jiān)控和控制移動(dòng)機(jī)器人的運(yùn)動(dòng)與建圖過程。Gmapping實(shí)驗(yàn)場(chǎng)景6.3基于激光雷達(dá)的Gmapping算法室內(nèi)地圖構(gòu)建實(shí)現(xiàn)SLAM實(shí)驗(yàn)環(huán)境與Gmapping應(yīng)用

Gmapping實(shí)驗(yàn)設(shè)置與場(chǎng)景：實(shí)驗(yàn)配置:本實(shí)驗(yàn)使用ROS平臺(tái)的Gmapping功能包，通過調(diào)節(jié)參數(shù)實(shí)現(xiàn)實(shí)驗(yàn)過程。啟動(dòng)文件運(yùn)行后，底盤驅(qū)動(dòng)、激光雷達(dá)、IMU、Gmapping以及遠(yuǎn)程控制節(jié)點(diǎn)將開始工作，完成機(jī)器人定位與地圖構(gòu)建。6.3基于激光雷達(dá)的Gmapping算法室內(nèi)地圖構(gòu)建實(shí)現(xiàn)SLAM實(shí)驗(yàn)環(huán)境與Gmapping應(yīng)用

Gmapping節(jié)點(diǎn)與坐標(biāo)關(guān)系：節(jié)點(diǎn)關(guān)系:如圖6-4所示，IMU與STM32發(fā)布慣導(dǎo)信息（/imu/data）與編碼器信息（/raw_vel），底盤節(jié)點(diǎn)提供里程計(jì)信息。Gmapping訂閱底盤里程計(jì)與激光雷達(dá)信息，進(jìn)行機(jī)器人位姿估計(jì)與地圖構(gòu)建。遠(yuǎn)程端通過/my_cmd_vel發(fā)送控制信息。Gmapping運(yùn)行節(jié)點(diǎn)圖6.3基于激光雷達(dá)的Gmapping算法室內(nèi)地圖構(gòu)建實(shí)現(xiàn)Gmapping地圖構(gòu)建重要參數(shù)配置:在啟動(dòng)建圖前，Gmapping需要配置多個(gè)參數(shù)，其中最重要的調(diào)節(jié)參數(shù)為：particles:設(shè)置粒子數(shù)量，本節(jié)實(shí)驗(yàn)中設(shè)置為50，以保證運(yùn)算效率和定位精度。minimumScore:設(shè)置掃描匹配的分?jǐn)?shù)閾值，本節(jié)設(shè)置為100，以判斷是否采用改進(jìn)建議分布。啟動(dòng)過程:啟動(dòng)Gmapping功能包、機(jī)器人底層Bringup、遠(yuǎn)程PC端的teleop_keyboard功能包和RVIZ可視化界面。通過遠(yuǎn)程控制機(jī)器人進(jìn)行SLAM建圖，完成實(shí)驗(yàn)場(chǎng)景的地圖構(gòu)建。6.3基于激光雷達(dá)的Gmapping算法室內(nèi)地圖構(gòu)建實(shí)現(xiàn)Gmapping地圖構(gòu)建實(shí)驗(yàn)結(jié)果:經(jīng)過調(diào)節(jié)，建圖精度滿足室內(nèi)導(dǎo)航需求。圖6-6展示了柵格地圖，墻壁和角落無明顯扭曲，實(shí)驗(yàn)室內(nèi)物體位置與真實(shí)場(chǎng)景吻合，成功實(shí)現(xiàn)了定位導(dǎo)航。Gmapping地圖構(gòu)建過程6.4Catorgrapher算法Cartographer算法概述什么是Cartographer?由Google提出的實(shí)時(shí)同時(shí)定位與地圖構(gòu)建（SLAM）算法。支持2D和3D激光雷達(dá)數(shù)據(jù)輸入，跨平臺(tái)使用，廣泛應(yīng)用于機(jī)器人、自動(dòng)駕駛和增強(qiáng)現(xiàn)實(shí)等領(lǐng)域。通過圖優(yōu)化方法進(jìn)行精準(zhǔn)的定位與地圖構(gòu)建，實(shí)時(shí)生成高分辨率的地圖。核心特點(diǎn)實(shí)時(shí)性:能夠在運(yùn)行時(shí)生成環(huán)境地圖，實(shí)時(shí)顯示更新的地圖信息。高精度:通過優(yōu)化算法和回環(huán)檢測(cè)機(jī)制，確保定位精度和地圖構(gòu)建的準(zhǔn)確性。子地圖方法:將地圖構(gòu)建分為多個(gè)子地圖，逐步拼接成完整地圖，降低誤差積累。工作原理：機(jī)器人通過傳感器數(shù)據(jù)（如激光雷達(dá)、IMU等）收集環(huán)境信息。連續(xù)掃描數(shù)據(jù)轉(zhuǎn)化為子地圖，通過掃描匹配算法與已建地圖進(jìn)行對(duì)齊，確保機(jī)器人定位精度。6.4Catorgrapher算法子圖構(gòu)建與掃描匹配子圖構(gòu)建：Cartographer使用子圖的方式構(gòu)建地圖。每次收集的掃描數(shù)據(jù)都會(huì)被劃分成多個(gè)子圖，通過掃描匹配算法將它們拼接成完整地圖。掃描匹配與優(yōu)化：激光雷達(dá)掃描轉(zhuǎn)換到子圖坐標(biāo)系，通過最小化誤差對(duì)齊掃描。使用Ceres求解器進(jìn)行非線性最小二乘優(yōu)化，保證精度。從激光雷達(dá)坐標(biāo)系到子圖坐標(biāo)系的剛體變換公式：6.4Catorgrapher算法網(wǎng)格點(diǎn)與命中/未命中網(wǎng)格示意圖

網(wǎng)格點(diǎn)和對(duì)應(yīng)的像素示意圖命中與未命中網(wǎng)格示意圖6.5ORB-SLAM3算法ORB-SLAM3算法概述ORB-SLAM3簡(jiǎn)介：ORB-SLAM3是一種視覺和視覺慣性SLAM系統(tǒng)，能夠處理單目、立體和RGB-D相機(jī)，并支持基于針孔和魚眼鏡頭模型的視覺、視覺–慣性和多地圖SLAM。它通過最大后驗(yàn)估計(jì)（MAP）方法進(jìn)行優(yōu)化，精度是以往方法的2至10倍。ORB-SLAM3的關(guān)鍵特點(diǎn)：多地圖系統(tǒng)：ORB-SLAM3可以處理多會(huì)話的地圖并支持地圖合并。高魯棒性與準(zhǔn)確性：即使在長(zhǎng)時(shí)間視覺信息不佳的情況下，也能持續(xù)穩(wěn)定地運(yùn)行。短期、中期、長(zhǎng)期數(shù)據(jù)關(guān)聯(lián)：ORB-SLAM3能夠在建圖區(qū)域?qū)崿F(xiàn)零漂移，提高了數(shù)據(jù)關(guān)聯(lián)精度。6.5ORB-SLAM3算法ORB-SLAM3的工作原理與關(guān)鍵技術(shù)最大后驗(yàn)估計(jì)與光束平差：ORB-SLAM3使用最大后驗(yàn)估計(jì)（MAP），類似光束平差（BundleAdjustment,BA）在視