李 剛，金鴻耀，冀同濤，張旭斌，任建平

一種無人駕駛賽車路徑規(guī)劃算法研究

李 剛，金鴻耀，冀同濤，張旭斌，任建平

（遼寧工業(yè)大學(xué) 汽車與交通工程學(xué)院，遼寧 錦州 121001）

針對無人駕駛方程式比賽，設(shè)計一種適合無人駕駛賽車應(yīng)用的局部路徑規(guī)劃算法。該算法利用四邊形重心求解方法確定賽車在錐桶賽道環(huán)境下需要的路徑離散點(diǎn)，并采用三次樣條插值方法擬合曲線，從而實(shí)現(xiàn)賽車局部路徑規(guī)劃功能。在ROS系統(tǒng)中進(jìn)行對無人駕駛賽車的路徑規(guī)劃仿真。結(jié)果表明：該算法能使賽車在不規(guī)則賽道中實(shí)現(xiàn)局部路徑規(guī)劃功能且曲線較為平滑，可以使賽車安全可靠地通過賽道，具有較好的魯棒性。

無人駕駛；路徑規(guī)劃；三次樣條插值

隨著汽車的普及程度不斷提高，我國在快速步入汽車社會的同時，也出現(xiàn)了一系列的新問題和新挑戰(zhàn)。而此時隨著計算機(jī)技術(shù)和互聯(lián)網(wǎng)技術(shù)的迅速發(fā)展，無人駕駛技術(shù)也開始出現(xiàn)在人們的視野中。無人駕駛技術(shù)主要包括環(huán)境感知，路徑規(guī)劃與決策控制等。路徑規(guī)劃是無人駕駛車輛環(huán)境感知和決策控制的中間橋梁，是汽車實(shí)現(xiàn)自動駕駛的基礎(chǔ)。目前，路徑規(guī)劃的方法可根據(jù)不同的特點(diǎn)分為傳統(tǒng)算法、智能優(yōu)化算法和基于強(qiáng)化學(xué)習(xí)的算法。傳統(tǒng)算法是較為基本的算法，傳統(tǒng)算法可以分為基于圖搜索的路徑規(guī)劃算法、基于采樣的路徑規(guī)劃算法和基于最優(yōu)化曲線的路徑規(guī)劃算法?；趫D搜索的路徑規(guī)劃算法主要有Dijkstra算法[1]、A*算法[2]?；诓蓸拥穆窂揭?guī)劃算法是通過均勻隨機(jī)采樣的方法來尋找狀態(tài)空間的連通性，其中最典型的是RRT算法[3]?；谧顑?yōu)化曲線生成的路徑規(guī)劃算法主要應(yīng)用于局部路徑規(guī)劃。目前，比較常用的高階曲線模型包括多項(xiàng)式曲線、貝塞爾曲線等[4]。在實(shí)際應(yīng)用中，為了保證所規(guī)劃的路徑具有較好的平滑性和可控性，生成的曲線至少二階連續(xù)可導(dǎo)，因此采用的高階曲線模型應(yīng)該至少是3次的。智能優(yōu)化算法是通過模擬生物的行為規(guī)律實(shí)現(xiàn)優(yōu)化路徑的目的。目前，典型的智能優(yōu)化算法包括蟻群算法[5]、觸須算法[6]和智能水滴算法[7]等。蟻群算法是通過迭代的方式來模擬螞蟻的覓食行為，從而實(shí)現(xiàn)最優(yōu)路徑的選擇。該算法具有較好的魯棒性，但存在計算量較大，計算過程中易陷入局部最優(yōu)解等問題。觸須算法是一種簡單易實(shí)現(xiàn)的新興智能仿生算法，其中觸須是指無人駕駛汽車的預(yù)計行駛軌跡，每一條觸須都是一條圓弧，用于感知障礙物和規(guī)劃最優(yōu)行駛路徑?；趶?qiáng)化學(xué)習(xí)的算法是指無人駕駛汽車通過傳感器不斷與外部環(huán)境相互作用獲取位置環(huán)境的知識。強(qiáng)化學(xué)習(xí)具有可以和環(huán)境交互進(jìn)行在線學(xué)習(xí)的優(yōu)勢，在學(xué)習(xí)中不斷改進(jìn)路徑方案。常用的強(qiáng)化學(xué)習(xí)算法主要包括順勢差分法和Q-learning算法[8]。Q-learning算法與環(huán)境模型無關(guān)，而且十分有效，具有在線學(xué)習(xí)的特點(diǎn)。強(qiáng)化學(xué)習(xí)在動態(tài)環(huán)境的路徑規(guī)劃中應(yīng)用十分廣泛，但如何加快算法收斂速度，提升學(xué)習(xí)能力一直是研究的難點(diǎn)。

路徑規(guī)劃的方法很多，但不同的方法應(yīng)用到的場景也不盡相同，基于對不同路徑規(guī)劃方法的特點(diǎn)以及賽道特性的分析，本文設(shè)計了一種適合無人駕駛賽車行駛的局部規(guī)劃算法，并對該算法進(jìn)行了仿真驗(yàn)證。

1 三次樣條插值曲線擬合原理

若路徑起點(diǎn)和終點(diǎn)已知，那么路徑生成的基本要求是能夠通過車輛的控制跟蹤這條路徑，一個簡化的處理方法就是能保證路徑的連續(xù)性和平滑性。采用函數(shù)擬合方式簡便有效，最簡單的方法是使用直線將離散點(diǎn)連接起來，但這樣擬合出來的路徑不夠光滑。而三次曲線的效果要好很多，但對比簡單的三次多項(xiàng)式擬合算法連接方式，三次樣條插值方法擬合出來的曲線更加符合車輛運(yùn)動實(shí)況，并且更加平滑。采用三次多項(xiàng)式的連接方式會在直線段處產(chǎn)生曲線弧度，不是期望的車輛運(yùn)動狀態(tài)，車輛在行駛時應(yīng)該盡可能地走直線，只是在轉(zhuǎn)彎方向的地方形成一個符合車輛轉(zhuǎn)彎特性的曲線路徑，而三次樣條插值算法有以下特質(zhì)，符合期望的汽車運(yùn)動狀態(tài)以及汽車運(yùn)動學(xué)規(guī)律。

（1）三次樣條曲線在銜接點(diǎn)處是光滑連續(xù)的； （2）三次樣條的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)是連續(xù)可導(dǎo)的；（3）自由邊界三次樣條的邊界二階導(dǎo)數(shù)是連續(xù)的；（4）單個點(diǎn)不會影響到整個插值曲線。

假定有3個點(diǎn)需要擬合，3個點(diǎn)集表示為：

那么，可以用一個三次函數(shù)來擬合1和2，另一個三次函數(shù)來擬合2和3，分別為：

將1,2和3帶入公式(2)和公式(3)，可得：

由于樣條曲線在銜接點(diǎn)處的導(dǎo)數(shù)連續(xù)，2個三次函數(shù)在2處的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)相等，對兩邊求一階導(dǎo)數(shù)和二階導(dǎo)數(shù)可得：

由于三次樣條是自由邊界，要求在起點(diǎn)和終點(diǎn)的二階導(dǎo)數(shù)也是連續(xù)的，所以：

綜合以上8個方程組，通過計算可以確定兩段三次樣條的多項(xiàng)式系數(shù)(,,,,,,,)。

其中,a,b,c,d為待定常數(shù)，這些常數(shù)可以由2個插值點(diǎn)條件，2-2個銜接點(diǎn)條件以及2個自由邊界條件來確定。

2 無人駕駛賽車路徑規(guī)劃算法設(shè)計

由于賽車需要識別的是錐桶，而錐桶起到的是引領(lǐng)路線的作用。行駛過程中并沒有障礙物，局部規(guī)劃的目的是使賽車能夠通過賽道的兩側(cè)錐桶，因此選取基于最優(yōu)化曲線的局部路徑規(guī)劃方法。賽車整個算法的設(shè)計思路是針對不規(guī)則賽道特性，由激光雷達(dá)獲取賽道錐桶位置信息，通過對錐桶位置的分析，確定路徑所要經(jīng)過的離散點(diǎn)，基于三次樣條插值算法計算分段多項(xiàng)式函數(shù)，得到所需路徑。

設(shè)計步驟如圖1所示。

圖1 算法設(shè)計步驟

（1）獲取錐桶位置并排序。

經(jīng)過對激光雷達(dá)得到的原始點(diǎn)云進(jìn)行濾波處理后，得到各個錐桶中心點(diǎn)，并對它們按照距離排序，得到處理后的點(diǎn)云。

（2）尋找離散點(diǎn)。

由于賽道中的錐桶并不是對應(yīng)的，且擺放位置不規(guī)則，具有隨意性，所以確定擬合曲線所需的離散點(diǎn)并不容易，這里采用確定四邊形重心的方法來尋找離散點(diǎn)。從排好序的錐桶點(diǎn)集(0,1,2,…,s)中取最近4個點(diǎn)(0,1,2,3)，這4個點(diǎn)可以組成一個四邊形，取四邊形的重心作為離散點(diǎn)，而四邊形的重心基本處于四邊形的中心位置，可以滿足對路徑的要求，得到離散點(diǎn)后對該點(diǎn)檢驗(yàn)，若該點(diǎn)與錐桶點(diǎn)的距離符合要求，將該點(diǎn)納入離散點(diǎn)集中。同理，再尋找(1,2,3,4)4個點(diǎn)組成的四邊形的重心，重復(fù)這個過程，直到將所有的錐桶點(diǎn)集遍歷完。最終確定擬合曲線所需的離散點(diǎn)。

四邊形重心確定方法：連接四邊形ABCD的一條對角線AC，將四邊形ABCD分成2個三角形ABC和ACD，求出2個三角形的重心E和F并連接成線段EF。同理，連接另一條對角線BD，得到另外兩個三角形重心G和H所連接的線段GH，而四邊形的重心即兩條線段的交點(diǎn)O。

四邊形重心原理如圖2所示。

圖2 四邊形重心原理圖

（3）檢驗(yàn)離散點(diǎn)。

在尋找完離散點(diǎn)后需要進(jìn)行檢驗(yàn)，將該離散點(diǎn)到不規(guī)則四邊形頂點(diǎn)距離進(jìn)行排序，計算該離散點(diǎn)到不規(guī)則四邊形頂點(diǎn)的最大距離與最小距離的差值、最大距離與次大距離差值1，次小距離與最小距離距離差值2，若小于閥值，1小于1，2小于2三者同時滿足，該離散點(diǎn)為擬合曲線所需要的離散點(diǎn)。

（4）擬合曲線。

在確定好離散點(diǎn)后，采用三次樣條插值算法得到分段多項(xiàng)式。在設(shè)計過程中，考慮過長的局部路徑規(guī)劃意義不大且激光雷達(dá)在遠(yuǎn)距離的識別過程中濾波效果不太好，可能會有雜亂點(diǎn)，對離散點(diǎn)有干擾，所以只對計算出來的最近兩個離散點(diǎn)以及激光雷達(dá)自身點(diǎn)這3個點(diǎn)進(jìn)行三次樣條插值曲線擬合。根據(jù)插值點(diǎn)條件，銜接點(diǎn)條件和邊界條件可以求出對應(yīng)的參數(shù)，得到三次樣條曲線。

算法框架如圖3所示。

圖3 路徑規(guī)劃算法框圖

3 無人駕駛賽車介紹

用于實(shí)驗(yàn)的無人駕駛賽車是由純電動賽車改裝而成的，主要加裝了以下幾個部分：

（1）Spatial NAV100組合導(dǎo)航設(shè)備：用于測量車輛狀態(tài)參數(shù)（加速度、橫擺角速度等）以及位置信息（經(jīng)緯度）；（2）環(huán)境感知傳感器：包括禾賽40線激光雷達(dá)和大恒雙目相機(jī)，用于檢測識別賽道錐桶位置和顏色；（3）研華工控機(jī)：用于運(yùn)行環(huán)境感知、路徑規(guī)劃以及跟蹤算法；（4）華?？焖僭涂刂破鳎河糜诮邮丈蠈拥目刂菩盘柌鬟f給執(zhí)行機(jī)構(gòu)；（5）EPS助力轉(zhuǎn)向電機(jī)和制動舵機(jī)：用于實(shí)現(xiàn)線控轉(zhuǎn)向和線控制動。整個系統(tǒng)通訊架構(gòu)如圖4。

圖4 無人駕駛賽車系統(tǒng)通訊架構(gòu)

無人駕駛賽車系統(tǒng)的整個系統(tǒng)通訊架構(gòu)主要分為上層通訊與底層通訊：其中上層通訊中，激光雷達(dá)和雙目相機(jī)是通過網(wǎng)口通信的方式與工控機(jī)進(jìn)行通訊，組合慣導(dǎo)是通過串口通信的方式與工控機(jī)進(jìn)行通訊，工控機(jī)與快速原型控制器通過CAN通信進(jìn)行數(shù)據(jù)通訊。在底層通訊中，而快速原型控制器與驅(qū)動電機(jī)、轉(zhuǎn)向電機(jī)的通訊方式為CAN通信，與制動電機(jī)的通訊方式為PWM通信。

無人駕駛賽車整車系統(tǒng)分為兩部分，分別為軟件開發(fā)系統(tǒng)和底層控制系統(tǒng)。軟件開發(fā)系統(tǒng)基于ROS進(jìn)行傳感器數(shù)據(jù)處理以及規(guī)劃跟蹤等功能開發(fā)。無人駕駛賽車基于ROS系統(tǒng)進(jìn)行各傳感器（組合慣導(dǎo)、激光雷達(dá)以及相機(jī)）數(shù)據(jù)的采集、處理，數(shù)據(jù)融合等過程，同時路徑規(guī)劃算法的設(shè)計也是基于ROS系統(tǒng)，這種分布式處理的特點(diǎn)可以降低代碼復(fù)雜性，各可執(zhí)行文件運(yùn)行松散耦合，為數(shù)據(jù)處理減輕了負(fù)擔(dān)，在一定程度上增強(qiáng)了系統(tǒng)實(shí)時性。而底層控制系統(tǒng)基于快速原型控制器RapidECU_S1實(shí)現(xiàn)對賽車驅(qū)動、制動和轉(zhuǎn)向等方面的控制。該控制器可以很好地用作純電動賽車整車控制器，基于模型開發(fā)并且在開發(fā)過程中隨時保持模型和代碼的同步狀態(tài)，底層系統(tǒng)成熟穩(wěn)定，包括實(shí)時操作系統(tǒng)、各種汽車級I/O信號調(diào)理、功率驅(qū)動、總線通信、標(biāo)定協(xié)議、引導(dǎo)程序、內(nèi)存管理等功能，可以很好地實(shí)現(xiàn)對賽車的有效控制。

4 實(shí)車實(shí)驗(yàn)

為驗(yàn)證所設(shè)計算法的實(shí)用性，以無人駕駛賽車為載體進(jìn)行了實(shí)車實(shí)驗(yàn)。

圖5為賽車在實(shí)車實(shí)驗(yàn)時的準(zhǔn)備狀態(tài)。在賽道未知的情況下，賽車在行駛過程中識別錐桶，進(jìn)行局部路徑規(guī)劃，該方式下的坐標(biāo)系為激光雷達(dá)坐標(biāo)系或者車體坐標(biāo)系，采用純追蹤算法進(jìn)行路徑跟蹤。預(yù)瞄距離設(shè)置為3 m，賽車驅(qū)動電機(jī)轉(zhuǎn)矩設(shè)置為1 500 Nm。賽車在跑動中識別錐桶并進(jìn)行局部規(guī)劃，確定行駛路徑，并沿參考路徑行駛，同時建立賽道地圖。賽車在實(shí)車實(shí)驗(yàn)過程中實(shí)時路徑規(guī)劃效果如圖6和圖7所示。

圖5 賽車準(zhǔn)備跑動圖

圖6 左彎道局部規(guī)劃圖

圖7 右彎道局部規(guī)劃圖

圖6和圖7為賽車實(shí)時局部規(guī)劃的效果圖，圖8為賽車建立賽道地圖的效果圖。實(shí)車實(shí)驗(yàn)表明所設(shè)計的算法可以實(shí)現(xiàn)賽車在不規(guī)則未知賽道中的路徑規(guī)劃功能，規(guī)劃出來的曲線較為平滑，可以使賽車安全可靠地通過賽道。

圖8 賽車建立賽道效果圖

5 結(jié)論

（1）本文針對賽道特性，通過激光雷達(dá)識別賽道錐桶，基于錐桶位置信息設(shè)計了一種適合無人駕駛賽車的局部路徑規(guī)劃方法。

（2）該方法不用通過相機(jī)等傳感器信息區(qū)分兩側(cè)賽道錐桶，只依靠錐桶位置信息實(shí)現(xiàn)局部路徑規(guī)劃功能，且曲線連續(xù)光滑，基本處于賽道中間，可使賽車安全平穩(wěn)地通過錐桶賽道。該算法有較好的魯棒性，對于無人駕駛車輛路徑規(guī)劃研究有一定的助力。

[1] Dijkstra E W. A note on two problems in connexion with graphs[J]. Numerische Mathematik, 1959, 1(1): 269-271.

[2] Hart P E, Nilsson N J, Raphael B. A formal basis for the heuristic determination of minimum cost paths[J]. IEEE Transactions on Systems Science and Cybernetics, 1968, 4(2): 100-107.

[3] 成怡, 王赟, 修春波. 一種改進(jìn)RRT算法在路徑規(guī)劃中的應(yīng)用研究[J]. 控制工程, 2020, 27(3): 567-571.

[4] 高嵩, 張金煒, 戎輝, 等. 基于貝塞爾曲線的無人車局部避障應(yīng)用[J]. 現(xiàn)代電子技術(shù), 2019, 42(9): 163-166.

[5]孫瑞, 張文勝. 基于改進(jìn)蟻群算法的移動機(jī)器人平滑路徑規(guī)劃[J]. 圖學(xué)學(xué)報, 2019, 40(2): 344-350.

[6] 張明環(huán), 張科. 智能車避障觸須算法中的障礙物探測研究[J]. 西北工業(yè)大學(xué)學(xué)報, 2012, 30(5): 763-767.

[7] Shah-Hosseini H. Optimization with the nature-inspired intelligent water drops algorithm[J]. International Journal of Bio-Inspired Computation, 2009, 1(1/2): 71-79.

[8] Watkins C J C H, Dayan P. Q-learning[J]. Machine Learning, 1992, 8(3/4): 279-292.

Research on a Path Planning Algorithm for Autonomous Race Cars

LI Gang, JIN Hong-yao, JI Tong-tao, ZHANG Xu-bin, REN Jian-ping

（School of Automobile and Traffic Engineering, Liaoning University of Technology, Jinzhou 121001, China）

Aiming at the driverless formula competition, a local path planning algorithm suitable for autonomous race applications is designed. This algorithm uses the quadrilateral gravity center solution method to determine the discrete points of the path required by the race car in the cone barrel track environment, and uses the cubic spline interpolation method to fit the curve to achieve the local path planning function of the race car. The path planning simulation of the autonomous race car is performed in the ROS system. The simulation results show that the algorithm can enable the race car to realize the local path planning function on irregular tracks, and the curve is relatively smooth, which can make the race car pass the track safely and reliably. The algorithm has better robustness.

autonomous driving; path planning; cubic spline interpolation

U469.72

A

1674-3261(2021)04-0228-04

10.15916/j.issn1674-3261.2021.04.004

2020-06-10

國家自然科學(xué)基金面上項(xiàng)目（51675257）；遼寧省“百千萬人才工程”培養(yǎng)經(jīng)費(fèi)資助

李剛(1979-)，男，遼寧朝陽人，教授，博士。

責(zé)任編校：劉亞兵