履帶平臺(tái)無(wú)人駕駛系統(tǒng)基于語(yǔ)義信息的模塊串聯(lián)方法

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

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

0 引言

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

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

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

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

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

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

1.1 系統(tǒng)框架

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

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

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

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

1.2 底盤(pán)執(zhí)行模塊

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

圖2 中型混合動(dòng)力地面無(wú)人履帶機(jī)動(dòng)平臺(tái)

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

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

圖3 三維可通行度語(yǔ)義柵格地圖

1.3.1 圖像語(yǔ)義分割與點(diǎn)云融合

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

圖4 激光雷達(dá)語(yǔ)義點(diǎn)云

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

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

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

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

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

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

(1)

本文選取以下3個(gè)矢量作為高斯聚類(lèi)的局部特征向量：

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

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

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

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

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

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

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

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

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

1.4 運(yùn)動(dòng)規(guī)劃模塊

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

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

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

1.4.1 行為運(yùn)動(dòng)基元的生成

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

s.t.

(2)

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

雙側(cè)獨(dú)立驅(qū)動(dòng)無(wú)人履帶平臺(tái)的運(yùn)動(dòng)學(xué)微分約束f如(3)式：

(3)

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

圖6 履帶平臺(tái)微分約束參量示意圖

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

(4)

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

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

(5)

由于運(yùn)動(dòng)規(guī)劃最終的輸出軌跡是由行為運(yùn)動(dòng)基元序列組成，在離線生成基元時(shí)需要考慮橫向和縱向參量的平滑過(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)

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

(7)

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

1.4.2 行為運(yùn)動(dòng)基元的拼接與過(guò)渡

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

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

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

(8)

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

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

(9)

行為運(yùn)動(dòng)基元的平滑度評(píng)價(jià)采用綜合曲率和長(zhǎng)度的軌跡能量函數(shù)，該值越小表明該運(yùn)動(dòng)基元的能量越低，軌跡越平滑，該代價(jià)值計(jì)算如(10)式：

(10)

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

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

(11)

(12)

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

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

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

(13)

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

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

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

1.5 運(yùn)動(dòng)控制模塊

(14)

行為運(yùn)動(dòng)基元組成的軌跡包括道路的坡度、路面不平度、曲率特征等語(yǔ)義信息，以上信息對(duì)運(yùn)動(dòng)控制模型約束以及目標(biāo)函數(shù)設(shè)計(jì)上提供更多的參考信息。在進(jìn)行模型預(yù)測(cè)軌跡跟蹤控制模型的建立過(guò)程中，主要考慮執(zhí)行機(jī)構(gòu)的機(jī)械約束、平臺(tái)的運(yùn)動(dòng)學(xué)約束以及平臺(tái)動(dòng)力學(xué)約束。根據(jù)道路類(lèi)型、曲率特征、平臺(tái)速度等信息，對(duì)電機(jī)外特性以及履帶車(chē)輛坡道轉(zhuǎn)向的動(dòng)力學(xué)分析，增加關(guān)于驅(qū)動(dòng)系統(tǒng)后備功率的約束，保證極限工況下履帶平臺(tái)的軌跡跟蹤的穩(wěn)定性。當(dāng)平臺(tái)在坡道上進(jìn)行低速勻速轉(zhuǎn)向時(shí)，結(jié)合參考軌跡上平臺(tái)姿態(tài)與規(guī)劃速度，對(duì)車(chē)輛在坡道上跟蹤行駛時(shí)所需的功率和力矩進(jìn)行預(yù)測(cè)，結(jié)合電機(jī)外特性的邊界值，對(duì)車(chē)輛的控制量進(jìn)行約束，使得平臺(tái)可以在坡道上進(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ī)外特性，道路坡道信息、車(chē)輛動(dòng)力學(xué)模型等，在保證車(chē)輛具備足夠的后備功率、轉(zhuǎn)向力矩的前提下，得到的該工況下車(chē)輛速度邊界值。Δumin、Δumax為驅(qū)動(dòng)電機(jī)調(diào)速的變化率約束。Np、Nc分別是預(yù)測(cè)時(shí)域和控制時(shí)域；yb為狀態(tài)量的約束輸出；k為預(yù)測(cè)時(shí)域內(nèi)的時(shí)刻。

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

(16)

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

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

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

2 實(shí)驗(yàn)結(jié)果與討論

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

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

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

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

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

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

2.2 運(yùn)動(dòng)規(guī)劃結(jié)果

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

圖9 運(yùn)動(dòng)規(guī)劃結(jié)果對(duì)比

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

圖10 A場(chǎng)景運(yùn)動(dòng)規(guī)劃結(jié)果對(duì)比

圖11 B場(chǎng)景運(yùn)動(dòng)規(guī)劃結(jié)果對(duì)比

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

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

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

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

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

2.3 運(yùn)動(dòng)控制結(jié)果

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

表3 軌跡跟蹤偏差對(duì)比

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

3 結(jié)論

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

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

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

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

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