基于微分平坦的分層軌跡規劃算法

周孝添， 任宏斌， 蘇波， 齊志權， 汪洋

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

收稿日期：2021-11-09

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

0 引言

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

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

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

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

1 基于貝塞爾曲線的局部路徑規劃

1.1 貝塞爾曲線

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

(1)

(2)

從上述推導可知，貝塞爾曲線本質是關于變量t的多項式，由于形狀由每個節點的坐標決定，具有如下性質：

1)它總是從第1個控制點開始，到最后一個控制點結束，不經過中間其他節點，但是曲線始終在所有節點連接構成的凸多邊形內。

2)曲線每個節點的導數由曲線節點的線性組合求得，即曲線的導數同樣也可以被約束在凸多邊形內。

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

1.2 避障軌跡規劃

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

圖1 避障軌跡規劃Fig.1 Obstacle avoidance trajectory planning

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

(3)

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

由于貝塞爾曲線的安全走廊存在一定范圍內的重疊，使得優化問題具有更高的自由度，對每一段貝塞爾曲線時間分配進行隱性調節。把三維空間中的軌跡規劃問題分解為兩個二維空間中的貝塞爾曲線優化問題，建立以曲線3次導數的平方和為目標函數：

(4)

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

設節點i的第f階導數為

(5)

(6)

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

(7)

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

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

(8)

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

(9)

(10)

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

2 車輛運動學模型的平坦輸出

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

(11)

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

(12)

使得系統的狀態量以及輸入量都能用輸出量z和z的有限階導數表示：

(13)

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

圖2 狀態空間與平坦空間的映射關系Fig.2 Mapping between state space and flat space

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

圖3 車輛運動學模型Fig.3 Vehicle kinematics model

則車輛的非完整性約束為

(14)

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

(15)

根據阿克曼轉角的幾何關系可以得到轉彎半徑R：

(16)

式中：ρ為曲率半徑。

以車輛狀態向量作為狀態量，以車速v和前輪轉角的角速度ω為模型輸入量，建立如下運動學模型：

(17)

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

(18)

(19)

(20)

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

由運動學模型可得

(21)

進一步推導可得

(22)

(23)

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

(24)

從而可以得到前輪轉角表達式：

(25)

3 基于粒子群的二次優化算法

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

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

首先設置粒子群內的粒子數np，由此可得粒子群矩陣如下：

X=(X1,X2,…,Xnp)

(26)

種群中每一個粒子包含3個參數：

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

(27)

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

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

(28)

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

(29)

(30)

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

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

Ft1=ω1dist(obs,Bezier)

(31)

式中：ω1為指標權重系數；obs表示周邊障礙物的全局坐標。

橫向乘坐舒適性指標是前輪轉角及其變化率，縱向舒適性評價指標為縱向加加速度，則總的舒適性評價指標為

(32)

式中：ω2、ω3為指標權重系數；xj、yj分別為橫縱向坐標；g為重力加速度。

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

(33)

式中：ω4為指標權重系數。

(34)

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

同理，利用Sigmoid函數設計質心側偏角和橫擺角速度評價指標：

(35)

(36)

式中：ω5、ω6為指標權重系數。

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

Ft6=ω7·s1+ω8·s2

(37)

式中：ω7、ω8為指標權重系數。故粒子自適應函數為

(38)

式中：w為貝塞爾曲線的粒子數，w∈{1,2,…,8}。式(31)～式(38)中，指標權重系數ω1,ω2,…,ω8根據不同的駕駛風格進行選取。

4 數值仿真實驗

換道避障是自動駕駛汽車實現結構化道路上無人駕駛的基礎技術，雙移線避障又可以看作單移線的疊加和改進。因此在考慮周圍靜態和動態車輛的環境下，如何規劃出一條綜合性能最優的行駛軌跡，就成為結構化道路下局部路徑規劃技術的關鍵。

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

(39)

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

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

(40)

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

(41)

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

(42)

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

(43)

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

4.1 迭代優化仿真

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

表1 仿真實驗參數Table 1 Simulation experiment parameters

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

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

4.2 硬件在環驗證

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

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

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

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

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

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

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

5 實車換道實驗

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

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

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

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

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

6 結論

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

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