--文末贈書--
當一個掃地機器人第一次來到你家時，它對家里的環境一無所知，所以第一次啟動時，它的主要工作是探索這個未知環境，使用的技術就是SLAM。
地圖建立完成后，就要正式開始干活了，接下來很多問題擺在機器人面前：如何完整走過家里每一個地方？如何躲避地圖中已知的墻壁、衣柜等障礙物？靜態的還好說，如果有“熊孩子”或者寵物，還有地上不時出現的各種雜物，機器人又該如何一一躲避？
這些問題就需要一套智能化的自主導航算法來解決。
機器人自主導航的流程并不復雜，和我們日常使用地圖App的導航功能非常相似。
首先，選擇一個導航的目標點，如圖1所示的Goal，可以在地圖App里直接輸入，也可以在機器人中人為給定，目的是明確機器人“去哪里”。
接下來，在進行路徑規劃前，機器人還得知道自己“在哪里”，地圖App可以通過手機中的GPS獲知定位，機器人在室外也可以用類似的方法。如果在室內，GPS的精度不夠，那么可以使用SLAM技術進行定位，也可以使用后面將要介紹的AMCL——一種全局定位的算法進行定位。
圖1 ?移動機器人的自主導航流程
回想一下地圖App中的操作，接下來App會畫出一條連接起點和終點的最優路徑，這就是路徑規劃的過程。規劃這條最優路徑的模塊被稱為全局規劃器，也就是站在全局地圖的視角，分析如何讓機器人以最優的路徑抵達目的地。
規劃出路徑后，機器人就開始移動了，在理想狀態下，機器人需要盡量沿著全局路徑運動，這個過程中難免會遇到臨時增加的障礙物等問題，需要機器人動態決策。此時，機器人會偏離全局路徑，動態躲避障礙物，這個過程就需要機器人搭載一個局部規劃器。
局部規劃器除了會實時規劃避障路徑，還會努力讓機器人沿著全局路徑運動，也就是規劃機器人每時每刻的運動速度，這個速度就是之前頻繁用到的cmd_vel話題。將速度指令傳輸給機器人底盤，底盤中的驅動就會控制機器人的電機按照某一速度運動，從而帶動機器人向目標前進。
先來了解一下Nav2自主導航框架的系統架構，如圖2所示，該框架需要和外部節點交互的信息較多。
首先是路點（waypoints），也就是一系列導航的目標位置，路點可以有一個或多個，例如從A到B，從B到C，再從C到D，這里的B、C、D就是路徑點的目標信息。
圖2 ?Nav2自主導航框架的系統架構
為了實現導航，Nav2還需要一些輔助信息幫助它明確位置和導航信息。例如tf坐標轉換信息，它表示機器人和里程計之間的關系，可以幫助機器人確定自身在地圖中的位置。對于機器人導航而言，只有知道自己在哪里、目標在哪里，才能進行路徑規劃。另一個重要信息是地圖，SLAM構建的地圖可以提供環境中的靜態障礙物信息，例如墻壁和桌子等。
此外，機器人還需要通過雷達動態檢測環境中的動態障礙物，例如突然出現在前方的人，并實時更新定位信息和避障策略。
有了以上信息，接下來實現Nav2框架。框架上方有一個行為樹導航服務器，它是一個組織和管理導航算法的機制。行為樹通過參數化配置，設置了導航過程中使用的插件和功能，用于組織和管理多個有具體功能的服務器。
類似導航App中的路徑規劃功能，規劃器服務器負責規劃全局路徑。根據全局代價地圖（Global Costmap），服務器規劃器計算從A點到B點的最佳路徑，從而繞過障礙物。其中可以選配的規劃算法種類較多，常見的如A*、Dijkstra算法等。
在全局路徑規劃完成后，控制器服務器提供路徑跟隨（Follow Path）服務，讓機器人盡量沿著預定的路徑移動。同時，它根據局部代價地圖（Local Costmap）和實時傳感器數據，動態調整機器人的路徑，確保機器人能夠避開動態障礙物并沿著全局路徑行進。
在規劃器服務器和控制器服務器輸出路徑后，平滑器服務器會獲取路徑，并通過一些處理使之更加平滑，減少大幅度的轉角或者加減速變化，確保機器人的運動過程更加順暢。
這三個服務器通過行為樹進行組織和管理。行為樹決定了各個功能模塊的執行順序和條件，確保導航過程的正確性和有效性。
行為樹就像一位組織者，協調各個模塊的工作。
Nav2框架完成軌跡的規劃控制后生成速度控制指令——cmd_vel，包括線速度和角速度。為了提高機器人底盤控制的平穩度，速度平滑器還會對算法輸出的速度進行平穩的加減速，然后檢測機器人按照該速度運動是否會發生碰撞，最后將結果傳遞給機器人的底盤控制器，驅動機器人沿著規劃的路徑移動，完成整個導航過程。
全局導航是由規劃器服務器負責的，它的主要任務是根據全局代價地圖計算從起點到目標點的最佳路徑。
那么全局代價地圖是什么呢？
Nav2中的代價（Cost）指的是機器人通過某個區域的難易程度。例如，空曠的區域代價低，機器人可以輕松通過；靠近障礙物的區域代價高，機器人需要小心避讓。全局代價地圖指的就是通過SLAM生成的靜態地圖和傳感器提供的動態數據構建的一個詳細的環境模型。
所以全局導航的關鍵在于利用地圖信息和傳感器數據，確保路徑規劃的準確性和有效性。其中，地圖信息通常通過SLAM技術生成，地圖中包含環境中的靜態障礙物，而傳感器數據則實時提供障礙物信息。綜合這些信息，規劃服務器就能規劃出一條安全且高效的路徑。
在Nav2中，全局規劃算法以插件的形式設置于行為樹XML文件中，常見的算法有A*和Dijkstra，兩種算法的效果對比如圖3所示。
圖3??Dijkstra與A*算法
Dijkstra算法。Dijkstra可以看作一種廣度優先算法，搜索過程會從起點開始一層一層輻射出去，直到發現目標點，由于搜索的空間大，往往可以找到全局最優解作為全局路徑，不過消耗的時間和內存資源相對較多，適合小范圍的路徑規劃，例如室內或者園區內的導航。
A*算法。由于加入了一個啟發函數，在搜索過程中會有一個搜索的方向，縮小了搜索的空間。但是啟發函數存在一定的隨機性，最終得到的全局路徑不一定是全局最優解。不過這種算法效率高，占用資源少，適合范圍較大的應用場景。
考慮到移動機器人的大部分應用場景范圍有限，而且計算資源豐富，所以在ROS 2導航中，還是以Dijkstra算法為主。
在Nav2框架中，局部導航由控制服務器（Controller Server）負責，它的主要任務是確保機器人在全局路徑規劃的基礎上能夠實時地沿著規劃好的路徑移動，并利用局部代價地圖動態避開環境中的障礙物。局部導航類似于手機導航App中的實時導航功能，當我們行駛在路上時，App會不斷調整路線，確保不會偏離預定路徑。
局部導航的關鍵在于實時性和靈活性。雖然全局路徑規劃提供了從起點到目標點的最優路徑，但在實際行進過程中，環境可能發生變化，例如突然出現的行人或移動的障礙物。控制器服務器通過不斷接收傳感器數據（如激光掃描和點云數據），實時更新機器人的位置和周圍環境信息，確保機器人能夠靈活應對這些變化。
局部代價地圖與全局代價地圖類似，但它專注于機器人周圍的局部環境，更新頻率通常高于全局代價地圖。
局部路徑規劃算法與全局路徑規劃算的原理不同，但是它的設置方式也在行為樹的XML文件中，Nav2框架中常見的算法有DWA、TEB算法等。
DWA算法。DWA（Dynamic Window Approaches）算法的輸入是全局路徑和本地代價地圖的參考信息，輸出是整個導航框架的最終目的——傳輸給機器人底盤的速度指令。這中間的處理過程是什么樣的呢？如圖4右側所示。
圖4 ?DWA算法框架
DWA首先將機器人的控制空間離散化，也就是根據機器人當前的運行狀態，采樣多組速度，然后使用這些速度模擬機器人在一定時間內的運動軌跡。得到多條軌跡后，再通過一個評價函數對這些軌跡打分，打分標準包括軌跡是否會導致機器人碰撞、是否在向全局路徑靠攏等，綜合評分最高的軌跡速度，就是傳輸給機器人的速度指令。
DWA算法的實現流程簡單，計算效率也比較高，但是不太適用于環境頻繁發生變化的場景。
TEB算法。TEB算法的全稱是Time Elastic Band，其中Elastic Band的中文意思是橡皮筋，可見這種算法也具備橡皮筋的特性：連接起點和目標點，路徑可以變形，變形的條件就是各種路徑約束，類似于給橡皮筋施加了一個外力。
如圖5所示，在TEB的算法框架中，機器人位于當前位置，目標點是全局路徑上的一個點，這兩個點類似橡皮筋的兩端，是固定的。接下來，TEB算法會在兩點之間插入一些機器人的姿態點控制橡皮筋的形變，為了顯示軌跡的運動學信息，還得定義點和點之間的運動時間，也就是這里Time的含義。
圖5??TEB算法框架
接下來這些離散的位姿就組成了一個優化問題：讓這些離散位姿組成的軌跡能實現時間最短、距離最短、遠離障礙物等目標，同時限制速度與加速度，符合機器人的運動學。
最終，滿足這些約束的機器人狀態，就作為局部導航輸入機器人底盤的速度指令。
在Nav2框架中，定位功能是機器人自主導航的基礎，主要任務是確定機器人在環境中的準確位置和姿態（位置和方向）。Nav2通常使用AMCL算法幫助機器人進行定位。
什么是AMCL算法呢？
AMCL功能包封裝了一套針對二維環境的蒙特卡羅定位方法，如圖6所示，算法的核心是粒子濾波器，它使用一系列粒子來表示機器人可能的狀態。
圖6??AMCL算法框架
每個粒子包含了機器人的位置和方向的估計值。在機器人移動時，這些粒子也會根據機器人的運動模型進行更新。同時，通過將機器人的傳感器數據（如激光雷達數據）與地圖進行比較，算法會評估每個粒子的權重，即該粒子代表的位置估計與實際觀測數據匹配的程度。在每次更新中，權重較高的粒子將有更大的機會被保留下來，而權重較低的粒子則可能被淘汰。這個過程被稱為重采樣。通過這種方式，粒子群逐漸聚焦于最可能代表機器人實際位置的區域，從而實現高精度的定位。?
如圖7所示，我們可以形象地描述AMCL算法：AMCL算法會在機器人的初始位姿周圍隨機撒很多粒子，每個粒子都可以看作機器人的分身，由于這些粒子是隨機撒下的，所以這些分身的姿態并不一致。
圖7??AMCL算法示例
接下來機器人開始運動，例如，機器人以1m/s的速度前進，那么這些粒子分身也會按照同樣的速度運動，由于姿態不同，每個粒子的運動方向不一致，也就會和機器人漸行漸遠，如何判斷這些粒子偏航了呢？這就要結合地圖信息了。
例如，機器人向前走了1m，這時通過傳感器我們可以發現機器人距離前方的障礙物從原來的10m變為9m，這個信息也會傳給所有粒子，那些和機器人漸行漸遠的粒子會被算法刪除，和機器人狀態一致的粒子則被保留，同時派生出一個同樣狀態的粒子，以避免最后所有粒子都被刪除了。
按照這樣的思路，以某個固定的頻率不斷對粒子進行篩選，基本一致的留下，不一致的刪除，最終這些粒子就會逐漸向機器人的真實位姿靠攏，聚集度最高的地方，就被看作機器人的當前位姿，也就是定位的結果。
以上就是AMCL算法的主要流程，更多相關內容可參閱《ROS 2智能機器人開發實踐》一書。??
↑限時優惠↑
限時優惠中，快快搶購吧！
互動有獎
按以下方式參與互動，即有機會獲贈圖書！
活動方式：在本文評論區留言參與與“你如何看待機器人帶來的機會”等話題互動，屆時會在參與的小伙伴中抽取3名幸運鵝各贈送《ROS 2智能機器人開發實踐》一本！
說明：留言區收到回復“恭喜中獎”者將免費獲贈本圖書，中獎者請在收到通知的24小時內將您的“姓名+電話+快遞地址”私發給我，未在規定時間內回復視作自動放棄兌獎資格。
活動時間：截至2024年12月27日開獎。
快快拉上你的小伙伴參與進來吧~~
溫馨提示：可以將“博文視點”、“嵌入式大雜燴”設為星標，以免錯過贈書活動哦！
