非結(jié)構(gòu)道路場(chǎng)景下輪式無人車輛避障算法

杜廣澤， 張旭東， 鄒淵， 鄭壯壯

(1.北京理工大學(xué) 機(jī)械與車輛學(xué)院， 北京 100081；2.北京電動(dòng)車輛協(xié)同創(chuàng)新中心， 北京 100081)

0 引言

地面無人車輛作為無人平臺(tái)的一種，無論是在民用領(lǐng)域還是在軍用領(lǐng)域，都承擔(dān)著越來越重要的功能與任務(wù)[1]。無人車輛系統(tǒng)避障是重要的一項(xiàng)功能要求，該功能涉及車輛環(huán)境感知技術(shù)、規(guī)劃技術(shù)與控制技術(shù)。以感知結(jié)果為基礎(chǔ)實(shí)現(xiàn)軌跡規(guī)劃，進(jìn)而進(jìn)行動(dòng)力學(xué)控制是無人化行駛的主要思路。一般來說，避障功能的主要目的是尋找一條符合車輛約束的、無碰撞的安全軌跡，并控制車輛安全通過障礙區(qū)域[2]。對(duì)于非結(jié)構(gòu)化道路下的輪式無人車輛而言，由于地形因素的影響，車輛運(yùn)動(dòng)多面臨側(cè)翻的風(fēng)險(xiǎn)。因此，車輛避障過程中的軌跡除了考慮車輛本身約束之外，還需要考慮車輛運(yùn)動(dòng)狀態(tài)的安全性，也應(yīng)考慮不同路面坡度的動(dòng)力學(xué)特性[3]。

針對(duì)無人車輛的避障需求，國(guó)內(nèi)外學(xué)者已提出許多算法。這些算法大多期望無人車輛在包含障礙物的環(huán)境中按照某些指標(biāo)運(yùn)動(dòng)，這些指標(biāo)通常與車輛運(yùn)動(dòng)的通過性、快速性、穩(wěn)定性、安全性相關(guān)，且大多數(shù)規(guī)劃出來的軌跡都滿足連續(xù)和平滑的要求[4]。這些軌跡規(guī)劃算法大致分為圖搜索類算法[5]、虛擬場(chǎng)類算法[6]、啟發(fā)類算法[7]、數(shù)學(xué)優(yōu)化類算法[8]4類。其中，圖搜索類算法與虛擬場(chǎng)類算法運(yùn)算速度快，但難以考慮車輛特性，啟發(fā)類算法需要額外的軌跡平滑手段，而數(shù)學(xué)優(yōu)化類算法可以考慮車輛特性，但需要較多的計(jì)算時(shí)間。

在數(shù)學(xué)優(yōu)化類算法中，基于模型預(yù)測(cè)控制(MPC)[9]的優(yōu)化類算法被廣泛應(yīng)用。現(xiàn)階段研究已經(jīng)證明了MPC在無人車輛軌跡規(guī)劃中的可行性。文獻(xiàn)[10]提出了一種基于MPC的分層式車輛避障算法，利用車輛運(yùn)動(dòng)學(xué)模型進(jìn)行車輛上層軌跡規(guī)劃，并利用動(dòng)力學(xué)模型進(jìn)行車輛軌跡跟蹤，證明了規(guī)劃軌跡的可行性；文獻(xiàn)[11]將凸近似的避障原理與MPC進(jìn)行結(jié)合，提出一種改進(jìn)的無人駕駛車輛軌跡規(guī)劃方法，并驗(yàn)證了其合理性；文獻(xiàn)[12]針對(duì)特定的換道問題提出一種基于MPC的換道軌跡規(guī)劃算法，該算法使用車輛動(dòng)力學(xué)模型，驗(yàn)證了軌跡規(guī)劃在車輛高速運(yùn)動(dòng)情況下的可行性與穩(wěn)定性；文獻(xiàn)[13]研究了高速無人車輛避障問題，使用MPC算法保證車輛安全性與路徑跟蹤的準(zhǔn)確性；文獻(xiàn)[14]使用MPC實(shí)現(xiàn)地面無人車輛的避障功能，證明了基于MPC避障算法的可行性；文獻(xiàn)[15]根據(jù)模型預(yù)測(cè)控制原理，提出一種考慮車輛安全性約束的軌跡規(guī)劃方法；文獻(xiàn)[16]提出一種適用于高速地面無人車輛的避障算法，使用了一種非線性MPC算法；文獻(xiàn)[17]針對(duì)乘用車輛實(shí)時(shí)避障問題，提出一種基于速度預(yù)測(cè)的模型預(yù)測(cè)控制器，將規(guī)劃與避障功能融合在一起。

上述研究大多以車輛動(dòng)力學(xué)為基礎(chǔ)，針對(duì)某一特定工況進(jìn)行避障研究，而在離線地圖與參考軌跡未知情況下進(jìn)行的車輛避障研究并不多見。

本文將非結(jié)構(gòu)化道路避障問題描述為分段最優(yōu)控制問題，針對(duì)地面無人車輛的動(dòng)力學(xué)特性，以得到車輛控制量序列為目的，提出在非結(jié)構(gòu)化道路下的避障算法。本文所提算法綜合了避障所需的規(guī)劃與控制兩部分，優(yōu)化求解完畢后，直接得到車輛的規(guī)劃軌跡、參考縱向加速度與前輪轉(zhuǎn)角。所提算法保證了車輛盡可能以目標(biāo)速度接近目標(biāo)點(diǎn)，確保車輛在安全性限制的條件下運(yùn)動(dòng)，提升了軌跡規(guī)劃與避障算法的環(huán)境適應(yīng)性。本文的創(chuàng)新點(diǎn)與價(jià)值在于：

1)提出一種不明確區(qū)分避障路徑規(guī)劃與避障路徑跟蹤的算法應(yīng)用，無需進(jìn)行地圖重建，提高了避障功能實(shí)現(xiàn)的準(zhǔn)確程度。非結(jié)構(gòu)道路包含更多復(fù)雜形狀的障礙物、樹木等元素，障礙物尺寸形狀都不易表達(dá)，受限于柵格形狀以及激光雷達(dá)在遠(yuǎn)處分辨率低的特點(diǎn)，在地圖重建過程中容易產(chǎn)生失真，影響無人車輛避障效果。本文基于MPC的避障方法，無需明確的路徑規(guī)劃部分，因此無需明確的地圖重建過程，僅需要根據(jù)激光雷達(dá)點(diǎn)云數(shù)據(jù)提取扇形邊界，這樣就減少了由于地圖重建造成的誤差。相比之下，在解決無人車輛非結(jié)構(gòu)化道路下的避障問題時(shí)，具有更少的步驟、更高的準(zhǔn)確程度。

2)針對(duì)非結(jié)構(gòu)化道路車輛行駛目的，結(jié)合無參考軌跡與離線地圖的工況，提出了合理的評(píng)價(jià)指標(biāo)與約束條件，保證車輛最大程度按照期望車速到達(dá)目標(biāo)點(diǎn)，并保證車輛運(yùn)動(dòng)的安全性。考慮到?jīng)]有參考軌跡與離線地圖，設(shè)計(jì)了基于車輛航向、運(yùn)行時(shí)間、車速以及距目標(biāo)距離綜合在內(nèi)的評(píng)價(jià)指標(biāo)，保證車輛運(yùn)行的高效性。針對(duì)非結(jié)構(gòu)化道路車輛行駛安全性特點(diǎn)，綜合考慮車輛動(dòng)力學(xué)約束與車輛載荷轉(zhuǎn)移下的安全性約束，將車輛橫縱向控制結(jié)合，達(dá)到無需后續(xù)設(shè)計(jì)控制率即可實(shí)現(xiàn)車輛縱向控制的目的。在安全性約束中，硬約束考慮了車輛車輪在實(shí)際運(yùn)動(dòng)中不可離地，在MPC的評(píng)價(jià)指標(biāo)中亦通過約束車輛橫縱向加速度與質(zhì)心側(cè)偏角保證車輛安全性。

3)采用變預(yù)測(cè)時(shí)域的方法來解決固定距離上的模型預(yù)測(cè)控制問題。受限于激光雷達(dá)探測(cè)范圍，車輛模型預(yù)測(cè)范圍最多到達(dá)激光雷達(dá)探測(cè)邊界，而由于車輛車速在該問題中不是定值，因此預(yù)測(cè)時(shí)域亦作為模型的狀態(tài)量參與優(yōu)化。這更符合非結(jié)構(gòu)道路下車輛避障的要求，也使得車輛在行進(jìn)中更好地保持連續(xù)性。在求解中，使用偽譜法將分段非線性最優(yōu)控制問題轉(zhuǎn)換為包含優(yōu)化變量的非線性問題，然后使用基于內(nèi)點(diǎn)法的非線性求解器進(jìn)行問題求解，從而達(dá)到變預(yù)測(cè)時(shí)域的目的。

1 基于MPC的避障算法設(shè)計(jì)

在MPC算法中，優(yōu)化問題通常需要定義好預(yù)測(cè)時(shí)域與控制時(shí)域，確定優(yōu)化問題的約束與評(píng)價(jià)函數(shù)，在實(shí)際使用中根據(jù)被控對(duì)象的反饋進(jìn)行滾動(dòng)優(yōu)化與校正。

然而，在非結(jié)構(gòu)化道路的避障問題中，在沒有離線地圖與參考軌跡的情況下，車輛運(yùn)動(dòng)僅依賴于動(dòng)態(tài)局部地圖確定的可通行區(qū)域，因此，車輛在避障軌跡終點(diǎn)處的狀態(tài)無法確定。由于車輛橫縱向運(yùn)動(dòng)的耦合性，無法明確定義預(yù)測(cè)時(shí)域與控制時(shí)域的長(zhǎng)度。并且由于動(dòng)態(tài)局部地圖的實(shí)時(shí)性，車輛在不同位置具有不同數(shù)學(xué)表達(dá)形式、不同數(shù)量的約束條件。本文提出基于MPC的分段優(yōu)化避障算法，通過構(gòu)建分段最優(yōu)控制問題，準(zhǔn)確地描述車輛在避障過程不同階段應(yīng)滿足的動(dòng)力學(xué)特性與狀態(tài)約束。因此，在綜合考慮車輛橫縱向動(dòng)力學(xué)時(shí)，車輛避障問題可以轉(zhuǎn)換為分段最優(yōu)控制問題。本文按照車輛在局部地圖中運(yùn)動(dòng)的不同區(qū)域，對(duì)問題進(jìn)行劃分。

根據(jù)模型預(yù)測(cè)控制原理與軌跡規(guī)劃問題，設(shè)計(jì)基于MPC的軌跡避障算法，圖1所示為基于MPC的避障算法框架。

圖1 基于MPC的避障算法框架Fig.1 Framework of MPC-based obstacle avoidance algorithm

該框架分被控對(duì)象和避障算法兩部分。其中被控對(duì)象指的是實(shí)際車輛和車載傳感器；避障算法分為感知、規(guī)劃、控制3層，感知層為算法的基礎(chǔ)，包括狀態(tài)估計(jì)與局部地圖的創(chuàng)建，規(guī)劃層包括了基于MPC的優(yōu)化問題描述、優(yōu)化問題分段、添加優(yōu)化問題約束、優(yōu)化問題的轉(zhuǎn)換與求解4步，規(guī)劃算法直接生成橫向控制量，即前輪轉(zhuǎn)角，并生成縱向參考加速度，縱向參考加速度由縱向控制算法進(jìn)行跟蹤，繼而生成縱向控制量。本文主要研究避障算法中的規(guī)劃層與控制層，實(shí)際應(yīng)用中避障算法根據(jù)車輛實(shí)際控制狀態(tài)進(jìn)行優(yōu)化求解，從而對(duì)車輛進(jìn)行修正，達(dá)到在非結(jié)構(gòu)化道路下避障的目的。

2 分段最優(yōu)控制問題構(gòu)建與求解

圖2所示為分段最優(yōu)控制問題原理，被控對(duì)象在不同的時(shí)間段內(nèi)，有不同的評(píng)價(jià)指標(biāo)、目標(biāo)值與約束，分段最優(yōu)控制問題的構(gòu)建要針對(duì)具體問題，選定某種規(guī)則進(jìn)行分段，紅色圓圈標(biāo)注區(qū)域?yàn)榉侄芜B接區(qū)域，Tp為控制時(shí)域。在車輛運(yùn)動(dòng)過程中，車輛運(yùn)動(dòng)的可通行區(qū)域范圍隨著車輛位置改變而改變，以車輛在局部地圖中的位置作為分段因素，可以將避障問題構(gòu)建為分段最優(yōu)控制問題。

圖2 分段最優(yōu)控制問題描述Fig.2 Problem description of multi-phrases optimal control

按照車輛運(yùn)動(dòng)的位置，將軌跡規(guī)劃問題構(gòu)建為分段最優(yōu)控制問題，由(1)式～(9)式描述。

(1)

(2)

(3)

(4)

S(i)[X(i)(t)]≤0,

(5)

R(i)[X(i)(t)]≤0,

(6)

X(i)(T(i))=X(i+1)(T(i)),

(7)

G[X(N)(T(N))]≥0,

(8)

T(0)=0 s,X(1)(T(0))=X(T(0)),T(N)≤Tmax，

(9)

2.1 代價(jià)函數(shù)

(1)式表示最優(yōu)控制問題的評(píng)價(jià)指標(biāo)，設(shè)定為代價(jià)函數(shù)J(參數(shù)示意圖見圖3)，如(10)式所示：

(10)

式中：w1、w2、w3、w4、w5為權(quán)重系數(shù)，w1對(duì)應(yīng)的指標(biāo)是在保證避障功能的前提下，期望規(guī)劃終點(diǎn)的車輛位置更靠近目標(biāo)點(diǎn)，w2對(duì)應(yīng)的指標(biāo)是期望規(guī)劃終點(diǎn)的車輛航向盡可能朝向目標(biāo)點(diǎn)，w3對(duì)應(yīng)的指標(biāo)是期望車輛以恒定的速度行駛，w4對(duì)應(yīng)的指標(biāo)是使得車輛盡可能貼近規(guī)劃起點(diǎn)與目標(biāo)點(diǎn)的連線，從而保持車輛不失控，w5對(duì)應(yīng)的指標(biāo)是期望車輛保持平穩(wěn)行駛，減少發(fā)生危險(xiǎn)的概率；s0、s1分別為規(guī)劃起點(diǎn)和規(guī)劃終點(diǎn)距離目標(biāo)點(diǎn)的距離，

(11)

(12)

在局部地圖坐標(biāo)系OSXSYS下，x和y分別表示車輛橫縱坐標(biāo)，xg和yg分別表示目標(biāo)點(diǎn)橫縱坐標(biāo)；γ0為目標(biāo)點(diǎn)方位角；φ為車輛航向；u為車輛縱向速度；ut為車輛行駛的期望速度；xref和yref分別為參考點(diǎn)橫縱軸方向坐標(biāo)；ω為車輛橫擺角速度。圖3中v為車輛橫向速度。

圖3 代價(jià)函數(shù)參數(shù)示意圖Fig.3 Cost function parameter diagram

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

(2)式表示輪式無人地面車輛的動(dòng)力學(xué)模型，如圖4所示，車輛運(yùn)動(dòng)的動(dòng)力學(xué)狀態(tài)由(13)式描述：

(13)

圖4 輪式無人車輛動(dòng)力學(xué)模型Fig.4 Dynamic model of wheeled unmanned vehicle

式中：ax為車輛縱向加速度；Ffy前軸車輪側(cè)向力；Fry為后軸車輪側(cè)向力；m為整車整備質(zhì)量；Lf為質(zhì)心到前軸的水平距離；Lr為質(zhì)心到后軸的水平距離；Iz為車輛z軸方向轉(zhuǎn)動(dòng)慣量；kf為前軸車輪側(cè)偏剛度；kr為后軸車輪側(cè)偏剛度；δ為轉(zhuǎn)角；β為側(cè)偏角；FRN為右側(cè)車輪接地垂向力；FLN為左側(cè)車輪接地垂向力；b為車輛輪距；θ為車輛側(cè)傾角；h為質(zhì)心位置距離地面高度。圖4中OXVYVZV為車輛坐標(biāo)系，O為車輛質(zhì)心，RT為轉(zhuǎn)彎半徑，af、ar分別為前、后軸車輪側(cè)偏角，F(xiàn)LT、FRT分別為左側(cè)和右側(cè)車輪接地切向力。選取狀態(tài)量X=[x,y,φ,u,v,ω]T,選取控制量μ=[ax,δ]T。

2.3 狀態(tài)量約束

對(duì)于本文針對(duì)的避障問題，只考慮車輛前進(jìn)方向半圓周內(nèi)的情形，記車輛最高縱向速度約束為umax，車輛最高橫向速度約束為vmax，車輛最高橫擺角速度為ωmax，則：

(14)

(15)

(14)式與(15)式表示了車輛狀態(tài)量約束，無窮作為約束界限表示該狀態(tài)量不需要進(jìn)行約束。

2.4 控制量約束

(4)式表示車輛控制量約束，對(duì)于該避障問題，車輛縱向加速度由驅(qū)動(dòng)系統(tǒng)與完成任務(wù)的需求共同決定，根據(jù)需求選定上下邊界值，記車輛最大縱向加速度為axmax. 車輛前輪轉(zhuǎn)角應(yīng)在極限轉(zhuǎn)角范圍內(nèi)，極限轉(zhuǎn)角由車輛機(jī)械結(jié)構(gòu)決定，記為δmax，最優(yōu)控制問題控制量約束表示為

(16)

(17)

2.5 安全指標(biāo)約束

(5)式表示車輛安全指標(biāo)約束，對(duì)于非結(jié)構(gòu)化道路，安全約束定義為車輛具有足夠大的垂向力且車輛質(zhì)心側(cè)偏角在一定范圍之內(nèi)，即雙側(cè)車輪接地垂向力均大于接地垂向力閾值。記車輪接地垂向力閾值為FNmin，記車輛最大質(zhì)心側(cè)偏角為βmax，路面附著系數(shù)為η，重力加速度為g，車輛最大質(zhì)心側(cè)偏角按照(18)式[18]確定：

βmax=arctan (0.02ηg).

(18)

結(jié)合(13)式，選定：

(19)

2.6 車輛位置約束

(6)式表示車輛位置約束，該約束體現(xiàn)了車輛應(yīng)通過的區(qū)域序列，在本算法中將所有的區(qū)域處理成為三角形。

經(jīng)過在線局部地圖的處理過程，得到如圖5所示局部地圖。在該局部地圖中，地圖坐標(biāo)系與車輛坐標(biāo)系在規(guī)劃起點(diǎn)處重合，車輛初始位置，即規(guī)劃起點(diǎn)，處于圓心處，車輛航向?yàn)闄M軸正方向。同時(shí)進(jìn)行地形評(píng)估，即局部地圖包含了地形起伏信息在內(nèi)，該地形起伏信息在優(yōu)化問題中體現(xiàn)在車輛側(cè)傾角中。

圖5 傳感器感知結(jié)果Fig.5 Results of sensor signal processing

將車輛可通行區(qū)域按形狀劃分，扇形區(qū)域表示該區(qū)域的邊界到達(dá)了傳感器可探知的最大范圍，即在扇形區(qū)域中無障礙物存在；三角形區(qū)域表示該區(qū)域的邊界未到達(dá)傳感器可探知的最大范圍，即在區(qū)域盡頭存在障礙物，如圖5(b)所示。

在避障問題中，車輛應(yīng)在滿足無碰撞需求的情況下，盡可能接近目標(biāo)點(diǎn)，即車輛的規(guī)劃終點(diǎn)應(yīng)在扇形區(qū)域中，由此可知，按照距離最近的原則，選取距離目標(biāo)點(diǎn)最近的扇形區(qū)域作為規(guī)劃終點(diǎn)所在的范圍。

使用數(shù)學(xué)方法描述車輛運(yùn)動(dòng)區(qū)域的邊界，如圖5(c)所示，圓弧用割線來代替，其中藍(lán)色界線為車輛運(yùn)動(dòng)區(qū)域的約束，在局部地圖坐標(biāo)系下用不等式描述邊界。

對(duì)于優(yōu)化問題的每一段來說，位置約束為3條直線所包圍的區(qū)域，選取：

R(i)[X(i)(t)]≤0?
P(i)x(i)(t)+Q(i)y(i)(t)

(20)

式中：P、Q、R均為長(zhǎng)度為3的向量。在圖5所示的例子中N=2，即優(yōu)化問題分為兩段，優(yōu)化問題的第1段對(duì)應(yīng)3號(hào)區(qū)域，優(yōu)化問題的第2段對(duì)應(yīng)4號(hào)區(qū)域。

2.7 連續(xù)性約束

(7)式表示在兩段之間狀態(tài)量應(yīng)是連續(xù)的，如圖2所示的虛線圓圈部分，這是車輛本身動(dòng)力學(xué)所決定的。此項(xiàng)約束表述為：對(duì)于分段問題的某一段，上一段最后時(shí)刻的狀態(tài)量與這一段初始時(shí)刻的控制量在連續(xù)狀態(tài)空間中應(yīng)保持一致。

2.8 端點(diǎn)約束

(8)式和(9)式表示車輛在規(guī)劃起點(diǎn)與規(guī)劃終點(diǎn)處的約束， MPC每次求解時(shí)，局部地圖坐標(biāo)系與規(guī)劃起點(diǎn)的車輛坐標(biāo)系重合，即在初始時(shí)刻：

X(T(0))=[0,0,0,u(T(0)),v(T(0)),ω(T(0))]T.

(21)

當(dāng)目標(biāo)點(diǎn)在局部地圖范圍之外時(shí)，選定規(guī)劃終點(diǎn)的坐標(biāo)盡可能貼近局部地圖邊界，即

(22)

式中：RL為局部地圖半徑；ε為半徑縮放因子，0<ε<1. 當(dāng)目標(biāo)點(diǎn)在局部地圖范圍之內(nèi)時(shí)，選定規(guī)劃終點(diǎn)的坐標(biāo)在目標(biāo)點(diǎn)附近，即

(23)

式中：λ為到達(dá)終點(diǎn)的距離閾值。

2.9 求解算法

對(duì)于(1)式～(9)式所描述的連續(xù)狀態(tài)空間非線性分段優(yōu)化問題，本文分兩步進(jìn)行求解。

首先針對(duì)連續(xù)狀態(tài)空間的分段優(yōu)化問題，使用hp自適應(yīng)偽譜法[19]將連續(xù)狀態(tài)空間的優(yōu)化問題轉(zhuǎn)換為含優(yōu)化變量的非線性問題，這就將避障過程這一分段優(yōu)化問題轉(zhuǎn)換為了含有車輛狀態(tài)量的非線性優(yōu)化問題。該方法使用Legendre-Gauss-Radau配點(diǎn)方法，融合有限元法與全局偽譜法的優(yōu)點(diǎn)，同時(shí)自適應(yīng)調(diào)節(jié)單元長(zhǎng)度與拉格朗日基函數(shù)的階次，能快速收斂到給定精度。經(jīng)過這一步的非線性優(yōu)化問題具有如(24)式～(26)式所示的形式：

(24)

s.t.bL≤g(X)≤bU,

(25)

XL≤X≤XU,

(26)

式中：f(X)是代價(jià)方程；bU和bL分別為約束方程g(X)的上、下邊界；XU和XL分別為變量向量X的上、下邊界。

其次使用非線性問題求解器IPOPT[20]對(duì)非線性問題進(jìn)行求解，IPOPT求解器使用內(nèi)點(diǎn)法求解非線性問題的局部最優(yōu)解。該方法將等式與不等式約束分解為一些列等式約束，然后用迭代的方法求解只含有等式約束的非線性問題。該方法可以使用精確的雅可比矩陣與海森矩陣，從而保證求解結(jié)果的準(zhǔn)確性，在求解完畢后即可得到車輛避障問題的優(yōu)化結(jié)果。

3 實(shí)車實(shí)驗(yàn)驗(yàn)證

3.1 車輛控制算法

通過實(shí)時(shí)優(yōu)化得到車輛在控制時(shí)域內(nèi)的狀態(tài)量與控制量序列，將求解得到的加速度序列作為控制層的參考加速度序列，實(shí)際縱向控制量為車輛油門開度，采用PID控制器跟蹤該參考控制量，實(shí)現(xiàn)軌跡跟蹤。(27)式表示PID控制器的控制規(guī)律，將求解得到的前輪轉(zhuǎn)角序列作為車輛橫向控制的實(shí)際控制量：

(27)

式中：pad(t)為車輛油門踏板開度；Kp、Ki、Kd為比例、積分、微分環(huán)節(jié)的增益系數(shù)；

e(t)=amx(t)-ax(t)，

(28)

amx(t)為車輛實(shí)際縱向加速度。

3.2 實(shí)驗(yàn)平臺(tái)搭建

實(shí)車平臺(tái)如圖6所示，被控車輛為北京理工大學(xué)輪式車輛平臺(tái)，在該實(shí)驗(yàn)車輛的基礎(chǔ)上，增加激光雷達(dá)、上位機(jī)、CAN協(xié)議轉(zhuǎn)以太網(wǎng)協(xié)議(CAN-NET)設(shè)備，進(jìn)行實(shí)驗(yàn)驗(yàn)證。

圖6 實(shí)車驗(yàn)證平臺(tái)架構(gòu)Fig.6 Real vehicle verification platform architecture

其中CAN-NET設(shè)備將以太網(wǎng)通信協(xié)議轉(zhuǎn)換為CAN通信協(xié)議，上述規(guī)劃與控制算法在上位機(jī)中運(yùn)行，算法結(jié)果通過以太網(wǎng)轉(zhuǎn)CAN設(shè)備下發(fā)到整車CAN總線上，從而控制制動(dòng)執(zhí)行器與電機(jī)，實(shí)現(xiàn)車輛控制。除了激光雷達(dá)之外，車輛還裝有視覺傳感器、前向毫米波雷達(dá)傳感器，可按照上述步驟準(zhǔn)確地得到相應(yīng)的局部地圖與劃分區(qū)域。實(shí)車實(shí)驗(yàn)參數(shù)如表1所示。

表1 實(shí)驗(yàn)相關(guān)參數(shù)Tab.1 Parameters related to experiment

3.3 實(shí)驗(yàn)驗(yàn)證

實(shí)際實(shí)驗(yàn)中，選取封閉園區(qū)內(nèi)一段道路作為測(cè)試路段，該路段是典型的非結(jié)構(gòu)化道路。該道路沒有車道線，不含明顯道路邊界，道路周圍障礙物種類復(fù)雜、數(shù)目較多，包含動(dòng)態(tài)行人與車輛。在此測(cè)試環(huán)境下可充分測(cè)試算法的可行性。

測(cè)試結(jié)果的車輛實(shí)際運(yùn)動(dòng)軌跡如圖7所示，將車輛實(shí)驗(yàn)軌跡標(biāo)注在三維點(diǎn)云地圖中，圖7(a)為俯視視角下的車輛軌跡，圖7(b)是軸測(cè)視角下的車輛軌跡。

圖7 車輛實(shí)際運(yùn)行軌跡Fig.7 Track of vehicle in experiment

測(cè)試過程中，車輛從起點(diǎn)由靜止出發(fā)，給定圖7中所示的終點(diǎn)為目標(biāo)點(diǎn)，在運(yùn)動(dòng)過程中記錄車輛在大地坐標(biāo)系下的位置。圖7中紅色區(qū)域?yàn)闊o障礙區(qū)域，綠色曲線為車輛實(shí)際運(yùn)動(dòng)軌跡，黑色區(qū)域?yàn)檎系K物區(qū)域。由圖7所示不同視角的車輛軌跡結(jié)果可知，車輛能到達(dá)目標(biāo)點(diǎn)并實(shí)現(xiàn)避障功能。圖7表明，車輛能夠在滿足無碰撞的條件下，在非結(jié)構(gòu)化道路中到達(dá)目標(biāo)點(diǎn)。圖7亦說明該算法能夠在障礙物大小與數(shù)目未知的環(huán)境中控制車輛到達(dá)期望位置。

圖8～圖13所示為在實(shí)際測(cè)試過程中，車輛在整個(gè)過程中的動(dòng)力學(xué)參數(shù)。

圖8 實(shí)驗(yàn)車輛縱向速度Fig.8 Longitudinal velocity of experimental vehicle

圖9 實(shí)驗(yàn)車輛縱向加速度Fig.9 Longitudinal acceleration of experimental vehicle

圖10 實(shí)驗(yàn)車輛前輪擺角Fig.10 Front wheel angle of experimental vehicle

圖11 實(shí)驗(yàn)車輛橫擺角速度Fig.11 Yaw rate of experimental vehicle

圖12 實(shí)驗(yàn)車輛垂直載荷Fig.12 Vertical load of experimental vehicle

圖13 實(shí)驗(yàn)車輛質(zhì)心側(cè)偏角Fig.13 Sideslip angle of experimental vehicle

由圖8和圖9可知，在縱向動(dòng)力學(xué)方面，在實(shí)驗(yàn)的第70 s、120 s、200 s附近時(shí)，車輛周圍出現(xiàn)了動(dòng)態(tài)障礙物，使得車輛減速避障，在其余時(shí)間中，車輛運(yùn)動(dòng)基本保持了車速在目標(biāo)車速附近。

車輛加速度經(jīng)由PID控制器控制之后，實(shí)際加速度與目標(biāo)加速度能保持良好的一致性。

由圖10和圖11可知，在橫向動(dòng)力學(xué)方面，車輛前輪轉(zhuǎn)角在運(yùn)動(dòng)過程中保持了在了約束范圍之內(nèi)，車輛橫擺角速度變化趨勢(shì)與前輪一致，且橫擺角速度在合理的范圍之內(nèi)。

由圖12和圖13可知，在安全性方面，車輛前后軸接地垂向力滿足了安全性約束，車輛質(zhì)心側(cè)偏角也在約束范圍之內(nèi)。

MPC算法的滾動(dòng)優(yōu)化部分保證了算法的閉環(huán)特性，在實(shí)際使用中，由于車輛是非線性系統(tǒng)，實(shí)時(shí)使用時(shí)需要將連續(xù)的狀態(tài)空間進(jìn)行離散化，進(jìn)而使用非線性求解器進(jìn)行求解，因此算法的實(shí)時(shí)性很大程度上依賴于非線性問題的求解速度。

在本文的實(shí)驗(yàn)中，hp自適應(yīng)偽譜法給定誤差精度為0.01，設(shè)置最大迭代次數(shù)為16次，最大迭代時(shí)間為0.05 s；內(nèi)點(diǎn)法求解非線性問題給定精度為0.01，設(shè)置最大迭代次數(shù)為32次，最大迭代時(shí)間為0.15 s，離線求解好目標(biāo)函數(shù)與約束條件的雅克比矩陣與海森矩陣的表達(dá)式，相比使用數(shù)值方法求解梯度，這樣可以加快收斂速度。由于使用了配點(diǎn)法構(gòu)建非線性問題，使得約束條件的光滑性更好，收斂性更好，特別是針對(duì)車輛動(dòng)力學(xué)優(yōu)化問題。

在實(shí)際使用中，選取控制時(shí)域?yàn)?.2 s，圖14所示為車輛實(shí)際運(yùn)行中的計(jì)算時(shí)間圖，實(shí)驗(yàn)使用計(jì)算機(jī)硬件配置為中央處理器i7-6700K,8 GB內(nèi)存，基于Ubuntu 16.04操作系統(tǒng)開發(fā)上位機(jī)系統(tǒng)。由圖14可知在整個(gè)過程中，總計(jì)1 175次計(jì)算，均未達(dá)到0.2 s的控制時(shí)域上限，平均計(jì)算時(shí)間為114.5 ms，滿足實(shí)時(shí)性要求。在車輛實(shí)際運(yùn)動(dòng)中，遇到動(dòng)態(tài)障礙物需要避障時(shí)的計(jì)算時(shí)間要比只有靜態(tài)障礙物時(shí)略長(zhǎng)，但仍在控制時(shí)域要求內(nèi)。實(shí)驗(yàn)結(jié)果顯示，算法能在0.2 s的控制時(shí)域內(nèi)更新完成，實(shí)現(xiàn)實(shí)時(shí)控制。

圖14 算法運(yùn)行時(shí)間Fig.14 Computation time of algorithm

在本文所提算法中，由于邊界的提取直接在點(diǎn)云數(shù)據(jù)上進(jìn)行操作，沒有任何地圖重建過程，該過程運(yùn)算速度非常快，相對(duì)于求解非線性問題，邊界提取的計(jì)算時(shí)間可以忽略不計(jì)。由于該算法沒有障礙物劃分、聚類等過程，在包含不同大小障礙物的非結(jié)構(gòu)化道路中，每次計(jì)算僅僅是對(duì)應(yīng)的邊界個(gè)數(shù)不同。邊界引發(fā)的車輛約束個(gè)數(shù)遠(yuǎn)遠(yuǎn)小于車輛狀態(tài)量序列引發(fā)的約束個(gè)數(shù)，因此障礙物個(gè)數(shù)變化對(duì)于實(shí)際求解運(yùn)算量影響不大，對(duì)于MPC控制器求解時(shí)間的影響可以忽略不計(jì)，對(duì)總體實(shí)驗(yàn)結(jié)果而言，多障礙物情況下依然可以實(shí)現(xiàn)車輛避障。

在實(shí)驗(yàn)過程中，第70 s、120 s、200 s附近時(shí)存在動(dòng)態(tài)障礙物出現(xiàn)的情況，而其運(yùn)算時(shí)間仍在給定的控制時(shí)域約束內(nèi)。實(shí)驗(yàn)結(jié)果表明，本文提出的MPC算法能夠保證車輛實(shí)時(shí)控制要求，并使得車輛在給定的約束下無碰撞行駛，使得車輛在運(yùn)行中實(shí)現(xiàn)避障。

4 結(jié)論

本文針對(duì)輪式地面無人車輛在非結(jié)構(gòu)化道路中，沒有參考軌跡與離線地圖情況下的避障問題，設(shè)計(jì)了一種基于模型預(yù)測(cè)控制的避障算法。該算法使用局部地圖信息構(gòu)建分段最優(yōu)控制問題，并在構(gòu)建最優(yōu)控制問題的過程中依據(jù)車輛動(dòng)力學(xué)添加約束，從而求解得到車輛前輪轉(zhuǎn)角與縱向參考加速度，繼而實(shí)現(xiàn)車輛控制，進(jìn)行避障，最后通過實(shí)車實(shí)驗(yàn)進(jìn)行驗(yàn)證。結(jié)果表明：

1) 本文所提算法可以依據(jù)局部地圖數(shù)據(jù)，明確代價(jià)函數(shù)與約束，將避障問題轉(zhuǎn)化，得到合理的最優(yōu)控制問題表達(dá)形式。

2) 本文所提避障算法可以在包含不同大小與數(shù)目障礙物的環(huán)境中，有效地引導(dǎo)車輛運(yùn)動(dòng)到目標(biāo)點(diǎn)，并在運(yùn)動(dòng)過程中實(shí)現(xiàn)避障。

本文后續(xù)研究將進(jìn)一步擴(kuò)展到越野環(huán)境下算法的適應(yīng)性與魯棒性。