基于線性誤差模型研究鏟運(yùn)機(jī)的路徑跟蹤與控制

沈琦

（201620 上海市 上海工程技術(shù)大學(xué) 機(jī)械與汽車工程學(xué)院）

0 引言

隨著采礦深度的增加，采礦條件也愈加惡劣,對人員安全的威脅程度也逐漸提高，遙控采礦、自動化礦山開采技術(shù)應(yīng)運(yùn)而生[1-2]。地下鏟運(yùn)機(jī)是地下自動采礦的關(guān)鍵設(shè)備，路徑跟蹤是地下鏟運(yùn)機(jī)實現(xiàn)自動化作業(yè)的基本任務(wù)之一，路徑跟蹤與控制的目的是使車輛以盡可能小的誤差跟蹤期望路徑，而地下空間多為狹窄受限的巷道空間，因此鏟運(yùn)機(jī)的路徑跟蹤與軌跡特性相較于普通車輛難度更大。

本文運(yùn)用 MATLAB 編寫工作裝置運(yùn)動仿真程序，通過程序仿真分析，得到工作裝置的軌跡、速度運(yùn)動學(xué)特性曲線，為提高工作裝置設(shè)計精度和設(shè)計效率提供了重要的分析數(shù)據(jù)，也為工作裝置的優(yōu)化設(shè)計奠定了基礎(chǔ)。

1 鏟運(yùn)機(jī)運(yùn)動學(xué)分析

1.1 轉(zhuǎn)向特點(diǎn)

鏟運(yùn)機(jī)是一種采用鉸接連接的工程車輛，采用“折腰式”設(shè)計[3]，前后車體作為相對獨(dú)立的部分存在，通過銷軸連接，這種連接方式可保證前后車體在同一水平面內(nèi)轉(zhuǎn)動，從而完成鏟運(yùn)機(jī)的轉(zhuǎn)向，同時車體間液壓缸的伸縮亦可達(dá)到轉(zhuǎn)向的目的。

1.2 轉(zhuǎn)向半徑分析

鏟運(yùn)機(jī)屬于鉸接式轉(zhuǎn)向車輛，通過轉(zhuǎn)向油缸的伸縮來改變鉸接角，進(jìn)而使鏟運(yùn)機(jī)轉(zhuǎn)向。其轉(zhuǎn)向角度取決于液壓缸伸縮長度，當(dāng)兩油缸伸縮量相等時，鏟運(yùn)機(jī)沿直線行駛，轉(zhuǎn)向半徑是鏟運(yùn)機(jī)行駛的重要參數(shù)。如圖1 所示，前后車橋兩軸線相交于C點(diǎn)，該點(diǎn)即為鏟運(yùn)機(jī)轉(zhuǎn)向速度瞬心，鏟運(yùn)機(jī)圍繞該點(diǎn)做無側(cè)滑的轉(zhuǎn)向運(yùn)動。

圖1 中，O 點(diǎn)——鏟運(yùn)機(jī)的鉸接點(diǎn)，即連接前后車體的銷軸所在點(diǎn)；A 點(diǎn)，B點(diǎn)——前后車橋中點(diǎn)；α——轉(zhuǎn)向角，與∠C 大小相同，鏟運(yùn)機(jī)為直線行駛時α=0°；lf，lr——前后車橋中心到鉸接點(diǎn)的距離；Rf，Rr——轉(zhuǎn)向半徑，計算公式如式（1）、式（2）。

同理可得Rr：

2 鏟運(yùn)機(jī)模型建立

2.1 模型推導(dǎo)

以前車體作為分析中心，該點(diǎn)速度與鏟運(yùn)機(jī)前進(jìn)方向一致，有利于后續(xù)分析與計算。參考Corke等人提出的經(jīng)典鏟運(yùn)機(jī)的運(yùn)動學(xué)模型[4-5]。由于鏟運(yùn)機(jī)通常在低速情況下行駛，所以此處可以忽略加速和制動及其它偏離確切運(yùn)動學(xué)模型的影響，將前車體行駛速度和鉸接角轉(zhuǎn)向速度為輸入量的情況下，計算前車體航向角的角速度表達(dá)式。

由剛體的平面運(yùn)動學(xué)[6]可知，剛體中的任意一點(diǎn)速度可通過基點(diǎn)分解成平動和轉(zhuǎn)動，其中平動速度與所選基點(diǎn)（牽連點(diǎn)）速度不同，而平面繞基點(diǎn)轉(zhuǎn)動的角速度、角加速度與基點(diǎn)的選取無關(guān)。由此可知，與前車體角速度一致，與后車體角速度一致。由于在O 點(diǎn)處兩剛體具有相同速度，分別選取基點(diǎn)A 與基點(diǎn)B 表達(dá)O 點(diǎn)的速度，根據(jù)剛體平面運(yùn)動學(xué)中基點(diǎn)法可得：

如圖2 所示，θf為前車體航向角，鏟運(yùn)機(jī)逆時針轉(zhuǎn)向時，航向角角速度為正，鉸接角α亦為正。圖中所處航向角與鉸接角α均為正。將4 個速度向量分別沿平行于前車體車橋方向與垂直于前車體車橋方向進(jìn)行分解，可以建立等式如式（4）。

其中，與順時針轉(zhuǎn)動時，航向角為負(fù)且不斷減小，鉸接角變大的同時，前車體航向角的變化率更大，同時求解

此處，將vA和vB改寫成vf和vr。假設(shè)鏟運(yùn)機(jī)處于無側(cè)滑情況下，故有

2.2 模型驗證

借鑒Altafini 和Corke 的文獻(xiàn)[7-8]驗證運(yùn)動學(xué)方程的準(zhǔn)確性。由于處于無側(cè)滑情況下，可列方程：

得到關(guān)于的表達(dá)式為

由此通過2 種方法證明所得運(yùn)動學(xué)模型方程是正確的。同時還可確定鏟運(yùn)機(jī)運(yùn)行時的速度瞬心：

當(dāng)鉸接角轉(zhuǎn)向速度=0 時，鏟運(yùn)機(jī)處于穩(wěn)態(tài)轉(zhuǎn)向狀態(tài)時，鏟運(yùn)機(jī)圍繞前后車橋的交叉點(diǎn)旋轉(zhuǎn)。速度瞬心表達(dá)式為：

通過比較可發(fā)現(xiàn)前后車橋交叉點(diǎn)與速度瞬心重合，即當(dāng)=0 時，鏟運(yùn)機(jī)的行駛曲率圓心位于前后車橋的交叉點(diǎn)處，再一次驗證了運(yùn)動學(xué)公式的準(zhǔn)確性。

鏟運(yùn)機(jī)運(yùn)動學(xué)模型狀態(tài)空間表達(dá)式為

3 LEMPC 路徑跟蹤控制

基于鏟運(yùn)機(jī)的運(yùn)動學(xué)方程設(shè)計了LEMPC 控制預(yù)測模型，MPC 控制具備以下特點(diǎn)：（1）預(yù)瞄功能，根據(jù)模型預(yù)測未來的狀態(tài)及參考狀態(tài)的偏差；（2）可處理MIMO 狀況；（3）約束功能，對控制量、輸出量、控制增量等進(jìn)行約束，提高系統(tǒng)穩(wěn)定性；（4）實時更新功能，每個時刻得到最優(yōu)輸入。

MPC 控制已廣泛應(yīng)用于移動機(jī)器人及自動駕駛中，而鏟運(yùn)機(jī)由于結(jié)構(gòu)特殊，關(guān)于路徑跟蹤的研究還較少。

圖3 為模型預(yù)測控制的計算流程。

對參考軌跡的當(dāng)前參考點(diǎn)進(jìn)行泰勒展開及雅克比線性化（Jacobian Linearization）：

式（15）、式（17）中下角標(biāo)ref 代表參考點(diǎn)（ωref即鉸接角的角速度）

處理得新的狀態(tài)空間表達(dá)式為

輸出量即為狀態(tài)變量，輸出方程為

列出預(yù)測方程，定義預(yù)測時域為Np，控制時域為Nc，且Nc≤Np。做如下假設(shè)：在控制域之外，輸入為0，即u（k+i）=0，i=Nc，Nc+1，Nc+2，…，Np-1，預(yù)測矩陣表達(dá)為

為使鏟運(yùn)機(jī)快速平穩(wěn)地行駛在預(yù)定軌跡上，將二次型的標(biāo)準(zhǔn)形式作為目標(biāo)函數(shù)來求解輸入，取[ΔUTε]T作為二次型的求解對象，目標(biāo)函數(shù)矩陣形式表示為

式中：Q，R——輸出量的調(diào)整矩陣；ε——松弛因子。Ψξ（k）對應(yīng)當(dāng)前時刻k 為已知值，將其定義為e，eT，QQ，e——常數(shù)。

由于鏟運(yùn)機(jī)的物理條件限制，還需要對輸入速度、鉸接角速度及鉸接角大小進(jìn)行約束：

本文使用MATLAB 嵌入的quadprog 函數(shù)對目標(biāo)函數(shù)求解，目標(biāo)函數(shù)輸入量為[ΔUTε]T?？刂圃隽喀=ΔU+Δuref。

則約束表達(dá)為

只存在鉸接角一個輸出量的表達(dá)式為

鉸接角約束為

公式中二次型目標(biāo)函數(shù)的約束Ax≤b。

基于鏟運(yùn)機(jī)誤差模型的預(yù)測控制已搭建完成，根據(jù)目標(biāo)函數(shù)求解得到每個周期的[ΔUTε]T，取其中的第一項，則需要的輸入為

4 仿真驗證

運(yùn)用MATLAB/Simulink 2021a 軟件設(shè)計仿真環(huán)境[9]。仿真系統(tǒng)中設(shè)計了鏟運(yùn)機(jī)的運(yùn)動學(xué)模型及MPC 控制器，如圖4 所示。在實際控制中，車速和鉸接角角速度的變化有時滯存在，因此在MPC控制器輸出端添置單位延遲環(huán)節(jié)，采用積極集法求解函數(shù)quadprog，期望路徑為一條直線。

控制器中各參數(shù)設(shè)定見表1。

表1 控制器參數(shù)Tab.1 Controller parameters

控制器權(quán)重參數(shù)為

圖5 為仿真結(jié)果。

可以看出，鏟運(yùn)機(jī)鏟裝路線與參考路徑基本吻合，殘差很小，對于地下鏟運(yùn)機(jī)的控制而言，誤差可以接受。

5 結(jié)語

本文通過參考Corke 的經(jīng)典運(yùn)動學(xué)模型,推導(dǎo)出鏟運(yùn)機(jī)運(yùn)動學(xué)模型表達(dá)式，運(yùn)用 MATLAB 進(jìn)行運(yùn)動仿真分析，為提高工作裝置設(shè)計精度和設(shè)計效率提供了重要的分析數(shù)據(jù)。以地下鏟運(yùn)機(jī)為例建立的跟蹤軌跡推算數(shù)學(xué)模型也同樣適用于其他鉸接式車輛。該跟蹤軌跡推算模型為地下鏟運(yùn)機(jī)導(dǎo)航控制器的設(shè)計提供了參考。