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

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

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

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

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

(xt,

M

|Z0:t,

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

=

f

(xt?1,

ut)

+

Vt機(jī)器人t時刻的狀態(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ī)器人傳感器測量數(shù)據(jù)，當(dāng)前時刻機(jī)器人對路標(biāo)位置的觀測方程可以描述為zt

=

h(xt,

M

)

+

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

(zt|xt,

M

)

=

H(xt,

M

)

+

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

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

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

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

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

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