基于貝塞爾曲線的無人車局部避障應(yīng)用

高嵩 張金煒 戎輝 王文揚(yáng) 郭蓬 何佳

摘 ?要： 文中將基于貝塞爾曲線的路徑規(guī)劃方法應(yīng)用在無人車局部避障中。首先通過最小化曲線的最大曲率與最小曲率之間的差值確定貝塞爾曲線所需的五個(gè)控制點(diǎn)，根據(jù)五個(gè)控制點(diǎn)確定貝塞爾曲線，再結(jié)合實(shí)驗(yàn)無人車和道路的實(shí)際情況從若干備選曲線中選擇最優(yōu)路線。實(shí)驗(yàn)仿真結(jié)果證明，該方法能夠達(dá)到無人車局部避障的要求，能滿足包括初始狀態(tài)約束、目標(biāo)狀態(tài)約束、曲率連續(xù)約束等行駛條件。

關(guān)鍵詞： 路徑規(guī)劃; 局部避障; 無人車; 四階貝塞爾曲線; 控制點(diǎn); 狀態(tài)約束

中圖分類號： TN99?34 ? ? ? ? ? ? ? ? ? ? ? ? ?文獻(xiàn)標(biāo)識碼： A ? ? ? ? ? ? ? ? ? ? ? ? ? 文章編號： 1004?373X（2019）09?0163?04

Application of unmanned vehicle local obstacle avoidance method based on Bezier curve

GAO Song1， 2， ZHANG Jinwei3， RONG Hui1， 3， WANG Wenyang1， GUO Peng1， 2， HE Jia1

（1. Automotive Engineering Research Institute， China Automotive Technology & Research Center Co.， Ltd.， Tianjin 300300， China;

2. Tianjin University， Tianjin 300072， China; 3. Hebei University of Technology， Tianjin 300132， China）

Abstract： The path planning method based on quartic Bezier curve is applied to the local obstacle avoidance of unmanned vehicles. The difference between the maximum curvature and minimum curvature of the minimum curve is used to determine the five control points required by Bezier curve. The Bezier curve is determined according to the five control points， and the optimal path is selected from some alternative curves by combining the experimental unmanned vehicle with actual road situation. The experimental results show that the proposed method can meet the requirements of unmanned vehicles for local obstacle avoidance， and satisfy the driving conditions of initial state constraint， target state constraint and curvature continuous constraint.

Keywords： path planning; local obstacle avoidance; unmanned vehicle; quartic Bezier curve; control point; state constraint

0 ?引 ?言

無人車的軟件層次一般由感知層、決策層和控制層三層組成[1]：感知層包括圖像識別、激光雷達(dá)與毫米波雷達(dá)數(shù)據(jù)處理、衛(wèi)星差分與慣性導(dǎo)航定位等算法;決策層包括全局路徑規(guī)劃和局部路徑規(guī)劃等內(nèi)容;控制層則主要涉及通過模型預(yù)測控制、PID、滑模變理論等對車輛的轉(zhuǎn)向、剎車、加速等底層執(zhí)行單元進(jìn)行控制。本文聚焦于無人車的局部路徑規(guī)劃，探討無人車在道路行駛過程中遇到障礙物時(shí)如何規(guī)劃出一條合理的曲線進(jìn)行避障行駛的問題。

無人車的局部路徑規(guī)劃吸引了國內(nèi)外的研究者進(jìn)行了廣泛的研究，現(xiàn)有的方法可以分為五大類，分別是：傳統(tǒng)路徑規(guī)劃算法（模擬退火法、人工勢場法等）[2]、啟發(fā)式搜索算法（Dijkstra算法、A*算法及其變種等）[3]、離散優(yōu)化算法（模型預(yù)測算法、幾何軌線算法等）[4]、隨機(jī)采樣算法（隨機(jī)路圖法、快速隨機(jī)拓展樹法等）[5?6]和智能仿生算法（遺傳算法、蟻群算法、神經(jīng)網(wǎng)絡(luò)等）[7]。幾何軌線法中的貝塞爾曲線方法是法國工程師Bezier在1962年為了設(shè)計(jì)汽車車身形狀提出的[8]，之后貝塞爾曲線由于具有良好的數(shù)學(xué)特性而被廣泛應(yīng)用到車輛路徑規(guī)劃領(lǐng)域[9?10]。本文研究的是將四階貝塞爾曲線應(yīng)用在無人車局部避障中的問題。

首先結(jié)合無人車的動力學(xué)屬性，設(shè)計(jì)局部避障應(yīng)該滿足的初始狀態(tài)約束、目標(biāo)狀態(tài)約束、曲率約束等條件，然后通過SQP最優(yōu)化算法[11]求出貝塞爾曲線的五個(gè)控制點(diǎn)，確定若干備選曲線，最后結(jié)合實(shí)際道路工況選出最優(yōu)行駛路線。Matlab仿真實(shí)驗(yàn)證明了本文方法的有效性，可用于實(shí)驗(yàn)車的局部路徑規(guī)劃。

1 ?無人車路徑規(guī)劃目標(biāo)

無人車須實(shí)現(xiàn)從初始點(diǎn)至目標(biāo)點(diǎn)的行駛，而初始點(diǎn)和目標(biāo)點(diǎn)的狀態(tài)則是貝塞爾曲線構(gòu)建的形成條件，初始點(diǎn)的狀態(tài)包括二維空間坐標(biāo)[（x，y）]，航向角[ψ]，曲率[κ]，如圖1所示。

曲率[κ]、轉(zhuǎn)彎半徑[r]、軸距[L]以及前輪轉(zhuǎn)角之間的關(guān)系如下所示：

無人車的軌跡可以看作是由初始狀態(tài)到目標(biāo)狀態(tài)的一組動作組成[11]，設(shè)初始狀態(tài)[XI=xI，yI，ψI，κI]和目標(biāo)狀態(tài)[XT=xT，yT，ψT，κT]。

2 ?基于貝塞爾曲線的路徑規(guī)劃

2.1 ?貝塞爾曲線性質(zhì)

本節(jié)僅介紹貝塞爾曲線的性質(zhì)，詳細(xì)的貝塞爾曲線介紹可參考文獻(xiàn)[12]。

如圖2所示，五個(gè)控制點(diǎn)可以唯一確定平面內(nèi)一條四階貝塞爾曲線，曲線具有仿射變換不變的特性，其參數(shù)化表達(dá)式如下：

圖1 ?無人車轉(zhuǎn)向模型

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

根據(jù)以上參數(shù)表達(dá)式可知：曲線經(jīng)過第一個(gè)[P0（u=0）]及第五個(gè)控制點(diǎn)[P4（u=1）];根據(jù)求導(dǎo)可得曲線在[P0]和[P4]處的切向量分別為[P（0）=4（P1-P0）]和[P（1）=4（P4-P3）]。曲線上任意一點(diǎn)的曲率公式為：

2.2 ?控制點(diǎn)表示

由于五個(gè)控制點(diǎn)即可確定貝塞爾曲線，因而可以根據(jù)無人車路徑規(guī)劃的要求先求解五個(gè)控制點(diǎn)的平面坐標(biāo)[13]，再對貝塞爾曲線進(jìn)行參數(shù)化表示，求解步驟如下：

1） 由于貝塞爾曲線具有仿射變換不變的特性，任意旋轉(zhuǎn)或平移該曲線都不會影響它的形狀，將曲線放在車輛坐標(biāo)系下，車輛坐標(biāo)系[（0，0）]為初始位置[P0]，初始位置狀態(tài)為[XI=0，0，π2，κI]。

2） 假設(shè)[P0]與[P1]的距離為[d1]，為了滿足初始航向角[π2]的約束，可求得[P1]的坐標(biāo)為[（0，d1）]。

3） 結(jié)合式（4）初始點(diǎn)曲率計(jì)算公式，為了滿足控制點(diǎn)[P0]的曲率約束，設(shè)[P2=（x2，y2）]，可得：

以上計(jì)算可以滿足無人車路徑規(guī)劃初始位置和目標(biāo)位置的約束條件，得到五個(gè)控制點(diǎn)的坐標(biāo)。在這五個(gè)控制點(diǎn)中只有三個(gè)自由變量，分別是[d1，d4，y2]，也就是說，通過這三個(gè)自由變量即可唯一確定一條四階貝塞爾曲線。

2.3 ?曲率約束條件

為了滿足無人車運(yùn)動約束條件，還須加上曲率約束。根據(jù)式（3）可知，當(dāng)且僅當(dāng)[xu=yu=0]時(shí)，曲率不連續(xù)，但是若滿足上述條件，貝塞爾曲線將縮成一個(gè)點(diǎn)，因此四階貝塞爾曲線曲率處處連續(xù)。

另外，假設(shè)實(shí)驗(yàn)無人車的最小轉(zhuǎn)彎半徑為[R]，可以得到最大曲率[κMax=1R]，最小曲率[κMin=0]。所以，曲率上下有界約束條件可以表示為：[κMin≤κ（u）≤κMax]。

2.4 ?軌跡優(yōu)化求解

本文采用最優(yōu)化的方法優(yōu)化三個(gè)參數(shù)[d1，d4，y2]，使得曲線的最大曲率與最小曲率之間的差值最小。優(yōu)化目標(biāo)函數(shù)為：

式中：[μ1，μ2∈0，1];目標(biāo)函數(shù)中[p]代表參數(shù)[d1，d4，y2]。另外，還須滿足實(shí)驗(yàn)無人車的最大曲率約束式（6）。曲率最大和最小值可通過曲率式（3）求取極值獲得。本文采用的最優(yōu)化算法為序列二次優(yōu)化（SQP）[14]，迭代次數(shù)為50次。

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

3.1 ?無人車情況

圖3所示為作者所在單位開發(fā)的無人觀光車——“小黃車”，外形尺寸為4 330 mm×1 510 mm×2 030 mm（長×寬×高），軸距為2 000 mm，輪距為1 200 mm，最小轉(zhuǎn)彎半徑為7 000 mm。車頂上具有GPS+慣導(dǎo)，負(fù)責(zé)采集無人車的位置和航向角等信息，車頭安裝有16線激光雷達(dá)和雙目攝像頭，負(fù)責(zé)采集行駛區(qū)域內(nèi)的路況和障礙物信息，后備箱安裝有工控機(jī)，負(fù)責(zé)處理感知信息，進(jìn)行軌跡規(guī)劃以及決策控制。

圖3 ?中國汽車技術(shù)研究中心研發(fā)的無人觀光車——“小黃車”

3.2 ?建立仿真道路

無人車在行進(jìn)中需要實(shí)時(shí)采集道路信息，由這些道路信息構(gòu)建環(huán)境地圖，而環(huán)境地圖是進(jìn)行局部路徑規(guī)劃的前提[15]。無人車通過車前的16線激光雷達(dá)感知環(huán)境信息，將世界坐標(biāo)系下的障礙物轉(zhuǎn)換到車輛質(zhì)心坐標(biāo)系，采用柵格法[16]在二維空間里建立周邊環(huán)境模型。

圖4為實(shí)驗(yàn)無人車避障實(shí)驗(yàn)道路，該道路位于中國汽車技術(shù)研究中心天津市東麗區(qū)的主院區(qū)內(nèi)，道路寬為7 m，長為120 m。

圖4 ?擬仿真的真實(shí)道路

本文基于以上道路進(jìn)行仿真實(shí)驗(yàn)，將道路范圍劃分為30×30的柵格，每個(gè)柵格的長和寬都設(shè)為0.5 m，中間白色地帶為7 m長道路，兩側(cè)黑色為草地，在道路上設(shè)置若干障礙物，并將其進(jìn)行膨脹處理，不能占滿一個(gè)柵格要按照占滿柵格處理。圖5和圖6為兩種仿真道路上障礙物的分布情況。

3.3 ?仿真實(shí)驗(yàn)結(jié)果

在Matlab仿真中，以無人車當(dāng)前所在的位置為原點(diǎn)建立平面[X?Y]坐標(biāo)系，通過計(jì)算兩段貝塞爾曲線實(shí)現(xiàn)局部避障。第一段貝塞爾曲線的初始狀態(tài)為[XI=0，0，π2，0]，第二段貝塞爾曲線的初始狀態(tài)設(shè)定為第一段曲線的目標(biāo)狀態(tài)，并將曲率設(shè)為0。

圖5 ?避障仿真實(shí)驗(yàn)（一）

圖6 ?避障仿真實(shí)驗(yàn)（二）

實(shí)驗(yàn)中，根據(jù)虛擬障礙物的平面坐標(biāo)，在[X]方向以1 m為間隔設(shè)置8個(gè)目標(biāo)點(diǎn)（涵蓋障礙物位置），并在每個(gè)目標(biāo)點(diǎn)取轉(zhuǎn)向角[ψT]分別為60°，90°和120°，因此兩次路徑規(guī)劃可分別得到24條貝塞爾曲線。實(shí)際應(yīng)用中通常不在目標(biāo)狀態(tài)加曲率約束，所以本文未在目標(biāo)狀態(tài)設(shè)置曲率約束。

考慮到無人車的橫向?qū)挾?，局部避障?guī)劃中最短路徑不一定就是最優(yōu)路徑，按照以下方式從計(jì)算得到的多條貝塞爾曲線中選取最優(yōu)路徑：由于本文無人車的寬度為1.4 m，因此選取1 m作為安全距離，首先要求待選路線上的每個(gè)點(diǎn)（代表無人車中心軸）與周圍障礙物的距離大于1 m，然后再從安全距離篩選出來的路徑中選擇出最短路徑，計(jì)算每個(gè)路線長度，選出最短路線。

兩種道路情況下的仿真結(jié)果如圖5和圖6所示（直線為原來的直線路徑，虛線為計(jì)算得到的貝塞爾曲線，實(shí)曲線為選取的避障路徑）。從仿真結(jié)果來看，本文使用的貝塞爾曲線路徑規(guī)劃方法可成功解決無人車在指定道路上的局部路徑規(guī)劃問題。

4 ?結(jié) ?語

本文將貝塞爾曲線的方法應(yīng)用到無人車的局部路徑規(guī)劃中，結(jié)合無人車和道路場景的具體數(shù)據(jù)進(jìn)行仿真實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果證明了本文方法的有效性，規(guī)劃出的行駛曲線可滿足無人車初始狀態(tài)約束、目標(biāo)狀態(tài)約束和曲率連續(xù)約束等條件，使得無人車能夠按照軌跡平滑行駛。未來會盡快將本文提出的方法運(yùn)用到車上，進(jìn)行實(shí)車驗(yàn)證。

參考文獻(xiàn)

[1] 王世峰，戴祥，徐寧，等.無人駕駛汽車環(huán)境感知技術(shù)綜述[J].長春理工大學(xué)學(xué)報(bào)（自然科學(xué)版），2017，40（1）：1?6.

WANG Shifeng， DAI Xiang， XU Ning， et al. Overview on environment perception technology for unmanned ground vehicle [J]. Journal of Changchun University of Science and Technology （Natural Science）， 2017， 40（1）： 1?6.

[2] 鮑慶勇，李舜酩，沈峘，等.自主移動機(jī)器人局部路徑規(guī)劃綜述[J].傳感器與微系統(tǒng)，2009，28（9）：1?4.

BAO Qingyong， LI Shunming， SHEN Huan， et al. Survey of local path planning of autonomous mobile robot [J]. Transducer and microsystem technologies， 2009， 28（9）： 1?4.

[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] BARRAQUAND J， LATOMBE J C. A Monte?Carlo algorithm for path planning with many degrees of freedom [C]// 1990 IEEE International Conference on Robotics and Automation. Cincinnati： IEEE， 1990： 1712?1717.

[6] KAVRAKI L， LATOMBE J C. Randomized preprocessing of configuration for fast path planning [C]// 1994 IEEE Interna?tional Conference on Robotics and Automation. San Diego： IEEE， 1994： 2138?2145.

[7] 陳剛，沈林成.復(fù)雜環(huán)境下路徑規(guī)劃問題的遺傳路徑規(guī)劃方法[J].機(jī)器人，2001，23（1）：40?44.

CHEN Gang， SHEN Lincheng. Genetic path planning algorithm for complex environment path planning [J]. Robot， 2001， 23（1）： 40?44.

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

[9] 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.

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

[11] 陳成，何玉慶，卜春光，等.基于四階貝塞爾曲線的無人車可行軌跡規(guī)劃[J].自動化學(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.

[12] DUNCAN M. Applied geometry for computer graphics and CAD [M]. Berlin： Springer， 2005.

[13] 余伶俐，龍子威，周開軍.基于貝塞爾曲線的機(jī)器人非時(shí)間軌跡跟蹤方法[J].儀器儀表學(xué)報(bào)，2016，37（7）：1564?1572.

YU Lingli， LONG Ziwei， ZHOU Kaijun. Non?time trajectory tracking method based on Bezier curve for robot [J]. Chinese journal of scientific instrument， 2016， 37（7）： 1564?1572.

[14] 李振浩.基于序列二次規(guī)劃：免疫記憶魚群算法的局放超聲定位研究[D].昆明：昆明理工大學(xué)，2017.

LI Zhenhao. Partial discharge ultrasound localization research based on sequence quadratic programming & immune memory fish swarm algorithm （SQP?IMAFSA） [D]. Kunming： Kunming University of Science and Technology， 2017.

[15] 胡玉文.城市環(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.

[16] CHE H， WU Z， KANG R， et al. Global path planning for explosion?proof robot based on improved ant colony optimization [C]// 2016 Asia?Pacific Conference Intelligent Robot Systems. Tokyo： IEEE， 2016： 36?40.