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

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

來進行機器人位姿

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

來進行機器人位姿

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

估計地圖m及機器人的運動位姿

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

采樣出t時刻的位姿粒子

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

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

表示柵格

為障礙物的概率。

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

。

是掃描終點在世界坐標系下的坐標，其函數(shù)表達式為式：4.3HectorSLAM算法1原理分析掃描匹配對于給定的任意一個初始估計

，尋找一個最優(yōu)估計

，使式對

進行一階泰勒展開，求取展開式的最小值，即對

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

（b）

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

為機器人參考點與障礙物之間的距離，

為勢場作用的最大范圍，

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

和引力

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

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

為IMU參考坐標系下第k幀的加速度計測量值

為第k幀的真實加速度值

為需要在線估計的第k幀的加速度零點誤差

為參考坐標系到世界坐標系的變換矩陣

為世界坐標系下的重力加速度加速度計測量模型g為當?shù)刂亓铀俣?/p>

為加速度白噪聲6.1慣性測量單元模型與標定1加速度計測量模型加速度計測量模型如下：加速度計測量模型通常，加速度白噪聲

符合零均值高斯分布：假定加速度零點誤差

隨時間隨機游走，其對時間的導(dǎo)數(shù)

符合零均值高斯分布：6.1慣性測量單元模型與標定1加速度計測量模型陀螺儀測量模型陀螺儀測量模型如下：

為第k幀的陀螺儀測量值

為第k幀的真實角速度值

為需要在線估計的第k幀的角速度零點誤差

為加速度白噪聲6.1慣性測量單元模型與標定1加速度計測量模型陀螺儀測量模型陀螺儀測量模型如下：通常，角速度白噪聲符合零均值高斯分布：假定角速度零點誤差

隨時間隨機游走，其對時間的導(dǎo)數(shù)

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

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

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

、

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

的低通濾波器下的方差

，然后將濾波器帶寬

作為X軸變量，方差作為Y軸變量，繪制對數(shù)曲線圖（Allan標準差圖），分析方差

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

的直線部分在

處的截距。其物理意義是，對于采樣頻率為f的陀螺儀（或加速度計），實際測量值具有標準差為

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

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

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

的直線部分在

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

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

，使得

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

，

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

，位姿為

，其中x、y表示二維坐標，

表示機器人的航向角。

為機器人運動圓心的坐標。6.3差速輪式移動機器人的運動里程計模型由速度與角速度的關(guān)系可知：機器人的運動速度不會跳變，在一個極短的時間

內(nèi)，運動速度為常量。采用三角法，在無噪聲情況下，機器人運動了

，同時轉(zhuǎn)向了

角度。因此，機器人位姿

可表示為下式。6.3差速輪式移動機器人的運動里程計模型下式中，

和

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