當一個掃地機器人第一次來到你家時,它對家裡的環境一無所知,所以第一次啟動時,它的主要工作是探索這個未知環境,使用的技術就是SLAM。
地圖建立完成後,就要正式開始幹活了,接下來很多問題擺在機器人面前:如何完整走過家裡每一個地方?如何躲避地圖中已知的牆壁、衣櫃等障礙物?靜態的還好說,如果有“熊孩子”或者寵物,還有地上不時出現的各種雜物,機器人又該如何一一躲避?
機器人自主導航的流程並不複雜,和我們日常使用地圖App的導航功能非常相似。
首先,選擇一個導航的目標點,如圖1所示的Goal,可以在地圖App裡直接輸入,也可以在機器人中人為給定,目的是明確機器人“去哪裡”。
接下來,在進行路徑規劃前,機器人還得知道自己“在哪裡”,地圖App可以透過手機中的GPS獲知定位,機器人在室外也可以用類似的方法。如果在室內,GPS的精度不夠,那麼可以使用SLAM技術進行定位,也可以使用後面將要介紹的AMCL——一種全域性定位的演算法進行定位。
回想一下地圖App中的操作,接下來App會畫出一條連線起點和終點的最優路徑,這就是路徑規劃的過程。規劃這條最優路徑的模組被稱為全域性規劃器,也就是站在全域性地圖的視角,分析如何讓機器人以最優的路徑抵達目的地。
規劃出路徑後,機器人就開始移動了,在理想狀態下,機器人需要儘量沿著全域性路徑運動,這個過程中難免會遇到臨時增加的障礙物等問題,需要機器人動態決策。此時,機器人會偏離全域性路徑,動態躲避障礙物,這個過程就需要機器人搭載一個區域性規劃器。
區域性規劃器除了會即時規劃避障路徑,還會努力讓機器人沿著全域性路徑運動,也就是規劃機器人每時每刻的運動速度,這個速度就是之前頻繁用到的cmd_vel話題。將速度指令傳輸給機器人底盤,底盤中的驅動就會控制機器人的電機按照某一速度運動,從而帶動機器人向目標前進。
ROS 2社群中有一套專業的機器人自主導航框架——Nav2。
Nav2是Navigation2的縮寫,它是一個專門為移動機器人提供導航功能的軟體包集合,支援多種型別的機器人的自主定位、路徑規劃、避障和運動控制等功能。
1. 系統框架
先來了解一下Nav2自主導航框架的系統架構,如圖2所示,該框架需要和外部節點互動的資訊較多。
首先是路點(waypoints),也就是一系列導航的目標位置,路點可以有一個或多個,例如從A到B,從B到C,再從C到D,這裡的B、C、D就是路徑點的目標資訊。
為了實現導航,Nav2還需要一些輔助資訊幫助它明確位置和導航資訊。例如tf座標轉換資訊,它表示機器人和里程計之間的關係,可以幫助機器人確定自身在地圖中的位置。對於機器人導航而言,只有知道自己在哪裡、目標在哪裡,才能進行路徑規劃。另一個重要資訊是地圖,SLAM構建的地圖可以提供環境中的靜態障礙物資訊,例如牆壁和桌子等。
此外,機器人還需要透過雷達動態檢測環境中的動態障礙物,例如突然出現在前方的人,並即時更新定位資訊和避障策略。
有了以上資訊,接下來實現Nav2框架。框架上方有一個行為樹導航伺服器,它是一個組織和管理導航演算法的機制。行為樹透過引數化配置,設定了導航過程中使用的外掛和功能,用於組織和管理多個有具體功能的伺服器。
(1)規劃器伺服器
類似導航App中的路徑規劃功能,規劃器伺服器負責規劃全域性路徑。根據全域性代價地圖(Global Costmap),伺服器規劃器計算從A點到B點的最佳路徑,從而繞過障礙物。其中可以選配的規劃演算法種類較多,常見的如A*、Dijkstra演算法等。
(2)控制器伺服器
在全域性路徑規劃完成後,控制器伺服器提供路徑跟隨(Follow Path)服務,讓機器人儘量沿著預定的路徑移動。同時,它根據區域性代價地圖(Local Costmap)和即時感測器資料,動態調整機器人的路徑,確保機器人能夠避開動態障礙物並沿著全域性路徑行進。
(3)平滑器伺服器
在規劃器伺服器和控制器伺服器輸出路徑後,平滑器伺服器會獲取路徑,並透過一些處理使之更加平滑,減少大幅度的轉角或者加減速變化,確保機器人的運動過程更加順暢。
這三個伺服器透過行為樹進行組織和管理。行為樹決定了各個功能模組的執行順序和條件,確保導航過程的正確性和有效性。
Nav2框架完成軌跡的規劃控制後生成速度控制指令——cmd_vel,包括線速度和角速度。為了提高機器人底盤控制的平穩度,速度平滑器還會對演算法輸出的速度進行平穩的加減速,然後檢測機器人按照該速度運動是否會發生碰撞,最後將結果傳遞給機器人的底盤控制器,驅動機器人沿著規劃的路徑移動,完成整個導航過程。
2. 全域性導航
全域性導航是由規劃器伺服器負責的,它的主要任務是根據全域性代價地圖計算從起點到目標點的最佳路徑。
Nav2中的代價(Cost)指的是機器人透過某個區域的難易程度。例如,空曠的區域代價低,機器人可以輕鬆透過;靠近障礙物的區域代價高,機器人需要小心避讓。全域性代價地圖指的就是透過SLAM生成的靜態地圖和感測器提供的動態資料構建的一個詳細的環境模型。
所以全域性導航的關鍵在於利用地圖資訊和感測器資料,確保路徑規劃的準確性和有效性。其中,地圖資訊通常透過SLAM技術生成,地圖中包含環境中的靜態障礙物,而感測器資料則即時提供障礙物資訊。綜合這些資訊,規劃伺服器就能規劃出一條安全且高效的路徑。
在Nav2中,全域性規劃演算法以外掛的形式設置於行為樹XML檔案中,常見的演算法有A*和Dijkstra,兩種演算法的效果對比如圖3所示。
-
Dijkstra演算法。Dijkstra可以看作一種廣度優先演算法,搜尋過程會從起點開始一層一層輻射出去,直到發現目標點,由於搜尋的空間大,往往可以找到全域性最優解作為全域性路徑,不過消耗的時間和記憶體資源相對較多,適合小範圍的路徑規劃,例如室內或者園區內的導航。
-
A*演算法。由於加入了一個啟發函式,在搜尋過程中會有一個搜尋的方向,縮小了搜尋的空間。但是啟發函式存在一定的隨機性,最終得到的全域性路徑不一定是全域性最優解。不過這種演算法效率高,佔用資源少,適合範圍較大的應用場景。
考慮到移動機器人的大部分應用場景範圍有限,而且計算資源豐富,所以在ROS 2導航中,還是以Dijkstra演算法為主。
3. 區域性導航
在Nav2框架中,區域性導航由控制伺服器(Controller Server)負責,它的主要任務是確保機器人在全域性路徑規劃的基礎上能夠即時地沿著規劃好的路徑移動,並利用區域性代價地圖動態避開環境中的障礙物。區域性導航類似於手機導航App中的即時導航功能,當我們行駛在路上時,App會不斷調整路線,確保不會偏離預定路徑。
區域性導航的關鍵在於即時性和靈活性。雖然全域性路徑規劃提供了從起點到目標點的最優路徑,但在實際行進過程中,環境可能發生變化,例如突然出現的行人或移動的障礙物。控制器伺服器透過不斷接收感測器資料(如雷射掃描和點雲資料),即時更新機器人的位置和周圍環境資訊,確保機器人能夠靈活應對這些變化。
區域性代價地圖與全域性代價地圖類似,但它專注於機器人周圍的區域性環境,更新頻率通常高於全域性代價地圖。
區域性路徑規劃演算法與全域性路徑規劃算的原理不同,但是它的設定方式也在行為樹的XML檔案中,Nav2框架中常見的演算法有DWA、TEB演算法等。
DWA首先將機器人的控制空間離散化,也就是根據機器人當前的執行狀態,取樣多組速度,然後使用這些速度模擬機器人在一定時間內的運動軌跡。得到多條軌跡後,再透過一個評價函式對這些軌跡打分,打分標準包括軌跡是否會導致機器人碰撞、是否在向全域性路徑靠攏等,綜合評分最高的軌跡速度,就是傳輸給機器人的速度指令。
DWA演算法的實現流程簡單,計算效率也比較高,但是不太適用於環境頻繁發生變化的場景。
如圖5所示,在TEB的演算法框架中,機器人位於當前位置,目標點是全域性路徑上的一個點,這兩個點類似橡皮筋的兩端,是固定的。接下來,TEB演算法會在兩點之間插入一些機器人的姿態點控制橡皮筋的形變,為了顯示軌跡的運動學資訊,還得定義點和點之間的運動時間,也就是這裡Time的含義。
接下來這些離散的位姿就組成了一個最佳化問題:讓這些離散位姿組成的軌跡能實現時間最短、距離最短、遠離障礙物等目標,同時限制速度與加速度,符合機器人的運動學。
最終,滿足這些約束的機器人狀態,就作為區域性導航輸入機器人底盤的速度指令。
4. 定位功能
在Nav2框架中,定位功能是機器人自主導航的基礎,主要任務是確定機器人在環境中的準確位置和姿態(位置和方向)。Nav2通常使用AMCL演算法幫助機器人進行定位。
AMCL功能包封裝了一套針對二維環境的蒙特卡羅定位方法,如圖6所示,演算法的核心是粒子濾波器,它使用一系列粒子來表示機器人可能的狀態。
每個粒子包含了機器人的位置和方向的估計值。在機器人移動時,這些粒子也會根據機器人的運動模型進行更新。同時,透過將機器人的感測器資料(如雷射雷達資料)與地圖進行比較,演算法會評估每個粒子的權重,即該粒子代表的位置估計與實際觀測資料匹配的程度。在每次更新中,權重較高的粒子將有更大的機會被保留下來,而權重較低的粒子則可能被淘汰。這個過程被稱為重取樣。透過這種方式,粒子群逐漸聚焦於最可能代表機器人實際位置的區域,從而實現高精度的定位。
如圖7所示,我們可以形象地描述AMCL演算法:AMCL演算法會在機器人的初始位姿周圍隨機撒很多粒子,每個粒子都可以看作機器人的分身,由於這些粒子是隨機撒下的,所以這些分身的姿態並不一致。
接下來機器人開始運動,例如,機器人以1m/s的速度前進,那麼這些粒子分身也會按照同樣的速度運動,由於姿態不同,每個粒子的運動方向不一致,也就會和機器人漸行漸遠,如何判斷這些粒子偏航了呢?這就要結合地圖資訊了。
例如,機器人向前走了1m,這時透過感測器我們可以發現機器人距離前方的障礙物從原來的10m變為9m,這個資訊也會傳給所有粒子,那些和機器人漸行漸遠的粒子會被演算法刪除,和機器人狀態一致的粒子則被保留,同時派生出一個同樣狀態的粒子,以避免最後所有粒子都被刪除了。
按照這樣的思路,以某個固定的頻率不斷對粒子進行篩選,基本一致的留下,不一致的刪除,最終這些粒子就會逐漸向機器人的真實位姿靠攏,聚集度最高的地方,就被看作機器人的當前位姿,也就是定位的結果。
以上就是AMCL演算法的主要流程,更多相關內容可參閱《ROS 2智慧機器人開發實踐》一書。
最後,評論區抽5本,隨機發表對機器人行業看法觀點,也可以提一些對公眾號的意見。