基于雙參數自適應優化的無人履帶車輛軌跡跟蹤控制

盧佳興, 劉海鷗, 關海杰, 李德潤, 陳慧巖, 劉龍龍

(北京理工大學 機械與車輛學院, 北京 100081)

0 引言

近年來,地面無人平臺在港口運輸、疫情防控、野外搜救以及戰場環境中得到了越來越多的應用,軌跡跟蹤控制作為無人駕駛的關鍵技術,其發展也得到了越來越多學者的關注,其中以模型預測的控制算法應用較多[1-4]。然而,目前大多數都是基于定參數的軌跡跟蹤控制算法,很難保證車輛在復雜多變工況下的跟蹤精度和穩定性。基于此,有學者提出自適應參數調整算法,包括更精確的車輛模型參數在線估計、更合理的控制系統結構設計以及更高效的優化算法架構設計等。Zhao等[5]結合瞬時轉向中心模型提出履帶車輛滑動參數實時估計算法,對越野工況下的履帶車輛軌跡跟蹤進行了探究。但由于越野工況下地面-履帶的相互作用關系難以精確表征,因此基于車輛模型參數在線估計的方法很難為系統提供穩定、可靠的輸入,且在線估計對系統的實時資源配置要求較高。Pereida等[6]提出了將模型預測控制(MPC)與底層控制器相結合的框架。Yuan等[7]基于模糊推理、預瞄模型、預瞄點搜索算法建立了自適應軌跡跟蹤控制系統,提升了不同速度下軌跡跟蹤的工況適應性。李睿等[8]基于自動駕駛履帶車輛的運動學與動力學耦合模型,采用自適應控制方法對模型進行前饋補償。Wang等[9]基于多層感知機(MLP)神經網絡和貝葉斯優化相結合的算法,提升了控制性能。這些研究成果證明了以MPC為基本框架,通過增加前饋、數據學習等方式提升軌跡跟蹤性能的有效性,但是缺乏在實際高速履帶平臺的驗證。

模型預測軌跡跟蹤算法中,影響控制效果的控制參數主要包括代價函數中的各權重系數、控制時域中的相關參數等,其中控制時域長度和控制時間步長影響較大[7,10]。基于學習的控制參數自適應軌跡跟蹤算法的主要建模過程包括最優參數組合評價與提取、在線最優控制參數預測兩部分。在最優參數提取方面,可變慣性權重的改進粒子群優化(PSO)算法已被學者廣泛應用。趙新等[11]基于改進PSO算法建立引信與戰斗部配合威力計算模型,實現了更高精度的最大殺傷面積計算和更快的收斂速度。譚頓等[12]將改進PSO算法用于尋找同步控制系統的最優PID系數,有效減小了系統超調與同步控制誤差,提升了系統響應速度。在線最優參數預測方面,常用的算法有高斯混合回歸、支持向量機[13]、分類與回歸決策樹[14]、神經網絡[15]等。其中,基于神經網絡的算法應用較廣。牛江川等[16]利用盾構施工案例的大量歷史數據作為樣本數據,基于PSO-反向傳播(BP)神經網絡進行模型訓練,實現盾構刀具配置方案的合理推薦,在計算精度和訓練時間上實現了較大的提升。但未見在無人履帶車輛方面的研究成果。

本文基于改進PSO-MLP算法,以控制時域長度tl和控制時間步長ts為優化目標,以離線和在線相結合的方式,設計一種雙參數自適應優化的無人履帶車輛軌跡跟蹤控制算法,總體框架如圖1所示。離線部分,首先對不同控制參數下的履帶車輛軌跡跟蹤響應狀態信息進行提取,得到包含運動基元類型、參考軌跡Sr、軌跡跟蹤響應信息、車輛狀態ξ(k)以及控制參數的車輛歷史數據集合DH;然后,基于改進PSO算法得到不同運動基元下的最優控制參數組合,并構建訓練數據集,利用學習率自適應優化算法完成對MLP神經網絡模型的訓練。在線部分,首先對運動基元類型和車輛狀態進行特征匹配,基于MLP神經網絡對控制時間步長ts和控制時域長度tl進行預測,為軌跡跟蹤控制模塊提供自適應雙參數輸入。然后,結合自適應參數輸入、車輛模型、約束條件C以及代價函數等建立軌跡跟蹤控制器,得到最優控制量u,實現雙側獨立電驅動履帶平臺的自適應軌跡跟蹤控制。

圖1 總體框架Fig.1 Overall framework

1 履帶車輛模型

本文基于履帶車輛運動學模型生成MPC控制器的狀態空間。同時,為彌補車輛運動學模型往往忽略車輛動力學特性的缺陷,引入履帶車輛動力學模型,為運動學模型提供安全邊界。

1.1 履帶車輛運動學模型

本文的研究對象為雙側獨立電驅動履帶車輛,根據履帶車輛的差速轉向特性,為了與規劃模塊生成的參考軌跡的坐標方向保持統一,建立圖2所示的履帶車輛運動學模型。

圖2 履帶車輛運動學模型Fig.2 Kinematics model of a tracked vehicle

圖2中:OXY為固連于地面的慣性坐標系,坐標軸OX、OY所在平面與路面重合;oxy為固連于履帶車輛并隨履帶車輛共同運動的牽連坐標系,o(x0,y0)位于履帶車輛的幾何中心,ox為車輛的橫向方向,oy為車輛的縱向方向,且ox、oy所在平面與路面平行;vx、vy分別為車輛的橫向速度、縱向速度,ωz為車輛的橫擺角速度,定義逆時針為正,φ為車輛的航向角;o′(x′0,y′0)為慣性坐標系中履帶車輛的瞬時轉向中心,oL(x′L,y′L)、oH(x′H,y′H)分別為低速側履帶、高速側履帶接地面的瞬時轉向中心(轉向極),三者中心在一條垂直于車輛縱向對稱面的直線上;vqL、vqH分別為低速側和高速側履帶上PL、PH點的牽連速度;vjL、vjH分別為低速側和高速側履帶的卷繞速度。

履帶車輛通過調整兩側履帶的速差實現轉向操控,在固連于地面的空間笛卡爾坐標系下,車輛的狀態向量可以表示為ξ(x0,y0,φ),由車輛位置坐標和車輛航向角組成,則履帶車輛的運動學模型可以描述為

(1)

基于傳統的運動學模型,當履帶的卷繞速度為0 m/s時,無法通過表達式反求出履帶的卷繞速度,基于瞬時轉向中心的運動學模型可以很好地解決這個問題[17]。根據圖2中建立的履帶車輛運動學模型坐標系體系,結合文獻[17]中的模型建立過程,推導得到如式(2)所示的基于瞬時轉向中心的履帶車輛運動學微分方程:

(2)

1.2 履帶車輛動力學模型

履帶車輛特殊的驅動方式,導致履帶和地面的相互作用關系的復雜性,為簡化建模,進行如下所示的假設[18]:

1)車輛在轉向過程中,不考慮履帶的沉陷作用和推土作用;

2)履帶載荷沿著履帶長度方向均勻分布;

3)在進行阻力分析時,忽略空氣阻力的作用;

4)在轉向過程中的行駛阻力系數與直駛過程相同。

圖3所示為履帶車輛的動力學模型。圖3中:C為車輛的重心;P為牽連坐標系中車體轉向中心;L為履帶接地段長度;B為履帶中心距;xC、yC分別為履帶車輛的重心與幾何中心的橫向、縱向距離;λC為由離心力作用產生的轉向中心縱向偏移量;R′為履帶車輛的穩態轉向半徑;R為慣性坐標系中履帶車輛的瞬時轉向中心與車輛重心的垂直距離;FC為車輛轉向時產生的離心力;FCx、FCy分別為離心力的橫向分量、縱向分量;FrL、FrH分別為低速側、高速側履帶接地段所受的地面變形阻力;FyL、FyH分別為低速側履帶制動力、高速側履帶驅動力;FxL、FxH分別為低速側、高速側履帶所受橫向阻力的合力;MμL、MμH分別為低速側、高速側履帶所受的轉向阻力矩,μ表示轉向阻力系數。

圖3 履帶車輛動力學模型Fig.3 Dynamic model of a tracked vehicle

基于達朗貝爾原理,在oxy坐標系中對車輛幾何中心建立履帶車輛動力學方程組:

(3)

式中:m為履帶車輛的整車質量;δ為履帶車輛旋轉質量換算系數;Mμ為轉向阻力矩,Mμ=MμL+MμH;J為履帶車輛繞z′軸的轉動慣量。

考慮履帶車輛在轉彎時,由于離心力作用導致的車輛轉向中心前移、載荷轉移,根據履帶車輛動力學方程組式(3),將FyL、FyH、λC作為未知量,車輛在進行平地轉向時為勻速轉向,在已知規劃路徑曲率特征、縱向規劃速度以及車輛的結構參數、性能參數等的情況下,對上述的3個未知量進行求解,可得

(4)

2 基于模型預測的軌跡跟蹤控制器

MPC模塊根據規劃模塊提供的參考路徑曲率特征以及參考速度、車輛狀態信息,結合雙參數自適應優化與預測模塊輸出的控制時域長度、時間步長,考慮車輛運動學邊界和電機外特性,進行最優控制序列的迭代求解,并將第1個時域內的控制量下發給執行器,實現最優控制。

2.1 目標函數

在進行軌跡跟蹤控制器的設計時,系統可以表達為

(5)

式中:u=[vjL,vjH]。由式(2)可知,要實現對車輛狀態的準確預測,還需要得到車輛瞬時中心的位置,該參數通過狀態觀測模塊提供給MPC控制器,在此不作贅述。

對式(5)中的連續系統模型進行離散化,可得

(6)

式中:ξ(k)、u(k)分別為k時刻的車輛狀態向量、控制輸入向量,k≥0;yc(k)為k時刻的控制輸出向量;yb(k)為k時刻約束輸出向量。控制量、控制量增量Δu(k)約束以及輸出約束分別為

(7)

式中:umin、umax分別為控制量下限值、上限值;Δumin、Δumax分別為控制量變化率的下限值、上限值;ymin(k+j)、ymax(k+j)分別為k+j時刻約束輸出的下限值、上限值。

在進行代價函數的構建時,需要綜合軌跡的跟蹤精度、穩定性。在k時刻,基于離散非線性模型的MPC優化問題可以描述為

(8)

(9)

式中:J(ξ(k),Uk)為目標函數

(10)

le、he、v分別為軌跡跟蹤的橫向偏差、航向偏差、實際車速,ldesired、hdesired、vdesired分別為期望橫向偏差、期望航向偏差以及期望車速,Q、R、P、M、N分別為對應項的權重系數矩陣;Np、Nc分別為預測時域、控制時域的個數,且Nc≤Np。

2.2 約束設置

為保證履帶車輛不發生側翻,車速需要滿足:

(11)

式中:kr為側翻閾值系數;β為車輛的側傾角度;hg為車輛重心高度。

為了保證車輛在轉彎時不發生失控的情況,車輛的轉向中心縱向偏移量需要滿足:

(12)

整理得:

(13)

式中:ks為側滑閾值系數;fφ為路面附著系數;α車輛俯仰角度。

在規劃軌跡已知的條件下,在入彎之前對車輛轉向的轉矩需求進行預測:

(14)

(15)

T(vy,ρ)=max [TL,TH]

(16)

式中:TL、TH分別為低速側、高速側履帶轉矩需求;η為機械傳動效率;i為綜合傳動比;r為驅動輪半徑;T為基于規劃軌跡的電機最大轉矩需求;ρ為路徑的曲率。

電機外特性是影響電機性能的邊界條件,圖4為本文履帶平臺的電機外特性。在基于轉速控制的軌跡跟蹤研究中,如果在當前電機轉速下,實際車輛行駛的轉矩需求達到或接近電機外特性曲線上的轉矩極限值,車輛的轉矩調節范圍就會受到約束,在當前轉速下的轉向能力就會得到制約,難以滿足車輛軌跡跟蹤控制的性能要求,嚴重時甚至會導致軌跡不可控。

圖4 電機外特性Fig.4 External characteristics of the motor

將電機的轉矩和功率作為約束輸出,并設置轉矩保護系數kT和后備功率保護系數kP,可得

T(vy,ρ)kT≤Tmax(vy)

(17)

P(vy,T)kP≤Pmax(vy)

(18)

式中:Tmax(vy)、Pmax(vy)分別為當前電機轉速下電機可以提供的最大轉矩和最大功率;P(vy,T)為當前工況下電機功率的需求。

3 雙參數自適應優化與預測模塊

雙參數自適應優化和預測模塊基于運動基元類型進行車輛歷史數據的提取,歷史數據集基于實驗室現有的雙側獨立電驅動無人履帶平臺,以機器人操作系統(ROS)作為通信系統完成對越野工況下車輛狀態信息的記錄和儲存[4]。通過離線優化和在線預測,為MPC控制器提供自適應最優控制參數輸入。

3.1 算法總體介紹

模塊以履帶車輛歷史數據集合為輸入,主要基于改進PSO-MLP算法,完成最優參數組合的離線提取和最優控制參數的在線預測。基于上述算法得到了運動基元類型與軌跡跟蹤最優控制參數(控制時域長度、控制時間步長)之間的映射關系,MPC控制器通過軌跡點匹配,實現實時工況下控制參數的自適應調整,以提高履帶車輛在復雜多變工況下的軌跡跟蹤精度和穩定性。具體的算法原理,如圖5所示。

圖5 改進PSO-MLP算法偽代碼

3.2 運動基元提取

基于真實駕駛數據對運動基元進行提取和再生成,以表征不同工況下車輛-道路之間復雜的耦合關系,是提升無人駕駛車輛系統適用性的重要途徑之一[19-20]。

本文在文獻[19]中的運動基元提取思想基礎上,在離線狀態下基于最優控制理論,根據大量無人履帶車輛在實際越野場景下的歷史數據,進行運動基元庫的生成。在線狀態下,根據感知模塊提供的柵格地圖、全局規劃的參考路徑以及運動基元庫,針對典型工況,軌跡模塊基于A*搜索算法進行運動基元的實時拼接和平滑過渡,以得到具備車輛位姿信息以及運動語義信息的參考軌跡:

Sr=[sx,sy,sz,syaw,spitch,sroll,sv,sρ,stype]

(19)

式中:sx、sy、sz為參考軌跡點在車體坐標下的坐標;syaw、spitch、sroll為參考軌跡的車輛三維姿態信息;sρ為參考軌跡的曲率;sv為車輛的參考速度;stype為履帶車輛運動基元的類型。

針對環境中具備的幾種典型越野工況,規劃模塊生成的參考軌跡中主要由以下4種運動基元組成:類直線運動基元PS、類小曲率轉向運動基元PG、類單移線運動基元PC、類U形彎運動基元PU。圖6所示為本文進行軌跡跟蹤控制的參考軌跡。

圖6 參考路徑圖Fig.6 Diagram of the reference path

3.3 基于改進PSO算法的最優參數組合提取

對于已經提取出來的包含運動語義信息、車輛狀態信息、以及軌跡跟蹤響應信息的歷史數據集DH,采用改進PSO算法,進行不同運動基元類型下的最優控制參數組合提取,為MLP神經網絡模型的訓練提供數據基礎。將輸入到MPC模型的控制時域長度和時間步長大小作為粒子,則粒子的位置可以表達為

(20)

對粒子的位置和速度進行初始化:

(21)

越野工況下,惡劣的環境條件使得硬件處于不良的工作狀態,導致MPC的控制量求解時間極不穩定。如果在一個控制周期內,MPC求解器無法求解出最新控制量,會嚴重威脅行車安全。因此,在對每種運動基元內的最優控制參數進行提取時,PSO算法中適應度函數的設置需要綜合考慮車輛的軌跡跟蹤精度、穩定性以及計算時間成本。跟蹤精度代價項通過橫向偏差和航向偏差體現,跟蹤穩定性代價項由控制量變化率體現,而計算時間成本代價項則由MPC求解器的迭代求解時間確定,一般來說,迭代求解時間隨著控制參數的變化而變化[10]。根據粒子的位置信息,結合履帶車輛歷史數據集合DH,可以匹配到相應的車輛軌跡跟蹤響應狀態信息,為適應度函數的評價提供依據。適應度函數的表達形式為

(22)

(23)

式中:t、t+1為迭代次數;vp(t)、vp(t+1)分別為第p個粒子在第t、t+1輪迭代的速度;w(t)為第t輪迭代的慣性因子,w(t)>0;c1、c2為學習因子,分別代表認知系數和社會系數,通常c1=c2∈(0,4);r1、r2為(0,1)之間的隨機數;Pp(t)為第p個粒子累計到第t輪迭代的局部最優值;xp(t)、xp(t+1)分別為第p個粒子在第t、t+1輪迭代的位置;G(t)為粒子群內所有粒子累計到第t輪迭代的全局最優值。為解決算法早熟以及迭代后期易在最優解附近振蕩的情況,這里采用線性可變權重系數的方法進行權重系數的設置:

(24)

式中:wmax、wmin分別為權重系數的最大值和最小值;tmax為最大迭代次數。

在進行狀態更新時,需要對位置和速度進行越界修正:

(25)

(26)

式中:vmax為粒子的最大速度;xmin、xmax分別為粒子位置的最小值和最大值。

3.4 基于MLP的最優控制參數預測

圖7 MLP算法原理圖Fig.7 Schematic diagram of the MLP algorithm

本文采用學習率自適應優化算法進行MLP神經網絡的訓練,具體流程如圖8所示。

MLP神經網絡具體算法流程如下:

步驟1初始化網絡深度l、模型權重系數矩陣W(q)、模型的偏置參數b(q)等參數,q∈{1,…,l}。模型的激活函數選擇tanh 函數:

(27)

步驟2基于模型輸入、模型參數進行模型輸出的求解,并得到損失函數L(,y),y為輸入x時的神經網絡輸出真值,為輸入x時的神經網絡輸出預測值。

圖8 學習率自適應優化算法流程偽代碼表

h(0)=xh(0)=x
h(k)=f(b(k)+W(k)h(k-1)),k∈{1,…,l}
=h

(28)

式中:h(k)為第k層隱藏單元的向量。

步驟3反向傳播更新。

未達到迭代終止條件時,需要根據誤差的梯度方向對不同網絡深度的權重系數矩陣和偏置參數進行更新,然后再重復步驟2,直至達到迭代收斂條件。

將改進PSO算法優化得到的數據集劃分為訓練集和測試集。訓練集的數據用于基于學習率自適應優化算法的MLP神經網絡的模型訓練,測試集數據用于對模型的精度進行評價,得到如圖9所示的預測結果。圖9中tstep為時間步長標簽值,step為時間步長預測值,tlr為時域長度標簽值,lr為時域長度預測值,step、lr可以對tstep、tlr進行良好預測。基于改進PSO-MLP算法,以當前車輛狀態、運動基元類型等作為輸入,可以對最優控制參數組合進行較好預測。

圖9 MLP神經網絡預測效果Fig.9 Prediction of the MLP neural network results

4 試驗驗證

為了驗證本文提出的算法框架的有效性,分別進行仿真實驗和實車試驗。仿真實驗由ROS-VREP聯合仿真環境測試完成,而實車試驗則通過一輛雙側獨立電驅動中型無人履帶平臺完成。試驗過程中的平臺結構、性能參數如表1所示。

表1 各平臺試驗參數表

4.1 ROS-VREP聯合仿真

仿真實驗是對算法初步的功能性驗證,用于保證實車測試的安全性,場景如圖10所示,平臺的具體參數如表1所示,仿真所需的參考軌跡為圖6所示的軌跡,仿真過程中的最高車速為20 km/h。為了體現數據真實規律,每組實驗重復3次,并選擇其中特征最明顯的一組作為對照組。

圖10 ROS-VREP聯合仿真環境Fig.10 ROS-VREP co-simulation environment

基于算法的收斂效果以及軌跡跟蹤響應狀態,設置對照組如下所示:對照組1設置為tl=2.7 s、ts=0.15 s;對照組2設置為tl=1.2 s、ts=0.10 s;對照組3設置為tl=0.54 s、ts=0.06 s。其中對照組1的時域長度和時間步長最大,計算成本也最大;對照組2與基于雙參數自適應優化算法的時間成本相同;對照組3的時域長度和時間步長最小,時間成本最小。

圖11 仿真實驗軌跡跟蹤效果Fig.11 Simulated trajectory tracking performance

表2 仿真實驗軌跡跟蹤效果統計表

結合圖11和表2對軌跡跟蹤效果的統計可以看出,對照組1雖然轉角狀態量變化最為穩定,但軌跡跟蹤的精度差,計算載荷大,本文提出的自適應參數優化法相比于對照組1分別降低橫向偏差48.9%,降低航向偏差31.4%,節省計算成本23.4%。對照組2的計算時間成本和轉角變化率均值幾乎一致,但本文算法的軌跡跟蹤精度較高,橫向偏差和航向偏差僅為對照組2的66.9%、81.7%。對照組3的計算成本最低,跟蹤精度也最高,但是車輛的轉角平均變化率為本文算法的2.3倍,尤其在PU、PS的運動基元路段,車輛的轉角出現了較大的波動,軌跡跟蹤處于失穩的狀態。本文算法在軌跡跟蹤的精度、行駛穩定性、時間成本上做到了較好的均衡。基于ROS-VREP的聯合仿真對算法進行了初步的驗證,為實車試驗的設計提供了重要的參考。

4.2 實車驗證

實車試驗基于如圖12所示的雙側獨立電驅動中型無人履帶平臺開展,平臺的具體結構、性能參數如表3所示。平臺裝配的設備包括:光雷達、GNSS接收機、慣性導航系統、感知工控機、規控工控機以及驅動電機等。搭載的規控工控機的處理器型號為八核八線程的Intel Core i7-9700,GPU型號為Nvidia RTX 2080Ti,內存32 GB,操作系統為Ubuntu 16.04。

圖12 無人駕駛履帶車輛試驗平臺Fig.12 Experimental platform of the unmanned tracked vehicle

同樣地,對軌跡跟蹤控制過程中的不同運動基元的橫向偏差均值、航向偏差均值、轉角狀態量變化率均值以及迭代求解時間進行提取,得到如圖13和表3所示的軌跡跟蹤效果及數據統計。

圖13 履帶車輛試驗軌跡跟蹤效果Fig.13 Track performance for the unmanned tracked vehicle

表3 實車試驗軌跡跟蹤效果統計表

通過對不同算法的軌跡跟蹤效果的統計分析可以看到,對照組1在全程的轉角狀態量變化率的均值最小,反映出該組實驗的軌跡跟蹤穩定性最好,但本文算法在橫向跟蹤誤差、航向跟蹤誤差以及時間成本上相對于對照組1分別降低34.6%、15.3%、2.7%。

對照組2的時間成本與本文算法相近,但軌跡跟蹤的精度和穩定性均較差。本文算法相對于對照組2,橫向偏差均值、航向偏差均值以及轉角變化率均值可以分別降低30.5%、17.2%、7.8%。對照組3的時間成本最低,軌跡跟蹤的精度也最高,但是軌跡跟蹤的穩定性是最差的,尤其在PU路段,車輛為了達到較高的跟蹤精度,出現了多次擺尾的情況,這無疑將嚴重影響履帶車輛的行車安全。綜上,本文提出的算法,在保證車輛具有較高的行駛穩定性、較為經濟的計算成本前提下,實現了對參考軌跡的精準跟蹤。

5 結論

本文針對模型預測軌跡跟蹤控制算法定參數輸入下軌跡跟蹤效果不穩定的情況,建立了雙參數自適應優化與預測模塊,為軌跡跟蹤控制模塊提供自適應參數輸入,并進行了ROS-VREP仿真實驗、實車試驗的算法驗證。得出主要結論如下:

1)通過對運動基元類型、軌跡跟蹤響應狀態,車輛狀態以及控制參數進行數據提取,基于改進PSO優化算法,實現了最優參數組合數據集的構建。

2)基于學習率自適應優化算法,結合最優參數組合數據集進行MLP神經網絡的模型訓練,根據規劃模塊實時信息和車輛狀態實時反饋,對最優控制參數組合進行了良好的預測,實現了為軌跡跟蹤控制模塊提供時變自適應的控制時域長度、控制時間步長的雙參數輸入。

3)通過ROS-VREP、實車試驗的結果分析,本文提出的算法在保證車輛具有較好軌跡跟蹤穩定性、計算成本經濟性的前提下,實現了較高的軌跡跟蹤精度。實車試驗中,與相同計算成本下的定參數軌跡跟蹤控制算法相比,橫向偏差均值、航向偏差均值以及轉角變化率均值可以分別降低30.5%、17.2%、7.8%,驗證了本文提出算法框架的有效性和可行性。