李 軍，周 偉，唐 爽

（1.重慶交通大學(xué)機(jī)電與車輛工程學(xué)院，重慶 400074；2.軌道交通車輛系統(tǒng)集成與控制重慶市重點(diǎn)實(shí)驗(yàn)室，重慶 400074）

前言

換道避障軌跡規(guī)劃是自動(dòng)駕駛車輛領(lǐng)域的關(guān)鍵技術(shù)之一［1］，穩(wěn)定的換道避障行為能夠提高車輛通行效率。換道避障軌跡規(guī)劃需要在安全避開(kāi)障礙物的前提下規(guī)劃一條連續(xù)且光滑的換道軌跡［2］。目前常用的避障軌跡規(guī)劃方法可以分為基于參數(shù)化曲線的算法［3］、基于網(wǎng)格的搜索算法［4］、基于采樣的規(guī)劃方法［5］、人工勢(shì)場(chǎng)算法［6］和基于離散化規(guī)劃的算法［7-9］5類?；趨?shù)化曲線算法的軌跡規(guī)劃方法由于路徑曲線形狀的限制存在不易實(shí)現(xiàn)實(shí)時(shí)避障的缺點(diǎn)?；诰W(wǎng)格的啟發(fā)式搜索軌跡規(guī)劃方法在進(jìn)行較大區(qū)域軌跡規(guī)劃時(shí)存在計(jì)算量較大的問(wèn)題?；诓蓸拥能壽E規(guī)劃方法由于采樣隨機(jī)的原因存在計(jì)算量較大和路徑不夠平滑的缺點(diǎn)?；谌斯?shì)場(chǎng)算法的軌跡規(guī)劃方法由于沒(méi)有考慮車輛運(yùn)動(dòng)學(xué)約束的原因存在所規(guī)劃的軌跡不可行的可能。基于離散化優(yōu)化的軌跡規(guī)劃方法對(duì)車輛行駛道路進(jìn)行有序的采樣，不存在難以避障、計(jì)算量大、盲目性大和所規(guī)劃的軌跡不可行的缺點(diǎn)。

模型預(yù)測(cè)控制算法的優(yōu)點(diǎn)是可以在控制過(guò)程中添加多種約束［10-16］，因此，模型預(yù)測(cè)控制算法被廣泛用于自動(dòng)駕駛車輛［17-27］、機(jī)器人和無(wú)人機(jī)等領(lǐng)域的軌跡規(guī)劃。Hu 等［22］基于模型預(yù)測(cè)控制算法搭建了車輛軌跡規(guī)劃和跟蹤的集成框架，能夠成功完成靜態(tài)和動(dòng)態(tài)障礙物的規(guī)避。Wang 等［23］基于模型預(yù)測(cè)控制算法設(shè)計(jì)了適合點(diǎn)障礙物的軌跡規(guī)劃方法，在動(dòng)態(tài)避障環(huán)境中取得了較好的效果。Gao 等［24］搭建了模型避障預(yù)測(cè)軌跡規(guī)劃的框架，實(shí)現(xiàn)了不同速度工況下的避障軌跡規(guī)劃，但軌跡規(guī)劃控制器未考慮軌跡的曲率問(wèn)題。Liu 等［25］基于模型預(yù)測(cè)控制設(shè)計(jì)了高速工況下的軌跡規(guī)劃方案，在靜態(tài)避障環(huán)境下獲得了良好效果，但沒(méi)有考察動(dòng)態(tài)避障環(huán)境。Li等［26］結(jié)合人工勢(shì)場(chǎng)理論，綜合考慮車輛狀態(tài)信息和障礙物的相對(duì)位置及速度信息，建立了懲罰函數(shù)，引入了優(yōu)化目標(biāo)函數(shù)，基于模型預(yù)測(cè)算法設(shè)計(jì)了避障軌跡規(guī)劃的方案，實(shí)現(xiàn)了車輛的靜態(tài)和動(dòng)態(tài)避障軌跡規(guī)劃，但未考慮到障礙物與車輛的相對(duì)最小距離。Park 等［27］考慮車輛與障礙物的視差角設(shè)計(jì)避障函數(shù)，基于模型預(yù)測(cè)算法設(shè)計(jì)避障軌跡規(guī)劃方案，實(shí)現(xiàn)了不同速度工況下的車輛避障軌跡規(guī)劃，但沒(méi)有考慮到速度對(duì)避障軌跡規(guī)劃的影響。

針對(duì)以上問(wèn)題，本文定義坐標(biāo)系和動(dòng)態(tài)換道避障場(chǎng)景，搭建被控車輛和障礙車輛模型，確定車輛換道避障軌跡規(guī)劃的行駛約束條件，考慮模型預(yù)測(cè)控制軌跡規(guī)劃算法提供的不包含曲率信息的一系列離散點(diǎn)可能生成不可行軌跡的問(wèn)題，設(shè)計(jì)一種基于五次貝塞爾曲線并考慮曲率信息的自適應(yīng)軌跡拼接擬合方法，進(jìn)一步基于模型預(yù)測(cè)控制算法設(shè)計(jì)自動(dòng)駕駛車輛換道避障軌跡規(guī)劃方法。

總之，本文提供了一種面向動(dòng)態(tài)環(huán)境的自動(dòng)駕駛車輛換道避障軌跡規(guī)劃方法，所提出的方法基于五次貝塞爾曲線自適應(yīng)分段擬合換道避障軌跡規(guī)劃的離散點(diǎn)序列，擬合軌跡包含曲率信息。所提出的方法基于模型預(yù)測(cè)控制算法進(jìn)行軌跡規(guī)劃，該方法考慮障礙車輛模型。因此，該方法可用于高速動(dòng)態(tài)避障環(huán)境。

1 動(dòng)態(tài)換道避障場(chǎng)景

1.1 定義坐標(biāo)系及換道避障場(chǎng)景

為描述車輛的運(yùn)動(dòng)定義兩個(gè)坐標(biāo)系：慣性坐標(biāo)系XOY和車身坐標(biāo)系xoy。當(dāng)前車輛在直道進(jìn)行換道避障，被控車輛行駛車道中心線前方和目標(biāo)車道中心線上對(duì)應(yīng)存在以一定速度行駛的障礙車輛，如圖1所示。

圖1 坐標(biāo)系及換道避障場(chǎng)景

1.2 換道避障軌跡規(guī)劃車輛行駛條件

避障軌跡規(guī)劃的需要在滿足車輛行駛條件下最小化參考軌跡與規(guī)劃軌跡之間的偏差，車輛行駛條件如下：

式中：vmin、v和vmax分別為允許最小車速、車速和允許最大車速；Rmin、R分別為車輛允許最小轉(zhuǎn)彎半徑、車輛轉(zhuǎn)彎半徑；amin和amax分別為車輛最小橫向加速度和車輛最大橫向加速度；b1和b2分別為兩條道路邊界線的橫向坐標(biāo)；Ym為規(guī)劃軌跡的橫向坐標(biāo)；μ為路面附著系數(shù)；g為重力加速度；W為車輛寬度。

1.3 被控車輛和障礙物車輛模型

對(duì)于動(dòng)態(tài)避障環(huán)境，需要搭建被控車輛和障礙車輛模型，換道避障軌跡規(guī)劃采用忽略車身尺寸和輪胎受力情況的剛體運(yùn)動(dòng)學(xué)模型，該模型可以表示如下［2］：

式中：ax和ay分別為車身坐標(biāo)系下車輛縱向加速度和橫向加速度；ψ和φ分別為車輛橫擺角和航向角；分別是車輛在慣性坐標(biāo)系中的縱向速度和橫向速度。

2 自適應(yīng)軌跡擬合算法

2.1 五次貝塞爾曲線表達(dá)形式

基于五次貝塞爾曲線的自適應(yīng)軌跡擬合算法被設(shè)計(jì)用以擬合軌跡規(guī)劃的離散點(diǎn)序列。

貝塞爾曲線［2］可以表示為

式中：q（τi）為在參數(shù)τi處的插值點(diǎn)；m為貝塞爾曲線的階次；Pk為貝塞爾曲線的控制點(diǎn)。

設(shè)P0、P1、P2、P3、P4、P5為貝塞爾曲線的控制點(diǎn)，五次貝塞爾曲線可以表示為

轉(zhuǎn)變?yōu)榫仃嚨男问娇梢员硎緸?/p>

貝塞爾曲線在任一點(diǎn)的曲率k可以表示為

由于貝塞爾曲線是被用于離散點(diǎn)序列的擬合，根據(jù)離散點(diǎn)的坐標(biāo)貝塞爾曲線的曲率可以表示為

2.2 軌跡拼接輔助點(diǎn)設(shè)計(jì)

在每?jī)啥蜗噜徢€處補(bǔ)充設(shè)計(jì)一個(gè)輔助控制點(diǎn)Q1，貝塞爾曲線拼接示意圖如圖2 所示。最優(yōu)輔助控制點(diǎn)Q1滿足條件如下：

圖2 貝塞爾曲線拼接示意圖

式中Q0、Q2和Q3分別是下一段拼接軌跡上除輔助控制點(diǎn)以外的前3個(gè)控制點(diǎn)。

其中，式（8）表示最小化線段P5-P4、線段P4-P3、線段P3-P2、線段Q2-Q1和線段Q1-Q0代數(shù)和的長(zhǎng)度。式（9）分別表示Q1處于Q0（P5）與Q2中間的合適位置；線段Q1-Q0的長(zhǎng)度小于線段Q2-Q1的長(zhǎng)度；線段P5-P4和線段Q1-Q0共線；P5=Q0表示第1 段末尾點(diǎn)與第2段第1個(gè)點(diǎn)重合。

2.3 軌跡自適應(yīng)擬合流程

可以得到1 階貝塞爾曲線的控制點(diǎn)坐標(biāo)，表示如下：

式中：p1，1、p1，2、p1，3、p1，4、p1，5為貝塞爾曲線的1 階控制點(diǎn)坐標(biāo)；p0、p1、p2、p3、p4、p5為貝塞爾曲線的控制點(diǎn)坐標(biāo)。

可以得到2 階貝塞爾曲線的控制點(diǎn)坐標(biāo)，表示如下：

式中p2，1、p2，2、p2，3、p2，4為貝塞爾曲線的2 階控制點(diǎn)坐標(biāo)。

可以得到3階貝塞爾曲線的控制點(diǎn)，表示如下：

式中p3，1、p3，2、p3，3為貝塞爾曲線的3階控制點(diǎn)坐標(biāo)。

可以得到4 階貝塞爾曲線的控制點(diǎn)坐標(biāo)，表示如下：

式中p4，1、p4，2為貝塞爾曲線的4階控制點(diǎn)坐標(biāo)。

可以得到5 階貝塞爾曲線的控制點(diǎn)坐標(biāo)，表示如下：

式中p5，1為貝塞爾曲線的5階控制點(diǎn)坐標(biāo)。

擬合殘差的平方和可以表示為

基于五次貝塞爾曲線的軌跡規(guī)劃的離散點(diǎn)序列擬合流程如圖3所示。

圖3 軌跡自適應(yīng)擬合流程

3 車輛換道避障模型預(yù)測(cè)軌跡規(guī)劃

3.1 車輛避障函數(shù)

考慮到車速的影響以及懲罰函數(shù)在避障目標(biāo)函數(shù)中所占的比例，避障函數(shù)［26］由下式給出：

式中：Jobs，i為避障函數(shù)；Sobs為避障權(quán)重；dmin為車輛距離障礙物的最小距離；ζ是一個(gè)極小的正數(shù)，用于避免分母為零。

3.2 離散化控制模型

3.2.1 狀態(tài)變量選取

3.2.2 線性化控制模型

式（17）是連續(xù)非線性方程，需要將該方程進(jìn)行線性化和離散化。

采用泰勒展開(kāi)，式（17）可以轉(zhuǎn)變?yōu)?/p>

式中：X0和u0為狀態(tài)方程當(dāng)前時(shí)刻的輸入；J（X）和J（u）為雅可比矩陣。

式（18）可以轉(zhuǎn)變?yōu)榫€性化狀態(tài)方程：

3.2.3 離散化控制模型

對(duì)式（19）進(jìn)行離散化處理，則式（19）轉(zhuǎn)變?nèi)缦拢?/p>

模型預(yù)測(cè)控制系統(tǒng)預(yù)測(cè)輸出與當(dāng)前狀態(tài)的關(guān)系為

其中，避障軌跡規(guī)劃控制器的橫向位移輸出矩陣為

避障軌跡規(guī)劃控制器的狀態(tài)矩陣表示如下：

避障軌跡規(guī)劃控制器的控制矩陣表示如下：

3.3 車輛換道避障軌跡規(guī)劃目標(biāo)函數(shù)

車輛換道避障軌跡規(guī)劃的控制目標(biāo)是避開(kāi)障礙物的條件下最小化規(guī)劃軌跡與參考軌跡的偏差，車輛換道避障軌跡規(guī)劃目標(biāo)函數(shù)可以表示為

式中：Q和R為權(quán)重矩陣；η和ηref分別為避障軌跡規(guī)劃控制器的預(yù)測(cè)軌跡和參考軌跡；umax和umin分別為控制量的最大值和最小值；△umax和△umin分別為控制增量的最大值和最小值。

3.4 車輛換道避障軌跡跟蹤目標(biāo)函數(shù)

軌跡跟蹤車輛模型選用考慮車輛輪胎側(cè)偏特性的2 自由度車輛動(dòng)力學(xué)模型［28］，換道避障軌跡跟蹤控制器采用文獻(xiàn)［2］中的模型預(yù)測(cè)標(biāo)準(zhǔn)目標(biāo)函數(shù)。

3.5 車輛換道避障穩(wěn)定性分析

為驗(yàn)證本文提出的軌跡規(guī)劃方法的穩(wěn)定性，利用橫擺角速度約束方程式（27）和質(zhì)心側(cè)偏角約束方程式（28）形成閉合包絡(luò)，如圖4所示［28］。

圖4 穩(wěn)定性約束包絡(luò)示意圖

車輛橫擺角速度的控制邊界可以表示如下：

式中：r是車輛的橫擺角速度；vx是車輛的縱向速度。

車輛質(zhì)心側(cè)偏角的控制邊界可以表示如下：

式中：β為車輛的質(zhì)心側(cè)偏角；Cαr為后輪等效側(cè)偏剛度；a和b分別為從車輛質(zhì)心到前軸和后軸的距離。

4 動(dòng)態(tài)避障仿真結(jié)果與分析

4.1 動(dòng)態(tài)避障方案設(shè)定

為驗(yàn)證本文車輛換道避障軌跡規(guī)劃方案的有效性，按照A、B 兩種方案進(jìn)行車輛動(dòng)態(tài)換道避障軌跡規(guī)劃仿真分析。仿真車輛模型的基本參數(shù)如表1所示。

表1 車輛模型基本參數(shù)

A.障礙車輛1 和障礙車輛2 以初始時(shí)刻的運(yùn)動(dòng)狀態(tài)勻速行駛。

B.障礙車輛1 以1 m/s2加速度加速到指定速度后以指定速度勻速行駛；障礙車輛2 以初始時(shí)刻的運(yùn)動(dòng)狀態(tài)勻速行駛。

4.2 動(dòng)態(tài)環(huán)境換道避障軌跡規(guī)劃驗(yàn)證

同時(shí)，為驗(yàn)證本文中提出的自適應(yīng)擬合算法的擬合軌跡的有效性，將自適應(yīng)擬合算法的擬合軌跡與多項(xiàng)式擬合軌跡進(jìn)行仿真對(duì)比分析。

車輛以方案A 在不同速度工況下?lián)Q道避障軌跡規(guī)劃結(jié)果如圖5所示。在圖5（a）中，障礙車輛1和障礙車輛2 在圖示位置以56 km/h 的速度勻速行駛，自車以70 km/h的初速度在圖示位置行駛，定義為工況1。從圖5（a）可以看出，本文的換道避障軌跡規(guī)劃方案在工況1 能夠完成障礙物的規(guī)避。在圖5（b）中，障礙車輛1和障礙車輛2在圖示位置以64 km/h的速度勻速行駛，自車以80 km/h的初速度在圖示位置行駛，定義為工況2。從圖5（b）可以看出，本文的換道避障軌跡規(guī)劃方法在工況2 能夠完成障礙物的規(guī)避。在圖7（c）中，障礙車輛1和障礙車輛2在圖示位置以72 km/h 的速度勻速行駛，自車以90 km/h 的初速度在圖示位置行駛，定義為工況3。從圖5（c）可以看出，本文的換道避障軌跡規(guī)劃方案能夠在工況3完成障礙物的規(guī)避。

圖5 方案A在不同工況下?lián)Q道避障軌跡規(guī)劃結(jié)果

同時(shí)，從圖5 可以看出，多項(xiàng)式擬合在擬合較多的離散點(diǎn)時(shí)，為逼近所有的點(diǎn)，會(huì)在擬合開(kāi)始和結(jié)束時(shí)產(chǎn)生震蕩現(xiàn)象，由于貝塞爾曲線的凸包特性，本文提出的自適應(yīng)擬合算法的擬合軌跡與多項(xiàng)式擬合軌跡相比更平滑。

車輛以方案B 在不同速度工況下?lián)Q道避障軌跡規(guī)劃結(jié)果如圖6 所示。在圖6（a）中，障礙車輛1 以1 m/s2加速度在圖示位置以56 km/h的初速度加速至70 km/h，障礙車輛2在圖示位置以56 km/h的速度勻速行駛，自車以70 km/h 的初速度在圖示位置行駛，定義為工況1。從圖6（a）可以看出，本文的換道避障軌跡規(guī)劃方法在工況1 能夠完成障礙物的規(guī)避。在圖6（b）中，障礙車輛1以1 m/s2加速度在圖示位置以64 km/h 的初速度加速至80 km/h，障礙車輛2 在圖示位置以64 km/h的速度勻速行駛，自車以80 km/h 的初速度在圖示位置行駛，定義為工況2。從圖6（b）可以看出，本文的換道避障軌跡規(guī)劃方法在工況2 能夠完成障礙物的規(guī)避。在圖6（c）中，障礙車輛1以1 m/s2加速度在圖示位置以72 km/h的初速度加速至90 km/h，障礙車輛2 在圖示位置以72 km/h 的速度勻速行駛，自車以90 km/h的初速度在圖示位置行駛定義為工況3。從圖6（c）可以看出，本文的換道避障軌跡規(guī)劃方法能夠在工況3 完成障礙物的規(guī)避。

圖6 方案B不同工況下?lián)Q道避障軌跡規(guī)劃結(jié)果

從圖6 可以看出，在擬合較多的離散點(diǎn)時(shí)，本文提出的自適應(yīng)擬合算法的擬合軌跡相比于多項(xiàng)式擬合軌跡更平滑。

4.3 自適應(yīng)擬合算法擬合殘差

車輛以方案A 在不同工況下?lián)Q道避障軌跡規(guī)劃提供的離散點(diǎn)序列與貝塞爾曲線對(duì)應(yīng)點(diǎn)的擬合殘差如圖7 所示。從圖7 可以看出，車輛以方案A 在不同工況下?lián)Q道避障軌跡規(guī)劃提供的離散點(diǎn)序列與貝塞爾曲線對(duì)應(yīng)點(diǎn)的擬合殘差都保持在擬合閾值以內(nèi)。

圖7 方案A在不同工況下?lián)Q道避障軌跡擬合殘差

車輛以方案B 在不同工況下?lián)Q道避障軌跡規(guī)劃提供的離散點(diǎn)序列與貝塞爾曲線對(duì)應(yīng)點(diǎn)的擬合殘差如圖8 所示。從圖8 可以看出，車輛以方案B在不同工況下?lián)Q道避障軌跡規(guī)劃提供的離散點(diǎn)序列與貝塞爾曲線對(duì)應(yīng)點(diǎn)的擬合殘差都保持在擬合閾值以內(nèi)。

圖8 方案B在不同工況下?lián)Q道避障軌跡擬合殘差

4.4 被控車輛動(dòng)力學(xué)參數(shù)分析

車輛以方案A 在不同速度工況下?lián)Q道避障軌跡規(guī)劃相關(guān)動(dòng)力學(xué)參數(shù)曲線如圖9所示。由圖9可知，被控車輛的前輪轉(zhuǎn)角、橫向加速度和輪胎側(cè)偏角都處于執(zhí)行機(jī)構(gòu)的機(jī)械飽和約束范圍內(nèi)。

圖9 方案A 在不同工況下?lián)Q道避障軌跡規(guī)劃的前輪轉(zhuǎn)角、橫向加速度和輪胎側(cè)偏角

車輛以方案B 在不同工況下?lián)Q道避障軌跡規(guī)劃相關(guān)動(dòng)力學(xué)參數(shù)曲線如圖10所示。由圖10可知，被控車輛的前輪轉(zhuǎn)角、橫向加速度和輪胎側(cè)偏角都處于執(zhí)行機(jī)構(gòu)的機(jī)械飽和約束范圍內(nèi)。

圖10 方案B 在不同工況下?lián)Q道避障軌跡規(guī)劃的前輪轉(zhuǎn)角、橫向加速度和輪胎側(cè)偏角

4.5 換道避障軌跡規(guī)劃方法穩(wěn)定性分析

車輛以方案A 在不同速度工況下?lián)Q道避障軌跡規(guī)劃穩(wěn)定性驗(yàn)證結(jié)果如圖11 所示。由圖11 可知，車輛的橫擺角速度和質(zhì)心側(cè)偏角一直處于包絡(luò)之內(nèi)，說(shuō)明被控車輛在不同工況下都處于穩(wěn)定狀態(tài)。

圖11 方案A在不同工況下的穩(wěn)定性約束驗(yàn)證

車輛以方案B 在不同速度工況下?lián)Q道避障軌跡規(guī)劃穩(wěn)定性驗(yàn)證結(jié)果如圖12所示。由圖12可知，車輛的橫擺角速度和質(zhì)心側(cè)偏角一直處于包絡(luò)之內(nèi)，說(shuō)明被控車輛處于穩(wěn)定狀態(tài)。

圖12 方案B在不同工況下的穩(wěn)定性約束驗(yàn)證

5 結(jié)論

（1）定義了動(dòng)態(tài)換道避障軌跡規(guī)劃環(huán)境，進(jìn)而確定車輛換道避障軌跡規(guī)劃的行駛約束條件，搭建了軌跡規(guī)劃被控車輛和障礙車輛模型。

（2）設(shè)計(jì)了基于五次貝塞爾曲線的自適應(yīng)分段擬合算法擬合換道避障軌跡規(guī)劃的離散點(diǎn)序列，擬合軌跡連續(xù)且光滑，換道避障軌跡規(guī)劃提供的離散點(diǎn)序列與貝塞爾曲線對(duì)應(yīng)點(diǎn)的擬合殘差都保持在擬合閾值內(nèi)。

（3）基于模型預(yù)測(cè)控制算法設(shè)計(jì)了換道避障軌跡規(guī)劃方法。仿真結(jié)果驗(yàn)證了本文中提出的自動(dòng)駕駛車輛換道避障模型預(yù)測(cè)軌跡規(guī)劃方法在動(dòng)態(tài)環(huán)境下的有效性和穩(wěn)定性。

后續(xù)可研究雷達(dá)和視覺(jué)系統(tǒng)等信息設(shè)備與避障軌跡規(guī)劃控制的集成框架，并對(duì)研究進(jìn)行實(shí)車驗(yàn)證。