王博洋， 龔建偉， 高天云，2， 張瑞增， 陳慧巖， 席軍強(qiáng)

(1.北京理工大學(xué) 機(jī)械與車輛學(xué)院， 北京 100081； 2.中國北方車輛研究所 兵器地面無人平臺研發(fā)中心， 北京 100072)

0 引言

駕駛員與車輛的交互控制系統(tǒng)是一個(gè)典型的人機(jī)交互系統(tǒng)，駕駛員在上述系統(tǒng)中可以被看作是一個(gè)自適應(yīng)優(yōu)化控制器[1]。離合器轉(zhuǎn)向機(jī)雖然具備結(jié)構(gòu)簡單、成本低廉以及使用維護(hù)方便等優(yōu)點(diǎn)，但由于車輛系統(tǒng)轉(zhuǎn)向過程中的不確定性，導(dǎo)致其轉(zhuǎn)向運(yùn)動可控性差。雖然上述車輛特性為路徑跟蹤控制帶來了挑戰(zhàn)，但熟練的人類駕駛員在長期與車輛的交互過程中，實(shí)現(xiàn)了對車輛動力學(xué)特性的深入了解，能夠依據(jù)不同的駕駛?cè)蝿?wù)，下發(fā)車輛縱向與橫向協(xié)同控制指令，實(shí)現(xiàn)對期望路徑的精確跟隨。因此，如何利用試驗(yàn)過程中的真實(shí)駕駛數(shù)據(jù)，構(gòu)建駕駛員跟蹤控制經(jīng)驗(yàn)的數(shù)學(xué)模型，對于實(shí)現(xiàn)無人行駛條件下的車輛路徑跟蹤控制具有重大意義。

目前對于無人車輛路徑跟蹤控制的研究受到國內(nèi)外許多研究機(jī)構(gòu)的重視，并取得了一些成果。這些成果從實(shí)現(xiàn)方式上可以分為兩類：一類是以車輛模型為基礎(chǔ)[2-5]，另一類是以駕駛員模型構(gòu)建為基礎(chǔ)[6-10]。文獻(xiàn)[2]在構(gòu)建輪式車動力學(xué)模型基礎(chǔ)上，利用模型預(yù)測控制，實(shí)現(xiàn)了車輛運(yùn)動軌跡的規(guī)劃與跟蹤。文獻(xiàn)[3]在構(gòu)建速差轉(zhuǎn)向車輛動力學(xué)模型基礎(chǔ)上，通過對滑移率的估計(jì)，完成對六輪獨(dú)立驅(qū)動輪胎作用力的優(yōu)化分配，并最終實(shí)現(xiàn)了速差轉(zhuǎn)向車輛對期望軌跡的跟蹤控制。文獻(xiàn)[4]以統(tǒng)計(jì)學(xué)分布規(guī)律表征地面參數(shù)，實(shí)現(xiàn)對地面參量不確定性的描述，繼而利用模型預(yù)測控制實(shí)現(xiàn)問題的求解。文獻(xiàn)[5]利用高斯回歸模型，構(gòu)建重復(fù)軌跡條件下的車輛擾動模型，實(shí)現(xiàn)對越野環(huán)境下速差轉(zhuǎn)向車輛模型不確定性的表述。雖然上述方法能夠從最優(yōu)控制的角度解決車輛路徑跟蹤問題，但對于履帶車輛而言，其轉(zhuǎn)向模型的不確定性極大地限制了車輛軌跡預(yù)測的精度，繼而影響到路徑跟蹤控制的效果[11-12]。

借助駕駛員“預(yù)瞄- 跟隨”理論，文獻(xiàn)[6-8]分別通過離散數(shù)表描述道路和車速關(guān)系，基于預(yù)測的自適應(yīng)尋優(yōu)以及強(qiáng)化學(xué)習(xí)算法，實(shí)現(xiàn)期望路徑中最優(yōu)預(yù)瞄點(diǎn)的選取。文獻(xiàn)[9]基于模糊控制器表征駕駛員的橫向操控經(jīng)驗(yàn)，并利用遺傳算法實(shí)現(xiàn)優(yōu)化。文獻(xiàn)[10]利用統(tǒng)計(jì)學(xué)建模方法實(shí)現(xiàn)車道保持過程中駕駛員個(gè)性化跟蹤經(jīng)驗(yàn)的表述。但上述駕駛員模型主要針對基于阿克曼轉(zhuǎn)向的輪式車輛，對于基于離合器轉(zhuǎn)向機(jī)的履帶車輛研究較少，普遍缺少對于車輛轉(zhuǎn)向不確定性以及多模態(tài)轉(zhuǎn)向模式切換問題的考慮。

本文針對基于離合器轉(zhuǎn)向機(jī)的履帶車輛路徑跟蹤控制問題，提出了一種基于雙層駕駛員模型的縱向與橫向協(xié)同路徑跟蹤方法。以高斯混合隱馬爾可夫模型(GMM-HMM)[13]和模糊邏輯，分別表征駕駛員的轉(zhuǎn)向模式選擇切換經(jīng)驗(yàn)以及縱向與橫向協(xié)同操縱經(jīng)驗(yàn)，實(shí)現(xiàn)對轉(zhuǎn)向模式操控序列的預(yù)測，并生成縱向、橫向控制量，最終實(shí)現(xiàn)在特定場景下軌跡跟蹤誤差小于1 m的路徑跟蹤控制。

1 無人速差轉(zhuǎn)向履帶車輛系統(tǒng)

1.1 縱向與橫向協(xié)同控制系統(tǒng)

縱向與橫向協(xié)同控制系統(tǒng)各控制器的功能以及控制器之間的信息傳遞關(guān)系，如圖1所示。

由圖1可知：跟蹤控制模塊根據(jù)規(guī)劃模塊所下發(fā)的期望路徑與限制速度，結(jié)合車輛自身的定位及狀態(tài)信息，解算離散控制量轉(zhuǎn)向模式與自動變速箱(AMT)擋位以及連續(xù)控制量期望轉(zhuǎn)向角度與期望行駛車速；上述跟蹤控制參量經(jīng)由綜合控制器解析后，分別向轉(zhuǎn)向控制器與AMT控制器下發(fā)縱向、橫向執(zhí)行器控制參量；轉(zhuǎn)向控制器以兩側(cè)操縱桿位置為依據(jù)控制底層伺服液壓轉(zhuǎn)向機(jī)構(gòu)；AMT控制器以期望擋位為依據(jù)實(shí)現(xiàn)AMT的擋位控制，并向發(fā)動機(jī)控制器轉(zhuǎn)發(fā)期望發(fā)動機(jī)轉(zhuǎn)速控制指令，實(shí)現(xiàn)對電子調(diào)速柴油機(jī)的控制。

1.2 車輛轉(zhuǎn)向狀態(tài)分析及轉(zhuǎn)向模式劃分

離合器轉(zhuǎn)向機(jī)的結(jié)構(gòu)形式如圖2所示，圖2中L1、L2分別代表左離合器和右離合器，T1、T2分別代表左制動器和右制動器。依據(jù)離合器與制動器狀態(tài)所劃分的行駛工況如表1所示。

表1 各行駛工況下內(nèi)側(cè)轉(zhuǎn)向離合器與制動器工作狀態(tài)

表1中直線行駛工況與制動轉(zhuǎn)向工況屬于兩類較為確定的轉(zhuǎn)向工況，分別對應(yīng)直駛模式與原地轉(zhuǎn)向模式，本文不對這兩種轉(zhuǎn)向模式的具體控制算法做詳細(xì)介紹。部分結(jié)合轉(zhuǎn)向工況、分離轉(zhuǎn)向工況以及部分制動轉(zhuǎn)向工況同屬行進(jìn)間轉(zhuǎn)向模式，此轉(zhuǎn)向模式下的縱向與橫向協(xié)同控制方法是本文討論的重點(diǎn)。

2 速差轉(zhuǎn)向履帶車輛雙層駕駛員模型

2.1 雙層駕駛員模型綜述

文獻(xiàn)[13]已經(jīng)完成了駕駛員操控經(jīng)驗(yàn)以及車輛特性的統(tǒng)計(jì)學(xué)描述，并利用基于GMM-HMM的駕駛員橫向控制模型實(shí)現(xiàn)了跟蹤控制過程中期望轉(zhuǎn)向模式的預(yù)測輸出。但縱向與橫向協(xié)同控制系統(tǒng)無法僅依靠轉(zhuǎn)向模式的預(yù)測序列，輸出實(shí)現(xiàn)車輛的路徑跟蹤控制指令，主要有以下兩點(diǎn)原因：

1)轉(zhuǎn)向模式輸出為離散量，對應(yīng)的是操縱桿動作區(qū)間，并非連續(xù)的位置數(shù)值，但控制系統(tǒng)需要精確的期望位置進(jìn)行引導(dǎo)。

2)轉(zhuǎn)向模式僅約束了控制量操縱桿位置的搜索區(qū)間，并未給出當(dāng)前狀態(tài)下的期望航向校正偏差，控制算法無法實(shí)現(xiàn)閉環(huán)。

因此本文提出一種基于雙層駕駛員模型的縱向與橫向協(xié)同路徑跟蹤控制方法，如圖3所示。

圖3中：

第1層駕駛員模型以航向變化偏差序列與速度序列聯(lián)合表述的軌跡序列點(diǎn)為輸入，利用經(jīng)過駕駛數(shù)據(jù)訓(xùn)練得到的GMM-HMM，生成基于駕駛員經(jīng)驗(yàn)的轉(zhuǎn)向模式預(yù)測序列；以高斯混合模型(GMM)這一統(tǒng)計(jì)學(xué)建模手段，表征當(dāng)前跟蹤狀態(tài)下操縱桿期望位置分布區(qū)間，并給出第2層駕駛員模型的操縱桿位置尋優(yōu)初值。

第2層駕駛員模型以Stanley跟蹤控制算法為基礎(chǔ)，生成路徑跟蹤過程中的期望航向校正偏差；以車輛橫擺角速度等狀態(tài)參量作為反饋，實(shí)現(xiàn)各轉(zhuǎn)向模式下的協(xié)同控制。其中，在行進(jìn)間轉(zhuǎn)向模式下，利用模糊邏輯表征駕駛員油門與操縱桿橫向與縱向協(xié)同控制配合規(guī)律，并生成控制量，實(shí)現(xiàn)行進(jìn)間操縱桿與油門的自適應(yīng)尋優(yōu)控制。

雙層駕駛員模型以基于車輛橫擺角速度的反饋校正為基礎(chǔ)，利用第1層駕駛員模型生成轉(zhuǎn)向操縱桿的尋優(yōu)控制起始值；以此為初值，在第2層駕駛員模型中，根據(jù)實(shí)際橫擺角速度與期望橫擺角速度的偏差，生成操縱桿位置與油門控制增量，解決行進(jìn)間轉(zhuǎn)向模式控制過程中轉(zhuǎn)向系統(tǒng)的不確定性問題。

2.2 第1層駕駛員模型

第1層駕駛員模型利用GMM的聚類結(jié)果作為轉(zhuǎn)向操縱桿的基本行進(jìn)間轉(zhuǎn)向基元，繼而依據(jù)期望軌跡輸入，利用隱馬爾可夫模型(HMM)預(yù)測生成的前向轉(zhuǎn)向基元概率，在線實(shí)時(shí)更新多變量GMM中單個(gè)高斯轉(zhuǎn)向基元的概率值，動態(tài)生成當(dāng)前概率下操縱桿位置區(qū)間的概率密度分布函數(shù)，并選取概率值為50%的操縱桿位置作為行進(jìn)間轉(zhuǎn)向模式下操縱桿的初始位置值，如圖4所示。

多變量GMM為

(1)

基于真實(shí)駕駛數(shù)據(jù)對模型進(jìn)行訓(xùn)練后，可以得到各擋位下的操縱桿區(qū)間概率分布，該概率分布表明了整個(gè)駕駛過程中操縱桿位置的分布情況，以1擋左側(cè)為例，其分布結(jié)果如圖5所示。

但上述概率密度反映的是整個(gè)駕駛過程中1擋左側(cè)操縱桿位置區(qū)間的概率密度分布情況，為了得到面向任一時(shí)刻動態(tài)的操縱桿位置分布規(guī)律，需要引入HMM.

HMM可以被定義為1個(gè)五元組，具體為

λ=(X,O,π,A,B),

(2)

式中：X與O為HMM狀態(tài)參量與觀測參量，X(t)={ll(t),lr(t)}，O(t)={v(t),θ(t)}，ll(t)為左側(cè)操縱桿狀態(tài)序列，lr(t)為右側(cè)操縱桿狀態(tài)序列，v(t)為車輛速度序列，θ(t)為航向變化偏差序列；π為初始狀態(tài)概率矩陣；A為狀態(tài)轉(zhuǎn)移矩陣；B為混淆矩陣。

任一狀態(tài)參量輸入條件下，第i個(gè)轉(zhuǎn)向基元能夠與其相對應(yīng)的概率值為

(3)

式中：Xi為隱含第i個(gè)狀態(tài)；Ot為t時(shí)刻的觀測量；λ為模型參數(shù)。

以上述概率值αi(Ot)分別替換(1)式中多變量高斯混合模型的先驗(yàn)概率值pi，可以實(shí)現(xiàn)操縱桿區(qū)間概率密度的在線實(shí)時(shí)更新：

(4)

以1擋左側(cè)轉(zhuǎn)向?yàn)槔?dāng)輸入的期望軌跡序列為v(t)=3 km/h、θ(t)=0.5°/s時(shí)，對應(yīng)的操縱桿概率密度分布如圖6所示。

在得到上述分布的基礎(chǔ)上，求解概率值為50%處所對應(yīng)的操縱桿位置，作為行進(jìn)間轉(zhuǎn)向操縱桿尋優(yōu)初值，如圖6所示。

2.3 第2層駕駛員模型

第2層駕駛員模型以第1層的轉(zhuǎn)向模式以及行進(jìn)間轉(zhuǎn)向模式下的操縱桿尋優(yōu)初值為輸入，結(jié)合Stanely路徑跟蹤控制算法，生成縱向、橫向控制量。由于直駛模式與原地轉(zhuǎn)向模式都只涉及油門的控制，操縱桿位置不會發(fā)生變化，控制算法較為簡單，在此不展開說明。

Stanely路徑跟蹤控制模塊如圖7所示。以車輛行駛速度v，與最近點(diǎn)的橫向偏差Δxi，以及路徑點(diǎn)航向與車輛行駛航向的偏差角度φi作為輸入，生成期望航向校正偏差角度值[14]為

(5)

式中：δi為當(dāng)前狀態(tài)下期望航向校正偏差角度；ke為橫向偏差敏感度增益參數(shù)。

行進(jìn)間轉(zhuǎn)向模式下基于駕駛員經(jīng)驗(yàn)的模糊控制邏輯框圖如圖8所示。

行進(jìn)間轉(zhuǎn)向縱向與橫向協(xié)同控制器包含油門增量與操縱桿增量兩個(gè)模糊控制器。兩個(gè)模糊控制器的輸入量均為橫擺角速度差值ε以及橫擺角速度差值變化率εc；模糊控制器的輸出分別為操縱桿增量εs與油門控制增量εa，分別在第1層駕駛員模型生成的操縱桿尋優(yōu)初始值，以及直駛模式下的油門初值上實(shí)現(xiàn)疊加，生成最終的縱向、橫向控制量。

橫擺角速度差值的連續(xù)取值范圍是[-10°/s,10°/s]，其論域?yàn)閇-20,20]；橫擺角速度差值變化率的連續(xù)取值范圍是[-5°/s2,5°/s2]，其論域?yàn)閇-20,20]；油門控制增量的連續(xù)取值范圍是[-10,5]，其論域?yàn)閇-3,1]；操縱桿位置控制增量的連續(xù)取值范圍是[-7,7]，其論域?yàn)閇-5,5]。采用如圖9所示的隸屬度函數(shù)和如表2、表3所示的模糊規(guī)則表，利用重心解模糊化法，最終求得油門控制增量與操縱桿位置控制增量。

表2 操縱桿增量模糊規(guī)則表

圖9中橫坐標(biāo)均為無量綱參數(shù)，參量NB、NM、NS、ZE、PS、PM、PB分別表征負(fù)大、負(fù)中、負(fù)小、零、正小、正中、正大。

表3 油門增量模糊規(guī)則表

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

3.1 試驗(yàn)平臺及試驗(yàn)場景

試驗(yàn)所使用的基于離合器轉(zhuǎn)向機(jī)無人速差轉(zhuǎn)向車輛試驗(yàn)平臺如圖10所示。車輛參數(shù)及傳感器參數(shù)如表4所示。

表4 無人車輛系統(tǒng)參數(shù)

試驗(yàn)中經(jīng)離線數(shù)據(jù)訓(xùn)練的HMM-GMM正向求解過程以及Stanely跟蹤控制指令生成由搭載Intel Core i7-6820EQ@2.8 GHz 4核處理器以及32 GB內(nèi)存的ADVANTECH研華工控機(jī)在50 ms的控制周期內(nèi)完成解算?；谀：壿嫷臋M向與縱向協(xié)同控制算法由時(shí)鐘主頻為16 MHz的MPC5644A飛思卡爾單片機(jī)，在10 ms控制周期內(nèi)生成控制量。

試驗(yàn)場景如圖11所示。共進(jìn)行了兩組測試，車輛的行駛擋位分別為1擋和2擋。其中，期望軌跡以基于運(yùn)動基元的搜索算法生成[15]，實(shí)際軌跡由車輛所搭載的即時(shí)定位與地圖構(gòu)建(SLAM)模塊輸出。此外，為了便于后續(xù)試驗(yàn)結(jié)果的討論，將整個(gè)路徑跟蹤試驗(yàn)分為3個(gè)階段。第1階段(t1～t2)為避障前的直線行駛階段，主要驗(yàn)證了車輛在進(jìn)行直線段路徑跟蹤時(shí)，小幅度的直駛糾偏跟蹤控制效果；第2階段(t2～t3)為換道過程中的轉(zhuǎn)向校正階段，主要驗(yàn)證車輛進(jìn)行大幅度航向校正跟蹤控制的穩(wěn)定性以及轉(zhuǎn)向模式切換的合理性；第3階段(t3～t4)為換道完成后的直線行駛恢復(fù)階段，主要驗(yàn)證車輛進(jìn)行小幅度航向校正跟蹤控制的穩(wěn)定性。

3.2 跟蹤控制縱向與橫向協(xié)同操控特性驗(yàn)證

1擋和2擋工況下跟蹤控制縱向與橫向協(xié)同操控特性如圖12和圖13所示。

1擋工況下跟蹤控制第1階段的時(shí)間區(qū)間為0～40 s，左側(cè)操縱桿進(jìn)行了多次間歇性動作，操縱桿位置峰值為70%，使得車輛實(shí)現(xiàn)了0.8°的航向校正；第2階段的時(shí)間區(qū)間為40～57 s，車輛經(jīng)歷了行進(jìn)間轉(zhuǎn)向模式和原地轉(zhuǎn)向模式的順次更迭，以及操縱桿位置的穩(wěn)步尋優(yōu)過程，實(shí)現(xiàn)了車輛的換道動作；第3階段的時(shí)間區(qū)間為57～65 s，車輛經(jīng)過多次小幅度行進(jìn)間轉(zhuǎn)向修正后，實(shí)現(xiàn)了1.1°的航向校正，車輛恢復(fù)直駛狀態(tài)，穩(wěn)定跟蹤直線。

2擋工況下跟蹤控制第1階段的時(shí)間區(qū)間為0～26 s，右側(cè)操縱桿進(jìn)行了3次間歇性動作，操縱桿位置峰值為42%，使得車輛實(shí)現(xiàn)了0.5°的航向校正；第2階段的時(shí)間區(qū)間為26～49 s，車輛經(jīng)歷了行進(jìn)間轉(zhuǎn)向模式的順次更迭，實(shí)現(xiàn)了車輛的換道動作；第3階段的時(shí)間區(qū)間為49～55 s，車輛經(jīng)過多次大幅度行進(jìn)間轉(zhuǎn)向修正后，實(shí)現(xiàn)了4°的航向校正，車輛恢復(fù)直駛狀態(tài)，穩(wěn)定跟蹤直線。

3.3 軌跡跟蹤控制精度驗(yàn)證

1擋和2擋工況下車輛實(shí)際跟蹤控制效果如圖14所示。兩種工況下車輛的軌跡跟蹤偏差對比如圖15所示。

如圖14所示，相對于僅基于Stanely算法未引入駕駛員模型的原有跟蹤控制邏輯，改進(jìn)后的基于雙層駕駛員模型的跟蹤控制算法，在實(shí)現(xiàn)1擋和2擋換道工況下的跟蹤控制時(shí)，超調(diào)量分別下降14%和11%，換道最終完成縱向偏移量分別減少17%和13%，換道過程開啟縱向偏移量分別縮短9%和15%. 改進(jìn)后的跟蹤控制算法從跟蹤精度以及響應(yīng)延遲兩個(gè)方面實(shí)現(xiàn)了跟蹤效果的提升。

如圖15所示，無論是1擋工況還是2擋工況，在第1階段和第3階段直線跟蹤過程中，改進(jìn)后的控制算法能夠?qū)⒖刂普`差限制在0.3 m以內(nèi)，在第2階段換道過程中將誤差限制在1.0 m以內(nèi)。

4 結(jié)論

1)基于HMM-GMM統(tǒng)計(jì)學(xué)習(xí)方法所建立的轉(zhuǎn)向模式預(yù)測模型，能夠?yàn)榭v向與橫向協(xié)同跟蹤控制系統(tǒng)提供基于駕駛員操控經(jīng)驗(yàn)的預(yù)測轉(zhuǎn)向模式以及行進(jìn)間轉(zhuǎn)向操縱桿位置搜索區(qū)間和搜索初值，縮短了系統(tǒng)的響應(yīng)延遲。

2)基于模糊邏輯表征的駕駛員行進(jìn)間轉(zhuǎn)向縱向與橫向配合規(guī)律，以期望橫擺角速度為目標(biāo)，可以實(shí)現(xiàn)行進(jìn)間轉(zhuǎn)向時(shí)油門與操縱桿的配合調(diào)整。

3)本文所提出的基于雙層駕駛員模型的縱向與橫向協(xié)同跟蹤控制方法具有良好的跟蹤控制效果，能夠在特定場景下實(shí)現(xiàn)跟蹤誤差不大于1.0 m的跟蹤控制。