ROS 導(dǎo)航堆棧對(duì)于移動(dòng)機(jī)器人從一個(gè)地方移動(dòng)到另一個(gè)地方是強(qiáng)大的可靠。

導(dǎo)航堆棧的工作是通過(guò)處理來(lái)自測(cè)距、傳感器和環(huán)境圖的數(shù)據(jù)來(lái)產(chǎn)生讓機(jī)器人執(zhí)行的安全路徑。

最大限度地提高此導(dǎo)航堆棧的性能需要對(duì)參數(shù)進(jìn)行一些精細(xì)的調(diào)整，這項(xiàng)工作并不像看起來(lái)那么簡(jiǎn)單。

如果對(duì)概念和推理了解不清楚，可能會(huì)隨意嘗試，這將浪費(fèi)大量的時(shí)間。

本文旨在引導(dǎo)讀者正確微調(diào)導(dǎo)航參數(shù)。當(dāng)在設(shè)置一些關(guān)鍵參數(shù)的時(shí)候，需要知道“如何”和“為什么”。

本指南假定讀者有已經(jīng)設(shè)置了導(dǎo)航堆棧并準(zhǔn)備優(yōu)化。這也是我總結(jié)在 ROS 導(dǎo)航堆棧的一個(gè)過(guò)程。

正文

1 速度和加速度

本節(jié)涉及同步驅(qū)動(dòng)機(jī)器人。動(dòng)力性（即速度和機(jī)器人的加速度）對(duì)于包括動(dòng)態(tài)窗口方法（DWA）和定時(shí)彈性帶（TEB）在內(nèi)的本地規(guī)劃器來(lái)說(shuō)是必不可少的。

在 ROS 導(dǎo)航堆棧，本地規(guī)劃器接收測(cè)距消息（“odom”主題）并輸出速度命令（“cmd_vel”主題）來(lái)控制機(jī)器人的運(yùn)動(dòng)。

最大/最小速度和加速度是移動(dòng)基站的兩個(gè)基本參數(shù)正確設(shè)置它們對(duì)于最佳的本地計(jì)劃行為非常有幫助。

在 ROS 中導(dǎo)航，我們需要知道平移和旋轉(zhuǎn)的速度和加速度。

1.1?獲得最大速度

通常可以參考你的移動(dòng)基站的手冊(cè)。例如，SCITOS G5 的最大速度是 1.4m/s。

在 ROS 中，你可以訂閱 odom 主題來(lái)獲取當(dāng)前的測(cè)距信息。

如果你可以手動(dòng)控制你的機(jī)器人（如通過(guò)操縱桿），你可以嘗試向前運(yùn)行，直到達(dá)到恒定速度，然后輸出里程計(jì)數(shù)據(jù)。

平移速度（m/s）是機(jī)器人在直線(xiàn)移動(dòng)時(shí)的速度。它的最大值就是上邊我們獲得的最大值。

旋轉(zhuǎn)速度（rad/s）等效于角速度，它的最大值是機(jī)器人旋轉(zhuǎn)到位時(shí)的速度。

為了獲得最大角速度，我們可以通過(guò)控制操縱桿來(lái)讓機(jī)器人旋轉(zhuǎn) 360°，直到它的角速度達(dá)到恒定值。

為了安全起見(jiàn)，我們傾向于設(shè)定最大平移速度和旋轉(zhuǎn)速度低于它們的實(shí)際值。

1.2?獲得最大加速度

如果手冊(cè)沒(méi)有說(shuō)明，有很多種方法可以獲得移動(dòng)基站的最大加速度。

在 ROS 中，我們可以輸出帶有時(shí)間戳的里程計(jì)數(shù)據(jù)，然后看機(jī)器人達(dá)到恒定的最大平移速度（ti）需要多長(zhǎng)時(shí)間，然后使用來(lái)自里程計(jì)數(shù)據(jù)（nav_msgs/Odometry message）來(lái)計(jì)算這個(gè)過(guò)程的加速度。多做幾次實(shí)驗(yàn)求平均值。

使用 tt、tr 分別來(lái)表征從靜態(tài)到達(dá)最大平移速度和最大旋轉(zhuǎn)速度的時(shí)間。

最大平移加速度約等于最大平移速度除以時(shí)間 tt，同樣的，最大旋轉(zhuǎn)加速度約等于最大旋轉(zhuǎn)速度除以旋轉(zhuǎn)時(shí)間 tr。

1.3?設(shè)置最小值

設(shè)置最小速度的方法與上述不同。對(duì)于最小平移速度，我們將它設(shè)置為一個(gè)大的負(fù)值，因?yàn)檫@將可以使機(jī)器人陷入困境中時(shí)可以后退，即使大多數(shù)情況下它都是前進(jìn)的。

對(duì)于最小的旋轉(zhuǎn)速度，如果參數(shù)允許，我們將其設(shè)置為負(fù)值，以便機(jī)器人可以在任意方向旋轉(zhuǎn)。要注意，DWA 本地規(guī)劃器采用的是機(jī)器人最小旋轉(zhuǎn)速度的絕對(duì)值。

1.4?x、y 方向速度

x 方向的速度是指平行于機(jī)器人直線(xiàn)運(yùn)動(dòng)方向的速度，這與移動(dòng)速度相同。

y 方向的速度垂直于直線(xiàn)運(yùn)動(dòng)，它被稱(chēng)之為“沖擊速度”。對(duì)于非整體機(jī)器人（如差速輪機(jī)器人），y 速度應(yīng)該設(shè)置為零。

2 全局路徑規(guī)劃

2.1?全局路徑規(guī)劃方法選擇

為了使用導(dǎo)航包中的 move_base 節(jié)點(diǎn)，我們需要有全局規(guī)劃器和局部規(guī)劃器。在導(dǎo)航包中有三種全局規(guī)劃器：carrot 規(guī)劃器、navfn 規(guī)劃器和 global 規(guī)劃器。

2.1.1 carrot 規(guī)劃器

這是最簡(jiǎn)單的一種規(guī)劃器。它檢查給定的目標(biāo)點(diǎn)是否是障礙物，如果是，則通過(guò)沿機(jī)器人和目標(biāo)點(diǎn)的向量來(lái)選擇靠近原始目標(biāo)的替代目標(biāo)。

最終他將給局部規(guī)劃器或內(nèi)部控制器一個(gè)有效的目標(biāo)點(diǎn)。因此，這種規(guī)劃器沒(méi)有任何全局路徑規(guī)劃。

如果你需要機(jī)器人靠近給定的目標(biāo)，即使目標(biāo)不可到達(dá)的情況下，這種方法是有效的。但在復(fù)雜的室內(nèi)環(huán)境中，這種方法不是很實(shí)用。

2.1.2 navfn 和 global 規(guī)劃器

navfn 使用 dijkstra 算法來(lái)在起點(diǎn)和終點(diǎn)之間尋找最小代價(jià)路線(xiàn)。global 規(guī)劃器建立了更靈活的替代 navfn 的選擇，這些選擇包括：

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

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

2.2?全局路徑規(guī)劃參數(shù)

因?yàn)?global 規(guī)劃器是一種更常使用的方法，我們來(lái)看它的一些關(guān)鍵參數(shù)。

注意：不是所有的參數(shù)都能在 ROS 網(wǎng)頁(yè)上找到,但是你可以通過(guò)運(yùn)行 rosrun rqt_reconfigure rqt_reconfigure 來(lái)查看。

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

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

除了這些參數(shù)外， 還有三個(gè)沒(méi)有列出來(lái)的參數(shù)會(huì)決定全局路徑規(guī)劃的性能。分別cost_factor, neutral_cost, lethal_cost。

事實(shí)上，這些參數(shù)在 navfn 算法中也提到了。這個(gè)開(kāi)源代碼詳細(xì)的解釋了 navfn 如何計(jì)算這些代價(jià)值。

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

navfn 代價(jià)值的計(jì)算方法如下：

cost = COST_NEUTRAL + COST_FACTOR * costmap_cost_value

傳入的代價(jià)值設(shè)置在 0 到 252 的范圍，進(jìn)一步分析：

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

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

實(shí)驗(yàn)觀(guān)察：實(shí)驗(yàn)也證實(shí)了這些解釋。將 cost_factor 設(shè)置過(guò)高或過(guò)低都會(huì)降低路徑質(zhì)量。這些路徑?jīng)]有穿過(guò)每側(cè)障礙物的中間，也沒(méi)有相對(duì)光滑的曲率。

極端的 COST _NEUTRAL 值也有類(lèi)似的效果。對(duì)于關(guān)鍵的 cost 值，如果設(shè)置的過(guò)低可能會(huì)無(wú)法產(chǎn)生任何路徑，即使可行路徑是明顯的。

圖 5-10 顯示了 COST_FACTOR 和 COS T_NEUTRAL 對(duì)全局路徑規(guī)劃的影響。綠色的線(xiàn)是全局規(guī)劃器產(chǎn)生的全局路徑。經(jīng)過(guò)幾次實(shí)驗(yàn).

我們發(fā)現(xiàn)當(dāng) COST_FACTOR=0.55，COST_NEUTRAL=66，cost=253 時(shí)，全局路徑是很好的。

3 局部路徑規(guī)劃

局部規(guī)劃器包含 dwa 局部規(guī)劃器、eband 局部規(guī)劃器和 teb 局部規(guī)劃器。

它們使用不同的算法來(lái)產(chǎn)生速度指令。通常 dwa 規(guī)劃器使用的較多，我們將詳細(xì)討論這種算法。其它規(guī)劃器的信息稍后將會(huì)提供。

3.1?DWA 本地規(guī)劃器

3.1.1 DWA 算法

dwa 局部規(guī)劃器采用動(dòng)態(tài)窗口方法，ROS 維基上提供了這種算法執(zhí)行過(guò)程的介紹：

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

2. 對(duì)于每一個(gè)采樣速度，從機(jī)器人的當(dāng)前狀態(tài)執(zhí)行正向模擬，以預(yù)測(cè)如果在短時(shí)間段內(nèi)采用采樣速度將會(huì)發(fā)生什么

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

4. 選擇得分最高的軌跡，將相關(guān)聯(lián)的速度發(fā)送給移動(dòng)基站

5. 清零然后重復(fù)以上過(guò)程DWA 算法是由 Dieter Fox 的論文：

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

根據(jù)這篇論文的介紹，DWA 算法的目的是生成一個(gè)動(dòng)作對(duì)（v，w），它代表了機(jī)器人最佳的圓形軌跡。

DWA 通過(guò)在下一個(gè)時(shí)間間隔內(nèi)搜索速度空間來(lái)達(dá)到此目的。

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

此外，DWA 將僅考慮動(dòng)態(tài)窗口內(nèi)的速度，其被定義為給定當(dāng)前平移和旋轉(zhuǎn)速度和加速度在下一時(shí)間間隔內(nèi)可到達(dá)的速度對(duì)集合。

現(xiàn)在，我們來(lái)看 ROS Wiki 的算法總結(jié)。第一步是在動(dòng)態(tài)窗口內(nèi)的速度空間中的采樣速度對(duì)（vx,vy,w）。

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

第三步是使用目標(biāo)函數(shù)來(lái)評(píng)估速度對(duì)，輸出軌跡得分。第四和第五步很容易了解：采取當(dāng)前最佳速度選項(xiàng)并重新計(jì)算。

這個(gè) DWA 規(guī)劃器取決于提供障礙物信息的本地成本圖。?

因此，調(diào)整本地成本圖的參數(shù)對(duì)于 DWA 本地規(guī)劃的最佳行為至關(guān)重要。接下來(lái)，我們將參考前向仿真，軌跡評(píng)分，成本地圖等參數(shù)。

3.1.2 DWA 本地規(guī)劃器：前向模擬

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

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

在機(jī)器人的一段時(shí)間間隔內(nèi)，每個(gè)速度樣本由仿真時(shí)間控制及仿真。

我們可以將模擬時(shí)間視為允許機(jī)器人以采樣速度移動(dòng)的時(shí)間。

通過(guò)實(shí)驗(yàn)，我們觀(guān)察到仿真時(shí)間越長(zhǎng)，計(jì)算負(fù)荷越大。此外，當(dāng)仿真時(shí)間變長(zhǎng)后，本地路徑規(guī)劃器產(chǎn)生路徑的時(shí)間也會(huì)變長(zhǎng)，這是合理的。

這里有一些關(guān)于如何調(diào)整仿真時(shí)間參數(shù)的建議。

如何設(shè)置仿真時(shí)間：如果將仿真時(shí)間設(shè)置為非常低的值（≤2.0）將導(dǎo)致性能有限，特別是當(dāng)機(jī)器人需要通過(guò)狹窄的門(mén)口或家具之間的間隙時(shí).

因?yàn)闆](méi)有足夠的時(shí)間來(lái)獲得最佳軌跡來(lái)通過(guò)狹窄的通道。另一方面，由于使用了 DWA 本地規(guī)劃器，所有的軌跡都是簡(jiǎn)單的圓弧，如果將仿真時(shí)間設(shè)置的非常高（≥5.0），將導(dǎo)致長(zhǎng)曲線(xiàn)不是非常靈活。

這個(gè)問(wèn)題并不是不可避免的，因?yàn)橐?guī)劃器在每個(gè)時(shí)間間隔后都會(huì)積極地重新規(guī)劃，可以進(jìn)行小的調(diào)整。對(duì)于高性能的計(jì)算機(jī)，4.0 秒的值也是足夠的。

除了仿真時(shí)間，還有幾個(gè)參數(shù)值得關(guān)注。

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

樣本的數(shù)量取決于你的計(jì)算能力。在大多數(shù)情況下，我們傾向于設(shè)置 vth_sample 高于平移速度樣本，因?yàn)橥ǔＰD(zhuǎn)比直線(xiàn)前進(jìn)更復(fù)雜。

如果將 y 向最大速度設(shè)置為零，則沒(méi)必要在 y 方向提取速度樣本，因?yàn)闆](méi)有可用的樣本。我們?cè)O(shè)置

vx_sample=20,vth_samples=40。

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

較低的值意味著高頻率，這需要更多的計(jì)算能力。對(duì)于 turtlebot 機(jī)器人來(lái)說(shuō)，0.025 的默認(rèn)值是足夠的。

3.1.3 DWA 本地規(guī)劃器：軌跡得分

如上所述，DWA 本地規(guī)劃器最大化目標(biāo)函數(shù)來(lái)獲得最佳速度對(duì)。

在其論文中，目標(biāo)函數(shù)的值依賴(lài)于三個(gè)組成部分：到目標(biāo)點(diǎn)的過(guò)程、清除障礙物和前進(jìn)速度。在 R OS 中，目標(biāo)函數(shù)的計(jì)算公式如下：

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))

目標(biāo)是獲得最小的代價(jià)。path_distance_bias 是本地規(guī)劃器與全局路徑保持一致的權(quán)重。較大的值將使本地規(guī)劃器更傾向于跟蹤全局路徑。

goal_distance_bias 是機(jī)器人嘗試到達(dá)目標(biāo)點(diǎn)的權(quán)重。實(shí)驗(yàn)顯示增加 goal_distance_bias 值將會(huì)使機(jī)器人與全局路徑的一致性偏低。

occdist_scale 是機(jī)器人嘗試躲避障礙物的權(quán)重，這個(gè)值偏大將導(dǎo)致機(jī)器人陷入困境。

在 SCITOS G5 上，我們?cè)O(shè)置 path_distance_bias 為 32，goal_distance_bias 為 20，occdist_sacle 為 0.02,仿真結(jié)果良好。

3.1.4 DWA 本地規(guī)劃器：其他參數(shù)

目標(biāo)距離公差（Goal distance tolerance）：

這些參數(shù)很容易理解，以下是他們?cè)?ROS 維基上的描述：yaw_goal_tolerance（double,默認(rèn)值：0.05），實(shí)現(xiàn)目標(biāo)時(shí)，偏航/旋轉(zhuǎn)中控制器弧度公差

xy_goal_tolerance（double,，默認(rèn)值：0.10），實(shí)現(xiàn)目標(biāo)時(shí)，在 x y 方向的距離公差

latch_xy_goal_tolerance（bool，默認(rèn)：false）如果目標(biāo)公差被鎖定，即使在目標(biāo)公差之前結(jié)束，如果機(jī)器人到達(dá)目標(biāo) xy 位置，它會(huì)簡(jiǎn)單旋轉(zhuǎn)到位

振蕩復(fù)位（Oscilation reset）：在通過(guò)門(mén)口的情況下，機(jī)器人可能會(huì)來(lái)回振蕩，是因?yàn)楸镜匾?guī)劃器正在產(chǎn)生通過(guò)兩個(gè)相反方向的路徑。

如果機(jī)器人保持振蕩，導(dǎo)航堆棧將讓機(jī)器人嘗試恢復(fù)行為。

oscillation_reset_dist（double，默認(rèn)值：0.05）在振蕩標(biāo)志復(fù)位之前，機(jī)器人以米為單位行走多遠(yuǎn)

4 代價(jià)地圖參數(shù)

如上所述，代價(jià)地圖參數(shù)對(duì)于本地規(guī)劃器（不僅僅是 DWA）是至關(guān)重要的。

在 RO S 中，代價(jià)地圖由靜態(tài)地圖層、障礙物圖層和膨脹層組成。

靜態(tài)地圖層直接給導(dǎo)航堆棧提供靜態(tài) SLAM 地圖解釋。障礙物圖層包含 2D 障礙物和 3D 障礙物（體素層）。

膨脹層是將障礙物膨脹來(lái)計(jì)算每個(gè) 2D 代價(jià)地圖單元的代價(jià)。

此外，有全局代價(jià)地圖，也有局部代價(jià)地圖。全局代價(jià)地圖是通過(guò)膨脹導(dǎo)航堆棧上的地圖障礙物來(lái)實(shí)現(xiàn)的。

局部代價(jià)地圖是通過(guò)將機(jī)器人傳感器檢測(cè)到的障礙物膨脹產(chǎn)生的.

4.1?足跡

足跡是移動(dòng)基站的輪廓。在 ROS 中，它由二維數(shù)組表示[x0,y0] ; [x1,y1] ; [x2,y2]……不需要重復(fù)第一個(gè)坐標(biāo)。

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

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

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

坐標(biāo)的起點(diǎn)應(yīng)該是機(jī)器人的中心?；蛘撸梢詫C(jī)器人移動(dòng)到一張大紙上，然后繪制基座的輪廓。

然后選擇一些頂點(diǎn)并使用標(biāo)尺來(lái)確定它們的坐標(biāo)。

4.2?膨脹

膨脹層由代價(jià)值為 0-255 的單元組成。每個(gè)單元可能會(huì)被占據(jù)、無(wú)障礙或未知三種情況。下圖介紹了膨脹值的計(jì)算方法。

inflation_radius 和 cost_scaling_factor 是決定膨脹的主要參數(shù)。

inflation_radius 控制零成本點(diǎn)距離障礙物有多遠(yuǎn)。

cost_scaling_factor 與單元的代價(jià)值成反比，設(shè)置高值將使衰減更陡峭。

Pronobis 博士建議，最佳的代價(jià)圖衰減曲線(xiàn)是具有相對(duì)較低斜率的曲線(xiàn)，以便最佳路徑盡可能遠(yuǎn)離每側(cè)的障礙物。

優(yōu)點(diǎn)是機(jī)器人可以在障礙物中間移動(dòng)。如圖14和圖15所示，具有相同的起點(diǎn)和目標(biāo)，當(dāng)代價(jià)圖曲線(xiàn)陡峭時(shí)，機(jī)器人往往靠近障礙物。

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

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

4.3?代價(jià)地圖精度 costmap resolution

本參數(shù)可以分別設(shè)置本地代價(jià)地圖和全局代價(jià)地圖，它們影響計(jì)算負(fù)荷和路徑規(guī)劃能力。

在低分別率（≥0.05）的情況下，障礙物區(qū)域可能重疊，導(dǎo)致本地規(guī)劃器無(wú)法找到可用路徑。

對(duì)于全局代價(jià)地圖精度，只要保持與提供給導(dǎo)航堆棧的地圖的分辨率相同即可。

如果有足夠的計(jì)算能力，可以查看激光掃描儀的分辨率，因?yàn)楫?dāng)使用 gmapping 建圖時(shí).

如果激光掃描儀的分辨率低于所需的地圖分辨率，則會(huì)有很多小的“未知點(diǎn)”，因?yàn)榧す鈷呙鑳x不能覆蓋該區(qū)域，如圖 16 所示。

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

4.4?障礙物層和體素層

這兩層負(fù)責(zé)標(biāo)注代價(jià)圖上的障礙，他們可以被稱(chēng)為障礙層。根據(jù) ROS 維基，障礙物層跟蹤二維的，體素層跟蹤三維的。

障礙物是根據(jù)機(jī)器人傳感器的數(shù)據(jù)進(jìn)行標(biāo)記（檢測(cè)）或清除（刪除），其中需要訂閱代價(jià)圖的主題。

在 ROS 執(zhí)行中，體素層從障礙物層繼承，并且都是通過(guò)使用激光雷達(dá)發(fā)布的 Point Cloud 或 PointCloud2 類(lèi)型的消息來(lái)獲取障礙物信息。

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

體素層如何工作：體素是空間中具有一定相對(duì)位置的 3D 立方體（類(lèi)似于 3D 像素）。它可以用于與附近體積的數(shù)據(jù)或?qū)傩韵嚓P(guān)聯(lián)。

例如，它的位置是夠是一個(gè)障礙。與體素與深度相機(jī)相關(guān)的 3D 重建已經(jīng)有很多研究了。

體素網(wǎng)格是一個(gè) ROS 包，它提供了一個(gè)高效的三維體素網(wǎng)格數(shù)據(jù)結(jié)構(gòu)的實(shí)現(xiàn)，它存儲(chǔ)三種狀態(tài)的體素：標(biāo)記、自由、未知。

體素網(wǎng)格占據(jù)了代價(jià)地圖區(qū)域內(nèi)的體積。在每次更新體素邊界期間，體素層根據(jù)傳感器的數(shù)據(jù)來(lái)標(biāo)記或去除體素網(wǎng)格中的一些體素。

它還執(zhí)行光線(xiàn)跟蹤，接下來(lái)會(huì)討論。請(qǐng)注意，在更新時(shí)，不會(huì)重新創(chuàng)建體素網(wǎng)格，而僅僅在更改本地代價(jià)圖的大小時(shí)才更新。

為什么要在障礙物層或體素層光線(xiàn)跟蹤：光線(xiàn)跟蹤為人所知是因?yàn)橛糜阡秩颈普娴?3D 圖形，所以可能會(huì)困惑為什么被用于處理障礙物。

一個(gè)重要的原因是可以通過(guò)傳感器來(lái)檢測(cè)不同類(lèi)型的障礙物。理論上，我們還可以知道障礙物是剛性的還是柔性的。

通過(guò)以上的理解，我們來(lái)研究一下障礙物層的一些參數(shù)，這些參數(shù)對(duì)于所有的傳感器都是適用的。

max_obstacle_height：插入代價(jià)圖中的障礙物的最大高度。該參數(shù)設(shè)置為稍高于機(jī)器人的高度，對(duì)于體素層，這基本上是體素網(wǎng)格的高度。

obstacle_range：障礙物距離機(jī)器人的最大距離，障礙物以米為單位摻入代價(jià)地圖，并可以在每個(gè)傳感器的基礎(chǔ)上進(jìn)行覆蓋

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

origin_z：地圖的 Z 軸原點(diǎn)，以米為單位

z_resolution：地圖 Z 軸精度

z_voxels：每個(gè)垂直列中的體素?cái)?shù)，網(wǎng)格的高度是 Z 軸分辨率*Z 軸體素?cái)?shù)

unknown_threshold：被認(rèn)為是“已知”的列中允許的未知單元的數(shù)量

mark_threshold：在被認(rèn)為是“自由”的列中允許的標(biāo)記單元的最大數(shù)量

實(shí)驗(yàn)觀(guān)察：實(shí)驗(yàn)進(jìn)一步闡明了體素層數(shù)的影響。我們使用華碩 Xtion Pro 作為我們的深度傳感器。

我們發(fā)現(xiàn) Xtion 的位置很重要，它決定了“盲區(qū)的范圍”，即深度傳感器看不到的區(qū)域。

此外，當(dāng)障礙物出現(xiàn)在 Xtion 范圍內(nèi)時(shí)，表示障礙物的體素會(huì)更新（標(biāo)記或清除）。

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

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

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

如果列中的體素?cái)?shù)太多但分辨率不夠也是沒(méi)用的，因?yàn)槊總€(gè)垂直列的高度都有限制。圖 18-20 顯示了不同體素參數(shù)設(shè)置之間的比較。

5 AMCL

AMCL 是處理機(jī)器人定位的 ROS 包。它是自適應(yīng)蒙特卡羅定位的縮寫(xiě)（AdaptiveMonte Carlo Localization），也被稱(chēng)為部分濾波定位器。

這種定位方法的原理如下：每個(gè)樣本存儲(chǔ)表示機(jī)器人姿態(tài)的位置和方向數(shù)據(jù)。

粒子是隨機(jī)抽樣的，當(dāng)機(jī)器人移動(dòng)時(shí)，粒子根據(jù)他們的狀態(tài)記憶機(jī)器人的動(dòng)作，采用遞歸貝葉斯估計(jì)進(jìn)行重采樣。稍后將提供 AMCL 參數(shù)調(diào)整的更多討論。

請(qǐng)參考 ROS 維基 http://wiki.ros.org/amcl 了解更多信息。關(guān)于原始算法的細(xì)節(jié)，可以參考 Chapter 8 of Probabilistic Robot ics, by Thrun, Burgard, and Fox.

6 恢復(fù)行為

機(jī)器人導(dǎo)航的一個(gè)討厭的事情就是機(jī)器人可能會(huì)卡住。幸運(yùn)的是，導(dǎo)航堆棧具有內(nèi)置的恢復(fù)行為。

即使如此，有時(shí)機(jī)器人會(huì)耗盡所有可用的恢復(fù)行為后保持靜止。因此，我們需要一個(gè)更強(qiáng)大的解決方案。

恢復(fù)行為的類(lèi)型：ROS 導(dǎo)航包有兩種恢復(fù)行為，分別是清除代價(jià)地圖恢復(fù)和旋轉(zhuǎn)恢復(fù)。

清除代價(jià)地圖恢復(fù)是將本地代價(jià)地圖還原成全局代價(jià)地圖的狀態(tài)。旋轉(zhuǎn)恢復(fù)是通過(guò)旋轉(zhuǎn) 360°來(lái)恢復(fù)。

解救機(jī)器人：有時(shí)由于旋轉(zhuǎn)故障，旋轉(zhuǎn)恢復(fù)將無(wú)法執(zhí)行。在這一點(diǎn)上，機(jī)器人可能會(huì)放棄，因?yàn)樗呀?jīng)嘗試了所有的恢復(fù)行為。

在大多數(shù)試驗(yàn)中，我們觀(guān)察到，當(dāng)機(jī)器人放棄時(shí)，實(shí)際上有很多方法可以解救機(jī)器人。

為了避免放棄，我們使用 SMAC H 來(lái)連續(xù)嘗試不同的恢復(fù)行為，通過(guò)其他額外的行為，例如設(shè)置非常接近機(jī)器人的臨時(shí)目標(biāo)，并返回到以前訪(fǎng)問(wèn)過(guò)得姿態(tài)（即退出）。

這些方法可以顯著提高機(jī)器人的耐久性，并且從以前觀(guān)察到的無(wú)望空間中解救出來(lái)。

參數(shù)：ROS 恢復(fù)行為的參數(shù)一般設(shè)為默認(rèn)值。

為了清除代價(jià)地圖恢復(fù)，你可以設(shè)置一個(gè)相對(duì)較高的模擬時(shí)間 sim_time，這意味著軌跡很長(zhǎng)，你可能需要考慮增加 res et_distance 參數(shù)的值，這樣可以消除本地代價(jià)地圖上更大的區(qū)域，并且有更好的機(jī)會(huì)尋找一條路徑。

7 動(dòng)態(tài)重新配置

關(guān)于 ROS 導(dǎo)航最靈活的方面之一是動(dòng)態(tài)重新配置，因?yàn)椴煌膮?shù)設(shè)置可能對(duì)某些情況（如機(jī)器人靠近目標(biāo)點(diǎn)時(shí)）更有幫助。

然后，沒(méi)有必要進(jìn)行大量的動(dòng)態(tài)重新配置。

示例：我們?cè)趯?shí)驗(yàn)中觀(guān)察到的一種情況是機(jī)器人常常區(qū)域脫離全局路徑，即使沒(méi)有必要這么做。

因此，我們?cè)黾勇窂骄嚯x偏差 path_distance_bias。

由于大的路徑距離偏差值會(huì)使機(jī)器人遵循全局路徑，但實(shí)際上由于公差不會(huì)最終到達(dá)目標(biāo)點(diǎn)，我們需要找到一種方法來(lái)使機(jī)器人毫不猶豫的到達(dá)目標(biāo)點(diǎn)。

因此我們選擇動(dòng)態(tài)減少路徑距離偏差，以便在機(jī)器人靠近目標(biāo)點(diǎn)時(shí)有一個(gè)小的值。畢竟，做更多的實(shí)驗(yàn)是為了找到解決問(wèn)題的方法。

8 問(wèn)題

8.1?陷入困境

在使用 ROS 導(dǎo)航的時(shí)候，這個(gè)問(wèn)題經(jīng)常出現(xiàn)，無(wú)論是在仿真還是實(shí)際中，機(jī)器人都可能陷入困境然后放棄目標(biāo)

8.2?不同方向的不同速度

在導(dǎo)航堆棧中我們觀(guān)察到一些奇怪的行為。當(dāng)目標(biāo)點(diǎn)設(shè)置在相對(duì)于 TF 原點(diǎn)的-x 方向時(shí)，dwa 局部規(guī)劃器規(guī)劃不穩(wěn)定（局部規(guī)劃路徑跳躍），而且機(jī)器人的移動(dòng)速度非常慢。

但是當(dāng)把目標(biāo)設(shè)置在+x 方向時(shí)，dwa 局部規(guī)劃器就比較穩(wěn)定了，并且移動(dòng)速度很快。

8.3?實(shí)際和仿真

實(shí)際與仿真是有區(qū)別的。在現(xiàn)實(shí)情況中，障礙物有各種各樣的形狀。

例如，在實(shí)驗(yàn)室中有一個(gè)垂直的柜子，防止門(mén)閉上，由于太細(xì)，機(jī)器人有時(shí)無(wú)法檢測(cè)到然后撞擊上去。而且實(shí)際中也會(huì)有更多復(fù)雜的人類(lèi)活動(dòng)。

8.4?前后矛盾

使用 ROS 導(dǎo)航堆?？赡軙?huì)出現(xiàn)不一致的行為。

例如進(jìn)門(mén)時(shí)，在不同時(shí)間本地代價(jià)地圖會(huì)一次又一次的生成，這可能會(huì)影響路徑規(guī)劃，特別是在分辨率較低的時(shí)候。

另外，機(jī)器人沒(méi)有內(nèi)存，它不記得上次從門(mén)進(jìn)入房間，所以每次嘗試進(jìn)門(mén)都需要重新開(kāi)始。

因此，如果沒(méi)有與以前相同的進(jìn)門(mén)角度，機(jī)器人可能會(huì)卡住并放棄目標(biāo)。翻譯自 ROS Navigation Tuning Guide（導(dǎo)航調(diào)試指南）

編輯：黃飛

?