摘 ?要：智能機器人的基礎(chǔ)功能之一就是自主導(dǎo)航，包括了地圖構(gòu)建和路徑規(guī)劃?；陂_源操作系統(tǒng)ROS的機器人，運用啟發(fā)式搜索A*算法進行全局路徑規(guī)劃。對機器人局部路徑規(guī)劃進行了集中研究，采用DWA算法進行局部實時路徑規(guī)劃，加入基于彈性時間帶碰撞TEB算法，優(yōu)化機器人位置和時間間隔變量，通過加權(quán)多目標(biāo)化獲取最優(yōu)的路徑規(guī)劃能力。實驗結(jié)果證實了TEB算法的規(guī)劃效果明顯優(yōu)于DWA算法，為移動機器人自動導(dǎo)航功能的優(yōu)化提供了思路。

關(guān)鍵詞：ROS;導(dǎo)航機器人;TEB算法;DWA算法;路徑規(guī)劃優(yōu)化

中圖分類號：TP242 ? ? ? 文獻標(biāo)識碼：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 ?引 ?言

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

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

1 ?自主導(dǎo)航

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

1.1 ?ROS操作系統(tǒng)

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

1.2 ?SLAM建圖原理

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

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

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

圖1 ?障礙距離與成本地圖值的關(guān)系

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

2 ?全局路徑規(guī)劃

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

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

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

3 ?局部路徑規(guī)劃

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

3.1 ?算法初步優(yōu)化

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

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

圖2 ?DWA算法流程

3.2 ?DWA算法原理

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

圖3 ?DWA算法示意圖

3.2.1 ?機器人的運動建模

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

當(dāng)旋轉(zhuǎn)速度w不等于0時，機器人坐標(biāo)為：

3.2.2 ?速度采樣

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

3.2.3 ?評價函數(shù)

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

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

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

圖4 ?方向角評價函數(shù)示意圖

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

3.3 ?算法再次優(yōu)化

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

3.4 ?TEB算法原理

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

圖5 ?帶TEB算法的機器人控制系統(tǒng)框圖

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

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

移動機器人的中心點坐標(biāo)與偏轉(zhuǎn)方向共同決定了其在環(huán)境中的位姿。首先定義機器人位姿：

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

其中xi和yi、βi分別對應(yīng)機器人在地圖坐標(biāo)系的位置和姿態(tài)朝向，則空間內(nèi)位姿序列可表示為：

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

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

B：=（Q，τ）

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

B*=argBminf（B）

為考慮各種約束后，將f（B）用作表示目標(biāo)函數(shù)，fk為各個目標(biāo)函數(shù)的權(quán)值。目標(biāo)函數(shù)在經(jīng)過G2O算法求解后，可最終得出最優(yōu)結(jié)果B*，即為局部路徑點序列[5，6]。

4 ?實驗

4.1 ?仿真實驗

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

圖7 ?DWA仿真

當(dāng)機器人接近障礙物時可能發(fā)生碰撞，調(diào)整權(quán)重后的局部規(guī)劃結(jié)果不再同全局路徑線重合，機器人由此不與障礙物發(fā)生碰撞。

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

圖8 ?TEB仿真一

圖9 ?TEB仿真二

4.2 ?實物環(huán)境實驗

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

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

4.3 ?TEB算法缺陷與改進

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

5 ?結(jié) ?論

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

參考文獻：

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

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

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

[4] 孫小飛.基于ROS的自動分揀機器人系統(tǒng)設(shè)計與實驗研究 [D].重慶：重慶大學(xué)，2020.

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

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

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

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