基于貝塞爾曲線的無人車局部避障應用

高嵩 張金煒 戎輝 王文揚 郭蓬 何佳

摘 ?要： 文中將基于貝塞爾曲線的路徑規劃方法應用在無人車局部避障中。首先通過最小化曲線的最大曲率與最小曲率之間的差值確定貝塞爾曲線所需的五個控制點，根據五個控制點確定貝塞爾曲線，再結合實驗無人車和道路的實際情況從若干備選曲線中選擇最優路線。實驗仿真結果證明，該方法能夠達到無人車局部避障的要求，能滿足包括初始狀態約束、目標狀態約束、曲率連續約束等行駛條件。

關鍵詞： 路徑規劃; 局部避障; 無人車; 四階貝塞爾曲線; 控制點; 狀態約束

中圖分類號： TN99?34 ? ? ? ? ? ? ? ? ? ? ? ? ?文獻標識碼： 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]：感知層包括圖像識別、激光雷達與毫米波雷達數據處理、衛星差分與慣性導航定位等算法;決策層包括全局路徑規劃和局部路徑規劃等內容;控制層則主要涉及通過模型預測控制、PID、滑模變理論等對車輛的轉向、剎車、加速等底層執行單元進行控制。本文聚焦于無人車的局部路徑規劃，探討無人車在道路行駛過程中遇到障礙物時如何規劃出一條合理的曲線進行避障行駛的問題。

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

首先結合無人車的動力學屬性，設計局部避障應該滿足的初始狀態約束、目標狀態約束、曲率約束等條件，然后通過SQP最優化算法[11]求出貝塞爾曲線的五個控制點，確定若干備選曲線，最后結合實際道路工況選出最優行駛路線。Matlab仿真實驗證明了本文方法的有效性，可用于實驗車的局部路徑規劃。

1 ?無人車路徑規劃目標

無人車須實現從初始點至目標點的行駛，而初始點和目標點的狀態則是貝塞爾曲線構建的形成條件，初始點的狀態包括二維空間坐標[（x，y）]，航向角[ψ]，曲率[κ]，如圖1所示。

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

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

2 ?基于貝塞爾曲線的路徑規劃

2.1 ?貝塞爾曲線性質

本節僅介紹貝塞爾曲線的性質，詳細的貝塞爾曲線介紹可參考文獻[12]。

如圖2所示，五個控制點可以唯一確定平面內一條四階貝塞爾曲線，曲線具有仿射變換不變的特性，其參數化表達式如下：

圖1 ?無人車轉向模型

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

根據以上參數表達式可知：曲線經過第一個[P0（u=0）]及第五個控制點[P4（u=1）];根據求導可得曲線在[P0]和[P4]處的切向量分別為[P（0）=4（P1-P0）]和[P（1）=4（P4-P3）]。曲線上任意一點的曲率公式為：

2.2 ?控制點表示

由于五個控制點即可確定貝塞爾曲線，因而可以根據無人車路徑規劃的要求先求解五個控制點的平面坐標[13]，再對貝塞爾曲線進行參數化表示，求解步驟如下：

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

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

3） 結合式（4）初始點曲率計算公式，為了滿足控制點[P0]的曲率約束，設[P2=（x2，y2）]，可得：

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

2.3 ?曲率約束條件

為了滿足無人車運動約束條件，還須加上曲率約束。根據式（3）可知，當且僅當[xu=yu=0]時，曲率不連續，但是若滿足上述條件，貝塞爾曲線將縮成一個點，因此四階貝塞爾曲線曲率處處連續。

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

2.4 ?軌跡優化求解

本文采用最優化的方法優化三個參數[d1，d4，y2]，使得曲線的最大曲率與最小曲率之間的差值最小。優化目標函數為：

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

3 ?仿真實驗與驗證

3.1 ?無人車情況

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

圖3 ?中國汽車技術研究中心研發的無人觀光車——“小黃車”

3.2 ?建立仿真道路

無人車在行進中需要實時采集道路信息，由這些道路信息構建環境地圖，而環境地圖是進行局部路徑規劃的前提[15]。無人車通過車前的16線激光雷達感知環境信息，將世界坐標系下的障礙物轉換到車輛質心坐標系，采用柵格法[16]在二維空間里建立周邊環境模型。

圖4為實驗無人車避障實驗道路，該道路位于中國汽車技術研究中心天津市東麗區的主院區內，道路寬為7 m，長為120 m。

圖4 ?擬仿真的真實道路

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

3.3 ?仿真實驗結果

在Matlab仿真中，以無人車當前所在的位置為原點建立平面[X?Y]坐標系，通過計算兩段貝塞爾曲線實現局部避障。第一段貝塞爾曲線的初始狀態為[XI=0，0，π2，0]，第二段貝塞爾曲線的初始狀態設定為第一段曲線的目標狀態，并將曲率設為0。

圖5 ?避障仿真實驗（一）

圖6 ?避障仿真實驗（二）

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

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

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

4 ?結 ?語

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

參考文獻

[1] 王世峰，戴祥，徐寧，等.無人駕駛汽車環境感知技術綜述[J].長春理工大學學報（自然科學版），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].傳感器與微系統，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] 陳剛，沈林成.復雜環境下路徑規劃問題的遺傳路徑規劃方法[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] 陳成，何玉慶，卜春光，等.基于四階貝塞爾曲線的無人車可行軌跡規劃[J].自動化學報，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].儀器儀表學報，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] 李振浩.基于序列二次規劃：免疫記憶魚群算法的局放超聲定位研究[D].昆明：昆明理工大學，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] 胡玉文.城市環境中基于混合地圖的智能車輛定位方法研究[D].北京：北京理工大學，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.