基于蟻群四次貝塞爾曲線的無人車路徑規(guī)劃

張金煒 王文揚(yáng) 郭蓬 高嵩

摘 ?要： 為了實(shí)現(xiàn)在障礙物環(huán)境下規(guī)劃出符合無人車行駛曲率并安全避障的路徑，提出一種蟻群算法與四次貝塞爾曲線融合的路徑規(guī)劃方法。首先在柵格地圖下利用蟻群算法規(guī)劃出從起始點(diǎn)到目標(biāo)點(diǎn)的全局最短路徑;然后將路徑分割成有限個(gè)位置點(diǎn)，在每兩個(gè)點(diǎn)之間生成具有行駛曲率、避障安全距離的貝塞爾曲線;最后形成一條連續(xù)的無人車行駛路徑。經(jīng)過仿真驗(yàn)證，證明了此方法的有效性和可行性。

關(guān)鍵詞： 路徑規(guī)劃; 蟻群算法; 貝塞爾曲線; 行駛曲率; 避障; 安全距離

中圖分類號(hào)： TN911.1?34 ? ? ? ? ? ? ? ? ? ? ? ?文獻(xiàn)標(biāo)識(shí)碼： A ? ? ? ? ? ? ? ? ? ? ? ? 文章編號(hào)： 1004?373X（2019）13?0113?04

Unmanned vehicle path planning based on ant colony quartic Bezier curve

ZHANG Jinwei1， 2， WANG Wenyang2， GUO Peng2， GAO Song2

（1. Hebei University of Technology， Tianjin 300132， China;

2. China Automotive Technology & Research Center Co.， Ltd.， Tianjin 300300， China）

Abstract： In order to plan the path conforming to driving curvature and safe obstacle avoidance of unmanned vehicle in the obstacle environment， a path planning method fusing ant colony optimization algorithm and quartic Bezier curve is proposed. The ant colony optimization algorithm is used to plan the global shortest path from the starting point to the target point under the grid map. The path is divided into a finite number of points， and the Bezier curve with driving curvature and safe obstacle avoidance distance is generated between every two points. After that， a continuous unmanned driving path is formed. The effectiveness and feasibility of this method are verified with simulation.

Keywords： path planning; ant colony optimization algorithm; Bezier curve; driving curvature; obstacle avoidance; safe distance

0 ?引 ?言

路徑規(guī)劃[1]是無人駕駛學(xué)中一個(gè)重要的領(lǐng)域，它的目的是在有障礙物的環(huán)境中按照某種優(yōu)化指標(biāo)（如路徑長度、時(shí)間、安全距離等）搜索出一條從出發(fā)點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)或近似最優(yōu)的路徑。

路徑規(guī)劃算法吸引了國內(nèi)外眾多研究者的廣泛研究，現(xiàn)有的方法可以分為五大類，分別是：傳統(tǒng)路徑規(guī)劃算法（模擬退火法、人工勢(shì)場法等）[2]、啟發(fā)式搜索算法（Dijkstra算法、A*算法及其變種等）[3]、離散優(yōu)化算法（模型預(yù)測(cè)算法、幾何軌線算法等）[4]、隨機(jī)采樣算法（隨機(jī)路圖法、快速隨機(jī)拓展樹法等）[5?6]和智能仿生算法（遺傳算法、蟻群算法、神經(jīng)網(wǎng)絡(luò)等）[7]。

蟻群算法又稱螞蟻算法，是一種在圖中搜索最優(yōu)或者次優(yōu)路線的智能算法。在1992年，MarccDorigo博士提出了蟻群算法[8]，它的主要思想是螞蟻在搜索食物過程中會(huì)形成一定的規(guī)則，在這種規(guī)則下每只螞蟻都能沿著相同路徑找到食物。

貝塞爾曲線方法是法國工程師Bezier在1962年為了設(shè)計(jì)汽車車身形狀提出的[9]，之后貝塞爾曲線由于具有良好的數(shù)學(xué)特性而被廣泛應(yīng)用到車輛路徑規(guī)劃領(lǐng)域[10?11]。

1 ?算法描述

1.1 ?蟻群算法

螞蟻尋找路徑不是單只螞蟻的行為，而是一個(gè)群體性的行為。它們互相協(xié)作，每只螞蟻都會(huì)在所走的路徑上留下信息素（路徑長度的倒數(shù)）[9?10]，當(dāng)下一只螞蟻路過該路徑時(shí)就會(huì)利用信息素做出下一步的判斷，并且會(huì)釋放出自己的信息素，這樣就形成了信息素的積累[11]，使得后續(xù)螞蟻可以選擇信息素強(qiáng)的路徑，隨著大量螞蟻在信息素的作用下不斷搜索路徑，最終會(huì)得到一條最優(yōu)或者次優(yōu)路徑[12?13]。

1.2 ?貝塞爾曲線

貝塞爾曲線的定義嚴(yán)格依賴于確定該段曲線控制點(diǎn)的個(gè)數(shù)，[N+1]個(gè)頂點(diǎn)可以定義[N]次多項(xiàng)式的曲線。貝塞爾曲線上各點(diǎn)的參數(shù)方程為：

2 ?算法融合的路徑規(guī)劃

算法流程圖如圖1所示。

圖1 ?融合算法流程圖

1） 構(gòu)造解空間

采用在二維空間內(nèi)構(gòu)建解空間的方法，構(gòu)造柵格地圖，用白色柵格表示可行駛區(qū)域，黑色柵格表示障礙物區(qū)域，對(duì)柵格進(jìn)行數(shù)字標(biāo)號(hào)處理，只有白色柵格才能成為搜索路徑的節(jié)點(diǎn)，從中設(shè)置出發(fā)點(diǎn)和目標(biāo)點(diǎn)，采用輪轉(zhuǎn)賭法選擇下一個(gè)節(jié)點(diǎn)，一直選擇到目標(biāo)點(diǎn)，這樣所有經(jīng)過的柵格就組成了解空間。

2） 節(jié)點(diǎn)選擇

每次迭代有[M]只螞蟻搜索路徑，一共經(jīng)過[N]次迭代，設(shè)置出發(fā)點(diǎn)[S]，目標(biāo)點(diǎn)[E]，每只螞蟻選擇下一個(gè)節(jié)點(diǎn)[j]的方法是：

3） 信息素更新

通常有兩種信息素更新方法，第一種如式（8）所示，叫作實(shí)時(shí)信息素更新，即當(dāng)前螞蟻在路徑搜索中每過一個(gè)節(jié)點(diǎn)就會(huì)對(duì)該節(jié)點(diǎn)進(jìn)行信息素更新。本文即采用該方法。

4） 分割路徑點(diǎn)

將蟻群算法輸出的路徑點(diǎn)從起始位置每隔3個(gè)點(diǎn)取一個(gè)路徑坐標(biāo)，一直取到目標(biāo)點(diǎn)。每兩個(gè)路徑坐標(biāo)之間可以通過5個(gè)控制點(diǎn)唯一確定平面內(nèi)一條四階貝塞爾曲線，如圖2所示。

圖2 ?四階貝塞爾曲線示例圖

5） 生成貝塞爾曲線

6） 路徑選擇

在生成貝塞爾曲線的算法中，設(shè)置多個(gè)規(guī)劃方向可以生成若干條路徑，以避障安全距離為選擇路徑的條件，可以從中得到符合要求的路徑，如下兩點(diǎn)為算法判斷邏輯：

① 在路徑上每個(gè)點(diǎn)與障礙物的距離都大于或等于避障安全距離。

② 若經(jīng)過條件①選擇出來的路徑大于一條，則進(jìn)行長度比較，選擇其中最短路徑。

3 ?仿真實(shí)驗(yàn)與驗(yàn)證

3.1 ?建立仿真環(huán)境

有兩大類構(gòu)建環(huán)境模型的方法：一是基于網(wǎng)絡(luò)或圖的模型方法，二是基于網(wǎng)格模型的方法。本文利用建立柵格圖[13]的方法，此方法屬于網(wǎng)格模型中的一種。柵格圖法結(jié)構(gòu)簡單、易于實(shí)現(xiàn)，是常用的建模方法。

建立20×20的柵格地圖，共有400個(gè)正方形柵格，按照從左往右、從上到下的順序?qū)鸥襁M(jìn)行1～400編號(hào)。白色柵格代表道路，黑色柵格代表障礙物。如圖3所示。

圖3 ?仿真環(huán)境圖

3.2 ?蟻群算法的仿真

在20×20的仿真地圖中，選擇390號(hào)位置為起始點(diǎn)，30號(hào)位置為目標(biāo)點(diǎn)，設(shè)置80只螞蟻，迭代200次搜索出最短路徑。圖4中線條為蟻群算法搜索出的路徑，圖5為收斂曲線，隨著迭代次數(shù)的增加，平均路徑長度和最小路徑長度趨于穩(wěn)定。

圖4 ?蟻群算法路徑圖

圖5 ?收斂曲線

3.3 ?四次貝塞爾曲線的仿真

圖6為四次貝塞爾曲線仿真圖，仿真中選擇390號(hào)位置為起始點(diǎn)，147號(hào)位置為目標(biāo)點(diǎn)，設(shè)置規(guī)劃的方向角度為60°，90°和120°，一共生成3條曲線，再設(shè)置安全距離為0.5個(gè)單位柵格長度，經(jīng)過3條曲線的長度比較，選擇最短路徑并符合安全距離要求。

3.4 ?融合算法的仿真

圖7為蟻群算法與貝塞爾曲線結(jié)合的路徑仿真圖，圖中粗實(shí)線為蟻群算法搜索的路徑，虛線為貝塞爾曲線，灰色線條為兩者融合后的路徑，經(jīng)過與圖4比較可以看出，此路徑符合無人車行駛曲率以及避障安全距離的條件，此次仿真證明了文中方法是優(yōu)于蟻群算法的路徑規(guī)劃。

4 ?結(jié) ?語

有關(guān)蟻群算法改進(jìn)的方法有很多，其中有兩大類方法：一類是基于蟻群算法的改進(jìn);一類是與其他算法結(jié)合的改進(jìn)。本文采用后一種方法，結(jié)合四次貝塞爾曲線，生成具有無人車行駛曲率和避障安全距離的最優(yōu)路徑，并且應(yīng)用Matlab進(jìn)行3組仿真實(shí)驗(yàn)，經(jīng)過對(duì)比驗(yàn)證了本文方法的有效性和可行性。

圖6 ?貝塞爾曲線仿真圖

圖7 ?融合仿真圖

參考文獻(xiàn)

[1] 楊俊成，李淑霞，蔡增玉.路徑規(guī)劃算法的研究與發(fā)展[J].控制工程，2017，24（7）：1473?1480.

YANG Juncheng， LI Shuxia， CAI Zengyu. Research and deve?lopment of path planning algorithm [J]. Control engineering， 2014， 24（7）： 1473?1480.

[2] 霍鳳財(cái)，任偉建，劉東輝.基于改進(jìn)的人工勢(shì)場法的路徑規(guī)劃方法研究[J].自動(dòng)化技術(shù)與應(yīng)用，2016，35（3）：63?67.

GUO Fengcai， REN Weijian， LIU Donghui. Research on path planning method based on improved artificial potential field method [J]. Automation technology and applications， 2016， 35（3）：63?67.

[3] ZHANG L， SUN L， ZHANG S， et al. Trajectory planning for an indoor mobile robot using quintic Bezier curves [C]// 2015 IEEE International Conference on Robotics and Biomimetics. Zhuhai： IEEE， 2015： 757?762.

[4] CHU K， LEE M， SUNWOO M. Local path planning for off?road autonomous driving with avoidance of static obstacles [J]. IEEE transactions on intelligent transportation systems， 2012， 13（4）： 1599?1616.

[5] 余卓平，李奕姍，熊璐.無人車運(yùn)動(dòng)規(guī)劃算法綜述[J].同濟(jì)大學(xué)學(xué)報(bào)（自然科學(xué)版），2017，45（8）：1150?1159.

YU Zhuoping， LI Yishan， XIONG Lu. Unmanned vehicle motion planning algorithm review [J]. Journal of Tongji University （Natural science edition）， 2017， 45（8）： 1150?1159.

[6] 孫豐財(cái)，張亞楠，史旭華.改進(jìn)的快速擴(kuò)展隨機(jī)樹路徑規(guī)劃算法[J].傳感器與微系統(tǒng)，2017，36（9）：129?131.

SUN Fengcai， ZHANG Yanan， SHI Xuhua. Improved rapid expansion of random tree path planning algorithm [J]. Transducer and microsystem technologies， 2017， 36（9）： 129?131.

[7] 于樹科，瞿國慶，祁宏宇，等.蟻群遺傳融合算法在移動(dòng)機(jī)器人路徑規(guī)劃中的應(yīng)用[J].火力與指揮控制，2017，42（12）：88?91.

YU Shuke， QU Guoqing， QI Hongyu， et al. Application of ant colony genetic fusion algorithm in mobile robot path planning [J]. Fire control and command control， 2017， 42（12）： 88?91.

[8] WANG Zhizhong. Based on improved ant colony algorithm for mobile robot path planning study [J]. Journal of mechanical design and manufacturing， 2018（1）： 242?244.

[9] FUNKE J， THEODOSIS P， HINDIYEH R， et al. Up to the limits： Autonomous Audi TTS [C]// 2012 Intelligent Vehicles Symposium. Alcala de Henares： IEEE， 2012： 541?547.

[10] JOLLY K G， KUMAR R S， VIJAYAKUMAR R. A Bezier curve based path planning in a multi?agent robot soccer system without violating the acceleration limits [J]. Robotics & autonomous systems， 2009， 57（1）： 23?33.

[11] CHOI J W， CURRY R E， ELKAIM G H. Curvature?conti?nuous trajectory generation with corridor constraint for autonomous ground vehicles [C]// The 49th IEEE Conference on Decision and Control. Atlanta： IEEE， 2011： 7166?7171.

[12] 陳成，何玉慶，卜春光，等.基于四階貝塞爾曲線的無人車可行軌跡規(guī)劃[J].自動(dòng)化學(xué)報(bào)，2015，41（3）：486?496.

CHEN Cheng， HE Yuqing， BU Chunguang， et al. Feasible trajectory generation for autonomous vehicles based on quartic Bezier curve [J]. Acta automatica sinica， 2015， 41（3）： 486?496.

[13] 胡玉文.城市環(huán)境中基于混合地圖的智能車輛定位方法研究[D].北京：北京理工大學(xué)，2014.

HU Yuwen. Hybrid map based localization method for unmanned ground vehicle in urban scenario [D]. Beijing： Beijing Institute of Technology， 2014.