導航堆棧的工作是通過處理來自測距、傳感器和環境圖的數據來產生讓機器人執行的安全路徑。

最大限度地提高此導航堆棧的性能需要對參數進行一些精細的調整，這項工作并不像看起來那么簡單。

如果對概念和推理了解不清楚，可能會隨意嘗試，這將浪費大量的時間。

本文旨在引導讀者正確微調導航參數。當在設置一些關鍵參數的時候，需要知道“如何”和“為什么”。

本指南假定讀者有已經設置了導航堆棧并準備優化。這也是我總結在 ROS 導航堆棧的一個過程。

正文

1 速度和加速度

本節涉及同步驅動機器人。動力性（即速度和機器人的加速度）對于包括動態窗口方法（DWA）和定時彈性帶（TEB）在內的本地規劃器來說是必不可少的。

在 ROS 導航堆棧，本地規劃器接收測距消息（“odom”主題）并輸出速度命令（“cmd_vel”主題）來控制機器人的運動。

最大/最小速度和加速度是移動基站的兩個基本參數正確設置它們對于最佳的本地計劃行為非常有幫助。

在 ROS 中導航，我們需要知道平移和旋轉的速度和加速度。

1.1 獲得最大速度

通�？梢詤⒖寄愕囊苿踊镜氖謨�。例如，SCITOS G5 的最大速度是 1.4m/s。

在 ROS 中，你可以訂閱 odom 主題來獲取當前的測距信息。

如果你可以手動控制你的機器人（如通過操縱桿），你可以嘗試向前運行，直到達到恒定速度，然后輸出里程計數據。

平移速度（m/s）是機器人在直線移動時的速度。它的最大值就是上邊我們獲得的最大值。

旋轉速度（rad/s）等效于角速度，它的最大值是機器人旋轉到位時的速度。

為了獲得最大角速度，我們可以通過控制操縱桿來讓機器人旋轉 360°，直到它的角速度達到恒定值。

為了安全起見，我們傾向于設定最大平移速度和旋轉速度低于它們的實際值。

1.2 獲得最大加速度

如果手冊沒有說明，有很多種方法可以獲得移動基站的最大加速度。

在 ROS 中，我們可以輸出帶有時間戳的里程計數據，然后看機器人達到恒定的最大平移速度（ti）需要多長時間，然后使用來自里程計數據（nav_msgs/Odometry message）來計算這個過程的加速度。多做幾次實驗求平均值。

使用 tt、tr 分別來表征從靜態到達最大平移速度和最大旋轉速度的時間。

最大平移加速度約等于最大平移速度除以時間 tt，同樣的，最大旋轉加速度約等于最大旋轉速度除以旋轉時間 tr。

1.3 設置最小值

設置最小速度的方法與上述不同。對于最小平移速度，我們將它設置為一個大的負值，因為這將可以使機器人陷入困境中時可以后退，即使大多數情況下它都是前進的。

對于最小的旋轉速度，如果參數允許，我們將其設置為負值，以便機器人可以在任意方向旋轉。要注意，DWA 本地規劃器采用的是機器人最小旋轉速度的絕對值。

1.4 x、y 方向速度

x 方向的速度是指平行于機器人直線運動方向的速度，這與移動速度相同。

y 方向的速度垂直于直線運動，它被稱之為“沖擊速度”。對于非整體機器人（如差速輪機器人），y 速度應該設置為零。

2 全局路徑規劃

2.1 全局路徑規劃方法選擇

為了使用導航包中的 move_base 節點，我們需要有全局規劃器和局部規劃器。在導航包中有三種全局規劃器：carrot 規劃器、navfn 規劃器和 global 規劃器。

2.1.1 carrot 規劃器

這是最簡單的一種規劃器。它檢查給定的目標點是否是障礙物，如果是，則通過沿機器人和目標點的向量來選擇靠近原始目標的替代目標。

最終他將給局部規劃器或內部控制器一個有效的目標點。因此，這種規劃器沒有任何全局路徑規劃。

如果你需要機器人靠近給定的目標，即使目標不可到達的情況下，這種方法是有效的。但在復雜的室內環境中，這種方法不是很實用。

2.1.2 navfn 和 global 規劃器

navfn 使用 dijkstra 算法來在起點和終點之間尋找最小代價路線。global 規劃器建立了更靈活的替代 navfn 的選擇，這些選擇包括：

（1）支持 A*算法（2）切換二次近似（3）切換網格路徑。navfn 和 global 規劃器都是基于這篇論文：

https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf

2.2 全局路徑規劃參數

因為 global 規劃器是一種更常使用的方法，我們來看它的一些關鍵參數。

注意：不是所有的參數都能在 ROS 網頁上找到,但是你可以通過運行 rosrun rqt_reconfigure rqt_reconfigure 來查看。

我們可以設置這些默認值：allow unknown(true), use dijkstra(true), use quadrati c(true), use grid path(false), old navfn behavior(false) 。

如果我們想在 RVIZ 中查看勢力圖可以將 visualize_potential 值由 FALSE 設為 TRUE。

除了這些參數外， 還有三個沒有列出來的參數會決定全局路徑規劃的性能。分別cost_factor, neutral_cost, lethal_cost。

事實上，這些參數在 navfn 算法中也提到了。這個開源代碼詳細的解釋了 navfn 如何計算這些代價值。

https://github.com/rosplanning/navigation/blob/indigo-devel/navfn/include/navfn/navfn.h

navfn 代價值的計算方法如下：

cost = COST_NEUTRAL + COST_FACTOR * costmap_cost_value

傳入的代價值設置在 0 到 252 的范圍，進一步分析：

COST_NEUTRAL 為 50 時，COST_FACTOR 需要約為 0.8，從而確保輸入值的分布能使輸出值在 50 到 253 間變化。

如果 COST_FACTOR 較高，cost 值將會在障礙物附近有一個峰值，然后規劃器將視狹窄走廊的整個寬度為較差，將不會沿著中心規劃路徑。

實驗觀察：實驗也證實了這些解釋。將 cost_factor 設置過高或過低都會降低路徑質量。這些路徑沒有穿過每側障礙物的中間，也沒有相對光滑的曲率。

極端的 COST _NEUTRAL 值也有類似的效果。對于關鍵的 cost 值，如果設置的過低可能會無法產生任何路徑，即使可行路徑是明顯的。

圖 5-10 顯示了 COST_FACTOR 和 COS T_NEUTRAL 對全局路徑規劃的影響。綠色的線是全局規劃器產生的全局路徑。經過幾次實驗.

我們發現當 COST_FACTOR=0.55，COST_NEUTRAL=66，cost=253 時，全局路徑是很好的。

3 局部路徑規劃

局部規劃器包含 dwa 局部規劃器、eband 局部規劃器和 teb 局部規劃器。

它們使用不同的算法來產生速度指令。通常 dwa 規劃器使用的較多，我們將詳細討論這種算法。其它規劃器的信息稍后將會提供。

3.1 DWA 本地規劃器

3.1.1 DWA 算法

dwa 局部規劃器采用動態窗口方法，ROS 維基上提供了這種算法執行過程的介紹：

1.將機器人的控制空間離散化（dx,dy,dtheta）

2. 對于每一個采樣速度，從機器人的當前狀態執行正向模擬，以預測如果在短時間段內采用采樣速度將會發生什么

3. 評估從正向模擬產生的每個軌跡使用包含諸如：障礙物接近度、目標接近度、全局路徑接近度和速度等特征的度量，丟棄非法軌跡（與障礙物相撞的軌跡）

4. 選擇得分最高的軌跡，將相關聯的速度發送給移動基站

5. 清零然后重復以上過程DWA 算法是由 Dieter Fox 的論文：

https://www.ri.cmu.edu/pub_files/pub1/fox_diet er_1997_1/fox_dieter_1997_1.pdf提出的。

根據這篇論文的介紹，DWA 算法的目的是生成一個動作對（v，w），它代表了機器人最佳的圓形軌跡。

DWA 通過在下一個時間間隔內搜索速度空間來達到此目的。

這個空間的速度被限制為可以接受的，這意味著機器人必須能夠在到達這些可接受速度所規定的圓形軌跡上的最接近的障礙物之前停止。

此外，DWA 將僅考慮動態窗口內的速度，其被定義為給定當前平移和旋轉速度和加速度在下一時間間隔內可到達的速度對集合。

現在，我們來看 ROS Wiki 的算法總結。第一步是在動態窗口內的速度空間中的采樣速度對（vx,vy,w）。

第二步是消除不可接受的速度（即消除不良軌跡）。

第三步是使用目標函數來評估速度對，輸出軌跡得分。第四和第五步很容易了解：采取當前最佳速度選項并重新計算。

這個 DWA 規劃器取決于提供障礙物信息的本地成本圖。 

因此，調整本地成本圖的參數對于 DWA 本地規劃的最佳行為至關重要。接下來，我們將參考前向仿真，軌跡評分，成本地圖等參數。

3.1.2 DWA 本地規劃器：前向模擬

前向模擬是 DWA 算法的第二步。

在這一步中，本地規劃器將機器人控制空間中的速度采樣，并檢查由這些速度樣本表示的圓形軌跡，并最終消除不良速度（軌跡與障礙物相交）。

在機器人的一段時間間隔內，每個速度樣本由仿真時間控制及仿真。

我們可以將模擬時間視為允許機器人以采樣速度移動的時間。

通過實驗，我們觀察到仿真時間越長，計算負荷越大。此外，當仿真時間變長后，本地路徑規劃器產生路徑的時間也會變長，這是合理的。

這里有一些關于如何調整仿真時間參數的建議。

如何設置仿真時間：如果將仿真時間設置為非常低的值（≤2.0）將導致性能有限，特別是當機器人需要通過狹窄的門口或家具之間的間隙時.

因為沒有足夠的時間來獲得最佳軌跡來通過狹窄的通道。另一方面，由于使用了 DWA 本地規劃器，所有的軌跡都是簡單的圓弧，如果將仿真時間設置的非常高（≥5.0），將導致長曲線不是非常靈活。

這個問題并不是不可避免的，因為規劃器在每個時間間隔后都會積極地重新規劃，可以進行小的調整。對于高性能的計算機，4.0 秒的值也是足夠的。

除了仿真時間，還有幾個參數值得關注。

速度采樣：在其它幾個參數中，vx_sample，vy_sample 確定在 x，y 方向上取多少平移速度樣本。vth_sample 控制旋轉速度樣本的數量。

樣本的數量取決于你的計算能力。在大多數情況下，我們傾向于設置 vth_sample 高于平移速度樣本，因為通常旋轉比直線前進更復雜。

如果將 y 向最大速度設置為零，則沒必要在 y 方向提取速度樣本，因為沒有可用的樣本。我們設置

vx_sample=20,vth_samples=40。

仿真粒度：sim_granularity 是在軌跡上的點之間采取的步長。它意味著要多頻繁的檢查軌跡上的點（檢測它們是否與障礙物相交）。

較低的值意味著高頻率，這需要更多的計算能力。對于 turtlebot 機器人來說，0.025 的默認值是足夠的。

3.1.3 DWA 本地規劃器：軌跡得分

如上所述，DWA 本地規劃器最大化目標函數來獲得最佳速度對。

在其論文中，目標函數的值依賴于三個組成部分：到目標點的過程、清除障礙物和前進速度。在 R OS 中，目標函數的計算公式如下：

cost = path_distance_bias*(distance(m) to path from the endpoint of the trajectory)+ goal_distance_bias (distance(m) to local goal from the endpoint of the trajectory)+ occdist_scale*(maximu m obstacle cost along the trajectory in obstacle cost (0-254))

目標是獲得最小的代價。path_distance_bias 是本地規劃器與全局路徑保持一致的權重。較大的值將使本地規劃器更傾向于跟蹤全局路徑。

goal_distance_bias 是機器人嘗試到達目標點的權重。實驗顯示增加 goal_distance_bias 值將會使機器人與全局路徑的一致性偏低。

occdist_scale 是機器人嘗試躲避障礙物的權重，這個值偏大將導致機器人陷入困境。

在 SCITOS G5 上，我們設置 path_distance_bias 為 32，goal_distance_bias 為 20，occdist_sacle 為 0.02,仿真結果良好。

3.1.4 DWA 本地規劃器：其他參數

目標距離公差（Goal distance tolerance）：

這些參數很容易理解，以下是他們在 ROS 維基上的描述：yaw_goal_tolerance（double,默認值：0.05），實現目標時，偏航/旋轉中控制器弧度公差

xy_goal_tolerance（double,，默認值：0.10），實現目標時，在 x y 方向的距離公差

latch_xy_goal_tolerance（bool，默認：false）如果目標公差被鎖定，即使在目標公差之前結束，如果機器人到達目標 xy 位置，它會簡單旋轉到位

振蕩復位（Oscilation reset）：在通過門口的情況下，機器人可能會來回振蕩，是因為本地規劃器正在產生通過兩個相反方向的路徑。

如果機器人保持振蕩，導航堆棧將讓機器人嘗試恢復行為。

oscillation_reset_dist（double，默認值：0.05）在振蕩標志復位之前，機器人以米為單位行走多遠

4 代價地圖參數

如上所述，代價地圖參數對于本地規劃器（不僅僅是 DWA）是至關重要的。

在 RO S 中，代價地圖由靜態地圖層、障礙物圖層和膨脹層組成。

靜態地圖層直接給導航堆棧提供靜態 SLAM 地圖解釋。障礙物圖層包含 2D 障礙物和 3D 障礙物（體素層）。

膨脹層是將障礙物膨脹來計算每個 2D 代價地圖單元的代價。

此外，有全局代價地圖，也有局部代價地圖。全局代價地圖是通過膨脹導航堆棧上的地圖障礙物來實現的。

局部代價地圖是通過將機器人傳感器檢測到的障礙物膨脹產生的.

4.1 足跡

足跡是移動基站的輪廓。在 ROS 中，它由二維數組表示[x0,y0] ; [x1,y1] ; [x2,y2]……不需要重復第一個坐標。

該占位面積將用于計算內切圓和外接圓的半徑，用于以適合此機器人的方式對障礙物進行膨脹。為了安全起見，我們通常將足跡稍大于機器人的實際輪廓。

要確定機器人的占地面積，最直接的方法是參考機器人的圖紙。 

此外，您可以手動拍攝其基座頂視圖。然后使用 CAD 軟件（如 Solidworks）適當縮放圖像，并將鼠標移動到基座輪廓上并讀取其坐標。 

坐標的起點應該是機器人的中心�；蛘�，您可以將機器人移動到一張大紙上，然后繪制基座的輪廓。

然后選擇一些頂點并使用標尺來確定它們的坐標。

4.2 膨脹

膨脹層由代價值為 0-255 的單元組成。每個單元可能會被占據、無障礙或未知三種情況。下圖介紹了膨脹值的計算方法。

inflation_radius 和 cost_scaling_factor 是決定膨脹的主要參數。

inflation_radius 控制零成本點距離障礙物有多遠。

cost_scaling_factor 與單元的代價值成反比，設置高值將使衰減更陡峭。

Pronobis 博士建議，最佳的代價圖衰減曲線是具有相對較低斜率的曲線，以便最佳路徑盡可能遠離每側的障礙物。

優點是機器人可以在障礙物中間移動。如圖14和圖15所示，具有相同的起點和目標，當代價圖曲線陡峭時，機器人往往靠近障礙物。

在圖14中，膨脹半徑=0.55，代價比例因子=5.0；在圖15中，膨脹半徑=1.75，代價比例因子=2.58

根據衰變曲線圖，我們要設定這兩個參數值，使得膨脹半徑幾乎覆蓋走廊，代價值的衰減中等，這意味著要降低代價比例因子 cost_scaling_factor 的值

4.3 代價地圖精度 costmap resolution

本參數可以分別設置本地代價地圖和全局代價地圖，它們影響計算負荷和路徑規劃能力。

在低分別率（≥0.05）的情況下，障礙物區域可能重疊，導致本地規劃器無法找到可用路徑。

對于全局代價地圖精度，只要保持與提供給導航堆棧的地圖的分辨率相同即可。

如果有足夠的計算能力，可以查看激光掃描儀的分辨率，因為當使用 gmapping 建圖時.

如果激光掃描儀的分辨率低于所需的地圖分辨率，則會有很多小的“未知點”，因為激光掃描儀不能覆蓋該區域，如圖 16 所示。

例如，Hokuyo URG-04LX-UG01 激光掃描儀的分辨率是 0.01mm，因此掃描分辨率≤0.01 的地圖將需要機器人旋轉幾次才能清除未知的點。我們發現 0.02 的精度就夠用了。

4.4 障礙物層和體素層

這兩層負責標注代價圖上的障礙，他們可以被稱為障礙層。根據 ROS 維基，障礙物層跟蹤二維的，體素層跟蹤三維的。

障礙物是根據機器人傳感器的數據進行標記（檢測）或清除（刪除），其中需要訂閱代價圖的主題。

在 ROS 執行中，體素層從障礙物層繼承，并且都是通過使用激光雷達發布的 Point Cloud 或 PointCloud2 類型的消息來獲取障礙物信息。

此外，體素層需要深度傳感器，如 Microsoft Kinect 或華碩 Xtion，3D 障礙物最終會被膨脹為二維代價圖。

體素層如何工作：體素是空間中具有一定相對位置的 3D 立方體（類似于 3D 像素）。它可以用于與附近體積的數據或屬性相關聯。

例如，它的位置是夠是一個障礙。與體素與深度相機相關的 3D 重建已經有很多研究了。

體素網格是一個 ROS 包，它提供了一個高效的三維體素網格數據結構的實現，它存儲三種狀態的體素：標記、自由、未知。

體素網格占據了代價地圖區域內的體積。在每次更新體素邊界期間，體素層根據傳感器的數據來標記或去除體素網格中的一些體素。

它還執行光線跟蹤，接下來會討論。請注意，在更新時，不會重新創建體素網格，而僅僅在更改本地代價圖的大小時才更新。

為什么要在障礙物層或體素層光線跟蹤：光線跟蹤為人所知是因為用于渲染逼真的 3D 圖形，所以可能會困惑為什么被用于處理障礙物。

一個重要的原因是可以通過傳感器來檢測不同類型的障礙物。理論上，我們還可以知道障礙物是剛性的還是柔性的。

通過以上的理解，我們來研究一下障礙物層的一些參數，這些參數對于所有的傳感器都是適用的。

max_obstacle_height：插入代價圖中的障礙物的最大高度。該參數設置為稍高于機器人的高度，對于體素層，這基本上是體素網格的高度。

obstacle_range：障礙物距離機器人的最大距離，障礙物以米為單位摻入代價地圖，并可以在每個傳感器的基礎上進行覆蓋

raytrace_range：用于使用傳感器數據在地圖中掃描出障礙物，以米為單位，可以在每個傳感器的基礎上進行覆蓋下面的這些參數僅適用于體素層

origin_z：地圖的 Z 軸原點，以米為單位

z_resolution：地圖 Z 軸精度

z_voxels：每個垂直列中的體素數，網格的高度是 Z 軸分辨率*Z 軸體素數

unknown_threshold：被認為是“已知”的列中允許的未知單元的數量

mark_threshold：在被認為是“自由”的列中允許的標記單元的最大數量

實驗觀察：實驗進一步闡明了體素層數的影響。我們使用華碩 Xtion Pro 作為我們的深度傳感器。

我們發現 Xtion 的位置很重要，它決定了“盲區的范圍”，即深度傳感器看不到的區域。

此外，當障礙物出現在 Xtion 范圍內時，表示障礙物的體素會更新（標記或清除）。

否則，一些體素的信息仍保持不變，在代價地圖中的膨脹信息也會保留。此外，Z 軸分辨率灰頂 Z 軸體素的密度。如果值很高，體素層會很密集。

如果值太低（例如 0.01），所有的體素將被放在一起，將不會獲得有效的代價圖信息。

如果將 Z 軸分辨率設置為較高的值，意圖是更好地獲得障礙物，因此需要增加 Z 軸體素數（該參數控制每個垂直列中的體素數）。

如果列中的體素數太多但分辨率不夠也是沒用的，因為每個垂直列的高度都有限制。圖 18-20 顯示了不同體素參數設置之間的比較。

5 AMCL

AMCL 是處理機器人定位的 ROS 包。它是自適應蒙特卡羅定位的縮寫（AdaptiveMonte Carlo Localization），也被稱為部分濾波定位器。

這種定位方法的原理如下：每個樣本存儲表示機器人姿態的位置和方向數據。

粒子是隨機抽樣的，當機器人移動時，粒子根據他們的狀態記憶機器人的動作，采用遞歸貝葉斯估計進行重采樣。稍后將提供 AMCL 參數調整的更多討論。

請參考 ROS 維基 http://wiki.ros.org/amcl 了解更多信息。關于原始算法的細節，可以參考 Chapter 8 of Probabilistic Robot ics, by Thrun, Burgard, and Fox.

6 恢復行為

機器人導航的一個討厭的事情就是機器人可能會卡住。幸運的是，導航堆棧具有內置的恢復行為。

即使如此，有時機器人會耗盡所有可用的恢復行為后保持靜止。因此，我們需要一個更強大的解決方案。

恢復行為的類型：ROS 導航包有兩種恢復行為，分別是清除代價地圖恢復和旋轉恢復。

清除代價地圖恢復是將本地代價地圖還原成全局代價地圖的狀態。旋轉恢復是通過旋轉 360°來恢復。

解救機器人：有時由于旋轉故障，旋轉恢復將無法執行。在這一點上，機器人可能會放棄，因為它已經嘗試了所有的恢復行為。

在大多數試驗中，我們觀察到，當機器人放棄時，實際上有很多方法可以解救機器人。

為了避免放棄，我們使用 SMAC H 來連續嘗試不同的恢復行為，通過其他額外的行為，例如設置非常接近機器人的臨時目標，并返回到以前訪問過得姿態（即退出）。

這些方法可以顯著提高機器人的耐久性，并且從以前觀察到的無望空間中解救出來。

參數：ROS 恢復行為的參數一般設為默認值。

為了清除代價地圖恢復，你可以設置一個相對較高的模擬時間 sim_time，這意味著軌跡很長，你可能需要考慮增加 res et_distance 參數的值，這樣可以消除本地代價地圖上更大的區域，并且有更好的機會尋找一條路徑。

7 動態重新配置

關于 ROS 導航最靈活的方面之一是動態重新配置，因為不同的參數設置可能對某些情況（如機器人靠近目標點時）更有幫助。

然后，沒有必要進行大量的動態重新配置。

示例：我們在實驗中觀察到的一種情況是機器人常常區域脫離全局路徑，即使沒有必要這么做。

因此，我們增加路徑距離偏差 path_distance_bias。

由于大的路徑距離偏差值會使機器人遵循全局路徑，但實際上由于公差不會最終到達目標點，我們需要找到一種方法來使機器人毫不猶豫的到達目標點。

因此我們選擇動態減少路徑距離偏差，以便在機器人靠近目標點時有一個小的值。畢竟，做更多的實驗是為了找到解決問題的方法。

8 問題

8.1 陷入困境

在使用 ROS 導航的時候，這個問題經常出現，無論是在仿真還是實際中，機器人都可能陷入困境然后放棄目標

8.2 不同方向的不同速度

在導航堆棧中我們觀察到一些奇怪的行為。當目標點設置在相對于 TF原點的-x 方向時，dwa 局部規劃器規劃不穩定（局部規劃路徑跳躍），而且機器人的移動速度非常慢。

但是當把目標設置在+x 方向時，dwa 局部規劃器就比較穩定了，并且移動速度很快。

8.3 實際和仿真

實際與仿真是有區別的。在現實情況中，障礙物有各種各樣的形狀。

例如，在實驗室中有一個垂直的柜子，防止門閉上，由于太細，機器人有時無法檢測到然后撞擊上去。而且實際中也會有更多復雜的人類活動。

8.4 前后矛盾

使用 ROS 導航堆�？赡軙霈F不一致的行為。

例如進門時，在不同時間本地代價地圖會一次又一次的生成，這可能會影響路徑規劃，特別是在分辨率較低的時候。

另外，機器人沒有內存，它不記得上次從門進入房間，所以每次嘗試進門都需要重新開始。

因此，如果沒有與以前相同的進門角度，機器人可能會卡住并放棄目標。翻譯自 ROS Navigation Tuning Guide（導航調試指南）