陳慧巖, 關(guān)海杰, 劉海鷗, 龔建偉, 吳賀禹

(1.北京理工大學(xué) 機(jī)械與車輛學(xué)院, 北京 100081; 2.中國兵器工業(yè)導(dǎo)航與控制技術(shù)研究所， 北京 100089)

0 引言

地面無人機(jī)動平臺能夠代替人類在越野環(huán)境下完成危險物資運輸、災(zāi)情救援、戰(zhàn)場環(huán)境偵察等任務(wù)，減輕或避免人類在越野危險環(huán)境中的負(fù)擔(dān)與消耗。地面無人機(jī)動平臺一般由環(huán)境感知、運動規(guī)劃、運動控制、底盤執(zhí)行4個基本模塊組成[1]。在越野環(huán)境中，如何利用更多的高階語義信息有效地串聯(lián)無人駕駛系統(tǒng)中的各個模塊以提升地面無人機(jī)動平臺在越野環(huán)境中的通行能力，成為重要的研究方向。

越野環(huán)境下無人駕駛系統(tǒng)環(huán)境感知模塊可通行區(qū)域的提取的準(zhǔn)確性與穩(wěn)定性對于提升地面無人機(jī)動平臺的通行能力至關(guān)重要[2]，而其中環(huán)境語義信息的獲取對于可通行區(qū)域的分類提取必不可少。目前環(huán)境感知模塊關(guān)于環(huán)境中語義信息的提取主要在城市道路等結(jié)構(gòu)化場景下進(jìn)行[3]，而越野環(huán)境相較于城市結(jié)構(gòu)化場景具有環(huán)境特征不明顯、地形地勢復(fù)雜、地表形式多樣等特點，對無人駕駛系統(tǒng)提出了更高的要求。目前越野環(huán)境語義信息分割主要是基于深度網(wǎng)絡(luò)學(xué)習(xí)人工標(biāo)注語義標(biāo)簽特征，利用端到端的方法將環(huán)境特征與語義信息建立映射關(guān)系，輸出環(huán)境信息中的語義特征[4]。文獻(xiàn)[5]采用對比學(xué)習(xí)的方法訓(xùn)練具有語義區(qū)分度的特征提取器，實現(xiàn)不同通行度的類別建模。

越野環(huán)境下無人駕駛系統(tǒng)運動規(guī)劃模塊利用環(huán)境感知模塊提供的信息的方式主要有兩種，第一種是基于平面假設(shè)的二維柵格地圖，將環(huán)境信息分為可通行與不可通行兩類，該類方法主要是對不可通行的障礙物進(jìn)行規(guī)避[6-7]，但是缺少環(huán)境感知模塊對于三維環(huán)境高度以及語義信息的輸入，在具有連續(xù)起伏路面或者有大量可通行植被，煙霧等環(huán)境中會出現(xiàn)運動規(guī)劃出現(xiàn)錯誤解甚至不可行解的情況。另一種是將環(huán)境高度特征進(jìn)行表征，又分為2.5D類型的地圖[8-10]、三維點云地圖[11]以及三維柵格地圖[12]。2.5D地圖是在地圖柵格中賦予高度或者其他相關(guān)信息，其中賦予高度信息的地圖類型又稱為數(shù)字高程地圖。文獻(xiàn)[8]考慮了環(huán)境中的坡度和粗糙度利用混合A*圖搜索算法進(jìn)行運動規(guī)劃。文獻(xiàn)[9]將環(huán)境分類為環(huán)境態(tài)勢場、道路層、威脅層、障礙層等多層數(shù)字高程地圖建立最終的態(tài)勢場模型利用人工勢場的方法進(jìn)行運動規(guī)劃。文獻(xiàn)[10]采用地形層，坡度層，地表特征分類層，地圖不確定性層考慮能量消耗進(jìn)行運動規(guī)劃。三維點云地圖是使用相機(jī)或者激光雷達(dá)等傳感器直接輸出的原始數(shù)據(jù)得到的地圖。文獻(xiàn)[11]采用三維點云地圖結(jié)合快速隨機(jī)擴(kuò)展樹(RRT)、改進(jìn)快速隨機(jī)擴(kuò)展樹(RRT*)和基于數(shù)值優(yōu)化的方法生成具有三維信息路徑。該方法在生成路徑的過程中，利用點云信息通過擬合平面得到當(dāng)前位置的三維空間姿態(tài)，通過包含三維空間姿態(tài)的代價函數(shù)進(jìn)行節(jié)點的選擇與路徑的優(yōu)化。三維柵格地圖是通過對點云信息進(jìn)行局部離散化計算，可以避免進(jìn)行大規(guī)模點云計算時間效率低下的問題。文獻(xiàn)[12]采用三維柵格地圖對環(huán)境中的局部障礙進(jìn)行分析，并通過平臺結(jié)構(gòu)與環(huán)境信息對于平臺的位姿進(jìn)行估計，用于運動規(guī)劃來避免平臺姿態(tài)的突變。

越野環(huán)境下無人駕駛系統(tǒng)運動控制模塊主要解決復(fù)雜環(huán)境下的地面無人機(jī)動平臺與接觸地面難以完全表征的關(guān)系，即模型的不確定性。文獻(xiàn)[13]針對越野環(huán)境的不確定性，設(shè)計了一種基于反饋校正的模型預(yù)測控制器。文獻(xiàn)[14]考慮控制延時與執(zhí)行機(jī)構(gòu)的執(zhí)行延時的模型預(yù)測控制器，針對平臺延時問題進(jìn)行了控制模型的設(shè)計。文獻(xiàn)[15]提出利用自適應(yīng)前饋調(diào)節(jié)器采用粒子群優(yōu)化算法和支持向量機(jī)的方法，通過軌跡中的曲率與速度信息對控制器進(jìn)行策略調(diào)整。文獻(xiàn)[16]針對行為軌跡與操控層上映射關(guān)系進(jìn)行了相關(guān)研究。

前期的研究針對無人駕駛系統(tǒng)中的各個模塊均開展了相應(yīng)的工作，但各個模塊之的串聯(lián)方式比較弱化。針對該問題，本文利用語義信息串聯(lián)環(huán)境感知模塊、運動規(guī)劃模塊、運動控制模塊，豐富了無人駕駛系統(tǒng)模塊之間的輸出信息，提升了地面無人機(jī)動平臺在越野環(huán)境的通行能力。

1 基于語義信息串聯(lián)的無人駕駛系統(tǒng)

1.1 系統(tǒng)框架

圖1為本文所提無人系統(tǒng)框架。圖1中，加粗字體為框架中包含語義信息的兩方面內(nèi)容：一是串聯(lián)環(huán)境感知與運動規(guī)劃模塊的三維語義柵格地圖，二是串聯(lián)運動規(guī)劃與運動控制模塊的行為語義軌跡；T(sx，sy，sz，syaw，spith，sroll，sv，stype)為三維姿態(tài)信息的軌跡，sx、sy、sz為軌跡點以在平臺局部三維坐標(biāo)系x軸、y軸、z軸的三維坐標(biāo)，syaw、spith、sroll為軌跡點在繞平臺局部三維坐標(biāo)系軸的旋轉(zhuǎn)角度量，sv為軌跡點上的期望速度，stype為軌跡點對應(yīng)行為基元的類型；(px、py、pz)為柵格的三維坐標(biāo)，pt為柵格的可通行度，pγ為柵格對應(yīng)的法向量；C(vl，vr)為運動控制模塊輸出，其中vl、vr分別為履帶平臺左右側(cè)履帶速度。

圖1 基于語義信息的無人駕駛系統(tǒng)框架

三維語義柵格地圖將環(huán)境基于圖像語義屬性、激光雷達(dá)點云特征更詳細(xì)地建模分類，形成更加豐富的通行度語義分類，運動規(guī)劃模塊基于該地圖通過利用離線生成的運動基元庫首先根據(jù)參考速度與平臺前方可通行區(qū)域的通行度選擇當(dāng)前最優(yōu)的運動基元簇，其中的運動基元是指在有限的時間并符合自身約束條件下車輛從起始位姿到目標(biāo)位姿的軌跡，運動基元簇是通過采樣不同的目標(biāo)位姿形成的軌跡簇。之后與根據(jù)與全局參考線的偏差、曲線能量、碰撞風(fēng)險、通行度代價等在線選擇出最優(yōu)的運動基元，生成具有平滑且姿態(tài)變化小的軌跡，根據(jù)不同行為語義的運動基元對生成軌跡進(jìn)行分段得到行為語義軌跡。

行為語義軌跡將規(guī)劃軌跡基于行為語義基元進(jìn)行分段，運動控制通過軌跡匹配，利用數(shù)據(jù)集合中的優(yōu)化參數(shù)在線實時調(diào)整控制參數(shù)，減小運動跟蹤控制跟蹤軌跡的橫向偏差、航向偏差、速度偏差。

1.2 底盤執(zhí)行模塊

本文采用圖2所示中型無人履帶平臺作為無人駕駛系統(tǒng)的底盤執(zhí)行模塊。該平臺通過串聯(lián)式混合動力輔助動力裝置(APU)系統(tǒng)結(jié)合能量管理算法為平臺提供驅(qū)動能量，同時動力電池可以單獨為平臺供能。該平臺的驅(qū)動形式為雙側(cè)獨立電驅(qū)動，兩側(cè)驅(qū)動鏈為驅(qū)動電機(jī)分別串聯(lián)兩擋機(jī)械式自動變速器(AMT)行星變速箱與側(cè)減速器將動力傳遞至履帶。由于兩側(cè)驅(qū)動電機(jī)之間無機(jī)械結(jié)構(gòu)連接限制，平臺可在復(fù)雜越野環(huán)境中靈活行進(jìn)。平臺搭載64線主激光雷達(dá)與2個16線激光雷達(dá)形成激光雷達(dá)組；采用3個相機(jī)作為圖像傳感器；采用定位信息接收機(jī)結(jié)合千尋網(wǎng)絡(luò)差分與慣性導(dǎo)航系統(tǒng)作為平臺的定位源。采用3臺工控機(jī)作為計算設(shè)備，其中一臺配備RTX 2080Ti作為圖像語義分割處理工控機(jī)，另外兩臺分別處理激光雷達(dá)點云和運動規(guī)劃控制程序。

圖2 中型混合動力地面無人履帶機(jī)動平臺

1.3 環(huán)境感知模塊

如圖3所示，環(huán)境感知模塊利用相機(jī)與激光雷達(dá)融合感知的方案，結(jié)合全局拓?fù)鋮⒖悸窂?，考慮障礙物、起伏地形、地表粗糙度、地面語義屬性以及地面無人機(jī)動平臺穩(wěn)定性等，輸出帶有局部可通行度語義柵格地圖。

圖3 三維可通行度語義柵格地圖

1.3.1 圖像語義分割與點云融合

首先基于BiSeNet V2[17]網(wǎng)絡(luò)在數(shù)據(jù)集上訓(xùn)練來提取圖像特征完成輸入圖像的語義分割，在圖像分割出的可通行語義的類別中，如土路、草地等，通過激光雷達(dá)系統(tǒng)與相機(jī)系統(tǒng)之間的坐標(biāo)轉(zhuǎn)化關(guān)系將圖像提取到的語義信息投影至激光雷達(dá)點云上形成激光雷達(dá)語義點云，將不可通行語義類別如車輛等，直接賦值為不可通行區(qū)域。之后采用激光雷達(dá)語義點云進(jìn)行可通行區(qū)域的粗提取。圖4所示為實際場景中激光雷達(dá)語義點云。

圖4 激光雷達(dá)語義點云

1.3.2 可通行區(qū)域粗提取

通過相機(jī)與激光雷達(dá)數(shù)據(jù)融合得到語義點云信息描述環(huán)境，考慮本文所用平臺的橫縱向通過性能，縱向包括最大坡度、垂直墻跨越高度，橫向包括平臺發(fā)生傾覆的最大橫坡角度。通過對三維點云的擬合法向量以及高度梯度進(jìn)行提取與履帶平臺通過能力的閾值進(jìn)行比較，提取不可通行和未知區(qū)域。其中，未知屬性可以根據(jù)點到車體距離遠(yuǎn)近和點云局部密度大小進(jìn)行判斷，距離平臺較遠(yuǎn)和點云密度過低的判定為未知屬性；不可通行屬性可以通過比較點云特征與平臺的坡度穩(wěn)定角、垂直墻最高高度等進(jìn)行判斷，點云特征值大于這些閾值的直接判斷為不可通行。最終完成越野環(huán)境下可通行區(qū)域的粗提取。

1.3.3 可通行區(qū)域內(nèi)可通行性分析

本文通過利用高斯混合聚類(GMM)方法訓(xùn)練出聚類模型，并對輸入點云進(jìn)行在線實時估計，分為三類，即易通行、可通行和難通行。

在點云特征的提取過程中，首先利用平臺搭載慣導(dǎo)對點云位姿進(jìn)行矯正，之后在平臺水平坐標(biāo)系計算所有點云特征，所有環(huán)境地形特征全部基于當(dāng)前時刻平臺所處位置進(jìn)行判斷。

點云特征的高斯聚類模型L(D)是根據(jù)點云樣本集D={p1,p2,…，pm}為過濾后的m個點，每個點的局部特征向量為F={f1,f2,f3}，高斯混合聚類的目標(biāo)是最大化對數(shù)似然函數(shù)，如(1)式：

(1)

本文選取以下3個矢量作為高斯聚類的局部特征向量：

f1：點云及其鄰近區(qū)域點云的法向量矢量和的模的均值，可以表示道路的不平度信息。

f2：點云及其鄰近區(qū)域點云的法向量矢量和在車體水平坐標(biāo)系Oyz平面投影與z軸的夾角，表示縱坡坡度。

f3：點云及其鄰近區(qū)域點云的法向量矢量和在車體水平坐標(biāo)系Oxz平面投影與z軸的夾角，表示橫坡坡度。

其中，領(lǐng)近區(qū)域選取平臺幾何形狀占據(jù)的區(qū)域。

將特征向量進(jìn)行歸一化處理，利用GMM方法進(jìn)行多元聚類，得到易通行、可通行和難通行三類可通行度標(biāo)簽。

在實際在線感知過程中，將點云的特征向量輸入至GMM模型，可得到當(dāng)前點云的對應(yīng)環(huán)境的可通行度。

1.3.4 三維語義柵格地圖構(gòu)建與地圖融合

本文采用八叉樹的數(shù)據(jù)管理地圖數(shù)據(jù)，采用基于貝葉斯核推理的柵格屬性推理，對三維柵格的可通行性屬性進(jìn)行建模，從而對點云缺失部分的柵格的屬性進(jìn)行推理，以實現(xiàn)連續(xù)的三維建圖。

地圖融合基于文獻(xiàn)[18]所提方法進(jìn)行融合。

1.4 運動規(guī)劃模塊

運動規(guī)劃模塊分為離線基于最優(yōu)控制理論的二維行為運動基元生成與在線基于三維語義柵格地圖與全局參考路徑的行為運動基元選擇擴(kuò)展。通過將二維運動基元向三維環(huán)境進(jìn)行投影，最終輸出具備三維姿態(tài)信息的軌跡為

T=[sx,sy,sz,syaw,spitch,sroll,sv,stype]

圖5所示為行為運動基元投影方法與拼接過渡方法示意圖，其中運動基元投影方法采用基于文獻(xiàn)[11]的方法。首先基于二維平面生成行為運動基元，得到離散的運動基元軌跡點p=[x,y] ，之后根據(jù)需要拼接的父行為運動基元末位姿確定拼接后的子行為運動基元軌跡點p′=[x′,y′]，之后將拼接后的二維軌跡點投影至三維環(huán)境中=[,,] ，并在車體范圍如圖5中綠色矩形框所示，利用主元分析法計算法向量進(jìn)而計算平臺在該處的俯仰角、側(cè)傾角、航向角。圖5中不同軌跡顏色為不同類型的行為運動基元，軌跡點上的坐標(biāo)系反映了當(dāng)平臺運動至該軌跡點時平臺的姿態(tài)信息。

1.4.1 行為運動基元的生成

行為運動基元的生成考慮履帶平臺的基本運動學(xué)約束、平滑過渡約束、平臺- 地形約束、平臺行為約束，以生成行為運動基元盡可能平滑為目標(biāo)，離線生成用于在線規(guī)劃時所用的行為運動基元庫。生成行為運動基元的優(yōu)化問題定義如(2)式：

s.t.

(2)

式中：f是履帶平臺的運動學(xué)約束；g(·)為考慮生成運動基元在路徑曲率和速度層的平滑性目標(biāo)函數(shù)；B為區(qū)分運動基元的行為約束；Ue為考慮平臺- 地形之間關(guān)系對控制量與狀態(tài)量的不等式約束；Up為考慮平臺執(zhí)行機(jī)械結(jié)構(gòu)與運動學(xué)約束的不等式約束；T為考慮運動基元拼接過渡的約束；優(yōu)化的控制量為u(t)=[ηx(t),αΔv(t)]T，ηx(t)為平臺縱向的沖擊度，αΔv(t)為速差變化率的導(dǎo)數(shù)；平臺的狀態(tài)參量s(t)=[x(t),y(t),θ(t),vx(t),ax(t),Δv(t),ωΔv(t)]T，其中x(t)、y(t)和θ(t)為全局坐標(biāo)系下的位置坐標(biāo)值和航向角，vx(t)為平臺坐標(biāo)系下沿平臺坐標(biāo)系x軸的縱向速度，ax(t)為相對應(yīng)的加速度，Δv(t)為兩側(cè)履帶的速差，ωΔv(t)為兩側(cè)履帶速差的變化率；t1為基元生成的起始時刻，tg為基元生成的終止時刻。

雙側(cè)獨立驅(qū)動無人履帶平臺的運動學(xué)微分約束f如(3)式：

(3)

式中：B為兩側(cè)履帶接地點中心距。圖6所示為履帶平臺微分約束中各參量示意圖，其中L為履帶接地長度。

圖6 履帶平臺微分約束參量示意圖

平臺行為約束主要對生成行為運動基元的起始與目標(biāo)狀態(tài)進(jìn)行限制，常見的行為如類直線行駛，類直角彎轉(zhuǎn)向，類U形彎大角度轉(zhuǎn)向，調(diào)頭，雙移線，上下坡等行為。起始與目標(biāo)狀態(tài)的約束如(4)式：

(4)

式中：d為首末位姿橫向移動的距離；β為首末位姿的角度差。

在運動基元生成過程中，由于平臺執(zhí)行機(jī)構(gòu)的機(jī)械結(jié)構(gòu)約束，以及平臺-地形之間關(guān)系，在運動基元生成過程需要滿足不等式約束如(5)式：

(5)

由于運動規(guī)劃最終的輸出軌跡是由行為運動基元序列組成，在離線生成基元時需要考慮橫向和縱向參量的平滑過渡，約束形式如(6)式：

Tt=[ax(t1),ηx(t1),Δv(t1),ωΔv(t1),αΔv(t1),ax(tg),ηx(tg),Δv(tg),ωΔv(tg),αΔv(tg)]T=0

(6)

行為運動基元生成的目標(biāo)函數(shù)從橫縱向兩個方面考慮了橫縱參量的平滑度，離散化的目標(biāo)函數(shù)表示如(7)式：

(7)

式中：c1、c2、c3、c4、c5為目標(biāo)函數(shù)中各參量的權(quán)重系數(shù)；N為生成運動基元的軌跡點數(shù)。

1.4.2 行為運動基元的拼接與過渡

行為基元的拼接與過渡考慮環(huán)境感知模塊提供的三維語義地形以及全局參考路徑的引導(dǎo)，以行為運動基元的擴(kuò)展代價為依據(jù)從離線行為運動基元庫中選出不同種類的行為運動基元進(jìn)行拼接與過渡，最終生成由多個行為基元序列組成的軌跡。

本文采用兩層的運動基元選擇邏輯，首先通過參考軌跡參考速度與地面無人平臺參考軌跡前方可通行區(qū)域的通行度選擇出合適的運動基元簇。之后在運動基元簇中通過比較運動基元的擴(kuò)展代價遍歷選擇出最優(yōu)的運動基元，其中擴(kuò)展代價包括行為運動基元與參考軌跡的橫縱向偏差Jo、碰撞風(fēng)險代價Jc、平滑代價Js、通行度代價Jt。

本文全局規(guī)劃采用基于谷歌地球衛(wèi)星地圖制作的環(huán)境拓?fù)涞貓D[20]，在環(huán)境拓?fù)涞貓D的基礎(chǔ)上利用A*搜索，完成經(jīng)過必經(jīng)點的全局規(guī)劃，輸出具有良好引導(dǎo)效果的全局參考軌跡[21]。因此被選擇的行為運動基元上的軌跡點需要與參考軌跡匹配點在橫向偏差、航向偏差、速度偏差盡可能小，代價計算如(8)式：

(8)

式中：Np為行為運動基元上等間隔采樣點的總數(shù)目；p為正態(tài)分布函數(shù)；jn為生成軌跡采樣點與參考線匹配點之間的橫縱向偏差代價；x為運動基元軌跡點距離軌跡末端點的距離；μ為軌跡末端點設(shè)定的距離值，一般取0，σ為軌跡長度的三分之一，dn為生成軌跡采樣點與參考線匹配點之間的橫向偏差，Δθn為生成軌跡采樣點與參考線匹配點之間的航向偏差，Δv為生成軌跡采樣點與參考線匹配點之間的速度偏差；ωd、ωh、ωv分別為橫向偏差、航向角偏差、速度偏差的系數(shù)。

碰撞風(fēng)險代價的計算首先將地面無人平臺利用文獻(xiàn)[6]所提6覆蓋圓的方法對地面無人平臺外形結(jié)構(gòu)進(jìn)行近似，采用6覆蓋圓圓心距離最近障礙物的距離do與覆蓋圓半徑r的差值作為運動基元的碰撞風(fēng)險代價，如(9)式：

(9)

行為運動基元的平滑度評價采用綜合曲率和長度的軌跡能量函數(shù)，該值越小表明該運動基元的能量越低，軌跡越平滑，該代價值計算如(10)式：

(10)

式中：κn為基元采樣點上的曲率值；Δsn為相鄰采樣點之間的間距。

在環(huán)境感知提供的三維語義柵格地圖下，運動基元的通行度代價計算包含兩個方面，一方面是將行為運動基元軌跡點匹配柵格附近平臺外形結(jié)構(gòu)大小區(qū)域的擬合平面法向量與垂直方向的夾角，另一方面是與匹配區(qū)域通行度代價的均值。擬合平面法向量計算方法如(11)式：

(11)

(12)

式中：pit為柵格的可通行度；I為垂向z軸單位向量。

行為基元的最終選擇是對多目標(biāo)進(jìn)行綜合評價，選擇代價的計算公式如(13)式：

J(mp)=Jo(mp)+ωsJs(mp)+ωtJt(mp)+ωcJc(mp)

(13)

式中：ωs、ωt、ωc為相應(yīng)代價的權(quán)重系數(shù)。

在線基元選擇的原則是選擇總代價值最小的行為運動基元，通過選擇基元向前擴(kuò)展，當(dāng)行為運動基元的末狀態(tài)與規(guī)劃終點區(qū)域后完成擴(kuò)展后，形成最終的具有時空信息的三維軌跡。

在選擇過程中，相鄰行為運動基元之間的過渡基元為變速直線運動，采用三次Hermite插值完成過渡段中速度的生成。

1.5 運動控制模塊

(14)

行為運動基元組成的軌跡包括道路的坡度、路面不平度、曲率特征等語義信息，以上信息對運動控制模型約束以及目標(biāo)函數(shù)設(shè)計上提供更多的參考信息。在進(jìn)行模型預(yù)測軌跡跟蹤控制模型的建立過程中，主要考慮執(zhí)行機(jī)構(gòu)的機(jī)械約束、平臺的運動學(xué)約束以及平臺動力學(xué)約束。根據(jù)道路類型、曲率特征、平臺速度等信息，對電機(jī)外特性以及履帶車輛坡道轉(zhuǎn)向的動力學(xué)分析，增加關(guān)于驅(qū)動系統(tǒng)后備功率的約束，保證極限工況下履帶平臺的軌跡跟蹤的穩(wěn)定性。當(dāng)平臺在坡道上進(jìn)行低速勻速轉(zhuǎn)向時，結(jié)合參考軌跡上平臺姿態(tài)與規(guī)劃速度，對車輛在坡道上跟蹤行駛時所需的功率和力矩進(jìn)行預(yù)測，結(jié)合電機(jī)外特性的邊界值，對車輛的控制量進(jìn)行約束，使得平臺可以在坡道上進(jìn)行穩(wěn)定的軌跡跟蹤。 最終建立如(15)式的約束不等式。

umin≤u(k+i)≤umax，0≤i≤Nc
Δumin≤Δu(k+i)≤Δumax
Δu(k+i)=u(k+i)-u(k+i-1)
ymin(k+i)≤yb(k+i)≤ymax(k+i),0≤i≤Np
Δu(k+i)=0，Nc≤i≤Np

(15)

式中：umin、umax的約束求解主要是結(jié)合電機(jī)外特性，道路坡道信息、車輛動力學(xué)模型等，在保證車輛具備足夠的后備功率、轉(zhuǎn)向力矩的前提下，得到的該工況下車輛速度邊界值。Δumin、Δumax為驅(qū)動電機(jī)調(diào)速的變化率約束。Np、Nc分別是預(yù)測時域和控制時域；yb為狀態(tài)量的約束輸出；k為預(yù)測時域內(nèi)的時刻。

基于模型預(yù)測控制方法的運動控制模塊根據(jù)運動規(guī)劃模塊提供的由行為運動基元組成的具有語義分類的參考軌跡，采用多組控制目標(biāo)函數(shù)權(quán)重系數(shù)對不同語義分類軌跡進(jìn)行跟蹤。根據(jù)控制目標(biāo)函數(shù)如(16)式中所考慮的軌跡跟蹤控制的橫向偏差、航向偏差以及控制量變化率為指標(biāo)反映的軌跡跟蹤穩(wěn)定性等構(gòu)建最優(yōu)軌跡跟蹤控制精度的評價體系，從而得出不同控制目標(biāo)函數(shù)權(quán)重系數(shù)以及不同的系數(shù)比例對最終軌跡跟蹤控制效果的影響規(guī)律，建立不同語義軌跡下的控制策略數(shù)據(jù)集。

(16)

式中：ωl為橫向偏差的權(quán)重系數(shù)；le為橫向偏差；ωh為航向偏差的權(quán)重系數(shù)；he為航向偏差；ωv為速度偏差的權(quán)重系數(shù)；vd為期望速度值；后兩項為兩側(cè)履帶卷繞速度的變化率；ωu為控制量變化率的權(quán)重系數(shù)。

通過試驗數(shù)據(jù)積累和分析，在平臺能夠進(jìn)行穩(wěn)定軌跡跟蹤控制的可行域內(nèi)，平臺的軌跡跟蹤效果主要受到橫向跟蹤權(quán)重以及航向跟蹤權(quán)重的影響，同時兩權(quán)重系數(shù)的比例關(guān)系在不同語義軌跡下平臺的表現(xiàn)具有較大差異。如圖7所示，平臺在跟蹤具有語義分類的參考軌跡時，在不同語義屬性的軌跡上采用不同的控制參數(shù)來提升軌跡跟蹤的精度與穩(wěn)定性。

圖7 控制參數(shù)語義匹配原則示意圖

2 實驗結(jié)果與討論

本節(jié)展示了在地形起伏的越野場景下環(huán)境感知以及運動規(guī)劃結(jié)果，基于二維柵格地圖與三維語義地圖的差異化的運動規(guī)劃結(jié)果進(jìn)行討論，同時本文對于具有行為運動基元語義的軌跡進(jìn)行了運動控制跟蹤試驗，對比分析了基于行為語義軌跡適配參數(shù)對于軌跡跟蹤效果的提升。

試驗場地選取越野環(huán)境場地，該場地道路不平度，道路表面屬性差異明顯，環(huán)境復(fù)雜度高。試驗平臺采用圖2中型混合動力地面無人履帶機(jī)動平臺進(jìn)行方法的實車測試，各模塊的試驗參數(shù)如表1所示。

表1 各模塊試驗參數(shù)

2.1 環(huán)境感知結(jié)果

圖8為環(huán)境感知模塊對越野環(huán)境的不同方式建模結(jié)果。圖8(a)是利用三維語義柵格地圖對試驗環(huán)境進(jìn)行建圖。圖8(b)是根據(jù)不可通行相對高度閾值h對環(huán)境進(jìn)行了二值分類。通過對比可以看出，對于環(huán)境進(jìn)行簡單的二值建模，造成環(huán)境中的很多信息丟失，而本文采用的三維語義柵格地圖盡可能地保留了環(huán)境中信息，對運動規(guī)劃模塊具有很好的約束引導(dǎo)作用。在輸入圖像寬高像素為640×480、點云過濾距離為20 m、限制融合幀數(shù)為80幀的情況下本文生成單幀三維語義柵格地圖的時間消耗平均為130 ms，生成單幀平面二維柵格地圖的時間為96 ms，雖然時間略有增加，但時間消耗仍可以滿足實時性要求。

圖8 環(huán)境感知建圖結(jié)果對比

2.2 運動規(guī)劃結(jié)果

圖9為運動規(guī)劃模塊基于不同類型地圖的運動規(guī)劃結(jié)果，圖9(a)中，S為直線行為運動基元，C為單移線行為運動基元，U為類U型彎行為運動基元，G為普通小曲率行為運動基元，R為類直角彎行為運動基元。

圖9 運動規(guī)劃結(jié)果對比

為了驗證基于三維語義柵格地圖的運動規(guī)劃方法，將本文與文獻(xiàn)[6]規(guī)劃方法進(jìn)行對比，圖9為整體規(guī)劃結(jié)果的對比，在整體結(jié)果對比之后選取A、B兩個典型場景進(jìn)行細(xì)化對比，其中A場景為類U型彎場景，如圖10所示；B場景為上縱坡場景，如圖11所示。無人履帶平臺在環(huán)境中行駛時，平臺的姿態(tài)一方面影響無人駕駛系統(tǒng)中環(huán)境感知與運動控制模塊的精度，另一方面較大的姿態(tài)變化容易對平臺硬件造成損害。本文選取平臺行駛過程中的平均俯仰角與平均側(cè)傾角對平臺的姿態(tài)變化進(jìn)行表征。最終選取的對比指標(biāo)包括規(guī)劃時間、平均俯仰角、平均側(cè)傾角、平均曲率。其中平均俯仰角、平均側(cè)傾角、平均曲率為全部路點相關(guān)量之和與路徑長度的比值。

圖10 A場景運動規(guī)劃結(jié)果對比

圖11 B場景運動規(guī)劃結(jié)果對比

通過圖9結(jié)合表2規(guī)劃結(jié)果的對比可以發(fā)現(xiàn)，本文規(guī)劃方法采用離線生成的運動基元、在線選擇運動基元規(guī)劃路徑，規(guī)劃耗時集中在對于豐富運動基元的選擇過程，與文獻(xiàn)[6]規(guī)劃時間基本持平。由于環(huán)境感知提供了可通度語義信息，運動規(guī)劃利用該信息在規(guī)劃時盡量選擇易通行區(qū)域通行，而文獻(xiàn)[6]規(guī)劃方法采用二值柵格地圖由于沒有相關(guān)信息，在提取道德可通行區(qū)域內(nèi)盡量選擇較短路徑，造成規(guī)劃路徑對應(yīng)的平均俯仰角、平均側(cè)傾角較本文方法較高，同時本文采用的運動基元在生成與選擇均考慮了軌跡平滑性，因此本文在軌跡平均曲率上具備一定的優(yōu)勢。

表2 規(guī)劃軌跡指標(biāo)對比

如圖10所示，在A場景中，本文規(guī)劃方法規(guī)劃路徑在路面不平度更低的區(qū)域，而文獻(xiàn)[6]方法由于沒有利用到該方面的信息，因此規(guī)劃路徑所在區(qū)域路面不平度較高，在入彎時文獻(xiàn)[6]規(guī)劃方法采用了切彎的策略，雖然使得路徑的平均曲率較小，但是軌跡在入彎處規(guī)劃軌跡的位姿發(fā)生較大變化，該種情況對于平臺的穩(wěn)定行駛具有較大的考驗，而本文規(guī)劃方法則繞開入彎處的難通行區(qū)域，在整個過彎過程中車輛位姿變化較小。

如圖11所示，在B場景中，本文規(guī)劃方法在通過縱坡類難通行區(qū)域時以相對較小的曲率通行，同時本文規(guī)劃方法在上坡時基本處于坡道正中央，規(guī)劃軌跡通過縱坡時側(cè)傾角變化不大，而文獻(xiàn)[6]規(guī)劃方法規(guī)劃軌跡通過縱坡時由于軌跡與和縱坡存在一定的夾角，因此規(guī)劃軌跡通過縱坡時側(cè)傾角變化較大。

本文在規(guī)劃時間與文獻(xiàn)[6]基本持平的基礎(chǔ)上，在平均俯仰角、平均側(cè)傾角、平均曲率等體現(xiàn)無人機(jī)動平臺通過穩(wěn)定性的重要指標(biāo)中具有顯著優(yōu)勢。同時本文所提運動規(guī)劃方法，根據(jù)行為運動基元的不同，將軌跡進(jìn)行了語義分段，對運動控制模塊的參數(shù)優(yōu)化起到參考作用。

2.3 運動控制結(jié)果

將運動規(guī)劃模塊生成的具有行為語義的軌跡作為參考軌跡，運動控制模塊修改目標(biāo)函數(shù)中的橫向偏差權(quán)重ωl以及航向偏差權(quán)重ωh分別對軌跡進(jìn)行跟蹤，通過對比跟蹤的航向偏差、橫向偏差以及速度偏差，將在不同行為語義軌跡下的最佳參數(shù)組成語義參數(shù)集合，對模型預(yù)測的目標(biāo)函數(shù)在線修正，形成變參數(shù)的運動控制模型。如圖12所示，將變參數(shù)模型與權(quán)重比例分別為1∶1，1∶5，1∶10的定參數(shù)控制模型的三項跟蹤偏差進(jìn)行對比，可以發(fā)現(xiàn)不同權(quán)重比例的模型對于不同行為語義軌跡的跟蹤效果具有偏差，而變參數(shù)模型由于針對不同行為語義軌跡進(jìn)行了參數(shù)適配，從表3中可以看出，變參數(shù)模型相較于固定參數(shù)比例的模型在橫向偏差最少提升20.7%，航向偏差最少提升2.7%，速度偏差最少提升6.9%；方法平均消耗時間為18.784 ms，相較于不變參算法計算成本基本沒有增加。

表3 軌跡跟蹤偏差對比

圖12 不同權(quán)重系數(shù)下軌跡跟蹤結(jié)果

3 結(jié)論

本文提出利用語義信息將無人駕駛系統(tǒng)中環(huán)境感知模塊、運動規(guī)劃模塊、運動控制模塊之間息進(jìn)行串聯(lián)的方案。結(jié)果表明：所提出的基于語義信息你串聯(lián)的無人駕駛系統(tǒng)，將難以標(biāo)準(zhǔn)量化的環(huán)境特征或軌跡特征以語義的形式表述，將定義的語義信息在無人駕駛系統(tǒng)模塊之間的傳遞，不同模塊利用模塊間傳遞的語義信息對模塊中模型或方法中的關(guān)鍵參量進(jìn)行限制或約束，豐富了模塊間信息交互的內(nèi)容。

本文的主要結(jié)論如下：

1) 環(huán)境感知模塊通過激光雷達(dá)點云與相機(jī)圖像融合感知的方案，利用圖像語義分割結(jié)合點云環(huán)境特征提取對環(huán)境進(jìn)行建模，最終輸出具有可通行度信息，地形語義信息的三維語義柵格地圖。相較于平面二值柵格地圖能夠提供更多的環(huán)境通行度信息。

2) 運動規(guī)劃模塊離線基于最優(yōu)控制理論生成行為運動基元，在線基于三維語義柵格地圖選擇合適的行為運動基元生成具有行為語義信息的軌跡。本文所提規(guī)劃方法相較于基于二值柵格地圖運動規(guī)劃方法在軌跡的平均俯仰角、平均側(cè)傾角、平均曲率分別降低46.1%、46.5%、14.2%。同時軌跡的語義類別屬性可用于運動控制模塊的參數(shù)優(yōu)化。

3) 運動控制模塊根據(jù)軌跡信息結(jié)合平臺動力學(xué)約束，基于行為基元類型對控制目標(biāo)函數(shù)權(quán)重進(jìn)行在線的優(yōu)化匹配，相較于定參數(shù)跟蹤軌跡的方法分別在橫向偏差、航向偏差、速度偏差降低17.1%、2.7%、6.1%。