周孝添， 任宏斌， 蘇波， 齊志權(quán)， 汪洋

(1.北京理工大學(xué) 機械與車輛學(xué)院， 北京 100081； 2.中國北方車輛研究所， 北京 100072)

收稿日期：2021-11-09

基金項目：國家自然科學(xué)基金項目(52002025)

0 引言

在人工智能的熱潮之下，智能汽車行業(yè)的發(fā)展也受到社會的廣泛關(guān)注，因為它將在降低交通事故發(fā)生率、減少交通擁堵、改善大氣環(huán)境、降低能源消耗等方面做出重要貢獻[1]。無人駕駛汽車是智能汽車發(fā)展的高級階段，避障軌跡規(guī)劃作為無人駕駛技術(shù)最核心的技術(shù)之一，對無人駕駛車輛的研究具有重大意義。在結(jié)構(gòu)化道路下，其基本任務(wù)是考慮車輛與前方障礙物的幾何關(guān)系，規(guī)劃出一條避免發(fā)生碰撞的換道軌跡，同時應(yīng)該具有較高的乘坐舒適性，從而提高出行效率[1-2]。

針對結(jié)構(gòu)化道路下的變道避障軌跡規(guī)劃，國內(nèi)外學(xué)者進行了各種研究[3-9]。Zhang等[4]和聶枝根等[5]利用三角函數(shù)去模擬車輛變道軌跡，此算法雖然簡單高效，但忽略了運動學(xué)約束，且存在變道終點已知的局限。Kanaris等[6]提出一種基于正反梯形橫向加速度的方法，通過對橫向加速度優(yōu)化求解、積分得到變道軌跡，此方法雖然有效結(jié)合了車輛橫向運動學(xué)，但不夠靈活，無法高效地調(diào)節(jié)整個變道過程。牛國臣等[7]設(shè)計了一種雙5次多項式換道避障軌跡規(guī)劃算法，雖然此算法通過優(yōu)化換道時間來改善乘坐舒性，但忽略了橫縱向耦合對行駛安全性乘坐舒適性的影響。Li等[8]設(shè)計了一種基于多階貝塞爾曲線的變道避障路徑規(guī)劃方法，此方法雖然能有效地調(diào)節(jié)變道效率和乘坐舒適性之間的平衡關(guān)系，但只適用于恒速變道。Shim等[9]設(shè)計了橫縱向上關(guān)于時間的6階多項式模型，通過求解非凸目標(biāo)函數(shù)得到了期望軌跡，雖然此方法考慮了乘坐舒適性和運動學(xué)約束，但因為是一個非凸優(yōu)化過程，所以求解效率較低，實用性差。

近年來，微分平坦定理在無人機和無人車運動規(guī)劃中得到的廣泛關(guān)注，其主要優(yōu)點是能將非線性系統(tǒng)的微分約束映射為平滑集合約束，有效減少了優(yōu)化空間的維數(shù)[10-12]。Cong等[10]利用車輛系統(tǒng)的平坦輸出建立運動學(xué)和穩(wěn)定性約束來彌補基本質(zhì)點模型規(guī)劃算法的不足。Wang等[11]利用車輛動力學(xué)模型的微分平坦特性線性化車輛非線性動力學(xué)模型，大大提高了預(yù)測控制精度。Guo等[12]基于微分平坦定理提出了一種具有輪胎穩(wěn)定性控制的最大功率控制器。

綜上所述，目前運動規(guī)劃問題普遍都是把三維的軌跡規(guī)劃(橫向- 縱向- 時間)解耦為兩個二維規(guī)劃問題，但McNaughton等[13]認(rèn)為解耦會損失車輛運動的最優(yōu)性。由于在橫縱向軌跡設(shè)計時并沒有考慮其耦合性，為提升規(guī)劃算法的最優(yōu)性且能滿足更多的運動學(xué)約束，本文提出一種分層段軌跡規(guī)劃算法，在底層構(gòu)建以貝塞爾曲線(Bézier curve)節(jié)點為自變量且滿足避障、速度、加速度約束等凸約束的二次規(guī)劃問題，保證算法的概率完備性和行車安全性。利用車輛運動學(xué)的微分平坦特性，構(gòu)建以乘坐舒適性、運動學(xué)約束、能耗、行車效率等指標(biāo)的目標(biāo)函數(shù)，利用粒子群優(yōu)化(PSO)算法優(yōu)化終止車速和行駛時間，得到一條滿足車輛多目標(biāo)約束的行駛軌跡。

1 基于貝塞爾曲線的局部路徑規(guī)劃

1.1 貝塞爾曲線

貝塞爾曲線是應(yīng)用于二維圖形應(yīng)用程序的數(shù)學(xué)曲線[14]。曲線定義起始點、終止點(也稱錨點)、控制點。通過調(diào)整控制點，貝塞爾曲線的形狀會發(fā)生變化。故基于貝塞爾曲線進行避障規(guī)劃是通過尋找貝塞爾曲線控制點的最優(yōu)位置來實現(xiàn)避障。已知貝塞爾曲線方程為

(1)

(2)

從上述推導(dǎo)可知，貝塞爾曲線本質(zhì)是關(guān)于變量t的多項式，由于形狀由每個節(jié)點的坐標(biāo)決定，具有如下性質(zhì)：

1)它總是從第1個控制點開始，到最后一個控制點結(jié)束，不經(jīng)過中間其他節(jié)點，但是曲線始終在所有節(jié)點連接構(gòu)成的凸多邊形內(nèi)。

2)曲線每個節(jié)點的導(dǎo)數(shù)由曲線節(jié)點的線性組合求得，即曲線的導(dǎo)數(shù)同樣也可以被約束在凸多邊形內(nèi)。

3)曲線自變量t的區(qū)間為[0，1]。

1.2 避障軌跡規(guī)劃

根據(jù)貝塞爾曲線形狀可知，當(dāng)給定控制點范圍約束時，可以保證整條曲線都位于此安全范圍內(nèi)，利用此性質(zhì)大大減少了約束的數(shù)量，提升了求解效率.如圖1所示，圖中點劃線為兩車道的中心線，黑色虛線為車道分割線，由于靜態(tài)障礙物和動態(tài)障礙物的存在，貝塞爾曲線節(jié)點的可行域是一個非凸域。為了使算法的求解效率更高，在非凸域上構(gòu)造兩個凸的安全走廊，以兩段7階貝塞爾曲線構(gòu)造變道曲線，變道起點和變道終點都位于車道中心線上，則把原始的非凸優(yōu)化問題轉(zhuǎn)化為在兩個安全走廊的凸優(yōu)化問題。

圖1 避障軌跡規(guī)劃Fig.1 Obstacle avoidance trajectory planning

由于貝塞爾曲線時間的取值在[0，1]內(nèi)，故需要對曲線進行歸一化，則實際變道曲線函數(shù)[15]為

(3)

式中：s1、s2為每一段貝塞爾曲線對應(yīng)的實際時間；μ∈{x,y}表示橫向和縱向兩個方位。

由于貝塞爾曲線的安全走廊存在一定范圍內(nèi)的重疊，使得優(yōu)化問題具有更高的自由度，對每一段貝塞爾曲線時間分配進行隱性調(diào)節(jié)。把三維空間中的軌跡規(guī)劃問題分解為兩個二維空間中的貝塞爾曲線優(yōu)化問題，建立以曲線3次導(dǎo)數(shù)的平方和為目標(biāo)函數(shù)：

(4)

式中：T為貝塞爾曲線對應(yīng)的時間；τ為仿真時間，τ∈[0,1]，節(jié)點編號i∈{0,1,2,3,4,5,6,7}；j∈{1,2}表示兩段貝塞爾曲線。

設(shè)節(jié)點i的第f階導(dǎo)數(shù)為

(5)

(6)

由于軌跡是由兩段曲線拼接而成，需要設(shè)置連續(xù)性約束：

(7)

由式(7)可知通過保證第1段曲線末尾節(jié)點與第2段曲線起始節(jié)點的初始位置、速度加速度相等來保證連續(xù)性約束。

為保證行車安全行，避免與障礙物發(fā)生碰撞，需要添加邊界約束：

(8)

實際車輛行駛過程中受到運動學(xué)約束，需要對橫縱方向上的速度、加速度進行約束：

(9)

(10)

由于目標(biāo)函數(shù)是一個二次型，且約束條件都是凸的，上述貝塞爾曲線優(yōu)化問題是一個凸二次規(guī)劃問題，求解效率極高。但是此問題的求解需要給定確切的行駛時間和末端車速，是一個局部條件下的最優(yōu)問題。為使求得的軌跡全局最優(yōu)，下面利用PSO算法對對參數(shù)進行優(yōu)化，同時在適應(yīng)度函數(shù)中加入乘坐舒適性和可跟蹤性等度量函數(shù)。

2 車輛運動學(xué)模型的平坦輸出

微分平坦最早是由法國Martin提出的優(yōu)化微分代數(shù)的概念[15]，之后得到迅速發(fā)展并慢慢引入控制領(lǐng)域。近年來廣泛運用于復(fù)雜約束的路徑規(guī)劃問題中，對于一個非線性系統(tǒng)：

(11)

如果能找到如下形式的輸出量：

(12)

使得系統(tǒng)的狀態(tài)量以及輸入量都能用輸出量z和z的有限階導(dǎo)數(shù)表示：

(13)

則稱系統(tǒng)是微分平坦系統(tǒng)。其中x為系統(tǒng)狀態(tài)量、y為系統(tǒng)輸出量、u為系統(tǒng)控制量，n、m分別為維度數(shù)量，F(xiàn)(x,u)表示非線性狀態(tài)方程，g(x)表示非線性輸出方程，z為平坦系統(tǒng)的平坦輸出，h為平坦函數(shù)，由于平坦輸出與狀態(tài)變量和控制輸入之間是一一對應(yīng)的關(guān)系，微分平坦的運動特征可以由平坦輸出唯一決定。這樣就把一個高維狀態(tài)空間映射到了平坦空間，降低了規(guī)劃空間維度，如圖2所示。圖2中，x(T)、u(T)為末尾狀態(tài)，x(0)、u(0)為初始狀態(tài)，F(xiàn)(0)=h(x(0)，u(0))與F(T)=h(x(T)，u(T))分別為平坦空間的初始狀態(tài)和末尾狀態(tài)。

圖2 狀態(tài)空間與平坦空間的映射關(guān)系Fig.2 Mapping between state space and flat space

為進一步優(yōu)化利用汽車的非完整性運動學(xué)約束得到的貝塞爾曲線，建立圖3所示車輛運動學(xué)模型，其中前輪為轉(zhuǎn)向輪、后輪為驅(qū)動輪，以后軸線中心為參考點建立坐標(biāo)系，(x,y)為車體位置坐標(biāo)，δ為前輪轉(zhuǎn)角，φ為車輛橫擺角，q=[x,y,δ,φ]為車體的位姿向量，R為轉(zhuǎn)彎半徑，l為前后軸輪距。

圖3 車輛運動學(xué)模型Fig.3 Vehicle kinematics model

則車輛的非完整性約束為

(14)

式中：(xf,yf)為前輪中心坐標(biāo)。上述等式表示前后軸無側(cè)向滑動，滿足如下幾何關(guān)系:

(15)

根據(jù)阿克曼轉(zhuǎn)角的幾何關(guān)系可以得到轉(zhuǎn)彎半徑R：

(16)

式中：ρ為曲率半徑。

以車輛狀態(tài)向量作為狀態(tài)量，以車速v和前輪轉(zhuǎn)角的角速度ω為模型輸入量，建立如下運動學(xué)模型：

(17)

已知橫縱向的貝塞爾曲線表達式為

(18)

(19)

(20)

式中：α、β矩陣由橫縱向貝塞爾曲線節(jié)點cx、cy確定。

由運動學(xué)模型可得

(21)

進一步推導(dǎo)可得

(22)

(23)

由式(21)和式(23)可得橫擺角速度:

(24)

從而可以得到前輪轉(zhuǎn)角表達式：

(25)

3 基于粒子群的二次優(yōu)化算法

貝塞爾曲線優(yōu)化問題是基于質(zhì)點模型的凸優(yōu)化，但對于車輛這樣一個非完整系統(tǒng)，應(yīng)該將車輛的運動學(xué)和輸入輸出狀態(tài)約束考慮在內(nèi)，保證規(guī)劃軌跡具有很高的可執(zhí)行性，否則會極大程度地增加軌跡跟蹤的難度。而且基于貝塞爾曲線的軌跡規(guī)劃問題只滿足避障和部分運動學(xué)等硬約束和部分平順性約束，因為目標(biāo)函數(shù)屬于車輛平順性的子集。為規(guī)劃出一條安全行、高效性、舒適性、可執(zhí)行性等綜合性能最優(yōu)的貝塞爾曲線，本文利用微分平坦輸出設(shè)計滿足上述條件的目標(biāo)函數(shù)，由于目標(biāo)函數(shù)對初始參數(shù)沒有明確的梯度表達式，利用PSO算法[16]完成對底層凸優(yōu)化問題的初始參數(shù)行駛時間s1、s2和最終車速vt的在線優(yōu)化，最終得到一條綜合性能最優(yōu)的行駛軌跡。具體流程算法流程如圖4所示。圖4中,k為當(dāng)前迭代次數(shù)，F(xiàn)為自適應(yīng)函數(shù)，F(xiàn)t1，F(xiàn)t2，…，F(xiàn)t6為粒子適應(yīng)度，Xij為種群位置，vij為種群速度，s1k，s2k，…，stk為每段貝塞爾曲線對應(yīng)的時間，gb為粒子在所有迭代周期內(nèi)的全局極值，Pib為粒子的個體極值。

圖4 算法流程Fig.4 Flow chart of the algorithm

首先設(shè)置粒子群內(nèi)的粒子數(shù)np，由此可得粒子群矩陣如下：

X=(X1,X2,…,Xnp)

(26)

種群中每一個粒子包含3個參數(shù)：

Xi=(s1,s2,vt),i=1,2,…,np

(27)

考慮粒子群內(nèi)所有粒子在所有迭代周期內(nèi)的全局極值、每個粒子維度下的速度和個體極值：

9月14日育苗，10月30日移栽，6行區(qū)，行長6.4米，行距0.5米，每小區(qū)面積19.2平方米，每畝密度4 000株。全生育期不進行中耕除草，僅是每次施氮肥時均用水澆施，提高肥效。在水稻收獲后不進行稻田翻犁，直接開行移栽油菜。重復(fù)間開溝，溝寬40厘米，溝深30厘米。2個品種隨機區(qū)組排列，3次重復(fù)。

(28)

在迭代過程中粒子會根據(jù)自身粒子的慣性，以及與自身極值Pi和全局極值gb之間的距離來更新自身的速度vi和位置Pi：

(29)

(30)

式中：j=1,2,3；ω為慣性權(quán)重因子；c1、c2為自身權(quán)重因子和種群權(quán)重因子；r1、r2分別為大小在[0，1]之間的2個隨機正整數(shù)。

從軌跡安全行、高效性、舒適性、可執(zhí)行性等各項性能出發(fā)設(shè)計自適應(yīng)函數(shù)Ft。首先考慮行車安全行，可由貝塞爾曲線與障礙物間的曼哈頓距離評價：

Ft1=ω1dist(obs,Bezier)

(31)

式中：ω1為指標(biāo)權(quán)重系數(shù)；obs表示周邊障礙物的全局坐標(biāo)。

橫向乘坐舒適性指標(biāo)是前輪轉(zhuǎn)角及其變化率，縱向舒適性評價指標(biāo)為縱向加加速度，則總的舒適性評價指標(biāo)為

(32)

式中：ω2、ω3為指標(biāo)權(quán)重系數(shù)；xj、yj分別為橫縱向坐標(biāo)；g為重力加速度。

由于軌跡規(guī)劃與控制模塊分離在不同的層中，導(dǎo)致二者的目標(biāo)側(cè)重不是完全切合，一般假設(shè)當(dāng)軌跡滿足一定約束條件時底層控制能夠以有界的誤差跟蹤軌跡。然而由于車輛模型的誤差 ，可能會使得規(guī)劃層規(guī)劃出不符合運動學(xué)約束的軌跡，一般用最大曲率ρmin=0.1 m-1進行限制[17]，為加快PSO算法收斂速度，運用Sigmoid函數(shù)[18]，設(shè)計可執(zhí)性評價指標(biāo)：

(33)

式中：ω4為指標(biāo)權(quán)重系數(shù)。

(34)

式中：μy為輪胎的橫向附著系數(shù)，對于干燥路面，一般μy取1[20]；φmax為最大偏航角；βmax為最大質(zhì)心側(cè)偏角。

同理，利用Sigmoid函數(shù)設(shè)計質(zhì)心側(cè)偏角和橫擺角速度評價指標(biāo)：

(35)

(36)

式中：ω5、ω6為指標(biāo)權(quán)重系數(shù)。

在實際的結(jié)構(gòu)化道路上行駛時，在保證智能汽車乘員舒適性前提下，變道避障時間越短越好，這樣能夠在宏觀層面上提高交通效率，設(shè)計避障效率指標(biāo)為

Ft6=ω7·s1+ω8·s2

(37)

式中：ω7、ω8為指標(biāo)權(quán)重系數(shù)。故粒子自適應(yīng)函數(shù)為

(38)

式中：w為貝塞爾曲線的粒子數(shù)，w∈{1,2,…,8}。式(31)～式(38)中，指標(biāo)權(quán)重系數(shù)ω1,ω2,…,ω8根據(jù)不同的駕駛風(fēng)格進行選取。

4 數(shù)值仿真實驗

換道避障是自動駕駛汽車實現(xiàn)結(jié)構(gòu)化道路上無人駕駛的基礎(chǔ)技術(shù)，雙移線避障又可以看作單移線的疊加和改進。因此在考慮周圍靜態(tài)和動態(tài)車輛的環(huán)境下，如何規(guī)劃出一條綜合性能最優(yōu)的行駛軌跡，就成為結(jié)構(gòu)化道路下局部路徑規(guī)劃技術(shù)的關(guān)鍵。

如圖5所示，車輛A以va=47 km/h的車速向前行駛，與前方障礙車輛C相距l(xiāng)ac=30 m時發(fā)現(xiàn)前方車輛車速急劇降低，此時車輛做出換到避障決策，左側(cè)車道車輛B車速為vb=40 km/h，相距l(xiāng)ab=20 m，左側(cè)車道車輛D車速為vd=40 km/h，相距l(xiāng)ad=40 m，車輛C的車速為vc。本文采用謹(jǐn)慎型駕駛風(fēng)格進行軌跡規(guī)劃[21]，由第2節(jié)可知，安全走廊重疊區(qū)域的作用是對行駛時間和乘坐舒適性的隱性調(diào)節(jié)，因此安全走廊的寬度取值一般比車道有效寬度略大一些，一般取值為1.25D(D為車道有效寬度)，故安全走廊的寬度邊界為

(39)

圖5 變道避障示意圖Fig.5 Schematic diagram of obstacle avoidance by lane changing

由于算法有性調(diào)節(jié)優(yōu)化參數(shù)功能，安全邊界按照車輛以最小能耗，即原始車速行駛進行計算。根據(jù)文獻[22]取安全距離s=5 m，則在第一段安全走廊行駛的最大時間為

(40)

第1段和第2段安全走廊的消耗最大時間為

(41)

由式(41)和式(42)可得縱向安全走廊邊界為

(42)

式中：v為本車車速。取粒子數(shù)np=5，則粒子群初始化參數(shù)為

(43)

在MATLAB 2020a軟件中進行軌跡規(guī)劃仿真，并搭建基于模型預(yù)測控制的Simulink模型和車輛動力學(xué)仿真軟件Carsim進行軌跡跟蹤仿真，對規(guī)劃得到的軌跡和軌跡跟蹤得到的仿真軌跡性能進行對比驗證，同時增加與基于采用的5次多項式軌跡[23]的對比分析。換道仿真的相關(guān)參數(shù)如表1所示。

4.1 迭代優(yōu)化仿真

圖6為5次多項式采樣算法的仿真過程，圖6(a)為在三維空間的采樣圖，把其投影到橫縱向坐標(biāo)系中得到圖6(b)，最優(yōu)5次多項式的仿真時間為t=3.55 s，終止車速為v=59.04 km/h，終止坐標(biāo)為(52 m,5.4 m)?；谖⒎制教狗謱榆壽E規(guī)劃仿真結(jié)果如圖7所示，圖7(a)為PSO算法尋找綜合性能最優(yōu)的避障換道軌跡過程中產(chǎn)生的軌跡曲線簇。由圖7(b)可知PSO算法收斂速度很快，基本上可以保證在10次迭代以內(nèi)收斂，因為此算法在基于貝塞爾曲線的凸優(yōu)化中加入了隱性調(diào)節(jié)約束，即安全走廊的重疊區(qū)域，隱性調(diào)節(jié)了兩段貝塞爾連接點的位置使求解出來的軌跡更加平滑，故加速了PSO算法的收斂速度。當(dāng)粒子群適應(yīng)度值收斂時，得到綜合性能最優(yōu)變道軌跡如圖7(c)所示，兩段貝塞爾曲線連接點的坐標(biāo)為(22.26 m,3.229 m)，終點坐標(biāo)為(52.55 m,5.4 m)，每段曲線對應(yīng)的行駛時間分別為t1=1.65 s，t2=1.94 s。

表1 仿真實驗參數(shù)Table 1 Simulation experiment parameters

圖6 5次多項式采樣過程Fig.6 Quintic polynomial sampling process

圖7 基于微分平坦分層軌跡規(guī)劃仿真過程Fig.7 Simulation process of hierarchical trajectory planning based on differential flatness

4.2 硬件在環(huán)驗證

為驗證本文規(guī)劃算法的實際性能，搭建圖8所示硬件在環(huán)(HIL)實驗平臺。根據(jù)車輛參數(shù)在Carsim中搭建車輛動力學(xué)模型，通過dSPACE提供實時接口(RTI)，利用DS1006處理板在dSPACE/Autobox中下載Carsim建立的模型。在Simulink軟件中搭建基于車輛3自由度跟蹤誤差模型的模型預(yù)測控制器[24]，整個模塊由模型預(yù)測控制器、車輛動力學(xué)模型、輸出及觀測器三大模塊組成[25]，并把分層軌跡規(guī)劃算法與軌跡跟蹤算法進行集成，然后通過MATLAB/Simulink軟件提供的實時工作空間(RTW)，將算法以C代碼的形式下載到車輛控制器中。利用CAN總線實現(xiàn)dSPACE/Autobox與車輛控制器之間信息通信，在獲得車輛狀態(tài)信息后，車輛控制器將控制命令發(fā)送到dSPACE/Autobox。上位機的控制臺接口可以通過TCP/IP或CAN-bus實現(xiàn)對車輛狀態(tài)的監(jiān)控和參數(shù)調(diào)整。仿真變道過程如圖9所示，其中紅線為傳感器測量到與周邊障礙車的距離。從圖9中可以看出，規(guī)劃的軌跡能在優(yōu)化得到的時間內(nèi)完成避障變道，且不存在任何碰撞風(fēng)險。

圖8 硬件在環(huán)(HIL)實驗平臺Fig.8 Hardware-in-the-loop (HIL) experimental platform

圖9 變道仿真過程Fig.9 Lane changing simulation process

換道軌跡跟蹤仿真結(jié)果如圖10所示。從圖10(a)中軌跡跟蹤結(jié)果可知，規(guī)劃層規(guī)劃得到的軌跡可跟蹤性高，仿真得到的軌跡跟蹤誤差較小。從圖10(b)換道速度跟蹤結(jié)果圖可知：速度跟蹤整體誤差較小，可跟蹤性較好，其橫向跟蹤誤差的最大值為0.021 m；車速的最大跟蹤誤差為0.39 km/h，在初始時刻車速會略微下降是因為剛開始轉(zhuǎn)向時會使車速降低，但車輛會迅速跟上規(guī)劃得到的速度曲線，速度曲線整體平滑、沒有突變，能實現(xiàn)無碰撞變道避障。

圖10 換道軌跡跟蹤仿真結(jié)果Fig.10 Simulation results of lane changingtrajectory tracking

基于迭代得到的最優(yōu)換道軌跡的最優(yōu)換道時間和速度進行5次多項式規(guī)劃，二者的車輛換道性能參數(shù)如圖11所示。圖11(a)所示的前輪轉(zhuǎn)角曲線整體平滑，符合駕駛員變道過程中前輪轉(zhuǎn)角的變化趨勢，Bezier規(guī)劃得到的前輪轉(zhuǎn)角曲線更為平滑，且最大值為1.32°，而五次多項式規(guī)劃得到的前輪轉(zhuǎn)角最大值為1.42°；圖11(b)為換道避障過程中的側(cè)向加速度曲線，從中可知二者得到的側(cè)向加速度都小于閾值0.4g，車輛具有良好的穩(wěn)定性；圖11(c)為換道避障過程中的質(zhì)心側(cè)偏角曲線，其中Bezier規(guī)劃得到的最大質(zhì)心側(cè)偏角為4.53°，5次多項式規(guī)劃得到的最大質(zhì)心側(cè)偏角為5.23°，二者均小于閾值8.88°；圖11(d)為換道避障過程中的橫擺角速度曲線，從中可知二者規(guī)劃得到橫擺角速度均遠小于閾值，具有良好的可跟蹤性。綜上所述可知，Bezier規(guī)劃得到的車輛換道避障性能更優(yōu)，具有更平滑、更佳的可控性。

圖11 車輛換道性能參數(shù)Fig.11 Lane changing performance parameters of the vehicle

5 實車換道實驗

將算法應(yīng)用于無人駕駛平臺進行實車實驗如圖12所示。實際換道交通場景如圖13所示，設(shè)定與前方障礙物車輛距離為30 m進行換道軌跡規(guī)劃并對得到的軌跡進行跟蹤?？紤]到實際平臺的特性和實驗安全性，取初始車速20 km/h。

圖12 無人駕駛平臺Fig.12 Self-driving platform

圖13 換道交通場景Fig.13 Lane changing traffic scene

圖14為換道軌跡跟蹤結(jié)果。從圖14(a)中可以看出：跟蹤軌跡在整體趨勢下遵循參考軌跡，跟蹤誤差最大值為0.12 m，規(guī)劃層規(guī)劃得到的路徑的可跟蹤性能高；圖14(b)表明，實際無人駕駛平臺可以跟蹤上規(guī)劃層規(guī)劃得到的速度軌跡，速度曲線符合車輛運動特性，跟蹤誤差最大值為0.4 km/h；由圖14(c)可知，規(guī)劃的軌跡可執(zhí)行性高，前輪轉(zhuǎn)角曲線平滑適合無人駕駛車輛進行控制，最大誤差0.21°；此外運動過程中與障礙物車輛保持4 m以上的安全距離。綜上所述可知，基于微分平坦的分層軌跡規(guī)劃算法得到的路徑曲線和速度曲線符合車輛運動學(xué)特性且具有良好的可跟蹤性。

圖14 換道軌跡跟蹤結(jié)果Fig.14 Tracking result for lane changing trajectory

6 結(jié)論

本文提出一種基于凸二次規(guī)劃和粒子群進化優(yōu)化的分層段軌跡規(guī)劃算法。利用貝塞爾曲線的凸包性，設(shè)計安全走廊約束，把不規(guī)則的非凸可行域劃分為兩個凸的可行域，以軌跡平滑性為目標(biāo)函數(shù)構(gòu)建一個凸二次規(guī)劃器。以底層規(guī)劃器得到的橫縱向貝塞爾曲線構(gòu)建車輛運動學(xué)模型的平坦輸出，得到系統(tǒng)的輸入量和狀態(tài)量的表達式，從而可以建立行車安全行、高效性、舒適性、可執(zhí)行性的自適應(yīng)函數(shù)，利用PSO算法去優(yōu)化基于貝塞爾曲線的凸優(yōu)化的先決條件，最終搜索到一條綜合性能最優(yōu)的變道避障軌跡。通過與基于采用的五次多項式算法進行對比可知，所提出的算法得到的軌跡綜合性能更優(yōu)。

該算法通過微分平坦定理把車輛狀態(tài)量集成到軌跡規(guī)劃過程中，并通過底層凸優(yōu)化上層進化優(yōu)化的框架來保證算法的概率完備性和最優(yōu)性。未來將進一步研究結(jié)合數(shù)據(jù)驅(qū)動和車輛動力學(xué)的微分平坦的軌跡規(guī)劃算法，用于提升極端工況下無人駕駛車輛的規(guī)劃控制性能。