朱元潤 周新智 胡雄

摘要：無人車路徑規(guī)劃與自主避障算法一直以來是無人駕駛技術(shù)研究的重要方向。通過改變啟發(fā)式函數(shù)，以及將距離計算方法改為考慮障礙物的實際距離，利用G2O圖優(yōu)化框架等方法來改進(jìn)A*算法、TEB算法，同時結(jié)合ROS Navigation框架和仿真平臺，探究了無人車路徑規(guī)劃算法與自主避障算法。研究結(jié)論不僅為全局路徑規(guī)劃和局部路徑規(guī)劃的自主避障提供了寶貴的參考，也為無人車的自主導(dǎo)航開辟了新的研究方向。

關(guān)鍵詞：路徑規(guī)劃；自主避障；融合；自主導(dǎo)航

中圖分類號：U469? 收稿日期：2024-01-05

DOI：1019999/jcnki1004-0226202405009

1 前言

隨著科技的發(fā)展，無人駕駛技術(shù)逐漸成為汽車行業(yè)的熱門話題。路徑規(guī)劃技術(shù)與自主避障算法在地面無人車自主導(dǎo)航技術(shù)中起著舉足輕重的作用， 它在地面無人車各個領(lǐng)域的應(yīng)用中都具有廣闊的發(fā)展空間和研究價值，已成為地面無人車研究的必然趨勢，雖然目前有許多學(xué)者提出各種各樣的算法來分析、解決這個問題，但是行之有效的方法并不多[1]。本文在改進(jìn)A*算法、TEB算法的基礎(chǔ)上，結(jié)合ROS Navigation框架及仿真平臺，深入探究無人車路徑規(guī)劃和自主避障。其目標(biāo)是實現(xiàn)無人車的自主導(dǎo)航，并為其提供實用的參考建議。

2 基于改進(jìn)A*算法的全局路徑規(guī)劃算法

全局規(guī)劃路徑是指在特定的地圖環(huán)境下，系統(tǒng)尋找從起始點到終點的最佳路徑。全局規(guī)劃路徑的研究在機(jī)器人技術(shù)、自動駕駛等領(lǐng)域具有舉足輕重的地位，能極大地提升機(jī)器和車輛在復(fù)雜環(huán)境中的導(dǎo)航和操作能力，對于推動相關(guān)產(chǎn)業(yè)的發(fā)展和技術(shù)進(jìn)步具有深遠(yuǎn)的影響。目前，全局規(guī)劃路徑研究的方法有很多，其中A*算法是最常用的一種[2]。在傳統(tǒng)的A*算法中，啟發(fā)式函數(shù)只考慮了起點到當(dāng)前點的代價，而沒有考慮當(dāng)前點到終點的代價。因此，改進(jìn)A*算法應(yīng)運(yùn)而生。本文將介紹一種基于改進(jìn)A*算法的全局規(guī)劃路徑方法，提高了路徑規(guī)劃的精度和速度。

21 基于代價函數(shù)的改進(jìn)

改進(jìn)A*算法的啟發(fā)式函數(shù)如下：

f（n）=g（n）+h（n）? ? ? ? ? ? ? ? ? ? ? ? ? ? （1）

改進(jìn)A*算法的基本思路和傳統(tǒng)A*算法一樣，只是啟發(fā)式函數(shù)發(fā)生了改變。改進(jìn)A*算法的優(yōu)點在于準(zhǔn)確地估計每個節(jié)點到終點的距離，從而更加精確地計了每個節(jié)點代價。

22 距離函數(shù)的改進(jìn)

歐幾里得距離函數(shù)[3]是一種常用的距離度量方法，通常用于全局路徑規(guī)劃中的A*算法。但是，在實際應(yīng)用中，歐幾里得距離函數(shù)有時會導(dǎo)致路徑規(guī)劃的效果不佳。

在歐幾里得距離函數(shù)中，兩個節(jié)點的距離被定義為它們在坐標(biāo)系中的直線距離。這種距離計算方法受到障礙物的影響較大，因此會導(dǎo)致A*算法在處理復(fù)雜環(huán)境下的路徑規(guī)劃問題時效果不佳。

為了解決這個問題，可以對歐幾里得距離函數(shù)進(jìn)行改進(jìn)。一種常見的方法是引入障礙物的影響因素，將距離計算方法改為考慮障礙物的實際距離，以此來更準(zhǔn)確地評估節(jié)點之間的距離。

通過查表的方式得到近似歐幾里得距離，如表1所示。

3 局部路徑規(guī)劃算法

31 基于時間彈性帶算法的局部路徑規(guī)劃

時間彈性帶算法（Temporal Elastic Bands，TEB）是一種常用的局部路徑規(guī)劃方法。該算法通過將機(jī)器人的運(yùn)動時間劃分為離散的時間步長，并在每個時間步長上計算機(jī)器人的可行運(yùn)動空間，從而生成可行的軌跡。TEB算法是一種簡單易用的局部路徑規(guī)劃方法，它能夠快速地生成機(jī)器人的可行軌跡，并考慮機(jī)器人的約束條件和優(yōu)先級[4]。因此，在許多機(jī)器人應(yīng)用中，TEB算法被廣泛應(yīng)用。

32 TEB的求解

TEB算法主要應(yīng)用非線性優(yōu)化算法來解決路徑規(guī)劃問題，如Levenberg-Marquardt算法、高斯-牛頓算法和擬牛頓法等。這些算法可以通過求解一個最小化誤差的優(yōu)化問題來獲得最優(yōu)路徑。TEB算法利用G2O圖優(yōu)化框架，定義頂點表和邊表，將地面無人車位姿、相鄰位姿間的時間間隔以及障礙物作為頂點，并將目標(biāo)函數(shù)作為邊，構(gòu)建出超圖（Hyper-graph），最終借助G2O框架中帶有的求解器完成求解過程，進(jìn)而得到優(yōu)化后的位置序列B*：=（Q，T）[5]。

4 混合路徑規(guī)劃仿真

41 仿真實驗平臺的搭建

構(gòu)建仿真實驗平臺的必要條件和技術(shù)要求如下：

a.車輛模型：采用妥當(dāng)?shù)能囕v模型對車輛的動力學(xué)特性和控制系統(tǒng)進(jìn)行準(zhǔn)確描述，可以使用MATLAB、Simulink、ADAMS等軟件進(jìn)行建模和仿真。

b.環(huán)境模擬：需要模擬車輛所處的環(huán)境，包括地圖、道路、障礙物等，可以使用Unity、Unreal Engine等游戲引擎進(jìn)行建模和仿真。

c.傳感器模擬：需要模擬車輛使用的傳感器進(jìn)行模擬，包括激光雷達(dá)、相機(jī)、超聲波傳感器等，可以利用ROS（機(jī)器人操作系統(tǒng)）進(jìn)行傳感器數(shù)據(jù)的模擬和處理。

d.控制系統(tǒng)：需要對車輛的路徑跟蹤、速度控制、避障等功能進(jìn)行設(shè)計，可以使用PID控制器、狀態(tài)反饋控制器等控制算法進(jìn)行設(shè)計和實現(xiàn)。

42 自主導(dǎo)航系統(tǒng)的設(shè)計

a.系統(tǒng)功能與需求。

自主導(dǎo)航系統(tǒng)是無人車實現(xiàn)自主行駛的核心，具有路徑規(guī)劃、自主避障、定位與控制等多種功能，其設(shè)計需滿足以下需求：

高精度定位：實時獲取無人車當(dāng)前位置，提供精確的導(dǎo)航坐標(biāo)。

快速響應(yīng)：具備快速響應(yīng)輸入指令的能力，保證無人車能夠及時響應(yīng)外部指令。

穩(wěn)定可靠：保證無人車穩(wěn)定性和安全性，避免出現(xiàn)意外情況。

高效運(yùn)行：能夠?qū)崿F(xiàn)高效的路徑規(guī)劃和自主避障，提供高速、穩(wěn)定的自主行駛能力。

b.硬件系統(tǒng)設(shè)計。

①無人車平臺選擇。

根據(jù)系統(tǒng)需求，無人車平臺應(yīng)具備較高的承載能力、良好的穩(wěn)定性和精準(zhǔn)的定位能力。在實際應(yīng)用中，常用的無人車平臺有小車底盤、四旋翼無人機(jī)等。本文選取小車底盤作為無人車平臺，其具有穩(wěn)定性好、載重能力強(qiáng)等優(yōu)點，便于實現(xiàn)路徑規(guī)劃和自主避障功能。

②傳感器系統(tǒng)設(shè)計與實現(xiàn)。

為了達(dá)成高精度定位以及自主避障功能，無人車需要搭載包括激光雷達(dá)、攝像頭、慣性導(dǎo)航儀等多種傳感器。激光雷達(dá)主要用于障礙物檢測和距離測量。在無人車行駛過程中，激光雷達(dá)通過掃描周圍環(huán)境，檢測出前方障礙物的位置和距離。本文選取Hokuyo URG-04LX-UG01型激光雷達(dá)，其具有測距范圍廣、數(shù)據(jù)穩(wěn)定等優(yōu)點。

攝像頭主要用于目標(biāo)檢測和路標(biāo)識別。在無人車行駛過程中，攝像頭通過拍攝周圍環(huán)境，識別出行駛路徑和路標(biāo)。本文選取Logitech C920 HD Pro型攝像頭，其具有高清晰度、光線適應(yīng)性好等優(yōu)點。慣性導(dǎo)航儀主要用于確定無人車運(yùn)動狀態(tài)，包括位置、速度和方向等。在無人車行駛過程中，慣性導(dǎo)航儀通過感知周圍環(huán)境的變化，確定無人車當(dāng)前狀態(tài)，提供定位信息。本文選取Xsens MTi-680型慣性導(dǎo)航儀，其具有高精度、穩(wěn)定性好等優(yōu)點。

③控制系統(tǒng)設(shè)計與實現(xiàn)。

為實現(xiàn)無人車的自主行駛能力，需要設(shè)計有效的控制系統(tǒng)，并進(jìn)行實現(xiàn)和測試。無人車的控制系統(tǒng)可以分為兩個層次，即底層控制和高層控制。底層控制主要負(fù)責(zé)無人車的運(yùn)動控制，包括電機(jī)控制、輪子轉(zhuǎn)速控制等。高層控制主要涉及路徑規(guī)劃、目標(biāo)識別等任務(wù)。本文選取Arduino Mega 2560作為底層控制器，其具有良好的擴(kuò)展性和穩(wěn)定性，支持多種通信協(xié)議[6]。高層控制主要負(fù)責(zé)無人車的路徑規(guī)劃和自主避障，包括路線規(guī)劃、避障決策等。本文采用ROS（Robot Operating System）作為高層控制平臺，其具有模塊化、跨平臺等優(yōu)點，便于進(jìn)行路徑規(guī)劃和自主避障算法的開發(fā)和實現(xiàn)。

43 全局路徑規(guī)劃模塊

本研究將采用改進(jìn)后的A*算法來替代原有算法，全局路徑規(guī)劃的流程圖如圖1所示。

44 局部路徑規(guī)劃模塊

本文研究使用改進(jìn)后的TEB算法對原框架中的動態(tài)窗口法進(jìn)行替換，如圖2所示的局部路徑規(guī)劃流程圖。

5 結(jié)語

本文通過改進(jìn)A*算法、TEB算法與ROS Navigation框架及仿真平臺，得出以下結(jié)論：

a.通過引入節(jié)點到終點的代價估計，路徑規(guī)劃的精度和速度得到提高，改進(jìn)A*算法的路徑規(guī)劃精度比傳統(tǒng)A*算法更高，搜索速度更快。該方法在機(jī)器人、自動駕駛汽車等領(lǐng)域具有重要的應(yīng)用價值。

b.改進(jìn)TEB算法能夠更快速地生成機(jī)器人的可行軌跡，并考慮機(jī)器人的約束條件和優(yōu)先級。

c.按照混合路徑規(guī)劃算法的運(yùn)行效果，驗證了其有效性。

以上結(jié)論為無人車路徑規(guī)劃與自主避障算法的持續(xù)改進(jìn)提供了指導(dǎo)，為全局路徑規(guī)劃以及局部路徑規(guī)劃兩者自主避障的實現(xiàn)提供了參考價值。

參考文獻(xiàn)：

[1]郭彥超無人車路徑規(guī)劃與自主避障算法研究[D]北京：北京交通大學(xué)，2021

[2]賈明超，馮斌，吳鵬，等一種融合改進(jìn)A*算法與改進(jìn)動態(tài)窗口法的文旅服務(wù)機(jī)器人路徑規(guī)劃[J]圖學(xué)學(xué)報，2024（1）：1-13

[3]謝永平基于ROS的無人配送車導(dǎo)航算法研究[D]廣州：廣東技術(shù)師范大學(xué)，2020

[4]郭志軍，尹亞昆，李亦軒，等融合改進(jìn)A～*和TEB算法的移動機(jī)器人路徑規(guī)劃[J]河南科技大學(xué)學(xué)報（自然科學(xué)版），2023，44（4）：57-65+7

[5]王月室內(nèi)AGV定位與路徑規(guī)劃算法研究[D]淮南：安徽理工大學(xué)，2022

[6]王瑜，白重陽，鄭帥勇，等基于Arduino Mega2560的指紋鎖設(shè)計[J]國外電子測量技術(shù)，2020，39（3）：109-113

作者簡介：

朱元潤，女，2001年生，本科在讀，研究方向為自動駕駛汽車。

周新智（通訊作者），男，2003年生，本科在讀，研究方向為新能源汽車。