基于ROS平臺的導航機器人局部路徑規劃的研究與優化

摘 ?要：智能機器人的基礎功能之一就是自主導航，包括了地圖構建和路徑規劃。基于開源操作系統ROS的機器人，運用啟發式搜索A*算法進行全局路徑規劃。對機器人局部路徑規劃進行了集中研究，采用DWA算法進行局部實時路徑規劃，加入基于彈性時間帶碰撞TEB算法，優化機器人位置和時間間隔變量，通過加權多目標化獲取最優的路徑規劃能力。實驗結果證實了TEB算法的規劃效果明顯優于DWA算法，為移動機器人自動導航功能的優化提供了思路。

關鍵詞：ROS;導航機器人;TEB算法;DWA算法;路徑規劃優化

中圖分類號：TP242 ? ? ? 文獻標識碼：A文章編號：2096-4706（2022）05-0144-05

Research and Optimization of Local Path Planning of Navigation Robot Based on ROS Platform

GUO Jiaying

（College of Electronic Science and Engineering， Jilin University， Changchun ?130061， China）

Abstract： One of the basic functions of smart robots is autonomous navigation， including map construction and path planning. The robot based on ROS， the open source operating system， uses the heuristic search A * algorithm for global path planning. The paper researches centrally the local path planning of the robot， uses the DWA algorithm for local real-time path planning， adds the TEB algorithm baesd on the elastic time-band collision， optimizes robot positions and time interval variables， and obtains the optimal path planning capability by weighted multi-object. The experimental results confirmes that the planning effect of the TEB algorithm is significantly better than the DWA algorithm， and it provides ideas for the optimization of mobile robot automatic navigation function.

Keywords： ROS; navigation robot; TEB algorithm; DWA algorithm; path planning optimization

0 ?引 ?言

從機器人軟件系統（Robot Operating System， ROS）在2007年問世，到現在已經經歷了14年，憑借著較低的技術壁壘和便捷的傳播因素，ROS現已成為全球主流的機器人操作系統。通過ROS這個平臺，不斷地有創新的知識和技術運用到機器人中，加快了人工智能的研發。而自動導航作為機器人基礎的功能之一，廣泛運用于各個應用層面，因此必須對其進行不斷地加強和完善。路徑規劃，包涵全局和局部路徑規劃，作為其關鍵性技術之一，直接決定了機器人任務的完成度和損耗值。

近年來，來自不同行業的研究人員對移動機器人的導航算法研究從未停止。在基于ROS的機器人局部移動規劃中，曾廣泛使用Trajectory Planner，后又逐漸發現了相同條件下樣本采樣空間更少的動態窗口（Dynamic Windows Approach，DWA）算法。因此本文選擇從DWA算法入手，通過數學邏輯推理和ROS機器人的仿真及實物實景下的測試結果，研究其對機器人的實際控制效果，以及能否找到局部移動規劃算法的更優解。

1 ?自主導航

智能機器人的基礎功能之一就是自主導航，包括了地圖構建、路徑規劃和自主移動，在機器人實際運用中是必不可少的部分。本文所用機器人使用LS01D 360°二維掃描測距激光雷達采集周圍距離信息，并采用SLAM算法在ROS機器人操作系統中構建實時地圖，搭載了用來獲得整體位姿和加速度信息的IMU姿態傳感器，通過51單片機作為下位機的控制器，進行編程和總線操作，控制直流無刷電機和舵機，從而實現對機器人方向和速度方面的控制。采用自適應蒙特卡洛定位 AMCL 實現定位功能，使用A*算法進行整體路徑規劃，從而實現地圖構建、路徑規劃和自主移動功能。

1.1 ?ROS操作系統

自主導航程序需要基于開源的機器人操作系統ROS。它提供了操作系統應有的服務，包含硬件抽象，底層設備控制，常用函數的完成，進程間消息傳遞，以及包管理。它也提供用于獲取、編譯、編寫、跨計算機運行代碼所需的工具和庫函數。ROS作為一個開源的軟件系統，在某些方面ROS為機器人軟件開發者提供了一個包含一系列工具、庫和約定的靈活框架，相當于一種“機器人框架（robot frameworks）”。此外，ROS是一個分布式的進程框架，將每個工作進程視為一個節點，并使用節點管理器進行統一管理。同時，ROS提供了一套消息傳遞機制，計算機視覺和語音識別等功能會帶來的實時計算壓力，可以由此消息傳遞機制進行分散，使機器人能夠克服遇到的挑戰。因此，所有的工程都可以通過ROS的基礎工具集成在一起。

1.2 ?SLAM建圖原理

SLAM（simultaneous localization and mapping），也被叫作CML（Concurrent Mapping and Localization），同步定位與地圖創建，或并行映射與定位。換言之，機器人被放置于未知環境中的未知位置，讓機器人在移動時逐漸繪制周圍障礙物和環境的地圖，并實時獲取機器人本身此時的位置信息。所謂完整的地圖（a consistent map）指的是無障礙地走到房間可達到的每個角落。

根據從編碼器和IMU慣性傳感器可以獲得機器人的測位，通過距離傳感器可以計算機器人與障礙物之間的直線距離。隨后，導航系統會將機器人的位置、傳感器姿態、障礙物信息以及通過SLAM地圖獲得的占用柵格地圖，調用到固定地圖（static map），作為占用區域（occupied area）、自由區域（free area）和未知區域（unknown area）。根據上述的四種信息，計算出障礙物存在的區域、預計碰撞障礙物的區域和可以無阻向前移動的區域，這些統稱為成本地圖（costmap）。成本地圖又可以根據導航類型被分為整體成本地圖（global_costmap）和局部成本地圖（local_costmap）兩部分。在全局移動路徑規劃中以整個區域為對象建立移動計劃，其輸出結果就是global_costmap。在局部移動路徑規劃中，local_costmap就是在以機器人為中心的部分限定區域中規劃實時的移動路徑，或是遇到需要躲避的障礙物時用到的地圖。然而，這兩種成本圖的目的不同，但表示方法卻是相同的。成本地圖可以用0到255之間的值來表示，數值的含義如表1和圖1所示。每個區域的計算取決于成本地圖中的配置參數。

機器人在實時定位的過程中，由于對機器人的運動進行過程估計時，所獲取的位置信息有可能不精確，從而產生誤差。當前的室內機器人用得最多的是導航推測（dead reckoning）。它的缺點是只能估算相對位置，然而，由于它僅用廉價的傳感器就能實現，而且研究進展已有較長的時間，因此可以獲得一定程度的位置估計值得到一定水平的位置估計值，從而得到了廣泛的應用[1]。導航推測技術使用機器人的車輪的旋轉量來估計機器人本身的移動量，但車輪的旋轉量具有不少的誤差。減少誤差的方式就是利用IMU傳感器等獲取慣性信息來補償位置和方向值、利用測距單元不斷地測量周圍環境的信息，實時地進行調整更正。

圖1 ?障礙距離與成本地圖值的關系

在完成了機器人的同步定位和地圖建模之后，就可以開始導航了。導航包括全局路徑規劃（global planner）和局部路徑規劃（local planner）。

2 ?全局路徑規劃

全局路徑規劃，它主要指基于 SLAM 這個系統，在二維地圖上預先規劃好當前一個機器人移動所處的地理位置，即一條全局路徑。ROS中間全局路徑規劃器算法采用的是global_planner，基于A*（A-star）的算法。A*算法，作為常見的路徑檢索和路徑遍歷的通用算法，具備了較高的驅動性能和工作精度。它采用啟發函數來估計地圖上機器人當前的位置到目標位置之間的距離，然后選擇最佳方向進行搜索，若失敗將選擇其他路徑繼續搜索直到找出最佳路徑。作為Dijkstra算法的一個擴展，又借助了基于啟發式算法的方式來進行引導，A*算法因此擁有更好更高效的路徑搜索結果[2]。

A*算法同樣也是求解靜態地圖最短路徑的最有效方法，適用于各種路徑規劃場景。在每次搜索中，選擇估計函數f（n）的最小值來規劃最優路徑，經典A*算法的估計函數f（n）可表示為：f（n）=g（n）+h（n）

從起點到當前搜索點的實際距離代價的值用g（n）式表示;從當前搜索點到終點的估價求值函數用h（n）式表示。

3 ?局部路徑規劃

僅僅是獲得了整體規劃路徑還是遠遠不夠的，實際生活當中可能會出現很多突發的情況，需要將原先規劃好的路徑進行及時調整，因此，還有另一個更深層次的路徑規劃模塊，即局部路徑規劃。

3.1 ?算法初步優化

在以往的ROS局部移動規劃中，曾廣泛使用Trajectory Planner。在給定機器人的加速度極限的情況下，Trajectory Planner在整個前向模擬周期內從可實現的速度集合中進行采樣。而DWA算法在相同前提下的采樣點，僅針對一個模擬步驟中的可用速度集合。

這意味著DWA的優點是更少的樣本空間和更高的采樣效率，但又知DWA不能對常加速度做前向模擬，似乎對于是低加速度的機器人來說，可能Trajectory Rollout更好。但是在實踐中，經過多次實驗發現兩種方法性能基本相當，由此DWA算法更勝一籌。調用dwa_local_planner包即可實現DWA算法的使用，算法流程如圖2所示[3]。

圖2 ?DWA算法流程

3.2 ?DWA算法原理

DWA算法中沒有用較為常見的x軸和y軸的位置坐標系，而是用圖3中的平移速度v和旋轉速度w為軸的速度搜索空間來表示機器人。在這個存在硬件限制的空間中，機器人具有最大允許速度，這被稱為動態窗口（Dynamic Window）。

圖3 ?DWA算法示意圖

3.2.1 ?機器人的運動建模

在動態窗口算法中，為了將機器人未來局部的軌跡模擬出來，需要知道機器人的運動模型。在這里，我們假設兩輪移動機器人的軌跡為圓弧形狀，那么一對（v，w）就代表一個圓弧軌跡。它做圓弧運動的半徑為：

當旋轉速度w不等于0時，機器人坐標為：

3.2.2 ?速度采樣

DWA的第二個核心是如何采樣速度：在速度（v，w）的二維空間中有無限多組速度。然而，根據機器人的極限速度限制、部件性能和環境約束，采樣速度可以被有效地控制在一定范圍內。

3.2.3 ?評價函數

DWA結合了機器人的動力學特性，在多組速度空間中對速度進行采樣，通過采集的數據模擬軌跡，然后根據事先設定的評分規則對模擬軌跡進行評分，評分最高的軌跡作為最優路徑。然而，ROS中DWA算法的評估規則與原始DWA不同。此處評分規則綜合考慮了路徑、目標、方向角三方面。在局部導航過程中，較好的避障性能、方向性能、速度性能三者缺一不可，所以評價函數G（v，w）可以被表示為：

G（v，w）=σ（α·h（v，w）+β·d（v，w）+γ·v（v，w））

式中的h（v，w）為方向角評價函數，當機器人以當前設定的采樣速度到達模擬軌跡末端時，方向角評價函數用于評估實際末端朝向和目標之間的角度間隙。如圖4所示，如果采用180-θ的方式來評價，則θ越小，評價得分越高。式中d（v，w）為空隙評價函數，表示當前軌跡上的機器人與最近障礙物之間的最短距離。如果在此軌道上不存在任何障礙物，那么將其設置為常量。式中的v（v，w）為速度評價函數用來評價當前軌跡的速度大小。通過設置σ、α、β、γ四個常數可調整權值，從而得到最優解[4]。

圖4 ?方向角評價函數示意圖

實際操作中，通常是利用base_local_planner包實現局部路徑實時規劃的，該包融合了Trajectory Rollout和 DWA算法。具體步驟是，先離散采樣機器人的控制空間（dx，dy，dθ），再對于每個采樣的速度控制量，以機器人當前的狀態為基礎，進行向前模擬預測，即如果繼續以該速度（v，w）前進一段距離后是否會遇到障礙物等情況。然后結合仿真預測中的障礙物接近目標、接近全局路徑和速度等特征，對每條軌跡的預測結果進行評價，丟棄不合適的路徑，如有障礙物碰撞的路徑。不斷重復以上步驟，最后選擇得分最高的軌跡，并將其相應的速度控制量發布給相關的移動平臺。

3.3 ?算法再次優化

但若實驗環境稍微復雜一些，運用dwa_local_planner包進行的局部實時規劃效果不是很理想。DWA算法無法設置轉向半徑參數，因此，此算法大多時候面向的是差分或者全向機器人，它們的共同點是可以做到原地自旋轉，而另一些機器人模型則無法做到自轉，比如阿克曼模型。于是選擇加入一種較新的局部路徑規劃器teb_local_planner，它實質上是ROS導航包dwa_local_planner的插件，在線優化了本地軌跡規劃器。使用TEB算法時，機器人在運動過程中會調整自己的位姿朝向，使之與目標朝向一致。

3.4 ?TEB算法原理

TEB（Timed-Elastic-Band，時間彈性帶）優化運動軌跡的方式是修正全局路徑規劃的初始軌跡。該算法主要優化與障礙物的距離、路徑的長度和軌跡的運行時間等。改進后的機器人控制系統框圖如圖5所示。

圖5 ?帶TEB算法的機器人控制系統框圖

如圖6所示，N個帶有時間信息的離散姿態構成TEB算法生成的軌跡，然后使用G2O（general graph optimization）算法對這些姿態進行優化，使生成的軌跡路徑長度最短、耗時最快、距離障礙物最遠，并對速度和加速度進行限制，使軌跡滿足機器人運動學要求。

圖6 ?TEB算法中的配置序列和時間間隔

移動機器人的中心點坐標與偏轉方向共同決定了其在環境中的位姿。首先定義機器人位姿：

χα=（xi，yi，βi）T∈R2×S1

其中xi和yi、βi分別對應機器人在地圖坐標系的位置和姿態朝向，則空間內位姿序列可表示為：

將機器人兩個位姿間的時間間隔定義為，記錄時間序列：

接著，將位姿和時間序列合并：

B：=（Q，τ）

TEB算法以姿態和時間間隔設置為待優化變量，在動態約束的條件下求解最優路徑。這些動態約束包括速度和加速度限制、路徑長度、障礙物與機器人之間的距離以及機器人在軌跡上的運行時間等等。隨后，通過設置加權多目標函數得到最優路徑Q：

B*=argBminf（B）

為考慮各種約束后，將f（B）用作表示目標函數，fk為各個目標函數的權值。目標函數在經過G2O算法求解后，可最終得出最優結果B*，即為局部路徑點序列[5，6]。

4 ?實驗

4.1 ?仿真實驗

DWA路徑規劃仿真結果如圖7所示，仿真系統會生成一條淡灰色軌跡的全局路徑和一條深黑色軌跡的局部路徑線。在局部代價地圖中，機器人通過行進途中不斷獲取與周圍障礙物的距離，在全局路徑規劃下實時調整局部路徑規劃。

圖7 ?DWA仿真

當機器人接近障礙物時可能發生碰撞，調整權重后的局部規劃結果不再同全局路徑線重合，機器人由此不與障礙物發生碰撞。

在配置好teb_local_planner中運動軌跡參數、運動速度和旋轉角速度閾值等等和權重參數后，開始進行TEB路徑規劃仿真實驗。仿真結果如圖8和圖9所示，淺色線條為全局規劃路徑，帶若干箭頭的深色軌跡代表局部路徑，其中一個箭頭代表一個路徑點及其朝向。圖9中機器人到達障礙物附件，TEB算法會在機器人前進的同時根據全局路徑規劃不斷調整局部規劃的軌跡。

圖8 ?TEB仿真一

圖9 ?TEB仿真二

4.2 ?實物環境實驗

利用基于ROS的機器人在實驗室進行避開障礙物路徑規劃算法的比較實驗。選擇A*算法作為全局路徑規劃算法，分別使用DWA算法和TEB算法作為局部路徑規劃算法[7]，在障礙物位置、個數不變的情況下，設置相同的起始點和目的點進行重復實驗25次，途中不碰到障礙物且到達指定地點即視為成功到達，結果如表2所示。

實驗結果表明在TEB算法的局部路徑規劃下，機器人在絕大多數情況下能夠穿過障礙物最終到達指定目標點且不存在碰撞或卡死現象，整體效果優于DWA算法。綜合考慮運動中需要躲避障礙物、行駛時效、動力學約束、路徑的平滑性等因素，TEB算法在復雜的環境中具有較為優秀的實驗結果。

4.3 ?TEB算法缺陷與改進

使用TEB進行局部路徑規劃時，如果在起始點處，機器人的定位朝向不是由起始點指向目標點，那么在啟動出發前，機器人會采取倒車旋轉指令來把朝向修正到正確位置。但是，在一些場合比如狹窄空間內，倒車和旋轉角度是無法實現的。因此，若機器人構造符合兩輪差速的運動模型，可設計程序先讓底盤在原地旋轉至正確朝向，然后再進行局部路徑規劃等一系列操作，得到最優路徑。同樣的，到達終點時也有類似的問題。解決方法是在設置參數的時候將角度容差值設為360度，等到達目標點之后再由IMU獲取實際角度數據，與理想目標點方向作差得出差值向量，再在原地旋轉該值即可。

5 ?結 ?論

對于局部路徑實時規劃，DWA算法以它評價函數多元化、采集樣本高效化的優勢，目前被廣泛運用于機器人自動導航。但是通過實驗證明它仍存在著前瞻性不足、動態避障效果差等缺點。于是本文介紹了一種較新的TEB算法，引入了一個新型局部路徑規劃器teb_local_planner。綜合考慮運動中避障、軌跡執行時間、動力學約束等因素，TEB算法適用于更為復雜的環境。

參考文獻：

[1] 表允晳，趙漢哲，鄭黎蝹，林泰勛.ROS機器人編程（從基本概念到機器人應用程序編程實戰） [M].首爾：ROBOTIS Co.，Ltd.，2017：318-338.

[2] 李崢.基于ROS平臺的導航機器人路徑規劃研究 [J].現代信息科技，2020，4（14）：163-165+168.

[3] 溫淑慧，問澤藤，劉鑫，等.基于ROS的移動機器人自主建圖與路徑規劃 [J].沈陽工業大學學報，2022，44（1）：90-94.

[4] 孫小飛.基于ROS的自動分揀機器人系統設計與實驗研究 [D].重慶：重慶大學，2020.

[5] 高佳佳.基于全局地圖的移動機器人路徑規劃研究 [D].西安：西安工業大學，2019.

[6] 郭柱梁.基于激光雷達的SLAM和動態避障機器人研制 [D].黑龍江：黑龍江大學，2021.

[7] 劉建娟，薛禮啟，張會娟，等.融合改進A*與DWA算法的機器人動態路徑規劃 [J].計算機工程與應用，2021，57（15）：73-81.

作者簡介：過佳穎（2001—），女，漢族，江蘇無錫人，本科在讀，研究方向：電子信息科學與技術。