基于高斯偽譜法的自動(dòng)駕駛車輛狀態(tài)研究*

唐曉峰，楊 林，袁靜妮

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

前言

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

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

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

1.1 環(huán)境建模

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

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

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

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

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

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

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

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

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

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

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

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

1.3 邊界條件與約束

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

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

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

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

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

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

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

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

h（x，y）≥0

1.4 性能指標(biāo)

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

2 高斯偽譜法

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

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

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

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

式中Li（τ）為L(zhǎng)agrange插值基函數(shù)：

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

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

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

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

令：

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

（5）目標(biāo)函數(shù)。應(yīng)用高斯積分來(lái)計(jì)算優(yōu)化函數(shù)中目標(biāo)函數(shù)的積分項(xiàng)，可得離散形式的近似目標(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∈

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

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

尋找滿足約束條件的狀態(tài)變量和控制變量，使J＝tf。計(jì)算主車1駛往L1段距離行駛過(guò)程中的狀態(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），可建立自動(dòng)駕駛車輛的優(yōu)化模型，其結(jié)果如圖3和圖4所示。

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

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

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

（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ì)算自動(dòng)駕駛經(jīng)過(guò)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），可建立自動(dòng)駕駛車輛的優(yōu)化模型，其結(jié)果如圖5和圖6所示。

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

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

4 結(jié)論

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

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