機(jī)器人操作系統(tǒng)ROS應(yīng)用實(shí)踐 課件 第4-6章 移動(dòng)機(jī)器人激光SLAM、移動(dòng)機(jī)器人自主導(dǎo)航、基于多傳感器的SLAM

第四章移動(dòng)機(jī)器人激光SLAM機(jī)器人操作系統(tǒng)ROS應(yīng)用實(shí)踐目錄contentSLAM基本原理4.1HectorSLAM算法4.3Gmapping算法4.2本章小結(jié)4.44.1SLAM基本原理1SLAM概述移動(dòng)機(jī)器人環(huán)境感知技術(shù)涉及的主要內(nèi)容包括定位（Localization）、建圖（Mapping）和路徑規(guī)劃（PathPlanning）/運(yùn)動(dòng)控制。SLAM技術(shù)屬于定位和建圖的交集部分，如圖所示。4.1SLAM基本原理1SLAM概述在未知環(huán)境中，機(jī)器人自主定位與建圖是相互關(guān)聯(lián)和相互依賴的，其既需要利用建立好的地圖信息來(lái)更新自身的位置和姿態(tài)，也需要利用準(zhǔn)確的位姿估計(jì)信息來(lái)建立環(huán)境地圖。SLAM的基本過(guò)程可用下圖描述。4.1SLAM基本原理1SLAM概述實(shí)現(xiàn)機(jī)器人的自主導(dǎo)航需要機(jī)器人能夠很好地感知周圍環(huán)境。在一個(gè)未知的復(fù)雜環(huán)境中，人類可以通過(guò)眼睛、耳朵、鼻子等多個(gè)器官來(lái)感知外界環(huán)境的變化，從而確定自身所處的狀態(tài)，這也體現(xiàn)了人類強(qiáng)大的感知能力。但是在未知環(huán)境中，機(jī)器人無(wú)法像人類一樣感知外界環(huán)境的動(dòng)態(tài)變化，需要依靠自身攜帶的傳感器來(lái)感知外界環(huán)境。這些傳感器通常包括相機(jī)、激光雷達(dá)、里程計(jì)、慣性測(cè)量單元等，機(jī)器人需要通過(guò)分析傳感器數(shù)據(jù)，進(jìn)行自身位姿的估計(jì)和環(huán)境地圖的構(gòu)建。4.1SLAM基本原理2移動(dòng)機(jī)器人坐標(biāo)系為了進(jìn)行位姿估計(jì)和地圖構(gòu)建，需要建立移動(dòng)機(jī)器人的坐標(biāo)系。ROS中，移動(dòng)機(jī)器人的坐標(biāo)系一般包括map、base_link、odom、base_laser等。map：地圖坐標(biāo)系，該坐標(biāo)系為固定坐標(biāo)系（FixedFrame），與機(jī)器人所在的世界坐標(biāo)系一致。base_link：機(jī)器人本體坐標(biāo)系，又稱基坐標(biāo)系，原點(diǎn)與機(jī)器人中心重合。4.1SLAM基本原理2移動(dòng)機(jī)器人坐標(biāo)系為了進(jìn)行位姿估計(jì)和地圖構(gòu)建，需要建立移動(dòng)機(jī)器人的坐標(biāo)系。ROS中，移動(dòng)機(jī)器人的坐標(biāo)系一般包括map、base_link、odom、base_laser等。odom：里程計(jì)坐標(biāo)系，可以看成一個(gè)隨移動(dòng)機(jī)器人運(yùn)動(dòng)而運(yùn)動(dòng)的坐標(biāo)系，只要機(jī)器人運(yùn)動(dòng)，就會(huì)發(fā)布base_link到odom的TF。機(jī)器人運(yùn)動(dòng)開(kāi)始前（初始時(shí)），如機(jī)器人在原點(diǎn)處時(shí)，odom和map是重合的，即map到odom的TF就是0。隨著時(shí)間的推移，由于里程計(jì)（根據(jù)運(yùn)動(dòng)學(xué)模型計(jì)算）的累計(jì)誤差，odom會(huì)出現(xiàn)偏差，這時(shí)就需要用傳感器進(jìn)行校正，也就是估計(jì)。4.1SLAM基本原理2移動(dòng)機(jī)器人坐標(biāo)系為了進(jìn)行位姿估計(jì)和地圖構(gòu)建，需要建立移動(dòng)機(jī)器人的坐標(biāo)系。ROS中，移動(dòng)機(jī)器人的坐標(biāo)系一般包括map、base_link、odom、base_laser等。base_laser：激光雷達(dá)坐標(biāo)系，由激光雷達(dá)的安裝位置確定，與base_link的坐標(biāo)系變換為固定的。4.1SLAM基本原理3ROS導(dǎo)航與定位過(guò)程下面以常見(jiàn)的雙輪差速移動(dòng)機(jī)器人為例，按照自底向上的順序介紹ROS導(dǎo)航與定位過(guò)程，如圖所示。4.1SLAM基本原理3ROS導(dǎo)航與定位過(guò)程最底層是輪速和底盤(pán)控制，差速輪結(jié)構(gòu)的移動(dòng)底盤(pán)一般包括兩個(gè)主動(dòng)輪和一個(gè)從動(dòng)輪，主動(dòng)輪上安裝有帶減速器的電機(jī)，每個(gè)電機(jī)軸上有編碼器，可以實(shí)現(xiàn)里程計(jì)功能。電機(jī)驅(qū)動(dòng)器上集成有轉(zhuǎn)速控制器，可以直接調(diào)用，ROS中也包含了很多第三方的驅(qū)動(dòng)器驅(qū)動(dòng)。4.1SLAM基本原理3ROS導(dǎo)航與定位過(guò)程往上是位置控制，如果期望機(jī)器人按照設(shè)定值，以某個(gè)速度前進(jìn)一定的距離，就需要應(yīng)用PID進(jìn)行閉環(huán)控制。如果知道當(dāng)前位置和朝向，以及目標(biāo)位置和朝向，ROS提供了move_base功能包控制機(jī)器人運(yùn)動(dòng)到目標(biāo)點(diǎn)，并實(shí)現(xiàn)避障和路徑規(guī)劃（PathPlanner）。4.1SLAM基本原理3ROS導(dǎo)航與定位過(guò)程再往上是利用通過(guò)Gmapping等功能包創(chuàng)建地圖，然后利用AMCL（AdaptiveMonteCarloLocalization，自適應(yīng)蒙特卡羅定位）功能包，根據(jù)當(dāng)前數(shù)據(jù)進(jìn)行定位。最上層是根據(jù)語(yǔ)義信息將命令解析成一系列動(dòng)作，然后驅(qū)動(dòng)下層執(zhí)行相應(yīng)命令，使機(jī)器人運(yùn)動(dòng)到目標(biāo)位置。4.1SLAM基本原理3ROS導(dǎo)航與定位過(guò)程ROS導(dǎo)航與定位一般是針對(duì)差速輪式機(jī)器人和完整約束輪式機(jī)器人的，移動(dòng)平臺(tái)只能接收x，y方向速度和偏航角（yaw）角速度；機(jī)器人需要激光雷達(dá)傳感器（或者深度數(shù)據(jù)模擬的激光數(shù)據(jù)）創(chuàng)建地圖和進(jìn)行定位；機(jī)器人的形狀應(yīng)是尺寸合適的方形或者圓形。4.1SLAM基本原理3ROS導(dǎo)航與定位過(guò)程ROS中的Navigation（導(dǎo)航）功能包為機(jī)器人的導(dǎo)航提供了一套完整的解決方案。NavigationStack（導(dǎo)航棧）是一個(gè)ROS的元功能包，其中包含了路徑規(guī)劃、定位、異常行為恢復(fù)、地圖服務(wù)器等功能，這些開(kāi)源工具包極大地減少了機(jī)器人應(yīng)用系統(tǒng)的開(kāi)發(fā)工作量，可以快速部署實(shí)現(xiàn)相關(guān)功能。Navigation功能包中包含了許多導(dǎo)航與定位相關(guān)的功能包，其中move_base功能包是一個(gè)強(qiáng)大的路徑規(guī)劃器，可以驅(qū)動(dòng)移動(dòng)底盤(pán)運(yùn)動(dòng)到世界坐標(biāo)系上的一個(gè)目標(biāo)點(diǎn)，其包含全局路徑規(guī)劃器和局部路徑規(guī)劃器，同時(shí)維護(hù)兩個(gè)costmap（代價(jià)地圖），一個(gè)給全局路徑規(guī)劃器，另一個(gè)給局部路徑規(guī)劃器。4.1SLAM基本原理3ROS導(dǎo)航與定位過(guò)程這兩個(gè)costmap可以配置多個(gè)地圖層：ObstacleMapLayer：障礙地圖層，用于動(dòng)態(tài)記錄傳感器感知到的障礙物信息。InflationLayer：膨脹層，在地圖上進(jìn)行膨脹（向外擴(kuò)張），以避免機(jī)器人撞上障礙物。StaticMapLayer：靜態(tài)地圖層，是基本不變的地圖層，通常是SLAM建立完成的靜態(tài)地圖。OtherLayers：通過(guò)插件形式實(shí)現(xiàn)的代價(jià)地圖層（目前已有一些開(kāi)源插件）。4.1SLAM基本原理4環(huán)境建圖與位姿估計(jì)地圖是對(duì)某種特定環(huán)境的描述，常見(jiàn)的地圖包括柵格地圖、點(diǎn)云地圖、拓?fù)涞貓D。移動(dòng)機(jī)器人的定位與建圖是兩個(gè)緊密相關(guān)的問(wèn)題，是可以同時(shí)進(jìn)行的。在SLAM的過(guò)程中，最常用的地圖為柵格地圖。4.1SLAM基本原理4環(huán)境建圖與位姿估計(jì)在ROS中，柵格地圖通常是一張pgm格式的灰度圖像，如圖所示。圖像中的白色像素點(diǎn)表示可行區(qū)域，黑色像素點(diǎn)表示障礙物，灰色部分是未知探索區(qū)域。4.1SLAM基本原理4環(huán)境建圖與位姿估計(jì)SLAM問(wèn)題本質(zhì)上是一個(gè)狀態(tài)估計(jì)問(wèn)題，即如何通過(guò)帶有噪聲的測(cè)量數(shù)據(jù)估計(jì)未知的狀態(tài)變量。其中，測(cè)量數(shù)據(jù)既可以來(lái)自機(jī)器人自身運(yùn)動(dòng)測(cè)量傳感器（里程計(jì)、慣性測(cè)量單元等），也可以來(lái)自機(jī)器人觀測(cè)外部環(huán)境的傳感器（相機(jī)、激光雷達(dá)），需要估計(jì)的未知狀態(tài)變量包括機(jī)器人的位姿和環(huán)境地圖。4.1SLAM基本原理4環(huán)境建圖與位姿估計(jì)從數(shù)學(xué)表述上來(lái)看，一個(gè)完整的SLAM過(guò)程由兩個(gè)基本方程組成：運(yùn)動(dòng)方程和觀測(cè)方程，如式所示。運(yùn)動(dòng)方程描述的是如何通過(guò)運(yùn)動(dòng)測(cè)量傳感器的測(cè)量值

來(lái)進(jìn)行機(jī)器人位姿

的估計(jì)，觀測(cè)方程描述的是如何通過(guò)外部環(huán)境觀測(cè)數(shù)據(jù)

來(lái)進(jìn)行機(jī)器人位姿

的估計(jì)和建圖（y）。4.1SLAM基本原理4環(huán)境建圖與位姿估計(jì)SLAM系統(tǒng)由前端與后端組成，如圖所示。前端的主要任務(wù)是完成傳感器數(shù)據(jù)處理、自身定位和局部建圖，實(shí)時(shí)更新機(jī)器人的姿態(tài)與地圖；而后端的主要任務(wù)是完成位姿優(yōu)化及地圖優(yōu)化，不斷優(yōu)化這些姿態(tài)的準(zhǔn)確性及地圖的準(zhǔn)確性。4.1SLAM基本原理4環(huán)境建圖與位姿估計(jì)根據(jù)后端處理方式不同，SLAM算法可以分為基于濾波器的算法與基于圖優(yōu)化的算法。在SLAM算法的早期應(yīng)用中，通常采用基于濾波器的算法，其利用貝葉斯原理，根據(jù)觀測(cè)信息及控制信息對(duì)系統(tǒng)狀態(tài)的后驗(yàn)概率進(jìn)行估計(jì)，而根據(jù)后驗(yàn)概率表示方法的不同，又存在著多種基于濾波器的算法。常用的基于濾波器的算法有卡爾曼濾波（KalmanFilter，KF）算法、擴(kuò)展卡爾曼濾波（ExtendedKalmanFilter，EKF）算法、無(wú)跡卡爾曼濾波（UnscentedKalmanFilter，UKF，也稱無(wú)損卡爾曼濾波）算法、粒子濾波（ParticleFilter，PF）算法等。讀者可查找相關(guān)資料對(duì)上述算法進(jìn)行進(jìn)一步學(xué)習(xí)。4.2Gmapping算法1原理分析使用RBPF的Gmapping算法解決SLAM問(wèn)題的核心思想是利用聯(lián)合概率密度

估計(jì)地圖m及機(jī)器人的運(yùn)動(dòng)位姿

。利用Rao-Blackwellized思想，將上述聯(lián)合概率分解為式。也就是說(shuō)，可以先估計(jì)機(jī)器人位姿，再利用位姿估計(jì)地圖。而對(duì)于位姿的估計(jì)，可以采用粒子濾波算法。4.2Gmapping算法2實(shí)施流程Gmapping算法的核心是RBPF，采用重要性重采樣（SamplingImportanceResampling，SIR）濾波，該算法的實(shí)施分為以下四個(gè)步驟。粒子采樣。對(duì)于每一個(gè)粒子i，利用建議分布（ProposalDistribution,PD），基于移動(dòng)機(jī)器人的里程計(jì)運(yùn)動(dòng)模型，從t?1時(shí)刻的位姿粒子中

采樣出t時(shí)刻的位姿粒子

。4.2Gmapping算法2實(shí)施流程Gmapping算法的核心是RBPF，采用重要性重采樣（SamplingImportanceResampling，SIR）濾波，該算法的實(shí)施分為以下四個(gè)步驟。權(quán)重更新。權(quán)重更新使用式進(jìn)行，為方便編程實(shí)現(xiàn)，將其變換成迭代形式：4.2Gmapping算法2實(shí)施流程Gmapping算法的核心是RBPF，采用重要性重采樣（SamplingImportanceResampling，SIR）濾波，該算法的實(shí)施分為以下四個(gè)步驟。粒子重采樣。每種粒子的數(shù)量與它們的權(quán)重正相關(guān)。由于只能采用有限的粒子來(lái)近似連續(xù)的分布，隨著時(shí)間的推移，粒子的多樣性將喪失，因此必須引進(jìn)重采樣，以增加粒子的多樣性。在目標(biāo)分布與建議分布不同的情況下，更應(yīng)引進(jìn)重采樣，這樣，經(jīng)過(guò)重采樣后所有粒子的權(quán)重相同，從而增加了粒子的多樣性。重采樣閾值依據(jù)式）確定：4.2Gmapping算法2實(shí)施流程Gmapping算法的核心是RBPF，采用重要性重采樣（SamplingImportanceResampling，SIR）濾波，該算法的實(shí)施分為以下四個(gè)步驟。地圖更新。Gmapping算法使用梯度下降法進(jìn)行搜索匹配，對(duì)每個(gè)粒子與地圖進(jìn)行掃描匹配，匹配相似度高的得分高，匹配相似度低的得分低，選取得分最高的粒子作為移動(dòng)機(jī)器人的當(dāng)前姿態(tài)，進(jìn)而用于更新地圖。掃描匹配準(zhǔn)則依據(jù)式確認(rèn)：4.2Gmapping算法2實(shí)施流程Gmapping算法完整的流程如圖所示。4.3HectorSLAM算法1原理分析雙線性地圖插值HectorSLAM算法為了獲取平滑的地圖，采取雙線性插值法對(duì)柵格地圖進(jìn)行插值。雙線性插值法的核心思想是：在x、y兩個(gè)方向分別進(jìn)行一次插值。由圖可知，

為障礙物的概率如式所示。4.3HectorSLAM算法1原理分析雙線性地圖插值其中，

表示柵格

為障礙物的概率。

的偏導(dǎo)數(shù)為4.3HectorSLAM算法1原理分析掃描匹配HectorSLAM算法掃描匹配的最優(yōu)準(zhǔn)則為即尋找激光雷達(dá)掃描結(jié)果與地圖最佳對(duì)準(zhǔn)時(shí)的最優(yōu)變換變量

。

是掃描終點(diǎn)在世界坐標(biāo)系下的坐標(biāo)，其函數(shù)表達(dá)式為式：4.3HectorSLAM算法1原理分析掃描匹配對(duì)于給定的任意一個(gè)初始估計(jì)

，尋找一個(gè)最優(yōu)估計(jì)

，使式對(duì)

進(jìn)行一階泰勒展開(kāi)，求取展開(kāi)式的最小值，即對(duì)

求取偏導(dǎo)數(shù)并令偏導(dǎo)數(shù)為0，求得式。4.4本章小結(jié)本章主要介紹SLAM基本原理及常見(jiàn)的激光SLAM算法，分析了SLAM中的兩大關(guān)鍵技術(shù)——環(huán)境建圖與位姿估計(jì)；然后舉例講解了移動(dòng)機(jī)器人坐標(biāo)變換方法；最后分析了兩種經(jīng)典的激光SLAM算法：Gmapping和HectorSLAM，并通過(guò)兩個(gè)任務(wù)，使讀者理解SLAM的關(guān)鍵技術(shù)，為將來(lái)進(jìn)一步學(xué)習(xí)或研究其他SLAM算法打下基礎(chǔ)。第五章移動(dòng)機(jī)器人自主導(dǎo)航機(jī)器人操作系統(tǒng)ROS應(yīng)用實(shí)踐目錄content基于地圖的定位5.15.3基于地圖的自主導(dǎo)航5.2本章小結(jié)5.1基于地圖的定位1蒙特卡羅定位蒙特卡羅定位（MonteCarloLocalization，MCL）算法是一種概率算法，其將機(jī)器人當(dāng)前位置看成許多粒子的密度模型，采用基于粒子濾波的方法來(lái)進(jìn)行定位。每個(gè)粒子可以看成機(jī)器人在此位置的假設(shè)。蒙特卡羅定位算法可以用在帶有外部距離傳感器（如激光雷達(dá)）的機(jī)器人系統(tǒng)上。激光雷達(dá)可測(cè)量各個(gè)方向上機(jī)器人與最近障礙物之間的距離。在每個(gè)時(shí)間點(diǎn)上，機(jī)器人都會(huì)獲得激光雷達(dá)的測(cè)量值。5.1基于地圖的定位1蒙特卡羅定位如圖所示，圖（a）中，三角形代表機(jī)器人，直線代表激光束，正方形格子是機(jī)器人在該激光束方向上檢測(cè)到的最近障礙物；圖（b）為占據(jù)柵格地圖（OccupancyGridMap），其中白色格子表示障礙物，黑色格子表示空白位置。（a）

（b）

5.1基于地圖的定位1蒙特卡羅定位如圖所示，蒙特卡羅定位算法的步驟如下。初始化粒子群。定義粒子群大小為M，在機(jī)器人初始位姿已知的情況下，可復(fù)制M個(gè)初始位姿作為初始粒子群；而在初始位姿未知的情況下，可利用高斯分布隨機(jī)采樣的方法初始化粒子群，即使粒子均勻分布在地圖上。5.1基于地圖的定位1蒙特卡羅定位如圖所示，蒙特卡羅定位算法的步驟如下。模擬粒子運(yùn)動(dòng)。模擬粒子運(yùn)動(dòng)可以根據(jù)機(jī)器人的運(yùn)動(dòng)模型，對(duì)任意粒子做下一時(shí)刻的運(yùn)動(dòng)估計(jì)，從而得到下一時(shí)刻的粒子群，這種方法需要準(zhǔn)確地對(duì)機(jī)器人做運(yùn)動(dòng)建模；然而有些時(shí)候機(jī)器人的運(yùn)動(dòng)模型難以建立，可以利用機(jī)器人運(yùn)動(dòng)的連續(xù)性（在兩個(gè)相鄰的采樣時(shí)間間隔內(nèi)機(jī)器人的運(yùn)動(dòng)范圍是有限的），使用隨機(jī)函數(shù)生成粒子來(lái)描述機(jī)器人可能運(yùn)動(dòng)到的位置。5.1基于地圖的定位1蒙特卡羅定位如圖所示，蒙特卡羅定位算法的步驟如下。計(jì)算粒子評(píng)分。對(duì)于激光雷達(dá)采集的每個(gè)點(diǎn)，激光雷達(dá)讀數(shù)與地圖的匹配有四種情況，如表所示。5.1基于地圖的定位1蒙特卡羅定位如圖所示，蒙特卡羅定位算法的步驟如下。粒子群重采樣。在每次評(píng)分結(jié)束后，會(huì)發(fā)現(xiàn)有些粒子評(píng)分很低，即這些粒子嚴(yán)重偏離了機(jī)器人的可能位姿，這些粒子必須舍棄掉，以提高粒子群的質(zhì)量與收斂速度；而有些粒子的評(píng)分則很高，并且這些粒子的評(píng)分都很接近，則需要保留這些粒子。但直接將評(píng)分過(guò)低的粒子舍棄必然會(huì)減少總粒子數(shù)，為了保持粒子群規(guī)模大小不變，可以對(duì)高得分的粒子進(jìn)行簡(jiǎn)單的復(fù)制重采樣，以保持粒子總數(shù)不變。這就是粒子群重采樣的過(guò)程。5.1基于地圖的定位1蒙特卡羅定位根據(jù)上述四個(gè)步驟可知，蒙特卡羅定位算法具有如下優(yōu)點(diǎn)。無(wú)須對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)建模即可以對(duì)機(jī)器人的全局位姿進(jìn)行估計(jì)。無(wú)須知道機(jī)器人的初始位姿。計(jì)算過(guò)程消耗的內(nèi)存較少，計(jì)算量較小，計(jì)算結(jié)果較為準(zhǔn)確。機(jī)器人運(yùn)動(dòng)位姿的估計(jì)非常平滑，適合做移動(dòng)機(jī)器人的導(dǎo)航控制。算法容易實(shí)現(xiàn)。5.1基于地圖的定位1蒙特卡羅定位根據(jù)上述四個(gè)步驟可知，蒙特卡羅定位算法具有如下缺點(diǎn)。一旦機(jī)器人位置出現(xiàn)跳變，蒙特卡羅定位算法將失效（存在“機(jī)器人綁架”問(wèn)題）。定位精度與粒子數(shù)有關(guān)，提高定位精度需要使用大量粒子。定位收斂速度較慢。5.1基于地圖的定位2自適應(yīng)蒙特卡羅定位為解決蒙特卡羅定位算法的一些弊端，人們開(kāi)發(fā)出了它的許多改進(jìn)版本，其中自適應(yīng)蒙特卡羅定位（AdaptiveMonteCarloLocalization，AMCL）算法頗為有效。由于蒙特卡羅定位算法在粒子群重采樣步驟中，可能會(huì)意外丟棄所有正確位姿附近的粒子。當(dāng)粒子數(shù)很小（如50個(gè)），并且擴(kuò)散到比較大的區(qū)域中（如全局定位過(guò)程）時(shí)，這個(gè)問(wèn)題的嚴(yán)重性就充分顯示出來(lái)了。5.1基于地圖的定位2自適應(yīng)蒙特卡羅定位這就需要對(duì)蒙特卡羅定位算法進(jìn)行優(yōu)化，自適應(yīng)蒙特卡羅定位算法的“自適應(yīng)”主要體現(xiàn)在兩方面：一是解決粒子數(shù)固定的問(wèn)題，當(dāng)移動(dòng)機(jī)器人定位收斂時(shí)，粒子基本都集中在一處，此時(shí)可以適當(dāng)減少粒子數(shù)；二是解決“機(jī)器人綁架”問(wèn)題，當(dāng)發(fā)現(xiàn)粒子的平均分?jǐn)?shù)突然降低了（正確的粒子在某次迭代中被拋棄了）時(shí)，在全局區(qū)域再重新散布一些粒子。5.1基于地圖的定位2自適應(yīng)蒙特卡羅定位amcl功能包的主題與服務(wù)amcl功能包訂閱的主題如下。tf（tf/tfMessage）：表示坐標(biāo)變換信息。initialpose（geometry_msgs/PoseWithCovarianceStamped）：表示用于初始化粒子濾波器的均值和協(xié)方差。scan（sensor_msgs/LaserScan）：表示激光雷達(dá)測(cè)量數(shù)據(jù)。map（nav_msgs/OccupancyGrid）：設(shè)置use_map_topic參數(shù)后，amcl會(huì)訂閱map主題來(lái)獲取地圖數(shù)據(jù)，用于激光雷達(dá)定位。5.1基于地圖的定位2自適應(yīng)蒙特卡羅定位amcl功能包的主題與服務(wù)amcl功能包發(fā)布的主題如下。amcl_pose（geometry_msgs/PoseWithCovarianceStamped）：表示機(jī)器人在地圖中的估計(jì)位姿，并帶有協(xié)方差信息。particlecloud（geometry_msgs/PoseArray）：表示由粒子濾波器維護(hù)的位姿估計(jì)集合。tf（tf/tfMessage）：表示發(fā)布從odom（可以通過(guò)~odom_frame_id參數(shù)重新映射）到map的變換。5.1基于地圖的定位2自適應(yīng)蒙特卡羅定位amcl功能包的主題與服務(wù)amcl功能包的服務(wù)如下。global_localization（std_srvs/Empty）：用于啟動(dòng)全局定位，所有粒子在地圖的空閑空間中隨機(jī)分散。request_nomotion_update（std_srvs/Empty）：用于手動(dòng)執(zhí)行更新和發(fā)布更新粒子的服務(wù)。5.1基于地圖的定位2自適應(yīng)蒙特卡羅定位amcl功能包的主題與服務(wù)amcl功能包調(diào)用的服務(wù)如下。static_map（nav_msgs/GetMap）：amcl調(diào)用該服務(wù)來(lái)獲取地圖數(shù)據(jù)。5.1基于地圖的定位2自適應(yīng)蒙特卡羅定位amcl功能包中的參數(shù)總體過(guò)濾器參數(shù)~min_particles（int，默認(rèn)值：100）：允許的最小粒子數(shù)。~max_particles（int，默認(rèn)值：5000）：允許的最大粒子數(shù)。~kld_err（double，默認(rèn)值：0.01）：真實(shí)分布和估計(jì)分布之間的最大誤差。~kld_z（double，默認(rèn)：0.99）：（1?p）的上標(biāo)準(zhǔn)正常分位數(shù)，其中p是估計(jì)分布誤差小于kld_err的概率。5.1基于地圖的定位2自適應(yīng)蒙特卡羅定位amcl功能包中的參數(shù)激光模型參數(shù)~laser_min_range（double，默認(rèn)值：?1.0）：最小掃描范圍，?1.0表示使用激光雷達(dá)默認(rèn)的最小掃描范圍。~laser_max_range（double，默認(rèn)值：?1.0）：最大掃描范圍，?1.0表示使用激光雷達(dá)默認(rèn)的最大掃描范圍。~laser_max_beams（int，默認(rèn)值：30）：表示在更新過(guò)濾器時(shí)要在每次掃描時(shí)使用多少束均勻間隔的光束。5.2基于地圖的自主導(dǎo)航1導(dǎo)航框架機(jī)器人導(dǎo)航包含定位和路徑規(guī)劃兩個(gè)核心內(nèi)容，ROS提供的amcl和move_base功能包能夠建立起一套完整的導(dǎo)航框架，實(shí)現(xiàn)機(jī)器人的導(dǎo)航功能。Navigation（導(dǎo)航）功能包的核心是move_base功能包，其包含了global_planner、local_planner、global_costmap、local_costmap、recovery_behaviors五個(gè)包。move_base功能包訂閱TF（坐標(biāo)系轉(zhuǎn)換）、odom（里程計(jì)數(shù)據(jù)）、map（地圖）、sensordata（激光雷達(dá)數(shù)據(jù)或點(diǎn)云）及goal（目標(biāo)位置）等主題，之后發(fā)布cmd_vel主題。5.2基于地圖的自主導(dǎo)航1導(dǎo)航框架ROS的導(dǎo)航功能框架如圖所示。5.2基于地圖的自主導(dǎo)航1導(dǎo)航框架TF：利用ROS的TF工具發(fā)布坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，包括：/map->/odom，/odom->/base_link，/base_link->/sensor等。odom：導(dǎo)航需要用到里程計(jì)數(shù)據(jù)，需將其用TF工具和nav_msgs/Odometry消息發(fā)布出來(lái)。map：在導(dǎo)航前，提供一張全局地圖，需要提前創(chuàng)建（但不是必須的）。sensordata：用于避障、建圖等，可以是激光雷達(dá)數(shù)據(jù)或點(diǎn)云數(shù)據(jù)（sensor_msgs/LaserScanorsensor_msgs/PointCloud）。goal：目標(biāo)位置在全局地圖中的坐標(biāo)，用geometry_msgs/PoseStamped消息格式發(fā)布。5.2基于地圖的自主導(dǎo)航1導(dǎo)航框架ROS的導(dǎo)航功能框架中幾個(gè)主要組件的功能如下。sensortransforms組件。該組件涉及使用TF進(jìn)行傳感器坐標(biāo)的變換，機(jī)器人中心往往不是傳感器中心，需要把傳感器的測(cè)量數(shù)據(jù)變換成以機(jī)器人中心為原點(diǎn)的坐標(biāo)信息。例如，若傳感器獲取的數(shù)據(jù)是基于激光雷達(dá)坐標(biāo)系base_laser的，而控制機(jī)器人時(shí)，是以機(jī)器人本體坐標(biāo)系base_link進(jìn)行的，則需要根據(jù)兩者的位置關(guān)系進(jìn)行坐標(biāo)變換。5.2基于地圖的自主導(dǎo)航1導(dǎo)航框架ROS的導(dǎo)航功能框架中幾個(gè)主要組件的功能如下。sensorsources組件。這是機(jī)器人導(dǎo)航傳感器的數(shù)據(jù)輸入組件，數(shù)據(jù)一般只有兩種：激光雷達(dá)數(shù)據(jù)、點(diǎn)云數(shù)據(jù)。odometrysource組件。該組件負(fù)責(zé)提供機(jī)器人導(dǎo)航需要輸入的里程計(jì)數(shù)據(jù)。5.2基于地圖的自主導(dǎo)航1導(dǎo)航框架ROS的導(dǎo)航功能框架中幾個(gè)主要組件的功能如下。basecontroller組件。該組件負(fù)責(zé)將導(dǎo)航過(guò)程中得到的數(shù)據(jù)封裝成具體的線速度和轉(zhuǎn)向角度，發(fā)布給硬件平臺(tái)。map_server組件。ROS的costmap地圖采用網(wǎng)格形式，每個(gè)網(wǎng)格值的范圍為0～255，分為三種狀態(tài)：占用（障礙物）、無(wú)用（自由的）、未知。5.2基于地圖的自主導(dǎo)航1導(dǎo)航框架盡管導(dǎo)航功能包設(shè)計(jì)得盡可能通用，但是其仍然對(duì)機(jī)器人的硬件有以下三個(gè)要求：導(dǎo)航功能包僅對(duì)差分輪式機(jī)器人和完整約束輪式機(jī)器人有效，并且假設(shè)可直接使用速度命令對(duì)機(jī)器人進(jìn)行控制，速度命令的格式為：x方向速度、y方向速度、速度向量角度。導(dǎo)航功能包要求機(jī)器人安裝有激光雷達(dá)等二維平面測(cè)距傳感器，以建圖和定位。5.2基于地圖的自主導(dǎo)航1導(dǎo)航框架盡管導(dǎo)航功能包設(shè)計(jì)得盡可能通用，但是其仍然對(duì)機(jī)器人的硬件有以下三個(gè)要求：導(dǎo)航功能包以方形或者圓形的機(jī)器人為模型進(jìn)行開(kāi)發(fā)，對(duì)于其他外形的機(jī)器人，雖然可以正常使用，但是支持度可能不佳。在ROS的導(dǎo)航功能框架中可以看到，move_base功能包提供了ROS導(dǎo)航的配置、運(yùn)行和交互接口，它主要包括兩個(gè)部分：全局路徑規(guī)劃和局部路徑規(guī)劃。5.2基于地圖的自主導(dǎo)航2全局路徑規(guī)劃全局路徑規(guī)劃是在已知的環(huán)境中，從機(jī)器人當(dāng)前位置找到一條到達(dá)目標(biāo)位置最佳路徑的過(guò)程。這里所說(shuō)的最佳路徑指的是路徑上沒(méi)有障礙物的情況下的最短路徑。如果機(jī)器人在按照規(guī)劃好的全局路徑進(jìn)行運(yùn)動(dòng)的過(guò)程中，遇到阻礙物，則要進(jìn)行局部路徑規(guī)劃，繞開(kāi)障礙物，到達(dá)目標(biāo)位置。5.2基于地圖的自主導(dǎo)航2全局路徑規(guī)劃Dijkstra算法Dijkstra（迪杰特斯拉）算法是典型的單源最短路徑算法，它用于計(jì)算圖中一個(gè)頂點(diǎn)到其他頂點(diǎn)之間的最短路徑。該算法的基本思想是：以起點(diǎn)為中心，每次找到離起點(diǎn)最近的一個(gè)頂點(diǎn)，然后以該頂點(diǎn)為中心向外逐層擴(kuò)展，直到擴(kuò)展到目標(biāo)點(diǎn)為止，最終得到起點(diǎn)到其余所有點(diǎn)的最短路徑。5.2基于地圖的自主導(dǎo)航2全局路徑規(guī)劃Dijkstra算法Dijkstra算法具體的步驟如下。初始化。S中只包含起點(diǎn)，即S={V0}，V0到V0的距離為0。U中包含除起點(diǎn)之外的其他點(diǎn)。若V0與U中的頂點(diǎn)Vi之間有邊可達(dá)，則Vi,V0之間有正常權(quán)值，一般就是距離值。若二者之間沒(méi)有邊可達(dá)，則Vi,V0之間的權(quán)值為無(wú)窮大（編程時(shí)設(shè)為一個(gè)較大的數(shù)值，如9999）。5.2基于地圖的自主導(dǎo)航2全局路徑規(guī)劃Dijkstra算法Dijkstra算法具體的步驟如下。從U中選取一個(gè)距離V0最近的頂點(diǎn)Vk，把Vk加入S中，選定的權(quán)值就是V0到Vk的最短路徑長(zhǎng)度。5.2基于地圖的自主導(dǎo)航2全局路徑規(guī)劃Dijkstra算法Dijkstra算法具體的步驟如下。以Vk為新的中間點(diǎn)，修改U中各頂點(diǎn)的權(quán)值：如果從V0經(jīng)過(guò)Vk到頂點(diǎn)Vm的距離比從V0直接到Vm的距離短，則修改頂點(diǎn)Vm的權(quán)值，使其等于V0到Vk的距離與Vk到Vm的距離之和；如果從V0經(jīng)過(guò)Vk到頂點(diǎn)Vm的距離比從V0直接到Vm的距離長(zhǎng)，則維持頂點(diǎn)Vm的權(quán)值不變。5.2基于地圖的自主導(dǎo)航2全局路徑規(guī)劃Dijkstra算法Dijkstra算法具體的步驟如下。重復(fù)以上步驟直到所有的頂點(diǎn)都被包含在集合S中。5.2基于地圖的自主導(dǎo)航2全局路徑規(guī)劃Dijkstra算法如圖所示是一個(gè)典型的無(wú)向圖。5.2基于地圖的自主導(dǎo)航2全局路徑規(guī)劃A*算法A*算法是在靜態(tài)網(wǎng)絡(luò)中求解最短路徑的算法，它把啟發(fā)式算法（HeuristicApproach）如BFS（Breadth-FirstSearch，廣度優(yōu)先搜索）算法和Dijkstra算法結(jié)合在一起，能保證找到一條最短路徑。A*算法的核心思想是找到一條路徑，使得機(jī)器人在該路徑上行走時(shí)，每一步的移動(dòng)代價(jià)G與估算成本H的和都是最小的。那么，問(wèn)題也就轉(zhuǎn)化為求取G與H的和最小的問(wèn)題，下面將詳細(xì)介紹G與H的意義及如何使G與H的和最小。5.2基于地圖的自主導(dǎo)航2全局路徑規(guī)劃A*算法實(shí)現(xiàn)A*算法需要以下條件。地圖：一個(gè)存儲(chǔ)靜態(tài)路網(wǎng)的結(jié)構(gòu)，由柵格組成。柵格：組成地圖的基本單位，也可以叫節(jié)點(diǎn)。每個(gè)柵格都具有五種屬性：坐標(biāo)、G、H、F（G和H的和）、父親。Open列表：用于存儲(chǔ)等待處理的柵格。Close列表：用于存儲(chǔ)實(shí)際路徑上的柵格。起點(diǎn)與目標(biāo)點(diǎn)：機(jī)器人初始位置與目標(biāo)位置，是算法的兩個(gè)輸入。5.2基于地圖的自主導(dǎo)航2全局路徑規(guī)劃A*算法以上這些存儲(chǔ)結(jié)構(gòu)是實(shí)現(xiàn)A*算法所需要的，下面給出地圖結(jié)構(gòu)體、Close列表、Open列表的抽象定義。地圖結(jié)構(gòu)體的定義包含了每個(gè)柵格的坐標(biāo)、可達(dá)性、相鄰柵格個(gè)數(shù)及柵格的值。5.2基于地圖的自主導(dǎo)航2全局路徑規(guī)劃A*算法以上這些存儲(chǔ)結(jié)構(gòu)是實(shí)現(xiàn)A*算法所需要的，下面給出地圖結(jié)構(gòu)體、Close列表、Open列表的抽象定義。Close列表定義如下：5.2基于地圖的自主導(dǎo)航2全局路徑規(guī)劃A*算法以上這些存儲(chǔ)結(jié)構(gòu)是實(shí)現(xiàn)A*算法所需要的，下面給出地圖結(jié)構(gòu)體、Close列表、Open列表的抽象定義。Open列表定義如下：5.2基于地圖的自主導(dǎo)航2全局路徑規(guī)劃Dijkstra算法和A*算法的區(qū)別Dijkstra算法和A*算法都是求解最短路徑的有效算法，它們之間的區(qū)別主要體現(xiàn)在以下方面。Dijkstra算法注重計(jì)算起點(diǎn)到其他所有點(diǎn)的最短路徑長(zhǎng)度，可以看成只有一個(gè)輸入，即指定的起點(diǎn)；A*算法關(guān)注的是點(diǎn)與點(diǎn)之間的最短路徑，每次計(jì)算會(huì)有兩個(gè)輸入：起點(diǎn)與目標(biāo)點(diǎn)，可以直接得出一條具體的路徑。5.2基于地圖的自主導(dǎo)航2全局路徑規(guī)劃Dijkstra算法和A*算法的區(qū)別Dijkstra算法和A*算法都是求解最短路徑的有效算法，它們之間的區(qū)別主要體現(xiàn)在以下方面。Dijkstra算法建立在比較抽象的圖論層面，而A*算法可以更直接地應(yīng)用于柵格地圖中。A*算法的搜索范圍小于Dijkstra算法，但A*算法的效率更高。5.2基于地圖的自主導(dǎo)航2全局路徑規(guī)劃Dijkstra算法和A*算法的區(qū)別Dijkstra算法和A*算法都是求解最短路徑的有效算法，它們之間的區(qū)別主要體現(xiàn)在以下方面。當(dāng)目標(biāo)點(diǎn)很多時(shí)，A*算法會(huì)代入較多重復(fù)的數(shù)據(jù)和復(fù)雜的估價(jià)函數(shù)，所以如果不要求獲得具體路徑而只要求比較路徑長(zhǎng)短時(shí)，Dijkstra算法會(huì)更加高效。5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃進(jìn)行局部路徑規(guī)劃時(shí)，需要對(duì)障礙物的情況進(jìn)行詳細(xì)判斷，特別是需要應(yīng)對(duì)多變的動(dòng)態(tài)環(huán)境，采用適應(yīng)性強(qiáng)的處理方式，并簡(jiǎn)化算法復(fù)雜度。如圖是在柵格地圖上進(jìn)行路徑搜索的流程圖，機(jī)器人先進(jìn)行全局路徑規(guī)劃，當(dāng)碰到障礙物時(shí)，會(huì)選取相應(yīng)環(huán)境參數(shù)進(jìn)行避障處理。5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃?rùn)C(jī)器人按照規(guī)劃好的路徑行走時(shí)，會(huì)利用紅外測(cè)距傳感器、超聲波雷達(dá)或激光雷達(dá)進(jìn)行障礙物檢測(cè)和實(shí)時(shí)測(cè)距，依據(jù)機(jī)器人的實(shí)際大小，當(dāng)機(jī)器人檢測(cè)到其與障礙物的距離小于或等于安全距離時(shí)，就獲取障礙物四周的情況進(jìn)行避障。因此，局部路徑規(guī)劃是根據(jù)機(jī)器人附近的障礙物信息進(jìn)行的實(shí)時(shí)避障路徑規(guī)劃。5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃如圖所示，根據(jù)障礙物的寬度和其與機(jī)器人之間的距離，將障礙物標(biāo)準(zhǔn)化為一個(gè)矩形。5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃人工勢(shì)場(chǎng)法斥力函數(shù)的選取由于障礙物是機(jī)器人不能靠近的，因此在勢(shì)場(chǎng)中，障礙物應(yīng)該對(duì)機(jī)器人表現(xiàn)出一種排斥效果，且距離越小，排斥效果越明顯，機(jī)器人具有的勢(shì)能越大，反之勢(shì)能越小。因此，勢(shì)能和距離成反比，取斥力函數(shù)如下：5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃人工勢(shì)場(chǎng)法斥力函數(shù)的選取其中，

為機(jī)器人參考點(diǎn)與障礙物之間的距離，

為勢(shì)場(chǎng)作用的最大范圍，

為加權(quán)系數(shù)。機(jī)器人所受到的斥力為5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃人工勢(shì)場(chǎng)法斥力函數(shù)的選取斥力的角度計(jì)算公式如下：5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃人工勢(shì)場(chǎng)法引力函數(shù)的選取機(jī)器人的運(yùn)動(dòng)始終是朝著目標(biāo)點(diǎn)進(jìn)行的，因而目標(biāo)點(diǎn)對(duì)機(jī)器人的作用力可以看成是引力，而且這種引力與機(jī)器人和目標(biāo)點(diǎn)之間的距離有關(guān)，二者相距越遠(yuǎn)，引力就越大。當(dāng)二者距離為0時(shí)，引力也就為0，此時(shí)機(jī)器人已經(jīng)到達(dá)目標(biāo)點(diǎn)。根據(jù)以上規(guī)律，可以將機(jī)器人的引力函數(shù)設(shè)置如下：5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃人工勢(shì)場(chǎng)法引力函數(shù)的選取引力的角度計(jì)算公式如下：由式（5-10）可以看出，引力的方向是指向目標(biāo)點(diǎn)的，且引力值實(shí)際上是一個(gè)常數(shù)。在實(shí)際應(yīng)用中，可以通過(guò)調(diào)節(jié)系數(shù)來(lái)設(shè)置引力值的大小。5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃人工勢(shì)場(chǎng)法合力的計(jì)算在算出機(jī)器人所受的引力和斥力后，通過(guò)力的分解與合成就可以得出機(jī)器人所受到的合力的大小和方向。計(jì)算方法如下：計(jì)算出機(jī)器人所受到的斥力

和引力

；將斥力和引力在x軸和y軸上進(jìn)行分解，得到斥力分量Frx與Fry，引力分量Fgx與Fgy，則合力大小為5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃動(dòng)態(tài)窗口法動(dòng)態(tài)窗口法（DynamicWindowApproaches，DWA）：在機(jī)器人速度空間中，采樣多組速度并模擬機(jī)器人在這些速度下一定時(shí)間內(nèi)的運(yùn)動(dòng)軌跡，得到多條路徑后，對(duì)這些路徑進(jìn)行評(píng)價(jià)，選取最優(yōu)路徑所對(duì)應(yīng)的速度來(lái)驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)，以避開(kāi)障礙物。5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃動(dòng)態(tài)窗口法該算法一般用于在二維平面上進(jìn)行局部路徑規(guī)劃，適用于靜態(tài)環(huán)境，需要的計(jì)算資源少，考慮了機(jī)器人的運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)模型，但其代價(jià)函數(shù)容易陷進(jìn)局部最優(yōu)。DWA的具體步驟如下：在機(jī)器人速度空間中進(jìn)行多組離散采樣，得到(dx,dy,

)；對(duì)每個(gè)采樣的速度進(jìn)行模擬，計(jì)算機(jī)器人以該速度運(yùn)動(dòng)一定時(shí)間后的狀態(tài)，得出多條路徑；評(píng)價(jià)得到的每條路徑并舍棄非法路徑，評(píng)價(jià)目標(biāo)如下：是否接近障礙物、是否接近目標(biāo)點(diǎn)、是否接近全局路徑；5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃動(dòng)態(tài)窗口法該算法一般用于在二維平面上進(jìn)行局部路徑規(guī)劃，適用于靜態(tài)環(huán)境，需要的計(jì)算資源少，考慮了機(jī)器人的運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)模型，但其代價(jià)函數(shù)容易陷進(jìn)局部最優(yōu)。DWA的具體步驟如下：選擇得分最高的路徑，發(fā)送對(duì)應(yīng)的速度給移動(dòng)機(jī)器人；重復(fù)上面的過(guò)程。DWA需要在機(jī)器人運(yùn)動(dòng)之前模擬生成許多運(yùn)動(dòng)軌跡，使得計(jì)算量增大，容易造成避障不及時(shí)的問(wèn)題。當(dāng)障礙物較多時(shí)，會(huì)造成機(jī)器人打轉(zhuǎn)，導(dǎo)致路徑不是最優(yōu)的等問(wèn)題，降低機(jī)器人的安全性和運(yùn)動(dòng)效率。5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃動(dòng)態(tài)窗口法動(dòng)態(tài)窗口子圖的更新原則是，只有當(dāng)機(jī)器人的運(yùn)動(dòng)距離超過(guò)設(shè)定的閾值時(shí)，才更新子圖。因此動(dòng)態(tài)窗口子圖的更新閾值很關(guān)鍵，若設(shè)置得太小，會(huì)導(dǎo)致整個(gè)系統(tǒng)計(jì)算量增加，降低實(shí)時(shí)性；若設(shè)置得太大，則會(huì)造成機(jī)器人避障的準(zhǔn)確性下降。同時(shí)，動(dòng)態(tài)窗口子圖范圍的大小設(shè)置也很重要，若設(shè)置得過(guò)小，會(huì)造成機(jī)器人避障的準(zhǔn)確性下降；若設(shè)置得太大，則增加系統(tǒng)的計(jì)算量。因此，動(dòng)態(tài)窗口子圖的更新閾值與子圖范圍的大小，需要根據(jù)實(shí)際效果進(jìn)行調(diào)整。5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃動(dòng)態(tài)窗口法當(dāng)把動(dòng)態(tài)窗口子圖中的障礙物坐標(biāo)變換到全局地圖坐標(biāo)系下之后，需要在全局地圖中將該坐標(biāo)對(duì)應(yīng)的位置標(biāo)識(shí)為障礙物區(qū)域，并進(jìn)行障礙物范圍擴(kuò)充處理，使機(jī)器人在運(yùn)動(dòng)時(shí)與障礙物之間存在合適的安全距離，然后控制機(jī)器人進(jìn)行避障。當(dāng)機(jī)器人繞過(guò)障礙物后，再清除動(dòng)態(tài)窗口子圖融合到全局地圖中的障礙物。這樣不斷循環(huán)，直到機(jī)器人運(yùn)動(dòng)到全局目標(biāo)點(diǎn)為止。5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃TEB算法連接起點(diǎn)和目標(biāo)點(diǎn)的路徑，會(huì)受到內(nèi)外力的影響而變形，將這條可以變形的路徑定義為ElasticBand（橡皮筋），變形的條件就是“橡皮筋”受到的內(nèi)外力約束變化。在起點(diǎn)和目標(biāo)點(diǎn)之間，存在N個(gè)使“橡皮筋”變形的控制點(diǎn)（機(jī)器人避障時(shí)改變運(yùn)動(dòng)方向的點(diǎn)），“橡皮筋”內(nèi)外力相互平衡，使路徑收縮，同時(shí)與障礙物保持一定的距離。由于機(jī)器人在這些點(diǎn)之間運(yùn)動(dòng)時(shí)需要一定的時(shí)間，因此這種算法稱為T(mén)EB算法。5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃TEB算法TEB算法的內(nèi)外力約束主要如下。跟隨全局規(guī)劃路徑和避障約束力：跟隨全局規(guī)劃路徑的約束力將“橡皮筋”拉向全局路徑，而避障約束力使得“橡皮筋”遠(yuǎn)離障礙物。速度和加速度約束：屬于運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)約束，即移動(dòng)機(jī)器人運(yùn)動(dòng)時(shí)的最大速度和加速度。5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃TEB算法TEB算法的內(nèi)外力約束主要如下。非完整（Non-Holonomic）運(yùn)動(dòng)學(xué)約束：差速移動(dòng)機(jī)器人在平面上運(yùn)動(dòng)時(shí)只有兩個(gè)自由度，只能以朝向的方向進(jìn)行直線運(yùn)動(dòng)或旋轉(zhuǎn)。這種運(yùn)動(dòng)學(xué)約束使得差速移動(dòng)機(jī)器人沿著由若干弧線組成的平滑軌跡運(yùn)動(dòng)。最短路徑時(shí)間約束：該約束為時(shí)間間隔序列的平方，該約束使得機(jī)器人獲得具有最短運(yùn)動(dòng)時(shí)間的路徑，而非傳統(tǒng)的空間上的最短路徑。5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃TEB算法對(duì)于兩輪差速移動(dòng)底盤(pán)，TED算法在運(yùn)動(dòng)中調(diào)節(jié)朝向會(huì)使運(yùn)動(dòng)路徑不流暢，機(jī)器人在起點(diǎn)和即將到達(dá)目標(biāo)點(diǎn)時(shí)會(huì)出現(xiàn)不必要的倒退。這在某些應(yīng)用場(chǎng)景里是不允許的，因?yàn)闄C(jī)器人倒退可能會(huì)碰到障礙物。因此，需要根據(jù)場(chǎng)景對(duì)TED算法進(jìn)行一些優(yōu)化：機(jī)器人在起點(diǎn)不朝向目標(biāo)點(diǎn)時(shí)，TEB算法規(guī)劃出的路徑會(huì)考慮倒退旋轉(zhuǎn)，修正朝向。但在一些場(chǎng)景中，這樣的倒退和修正是不允許的。所以針對(duì)兩輪差速的運(yùn)動(dòng)模型，可以先讓底盤(pán)原地旋轉(zhuǎn)，使其朝向目標(biāo)點(diǎn)，再執(zhí)行優(yōu)化函數(shù)，得到最優(yōu)軌跡。5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃TEB算法對(duì)于兩輪差速移動(dòng)底盤(pán)，TED算法在運(yùn)動(dòng)中調(diào)節(jié)朝向會(huì)使運(yùn)動(dòng)路徑不流暢，機(jī)器人在起點(diǎn)和即將到達(dá)目標(biāo)點(diǎn)時(shí)會(huì)出現(xiàn)不必要的倒退。這在某些應(yīng)用場(chǎng)景里是不允許的，因?yàn)闄C(jī)器人倒退可能會(huì)碰到障礙物。因此，需要根據(jù)場(chǎng)景對(duì)TED算法進(jìn)行一些優(yōu)化：將機(jī)器人到達(dá)目標(biāo)點(diǎn)時(shí)的朝向緩存下來(lái)，當(dāng)機(jī)器人到達(dá)目標(biāo)點(diǎn)時(shí)，對(duì)比此時(shí)朝向與緩存的目標(biāo)朝向，當(dāng)兩個(gè)朝向有差異時(shí)，原地旋轉(zhuǎn)到目標(biāo)朝向。5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃TEB算法對(duì)于兩輪差速移動(dòng)底盤(pán)，TED算法在運(yùn)動(dòng)中調(diào)節(jié)朝向會(huì)使運(yùn)動(dòng)路徑不流暢，機(jī)器人在起點(diǎn)和即將到達(dá)目標(biāo)點(diǎn)時(shí)會(huì)出現(xiàn)不必要的倒退。這在某些應(yīng)用場(chǎng)景里是不允許的，因?yàn)闄C(jī)器人倒退可能會(huì)碰到障礙物。因此，需要根據(jù)場(chǎng)景對(duì)TED算法進(jìn)行一些優(yōu)化：根據(jù)障礙物信息，找到前方距離障礙物的最短距離，然后根據(jù)障礙物的遠(yuǎn)近調(diào)整最大線速度（max_vel_x）約束，改善避障效果。5.2基于地圖的自主導(dǎo)航3局部路徑規(guī)劃TEB算法對(duì)于兩輪差速移動(dòng)底盤(pán)，TED算法在運(yùn)動(dòng)中調(diào)節(jié)朝向會(huì)使運(yùn)動(dòng)路徑不流暢，機(jī)器人在起點(diǎn)和即將到達(dá)目標(biāo)點(diǎn)時(shí)會(huì)出現(xiàn)不必要的倒退。這在某些應(yīng)用場(chǎng)景里是不允許的，因?yàn)闄C(jī)器人倒退可能會(huì)碰到障礙物。因此，需要根據(jù)場(chǎng)景對(duì)TED算法進(jìn)行一些優(yōu)化：根據(jù)機(jī)器人當(dāng)前位置距離全局目標(biāo)點(diǎn)的距離來(lái)調(diào)整最大線速度約束，距離全局目標(biāo)點(diǎn)遠(yuǎn)時(shí)可以將最大線速度設(shè)置得大一點(diǎn)，距離全局目標(biāo)點(diǎn)近時(shí)可以將最大線速度設(shè)置得小一點(diǎn)，以防止出現(xiàn)過(guò)沖現(xiàn)象。5.2基于地圖的自主導(dǎo)航4導(dǎo)航功能包move_base功能包接口move_base功能包訂閱的動(dòng)作move_base/goal（move_base_msgs/MoveBaseActionGoal）：表示move_base功能包的運(yùn)動(dòng)規(guī)劃目標(biāo)。move_base/cancel（actionlib_msgs/GoalID）：表示撤銷指定目標(biāo)的請(qǐng)求。5.2基于地圖的自主導(dǎo)航4導(dǎo)航功能包move_base功能包接口move_base功能包發(fā)布的動(dòng)作move_base/feedback（move_base_msgs/MoveBaseActionFeedback）：表示反饋信息，包含機(jī)器人底盤(pán)的坐標(biāo)。move_base/status（actionlib_msgs/GoalStatusArray）：表示發(fā)布到move_base功能包的目標(biāo)狀態(tài)信息。move_base/result（move_base_msgs/MoveBaseActionResult）：表示對(duì)move_base功能包的操作結(jié)果為空。5.2基于地圖的自主導(dǎo)航4導(dǎo)航功能包move_base功能包接口move_base功能包訂閱的主題move_base_simple/goal（geometry_msgs/PoseStamped）：為用戶提供一個(gè)到move_base功能包的非動(dòng)作接口，不需要關(guān)注跟蹤目標(biāo)的執(zhí)行狀態(tài)。5.2基于地圖的自主導(dǎo)航4導(dǎo)航功能包move_base功能包接口move_base功能包發(fā)布的主題cmd_vel（geometry_msgs/Twist）：輸入機(jī)器人底盤(pán)的速度命令。5.2基于地圖的自主導(dǎo)航4導(dǎo)航功能包move_base功能包接口move_base功能包的服務(wù)~make_plan（nav_msgs/GetPlan）：允許用戶從move_base功能包請(qǐng)求一個(gè)到給定位姿的規(guī)劃，但不會(huì)使move_base功能包執(zhí)行該規(guī)劃。~clear_unknown_space（std_srvs/Empty）：允許用戶清理機(jī)器人周圍區(qū)域的未知空間。當(dāng)costmap（代價(jià)地圖）在環(huán)境中已經(jīng)停止了很長(zhǎng)時(shí)間，并在一個(gè)新的位置重新啟用時(shí)有用。5.2基于地圖的自主導(dǎo)航4導(dǎo)航功能包move_base功能包接口move_base功能包中的參數(shù)~base_global_planner（string,默認(rèn)值：navfn/NavfnROS）：設(shè)置move_base功能包使用的全局路徑規(guī)劃插件的名稱。~base_local_planner（string,默認(rèn)值：base_local_planner/TrajectoryPlannerROS）：設(shè)置move_base功能包使用的局部路徑規(guī)劃插件的名稱。~controller_frequency（double,默認(rèn)值：20.0）：發(fā)布控制命令的循環(huán)頻率（單位：Hz）。5.2基于地圖的自主導(dǎo)航4導(dǎo)航功能包代價(jià)地圖配置通用配置文件導(dǎo)航功能包使用代價(jià)地圖存儲(chǔ)障礙物信息，創(chuàng)建一個(gè)名為costmap_common_params.yaml的通用配置文件，具體內(nèi)容如下：5.2基于地圖的自主導(dǎo)航4導(dǎo)航功能包代價(jià)地圖配置通用配置文件導(dǎo)航功能包使用代價(jià)地圖存儲(chǔ)障礙物信息，創(chuàng)建一個(gè)名為costmap_common_params.yaml的通用配置文件，具體內(nèi)容如下：5.2基于地圖的自主導(dǎo)航4導(dǎo)航功能包代價(jià)地圖配置全局規(guī)劃配置文件全局規(guī)劃配置文件名為global_costmap_params.yaml，用于存儲(chǔ)配置全局代價(jià)地圖的參數(shù)，內(nèi)容如下：5.2基于地圖的自主導(dǎo)航4導(dǎo)航功能包代價(jià)地圖配置局部規(guī)劃配置文件局部規(guī)劃配置文件名為local_costmap_params.yaml，用于存儲(chǔ)配置局部代價(jià)地圖的參數(shù)，內(nèi)容如下：5.2基于地圖的自主導(dǎo)航4導(dǎo)航功能包局部路徑規(guī)劃器配置base_local_planner局部路徑規(guī)劃器的主要作用是根據(jù)規(guī)劃的全局路徑計(jì)算發(fā)送給機(jī)器人底盤(pán)的速度控制命令。局部路徑規(guī)劃器需要根據(jù)機(jī)器人規(guī)格尺寸進(jìn)行參數(shù)配置。5.3本章小結(jié)本章介紹了如何利用傳感器信息使機(jī)器人獲取在已知地圖中的定位，并進(jìn)行自主導(dǎo)航。通過(guò)自適應(yīng)蒙特卡羅定位功能包amcl獲取機(jī)器人在地圖中的準(zhǔn)確姿態(tài)，然后利用move_base功能包完成全局路徑規(guī)劃和局部路徑規(guī)劃，實(shí)現(xiàn)機(jī)器人避障。第六章基于多傳感器的SLAM機(jī)器人操作系統(tǒng)ROS應(yīng)用實(shí)踐目錄content慣性測(cè)量單元模型與標(biāo)定6.16.5激光雷達(dá)與IMU的外參標(biāo)定6.2Cartographer算法差速輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)里程計(jì)模型6.3基于卡爾曼濾波的多傳感器融合6.46.6本章小結(jié)6.1慣性測(cè)量單元模型與標(biāo)定六軸IMU包含加速度計(jì)和陀螺儀，加速度計(jì)的三個(gè)軸與陀螺儀的三個(gè)軸分別對(duì)應(yīng)于三維笛卡爾坐標(biāo)系的X軸、Y軸、Z軸。加速度計(jì)通常由質(zhì)量塊、阻尼器、彈性元件、敏感軟件和處理電路等部分組成，用于測(cè)量軸向的加速度。陀螺儀的基本原理是，在物體旋轉(zhuǎn)時(shí)，旋轉(zhuǎn)軸所指的方向在沒(méi)有外力干擾下會(huì)保持不變，據(jù)此可測(cè)量物體繞軸旋轉(zhuǎn)的角速度。6.1慣性測(cè)量單元模型與標(biāo)定1加速度計(jì)測(cè)量模型加速度計(jì)測(cè)量模型如下：

為IMU參考坐標(biāo)系下第k幀的加速度計(jì)測(cè)量值

為第k幀的真實(shí)加速度值

為需要在線估計(jì)的第k幀的加速度零點(diǎn)誤差

為參考坐標(biāo)系到世界坐標(biāo)系的變換矩陣

為世界坐標(biāo)系下的重力加速度加速度計(jì)測(cè)量模型g為當(dāng)?shù)刂亓铀俣?/p>

為加速度白噪聲6.1慣性測(cè)量單元模型與標(biāo)定1加速度計(jì)測(cè)量模型加速度計(jì)測(cè)量模型如下：加速度計(jì)測(cè)量模型通常，加速度白噪聲

符合零均值高斯分布：假定加速度零點(diǎn)誤差

隨時(shí)間隨機(jī)游走，其對(duì)時(shí)間的導(dǎo)數(shù)

符合零均值高斯分布：6.1慣性測(cè)量單元模型與標(biāo)定1加速度計(jì)測(cè)量模型陀螺儀測(cè)量模型陀螺儀測(cè)量模型如下：

為第k幀的陀螺儀測(cè)量值

為第k幀的真實(shí)角速度值

為需要在線估計(jì)的第k幀的角速度零點(diǎn)誤差

為加速度白噪聲6.1慣性測(cè)量單元模型與標(biāo)定1加速度計(jì)測(cè)量模型陀螺儀測(cè)量模型陀螺儀測(cè)量模型如下：通常，角速度白噪聲符合零均值高斯分布：假定角速度零點(diǎn)誤差

隨時(shí)間隨機(jī)游走，其對(duì)時(shí)間的導(dǎo)數(shù)

符合零均值高斯分布：6.1慣性測(cè)量單元模型與標(biāo)定2系統(tǒng)誤差的預(yù)標(biāo)定加速度計(jì)與陀螺儀的測(cè)量模型僅考慮了傳感器的零點(diǎn)誤差與白噪聲，但是對(duì)于實(shí)際的IMU來(lái)說(shuō)，這不足以建立完全的實(shí)際測(cè)量模型。在使用IMU之前，需要明確IMU的系統(tǒng)誤差和隨機(jī)誤差，對(duì)IMU進(jìn)行預(yù)標(biāo)定，以進(jìn)一步補(bǔ)償測(cè)量誤差，本節(jié)介紹系統(tǒng)誤差的預(yù)標(biāo)定。6.1慣性測(cè)量單元模型與標(biāo)定2系統(tǒng)誤差的預(yù)標(biāo)定系統(tǒng)誤差也叫確定性誤差，包括軸向誤差和比例系數(shù)，軸向誤差是用來(lái)修正由于制造工藝精度導(dǎo)致的X、Y、Z三個(gè)軸無(wú)法準(zhǔn)確正交的問(wèn)題的；比例系數(shù)也叫尺度因子，是用來(lái)修正真實(shí)值和傳感器輸出的測(cè)量值之間的比值的，即修正傳感器數(shù)字信號(hào)到物理量轉(zhuǎn)換的誤差。通常情況下，IMU在出廠時(shí)會(huì)標(biāo)定好確定性誤差，從而我們可以將軸向誤差和比例系數(shù)視為確定值。軸向誤差和比例系數(shù)如圖所示。6.1慣性測(cè)量單元模型與標(biāo)定2系統(tǒng)誤差的預(yù)標(biāo)定這里介紹一種無(wú)須外部基準(zhǔn)裝置的IMU標(biāo)定方法，能夠提供“軸向誤差”“比例系數(shù)”“零點(diǎn)誤差”的標(biāo)定結(jié)果。設(shè)加速度計(jì)測(cè)量模型為a0為正交坐標(biāo)系下的加速度計(jì)測(cè)量值as為加速度計(jì)實(shí)際坐標(biāo)系下的原始測(cè)量值加速度計(jì)軸向誤差6.1慣性測(cè)量單元模型與標(biāo)定2系統(tǒng)誤差的預(yù)標(biāo)定設(shè)加速度計(jì)測(cè)量模型為比例系數(shù)零點(diǎn)誤差

為加速度白噪聲6.1慣性測(cè)量單元模型與標(biāo)定2系統(tǒng)誤差的預(yù)標(biāo)定設(shè)陀螺儀測(cè)量模型為w0為正交坐標(biāo)系下的陀螺儀測(cè)量值ws為陀螺儀實(shí)際坐標(biāo)系下的原始測(cè)量值陀螺儀軸向誤差比例系數(shù)零點(diǎn)誤差

為加速度白噪聲6.1慣性測(cè)量單元模型與標(biāo)定2系統(tǒng)誤差的預(yù)標(biāo)定IMU的系統(tǒng)誤差預(yù)標(biāo)定過(guò)程分為以下步驟。將IMU接入采集設(shè)備，并靜置50s，以估計(jì)陀螺儀零點(diǎn)誤差。拿起IMU，沿著IMU每個(gè)軸的正、負(fù)方向進(jìn)行足夠快的旋轉(zhuǎn)（每次旋轉(zhuǎn)后靜置5s），直到對(duì)陀螺儀三個(gè)軸的正、負(fù)共六個(gè)方向進(jìn)行充分的旋轉(zhuǎn)激勵(lì)，以標(biāo)定陀螺儀的“軸向誤差”和“比例系數(shù)”。6.1慣性測(cè)量單元模型與標(biāo)定2系統(tǒng)誤差的預(yù)標(biāo)定IMU的系統(tǒng)誤差預(yù)標(biāo)定過(guò)程分為以下步驟。手拿IMU，然后沿著IMU每個(gè)軸的正、負(fù)方向進(jìn)行快速直線運(yùn)動(dòng)（每次運(yùn)動(dòng)后靜置5s），直到對(duì)加速度計(jì)三個(gè)軸的正、負(fù)共六個(gè)方向進(jìn)行有效的激勵(lì)，以此標(biāo)定加速度計(jì)的“軸向誤差”“比例系數(shù)”“零點(diǎn)誤差”。將記錄的傳感器數(shù)據(jù)轉(zhuǎn)換為合適的格式后，使用開(kāi)源工具imu_tk，標(biāo)定上述Ta、ka、ba、Tg、Kg、bg。6.1慣性測(cè)量單元模型與標(biāo)定2系統(tǒng)誤差的預(yù)標(biāo)定IMU的系統(tǒng)誤差預(yù)標(biāo)定過(guò)程分為以下步驟。編寫(xiě)ROS節(jié)點(diǎn)，將實(shí)際坐標(biāo)系下的原始傳感器測(cè)量值as、ws，分別使用預(yù)標(biāo)定得到的參數(shù)進(jìn)行補(bǔ)償，將得到的正交坐標(biāo)系下的傳感器測(cè)量值a0、w0作為式（6-1）與式（6-4）中的

、

，用于機(jī)器人的位姿估計(jì)。6.1慣性測(cè)量單元模型與標(biāo)定3隨機(jī)誤差的預(yù)標(biāo)定由于外界環(huán)境不斷變化，加速度計(jì)與陀螺儀的測(cè)量模型中會(huì)攜帶隨機(jī)白噪音信號(hào)，且該信號(hào)符合零均值高斯分布，同時(shí)兩者的零點(diǎn)誤差也會(huì)隨時(shí)間游走，其對(duì)時(shí)間的導(dǎo)數(shù)也符合零均值高斯分布，這兩類隨機(jī)噪音信號(hào)會(huì)造成測(cè)量值在采集過(guò)程中攜帶誤差，影響精度，因此，需要標(biāo)定加速度計(jì)與陀螺儀的白噪音參數(shù)和零點(diǎn)誤差隨機(jī)游走參數(shù)。隨著IMU數(shù)據(jù)的不斷積分，隨機(jī)誤差會(huì)越來(lái)越大。因此，有必要對(duì)IMU隨機(jī)誤差進(jìn)行標(biāo)定。6.1慣性測(cè)量單元模型與標(biāo)定3隨機(jī)誤差的預(yù)標(biāo)定在IEEE標(biāo)準(zhǔn)中，使用Allan方差法進(jìn)行單軸光纖陀螺儀的測(cè)試。Allan方差法是一種時(shí)域分析方法，用于確定信號(hào)的噪聲。下面使用Allan方差法估計(jì)加速度計(jì)與陀螺儀的隨機(jī)誤差。首先計(jì)算原始測(cè)量數(shù)據(jù)在不同帶寬

的低通濾波器下的方差

，然后將濾波器帶寬

作為X軸變量，方差作為Y軸變量，繪制對(duì)數(shù)曲線圖（Allan標(biāo)準(zhǔn)差圖），分析方差

隨濾波器帶寬的變化趨勢(shì)；最后，根據(jù)Allan標(biāo)準(zhǔn)差圖，得到陀螺儀與加速度計(jì)信號(hào)的以下參數(shù)。6.1慣性測(cè)量單元模型與標(biāo)定3隨機(jī)誤差的預(yù)標(biāo)定N——白噪聲參數(shù)。陀螺儀數(shù)據(jù)的單位：加速度計(jì)數(shù)據(jù)的單位：該值對(duì)應(yīng)Allan標(biāo)準(zhǔn)差圖中斜率為

的直線部分在

處的截距。其物理意義是，對(duì)于采樣頻率為f的陀螺儀（或加速度計(jì)），實(shí)際測(cè)量值具有標(biāo)準(zhǔn)差為

的白噪聲。6.1慣性測(cè)量單元模型與標(biāo)定3隨機(jī)誤差的預(yù)標(biāo)定B——零點(diǎn)誤差不穩(wěn)定性參數(shù)。陀螺儀數(shù)據(jù)的單位：rad/s加速度計(jì)數(shù)據(jù)的單位：m/s2該值對(duì)應(yīng)Allan標(biāo)準(zhǔn)差圖中

的最小值。陀螺儀零點(diǎn)誤差不穩(wěn)定性參數(shù)的物理意義是，陀螺儀在準(zhǔn)確地補(bǔ)償靜態(tài)零點(diǎn)誤差后，每小時(shí)角度積分的誤差小于

。6.1慣性測(cè)量單元模型與標(biāo)定3隨機(jī)誤差的預(yù)標(biāo)定K——速度零點(diǎn)誤差隨機(jī)游走參數(shù)。陀螺儀數(shù)據(jù)的單位：加速度計(jì)數(shù)據(jù)的單位：該值對(duì)應(yīng)Allan標(biāo)準(zhǔn)差圖中斜率為

的直線部分在

處的截距。其物理意義是，對(duì)于采樣頻率為f的陀螺儀（或加速度計(jì)），零點(diǎn)誤差對(duì)時(shí)間的導(dǎo)數(shù)符合標(biāo)準(zhǔn)差為

的高斯分布。6.1慣性測(cè)量單元模型與標(biāo)定3隨機(jī)誤差的預(yù)標(biāo)定隨機(jī)誤差的具體標(biāo)定過(guò)程分為以下步驟。將IMU固定于穩(wěn)固平面上，避免振動(dòng)影響標(biāo)定精度，與安裝在機(jī)器人上時(shí)的安裝方向相同，以減少重力加速度對(duì)陀螺儀測(cè)量的影響。將IMU通過(guò)延長(zhǎng)線連接到采集設(shè)備上，與PC建立通信。靜止等待若干分鐘，等待IMU進(jìn)入穩(wěn)定狀態(tài)。6.1慣性測(cè)量單元模型與標(biāo)定3隨機(jī)誤差的預(yù)標(biāo)定隨機(jī)誤差的具體標(biāo)定過(guò)程分為以下步驟。使用ROS的rosbag工具記錄2～5小時(shí)的IMU數(shù)據(jù)，這期間保持IMU靜止，否則會(huì)影響標(biāo)定結(jié)果；使用基于Allan方差法的開(kāi)源工具imu_utils，將源碼中IMU的采樣頻率修改為實(shí)際值后，通過(guò)公式擬合，得到標(biāo)定結(jié)果。6.1慣性測(cè)量單元模型與標(biāo)定3隨機(jī)誤差的預(yù)標(biāo)定BMI055的隨機(jī)誤差標(biāo)定結(jié)果見(jiàn)下表，標(biāo)定結(jié)果與數(shù)據(jù)手冊(cè)中給出的數(shù)值基本一致。6.2激光雷達(dá)與IMU的外參標(biāo)定在基于多傳感器的SLAM系統(tǒng)中，傳感器的數(shù)據(jù)類型不同，采樣頻率也不同。例如，一個(gè)移動(dòng)機(jī)器人系統(tǒng)中，激光雷達(dá)的采樣頻率為10Hz，相機(jī)的采樣頻率為15Hz，IMU的采樣頻率為400Hz。在進(jìn)行數(shù)據(jù)融合之前，需要保證不同傳感器的數(shù)據(jù)是同一時(shí)刻獲得的，即進(jìn)行數(shù)據(jù)時(shí)間同步。選擇這些傳感器中最低的采樣頻率作為基準(zhǔn)，即將激光雷達(dá)的采樣時(shí)間作為對(duì)齊標(biāo)志，如圖所示。6.2激光雷達(dá)與IMU的外參標(biāo)定通過(guò)ROS功能包采集的數(shù)據(jù)每一幀都被標(biāo)記上了時(shí)間戳，由于各傳感器之間沒(méi)有硬件同步功能，因此無(wú)法保證采集到的數(shù)據(jù)幀中有相同的時(shí)間戳。下面以激光雷達(dá)為基準(zhǔn)，保留距離激光雷達(dá)數(shù)據(jù)幀時(shí)間戳最近的相機(jī)幀。對(duì)于IMU傳感器來(lái)說(shuō)，則通過(guò)加權(quán)均值法進(jìn)行時(shí)間同步處理，如式所示：6.2激光雷達(dá)與IMU的外參標(biāo)定激光雷達(dá)獲得的是點(diǎn)云數(shù)據(jù)，粗略地反映了場(chǎng)景中物體的輪廓。激光雷達(dá)隨著移動(dòng)機(jī)器人的運(yùn)動(dòng)而運(yùn)動(dòng)，以激光雷達(dá)的初始位姿所在的坐標(biāo)系作為世界坐標(biāo)系，如圖所示。6.2激光雷達(dá)與IMU的外參標(biāo)定ICP算法假定有兩個(gè)待處理的點(diǎn)云數(shù)據(jù)集合P,Q，記P為源點(diǎn)云數(shù)據(jù)集合，Q為目標(biāo)點(diǎn)云數(shù)據(jù)集合，存在未知空間變換T=[Rt]使得它們能夠進(jìn)行空間匹配，即將P所在的坐標(biāo)系變換到Q所在的坐標(biāo)系下。設(shè)向量，n為兩張點(diǎn)云圖中最近鄰近點(diǎn)云對(duì)數(shù)，ICP算法的目的就是找出最鄰近的點(diǎn)云對(duì)

，使得

。用點(diǎn)云之間的歐氏距離來(lái)度量匹配誤差：于是可以建立最小二乘的優(yōu)化目標(biāo)函數(shù)：6.2激光雷達(dá)與IMU的外參標(biāo)定結(jié)束迭代優(yōu)化的條件有兩個(gè)，滿足其中之一即可：迭代次數(shù)達(dá)到設(shè)定閾值；前后兩次均方根誤差之差的絕對(duì)值小于設(shè)定閾值，并且前后兩次內(nèi)點(diǎn)比例之差的絕對(duì)值小于設(shè)定閾值。在實(shí)際處理過(guò)程中，源點(diǎn)云和目標(biāo)點(diǎn)云是不完全一樣的，需要剔除誤匹配的點(diǎn)云對(duì)，從而使迭代優(yōu)化收斂得更準(zhǔn)確。可使用自適應(yīng)閾值法進(jìn)行誤匹配剔除，具體方法是，對(duì)點(diǎn)云對(duì)進(jìn)行統(tǒng)計(jì)分析，計(jì)算出自適應(yīng)閾值，從而剔除大于該閾值的點(diǎn)云對(duì)。6.2激光雷達(dá)與IMU的外參標(biāo)定計(jì)算出每次迭代過(guò)程中，對(duì)應(yīng)點(diǎn)距離值的均值和標(biāo)準(zhǔn)差：將自適應(yīng)閾值設(shè)定為6.2激光雷達(dá)與IMU的外參標(biāo)定下表是某個(gè)移動(dòng)機(jī)器人系統(tǒng)的激光雷達(dá)-IMU外參變換矩陣標(biāo)定結(jié)果，其中平移向量單位為m。于是，可以根據(jù)傳感器兩兩之間的位姿變換關(guān)系，建立起移動(dòng)機(jī)器人平臺(tái)上的所有傳感器坐標(biāo)系的變換矩陣，為后續(xù)的多傳感器融合算法奠定基礎(chǔ)。6.3差速輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)里程計(jì)模型SLAM算法都需要依據(jù)機(jī)器人的運(yùn)動(dòng)模型進(jìn)行位姿估計(jì)，然后結(jié)合其他傳感器觀測(cè)進(jìn)行位姿校正。假設(shè)機(jī)器人在二維平面上運(yùn)動(dòng)，且不存在車輪打滑的情況，分別在機(jī)器人左右輪上安裝編碼器，基于上述假定條件，對(duì)差速輪式移動(dòng)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模。6.3差速輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)里程計(jì)模型如圖所示，假定機(jī)器人在XY平面上運(yùn)動(dòng)，機(jī)器人中心點(diǎn)在左輪與右輪中間，左輪與右輪之間的距離為2L，左側(cè)輪式編碼器測(cè)得的速度為vL，右側(cè)輪式編碼器測(cè)得的速度為vR，則差速輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)參數(shù)由速度v和角速度ω構(gòu)成，它們可以根據(jù)機(jī)器人的左輪速度和右輪速度求解得出。6.3差速輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)里程計(jì)模型設(shè)差速輪式移動(dòng)機(jī)器人繞著圓心A做圓周運(yùn)動(dòng)，圓周半徑為AB1=R，當(dāng)機(jī)器人沿著圓心A運(yùn)動(dòng)很短的時(shí)間t后，差速輪式移動(dòng)機(jī)器人由A1運(yùn)動(dòng)到A2，則左輪運(yùn)動(dòng)的距離為ΔSL，右輪運(yùn)動(dòng)距離為ΔSR，機(jī)器人整體運(yùn)動(dòng)距離為ΔS。根據(jù)圓周的弧長(zhǎng)與半徑R、夾角θ的關(guān)系，可得方程組：左輪與右輪運(yùn)動(dòng)的弧長(zhǎng)可由輪式編碼器對(duì)時(shí)間積分獲得，即ΔSL、ΔSR、L為已知量，聯(lián)合上式中的兩式，可分別求得差速輪式移動(dòng)機(jī)器人做圓周運(yùn)動(dòng)的半徑R與夾角θ。6.3差速輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)里程計(jì)模型上式中，

，

。機(jī)器人沿著圓心A運(yùn)動(dòng)很短的時(shí)間t后，可由運(yùn)動(dòng)弧長(zhǎng)求解圓周的半徑R與夾角θ。當(dāng)時(shí)間t逐漸趨近于零時(shí)，對(duì)式中的θ與ΔS中的時(shí)間t做微分，可得機(jī)器人速度v與角速度w如下：6.3差速輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)里程計(jì)模型如圖所示，假設(shè)某一時(shí)刻機(jī)器人速度為v，角速度為

，位姿為

，其中x、y表示二維坐標(biāo)，

表示機(jī)器人的航向角。

為機(jī)器人運(yùn)動(dòng)圓心的坐標(biāo)。6.3差速輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)里程計(jì)模型由速度與角速度的關(guān)系可知：機(jī)器人的運(yùn)動(dòng)速度不會(huì)跳變，在一個(gè)極短的時(shí)間

內(nèi)，運(yùn)動(dòng)速度為常量。采用三角法，在無(wú)噪聲情況下，機(jī)器人運(yùn)動(dòng)了

，同時(shí)轉(zhuǎn)向了

角度。因此，機(jī)器人位姿

可表示為下式。6.3差速輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)里程計(jì)模型下式中，

和

為誤差（噪聲）模型。因此，在使用里程計(jì)進(jìn)行機(jī)器人位姿估計(jì)時(shí)，位姿估計(jì)精度低，可靠性差，需結(jié)合其他傳感器觀測(cè)數(shù)據(jù)共同求解。6.4基于卡爾曼濾波的多傳感器融合SLAM的過(guò)程，可以認(rèn)為是機(jī)器人在未知環(huán)境中從某個(gè)位姿開(kāi)始運(yùn)動(dòng)，在運(yùn)動(dòng)過(guò)程中不斷重復(fù)利用自身攜帶的傳感器（如激光雷達(dá)、IMU、相機(jī)等）觀測(cè)到的地圖特征（如墻角、柱子等）進(jìn)行自身位姿的確