唐曉峰，楊 林，袁靜妮

（上海交通大學(xué)機械與動力工程學(xué)院，上海 200240）

前言

自動駕駛車輛主要采用雷達、機器視覺、地圖導(dǎo)航等傳感器實現(xiàn)對道路環(huán)境的感知，應(yīng)用決策、規(guī)劃和控制技術(shù)實現(xiàn)避障功能，其中合理的決策與路徑規(guī)劃是實現(xiàn)自動駕駛車輛進行避障的核心環(huán)節(jié)之一。目前常用的自動駕駛路徑規(guī)劃方法主要有4類：第1類方法是基于幾何模型搜索方法［1］，例如常用的單元體分解法、可視圖法等，其原理是利用圖像搜索方法搜索可行區(qū)域，其缺點是只關(guān)注避障路徑本身，很難增加車輛動力學(xué)的安全性約束；第2類方法是基于力學(xué)場和導(dǎo)航方法，例如人工勢場方法和諧波函數(shù)場等，其原理是將道路和動靜障礙物抽象為具有引力或斥力的勢能場，以過程中勢能最小為目標(biāo)進行規(guī)劃路徑，其缺點是算法只能產(chǎn)生瞬時指令，因此其行為不可預(yù)測［2］；第3類方法是基于元啟發(fā)式方法，例如粒子群算法、遺傳算法等，其原理是結(jié)合啟發(fā)式算法，在復(fù)雜的非結(jié)構(gòu)化約束下隨機找到有效的近似最優(yōu)解，缺點是由于搜索空間是離散的，所以生成的軌跡不平滑且計算量大［3］；第4類是基于數(shù)學(xué)優(yōu)化方法，例如常見的線性二次型、模型預(yù)測控制等，其原理是采用數(shù)學(xué)優(yōu)化的方法處理數(shù)值最小的成本函數(shù)，包含了嚴(yán)格且系統(tǒng)性較高的車輛動力學(xué)約束和各類別的安全約束等。將自動駕駛車輛的路徑規(guī)劃問題轉(zhuǎn)換成性能指標(biāo)的優(yōu)化問題是未來的發(fā)展趨勢。

模型預(yù)測控制方法是優(yōu)化路徑的一種常用算法，它具有建立約束模型、車輛動力學(xué)模型和道路模型的優(yōu)點。然而，自動駕駛車輛存在傳感器失效或不準(zhǔn)確、車輛的位置與航向角等狀態(tài)無法準(zhǔn)確測量、車輛駛過特定道路時需要多階段軌跡優(yōu)化和不同狀態(tài)的約束等情況，因此模型預(yù)測控制存在處理問題時要求狀態(tài)邊界準(zhǔn)確性較高且容易陷入局部最優(yōu)問題等缺點。據(jù)此，本文中提出了基于高斯偽譜法的自動駕駛車輛的多階段軌跡優(yōu)化方法。偽譜法最早應(yīng)用于航空航天領(lǐng)域，例如當(dāng)火箭發(fā)射升空時會呈現(xiàn)多階段軌跡和每一階段的狀態(tài)約束變化，偽譜法能實現(xiàn)多階段軌跡優(yōu)化和路徑規(guī)劃的全局最優(yōu)。將偽譜法應(yīng)用到自動駕駛車輛的路徑規(guī)劃中，有助于實現(xiàn)自動駕駛在多個道路階段的軌跡優(yōu)化，且可根據(jù)每一階段的要求設(shè)定車輛行駛狀態(tài)的不同約束，以實現(xiàn)車輛在路徑規(guī)劃上的全局最優(yōu)。偽譜法包括勒讓德偽譜法、高斯偽譜法和切比雪夫偽譜法3種，其特點是采用全局正交多項式逼近狀態(tài)變量和控制變量，具有快速收斂、對初值敏感度不高和可設(shè)立多階段的軌跡優(yōu)化等優(yōu)點。其中，高斯偽譜法結(jié)合了有限元區(qū)間劃分的思想進行自適應(yīng)調(diào)節(jié)，求解精度更高［4］。本文中以車輛自動駛過特定的道路（容易發(fā)生交通事故的區(qū)域）為模型，研究基于高斯偽譜法的多階段軌跡優(yōu)化問題。

1 道路空間自動駕駛路徑規(guī)劃模型

1.1 環(huán)境建模

自動駕駛車輛通常根據(jù)預(yù)測的確定性障礙物的未來行駛意圖規(guī)劃其自身的軌跡，然而不確定性動態(tài)障礙物的行駛意圖很難預(yù)測，因此通常將車輛置于特定的行駛環(huán)境中，進行環(huán)境建模來規(guī)劃路徑。環(huán)境模型通常分為決策性模型、基于集類、隨機類和基于場景模型等4類，決策性環(huán)境建模為每個場景的障礙物提供單一的預(yù)測軌跡，其特點是簡化了控制過程，但無法處理不確定的情況；基于集類的環(huán)境建模是給定一個預(yù)測模型和關(guān)系來近似目標(biāo)車輛的未來空間；隨機類環(huán)境建模是使用可能性分配函數(shù)將駕駛行為模型化，缺點是難以計及交通規(guī)則與車輛之間的關(guān)系，增加了復(fù)雜性；基于場景的環(huán)境建模可充分克服上述缺點，可描述不確定的行駛環(huán)境，同時增加了交通場景的理解程度［5］。本文中采用基于場景的環(huán)境建模來研究自動駕駛車輛在特定道路環(huán)境下的軌跡規(guī)劃問題。

為不失一般性，本文中建立了如圖1所示的場景模型。假設(shè)主車1要經(jīng)過道路前方容易發(fā)生交通事故的區(qū)域，其中L段較長距離為主車1的起始行駛區(qū)域或周圍車輛的換道行駛區(qū)域，在容易發(fā)生交通事故的區(qū)域有3輛車，主車2、6是臨近車道的周圍車輛，其車道保持行駛或換道行駛狀態(tài)影響主車1的行駛狀態(tài)約束設(shè)置，換道行駛須在L段距離內(nèi)完成。為便于更好地實現(xiàn)主車對前方車輛的有效感知，在道路環(huán)境上設(shè)定了虛擬車輛，當(dāng)主車臨近虛擬車輛位置時，可通過通信技術(shù)通知主車實時調(diào)整路徑軌跡和行駛狀態(tài)，例如加減速、巡航或換道等行駛行為。從圖1中可以看出，只要進行主車1在L1和L2段距離的軌跡規(guī)劃和車輛自身及周圍車輛的行駛狀態(tài)約束設(shè)置，便可實現(xiàn)主車1對前方道路環(huán)境的有效感知。

圖1 基于場景的自動駕駛環(huán)境建模

首先定義虛擬車輛與車輛4的相對安全距離，采用傳統(tǒng)的Mazda模型［6］，當(dāng)主車1行駛到虛擬車輛的位置時，須采取合理的行駛行為，保證車輛的安全行駛。

定義1：虛擬車輛位置與車輛4位置的相對制動安全距離設(shè)定為

式中：v為虛擬車輛的速度；vrel為相對車速；a1為虛擬車輛的最大減速度；a2為車輛4的最大減速度；t1為處理器等響應(yīng)延遲時間；t2為制動器延遲時間；t3為傳感器感知車輛4的延遲時間；d0為最小停車距離。

L3段距離的約束局限范圍是：L3≥d0

其次定義虛擬車輛的形狀。采用基于P準(zhǔn)則的障礙物模型構(gòu)建函數(shù)，可方便模擬車輛為正方形、矩形或者圓形［7］，主要形式為

1.2 車輛動力學(xué)建模

車輛的動力學(xué)特性會影響自動駕駛車輛的行駛軌跡和跟蹤控制等任務(wù)，因此，本文中增加了車輛動力學(xué)模型，并將其簡化為如圖2所示的模型［8］，車輛動力學(xué)模型可描述如下：

式中：x、y、θ為車輛給定的位置和行駛方向；v、L為車輛速度和軸距；γ為橫擺角；a、w為車輛的加速度和橫擺角速度。令u為控制變量，u＝［a，w］T。

圖2 車輛動力學(xué)模型

1.3 邊界條件與約束

自動駕駛車輛在進行軌跡規(guī)劃時須考慮車輛動力學(xué)約束、交通規(guī)則和每一階段的起始和終了條件，以保證車輛可規(guī)劃出一條安全有效的路徑，因此在進行軌跡規(guī)劃時須設(shè)定各種邊界條件，包括初始邊界條件和終了邊界條件，初始邊界條件是自動駕駛車輛起始的狀態(tài)變量初值，終了邊界條件是在軌跡規(guī)劃的終點需要滿足的條件，首先設(shè)定L1段的起始和終了條件，具體過程如下。

車輛在經(jīng)過L1段距離的起始條件為

車輛在經(jīng)過L1段距離后的終了條件為

車輛在L1段各狀態(tài)的約束為

車輛經(jīng)過L2段距離時狀態(tài)的起始條件是L1段距離的終了條件，這種約束稱為鏈接約束，即車輛在L2段距離的終了條件為

車輛在L2段各狀態(tài)的約束為

主車在L2路段中行駛時，須設(shè)定虛擬車輛的路徑約束，根據(jù)式（2）可知，為便于計算有效性，定義虛擬車輛的約束為

式中：（xc，yc）為障礙物中心坐標(biāo)；a、b、c為確定參數(shù)。這樣，路徑約束為

h（x，y）≥0

1.4 性能指標(biāo)

自動駕駛車輛的路徑規(guī)劃要從起始位置到終了位置，其性能指標(biāo)可設(shè)置計算耗能最低、計算快速通行能力等。本文中研究車輛在低速行駛下對圖1中L1和L2段道路環(huán)境感知情況，為保證兩段道路環(huán)境的暢通性，采取快速通行能力的性能優(yōu)化指標(biāo)，目標(biāo)是得到時間最短的軌跡，因此，性能指標(biāo)函數(shù)為

2 高斯偽譜法

使用高斯偽譜法計算優(yōu)化問題的離散解可滿足間接法的1階最優(yōu)條件，通過一系列變換將連續(xù)優(yōu)化問題離散為帶有代數(shù)約束的參數(shù)優(yōu)化問題，即非線性規(guī)劃問題。高斯偽譜法在控制變量、狀態(tài)變量和協(xié)調(diào)變量上的近似精度和收斂速度具有明顯的優(yōu)勢［9］，具體步驟如下［10］。

（1）時間區(qū)間轉(zhuǎn)換。高斯偽譜法實現(xiàn)最優(yōu)控制問題的時間區(qū)間是τ∈［-1，1］，將姿態(tài)優(yōu)化問題的時間區(qū)間［t0，tf］轉(zhuǎn)換為［-1，1］，定義時間變量 t為

（2）離散狀態(tài)變量與控制變量。高斯偽譜法主要采用拉格朗日插值和高斯求積公式將一系列最優(yōu)問題在Legendre-Gauss（LG）點進行離散化處理。其中，LG點為K階Legendre多項式：

則狀態(tài)變量近似為

式中Li（τ）為Lagrange插值基函數(shù)：

高斯偽譜法控制變量近似為

（3）終了狀態(tài)約束的離散狀態(tài)方程約束中考慮了末端節(jié)點，因此須計算出末端節(jié)點方程。對動力學(xué)微分方程在區(qū)間［-1，1］上積分，得

式中：τk為 LG點；μk為 Gauss權(quán)重。離散化并用高斯積分來近似可得

（4）動力學(xué)微分方程的轉(zhuǎn)換。高斯偽譜法的狀態(tài)變量通過正交插值多項式來近似，因此其導(dǎo)數(shù)可通過對式（12）求導(dǎo)得到，即

令：

式中k＝1，2，…，K。再代入動力學(xué)微分方程，可得到離散表示式：

（5）目標(biāo)函數(shù)。應(yīng)用高斯積分來計算優(yōu)化函數(shù)中目標(biāo)函數(shù)的積分項，可得離散形式的近似目標(biāo)函數(shù)：

3 仿真結(jié)果分析

主車1在 L1階段的起始條件：x10＝10 m，y10＝0.1 m，θ10＝0.2 rad，v10＝5 m／s，γ10＝0.2 rad／s。

主車1在 L1階段的終了條件：x1f＝50 m，y1f＝0，θ1f＝0，v1f＝8 m／s，γ1f＝0。

主車1在 L1段各狀態(tài)的約束：x∈［0，200］，y∈

本試驗在MATLAB2018b環(huán)境下，進行自動駕駛車輛的多階段研究，設(shè)定終點約束為

E（x（t0），x（tf），t0，tf）＝tf

尋找滿足約束條件的狀態(tài)變量和控制變量，使J＝tf。計算主車1駛往L1段距離行駛過程中的狀態(tài)，并考慮主車1在低速和高速兩種行駛工況。

（1）首先，研究主車1的低速行駛情況，其仿真數(shù)據(jù)約束條件如下。［0，3］，θ∈［-3π，3π］，v∈［5，8］，γ∈［-3π，3π］，a∈［-3，3］。

主車1在L2階段的起始條件：x20＝50 m，y20＝0，θ20＝0，v20＝8 m／s，γ20＝0。

主車1在 L2階段的終了條件：x2f＝100 m，y2f＝0，θ2f＝0.1 rad，v2f＝10 m／s，γ2f＝0。

主車1在 L2段各狀態(tài)的約束：x∈［0，200］，y∈［0，3］，θ∈［-3π，3π］，v∈［5，8］，γ∈［-3π，3π］，a∈［-3，3］。

聯(lián)立式（3）～式（5）、式（9）和式（15），可建立自動駕駛車輛的優(yōu)化模型，其結(jié)果如圖3和圖4所示。

圖3 L1和L2階段的軌跡和速度

圖4 L1和L2階段的車輛狀態(tài)

從圖3和圖4可以看出：主車1在L1階段時，車速、加速度和橫擺角速度在狀態(tài)約束范圍內(nèi)有效行駛，并設(shè)計出了L1階段的行駛軌跡；當(dāng)主車1在L2階段時，隨著車速、加速度和橫擺角速度等車輛狀態(tài)的臨界約束條件的變化，L2階段的行駛軌跡也隨著變化。主車1在L1和L2階段的車輛狀態(tài)臨界約束條件和起始、終了條件的變化以及在兩個階段的起始和終了狀態(tài)約束的充分鏈接性，保證主車1在L1和L2階段以不同的狀態(tài)行駛，并能根據(jù)道路環(huán)境調(diào)整行駛狀態(tài)約束，以產(chǎn)生合適的行駛路徑。因此，當(dāng)主車1在容易發(fā)生交通事故區(qū)域等特定的交通環(huán)境行駛時，多階段路徑優(yōu)化有助于主車1在每一階段調(diào)整合適的行駛狀態(tài)及其臨界約束條件，保證車輛在越來越臨近特定的行駛環(huán)境時，能實現(xiàn)優(yōu)化控制在全局上的最優(yōu)，進而實現(xiàn)車輛對道路前方的有效感知。

（2）其次，考慮主車1在較高行駛速度環(huán)境下的狀態(tài)，其仿真數(shù)據(jù)約束條件如下。

主車1在L1階段的起始條件：x10＝10 m，y10＝0.1 m，θ10＝0.2 rad，v10＝15 m／s，γ10＝0.2 rad／s。

主車1在L1階段的終了條件：x1f＝100 m，y1f＝0，θ1f＝0，v1f＝20 m／s，γ1f＝0。

主車1在 L1段各狀態(tài)的約束：x∈［0，200］，y∈［0，3.5］，θ∈［-3π，3π］，v∈［15，25］，γ∈［-3π，3π］，a∈［-3，3］，w∈［-0.3，0.3］。

計算自動駕駛經(jīng)過L2段距離中的狀態(tài)，其仿真數(shù)據(jù)約束條件如下。

主車1在L2階段的起始條件：x20＝100 m，y20＝0，θ20＝0，v20＝20 m／s，γ20＝0。

主車1在L2段各狀態(tài)的約束：x2f＝250 m，y2f＝0.2 m，θ2f＝0，v2f＝25 m／s，γ2f＝0。

主車1在 L2段各狀態(tài)的約束：x∈［0，200］，y∈［0，3］，θ∈［-3π，3π］，v∈［20，30］，γ∈［-3π，3π］，a∈［-3，3］，w∈［-0.3，0.3］。

聯(lián)立式（3）、式（6）、式（7）、式（9）和式（16），可建立自動駕駛車輛的優(yōu)化模型，其結(jié)果如圖5和圖6所示。

圖5 L1和L2階段的軌跡和速度

從圖5中可以看出：當(dāng)主車1以較高速度行駛時，其行駛軌跡和車輛狀態(tài)在不同階段實時變化；在L1階段，車速以勻加速增加，直到軌跡達到L1段距離的末端約束；在L2階段，車輛的初始位置從L1階段的終點開始，且其初始條件與L1階段的終了條件相同，主車1在L2階段可實時調(diào)整其行駛軌跡，以適應(yīng)具體的行駛環(huán)境。從圖6中可以看出，主車1的加速度和橫擺角速度隨著不同階段路徑的變化而產(chǎn)生較好的效果，保證自動駕駛在對特定的行駛環(huán)境感知時，能實時調(diào)節(jié)其行駛狀態(tài)，進而實現(xiàn)車輛對道路前方的有效感知。

4 結(jié)論

本文中以自動駕駛車輛的多階段軌跡優(yōu)化為目標(biāo)，建立了車輛動力學(xué)模型和環(huán)境模型，在滿足車輛的速度、加速度和橫擺角速度等狀態(tài)約束以及狀態(tài)的起始和終了條件下，采用高斯偽譜法進行自動駕駛車輛的多階段軌跡優(yōu)化問題求解，將自動駕駛車輛的軌跡規(guī)劃問題轉(zhuǎn)化成優(yōu)化問題。從本文研究中可以看出，在對多階段軌跡優(yōu)化過程中，高斯偽譜法可根據(jù)行駛環(huán)境設(shè)定具體的車輛狀態(tài)約束及狀態(tài)的起始、終了條件，進而實現(xiàn)自動駕駛車輛在全局求解性能上的最優(yōu)，保證自動駕駛車輛對前方道路環(huán)境的實時性有效感知。該方法計算精度高、求解速度快，具有較強的魯棒性，但仍存在一些挑戰(zhàn)，例如偽譜法在節(jié)點上的插值部分可能出現(xiàn)不滿足的情況。

圖6 L1和L2階段的車輛狀態(tài)