基于自適應擬合的智能車換道避障軌跡規(guī)劃*

李 軍，周 偉，唐 爽

（1.重慶交通大學機電與車輛工程學院，重慶 400074；2.軌道交通車輛系統(tǒng)集成與控制重慶市重點實驗室，重慶 400074）

前言

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

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

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

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

1 動態(tài)換道避障場景

1.1 定義坐標系及換道避障場景

為描述車輛的運動定義兩個坐標系：慣性坐標系XOY和車身坐標系xoy。當前車輛在直道進行換道避障，被控車輛行駛車道中心線前方和目標車道中心線上對應存在以一定速度行駛的障礙車輛，如圖1所示。

圖1 坐標系及換道避障場景

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

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

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

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

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

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

2 自適應軌跡擬合算法

2.1 五次貝塞爾曲線表達形式

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

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

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

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

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

貝塞爾曲線在任一點的曲率k可以表示為

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

2.2 軌跡拼接輔助點設(shè)計

在每兩段相鄰曲線處補充設(shè)計一個輔助控制點Q1，貝塞爾曲線拼接示意圖如圖2 所示。最優(yōu)輔助控制點Q1滿足條件如下：

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

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

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

2.3 軌跡自適應擬合流程

可以得到1 階貝塞爾曲線的控制點坐標，表示如下：

式中：p1，1、p1，2、p1，3、p1，4、p1，5為貝塞爾曲線的1 階控制點坐標；p0、p1、p2、p3、p4、p5為貝塞爾曲線的控制點坐標。

可以得到2 階貝塞爾曲線的控制點坐標，表示如下：

式中p2，1、p2，2、p2，3、p2，4為貝塞爾曲線的2 階控制點坐標。

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

式中p3，1、p3，2、p3，3為貝塞爾曲線的3階控制點坐標。

可以得到4 階貝塞爾曲線的控制點坐標，表示如下：

式中p4，1、p4，2為貝塞爾曲線的4階控制點坐標。

可以得到5 階貝塞爾曲線的控制點坐標，表示如下：

式中p5，1為貝塞爾曲線的5階控制點坐標。

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

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

圖3 軌跡自適應擬合流程

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

3.1 車輛避障函數(shù)

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

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

3.2 離散化控制模型

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

3.2.2 線性化控制模型

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

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

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

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

3.2.3 離散化控制模型

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

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

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

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

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

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

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

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

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

軌跡跟蹤車輛模型選用考慮車輛輪胎側(cè)偏特性的2 自由度車輛動力學模型［28］，換道避障軌跡跟蹤控制器采用文獻［2］中的模型預測標準目標函數(shù)。

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

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

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

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

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

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

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

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

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

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

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

A.障礙車輛1 和障礙車輛2 以初始時刻的運動狀態(tài)勻速行駛。

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

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

同時，為驗證本文中提出的自適應擬合算法的擬合軌跡的有效性，將自適應擬合算法的擬合軌跡與多項式擬合軌跡進行仿真對比分析。

車輛以方案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é)果

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

車輛以方案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 可以看出，在擬合較多的離散點時，本文提出的自適應擬合算法的擬合軌跡相比于多項式擬合軌跡更平滑。

4.3 自適應擬合算法擬合殘差

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

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

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

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

4.4 被控車輛動力學參數(shù)分析

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

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

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

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

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

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

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

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

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

5 結(jié)論

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

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

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

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