基于線性誤差模型研究鏟運機的路徑跟蹤與控制

沈琦

（201620 上海市 上海工程技術大學 機械與汽車工程學院）

0 引言

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

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

1 鏟運機運動學分析

1.1 轉向特點

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

1.2 轉向半徑分析

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

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

同理可得Rr：

2 鏟運機模型建立

2.1 模型推導

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

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

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

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

此處，將vA和vB改寫成vf和vr。假設鏟運機處于無側滑情況下，故有

2.2 模型驗證

借鑒Altafini 和Corke 的文獻[7-8]驗證運動學方程的準確性。由于處于無側滑情況下，可列方程：

得到關于的表達式為

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

當鉸接角轉向速度=0 時，鏟運機處于穩態轉向狀態時，鏟運機圍繞前后車橋的交叉點旋轉。速度瞬心表達式為：

通過比較可發現前后車橋交叉點與速度瞬心重合，即當=0 時，鏟運機的行駛曲率圓心位于前后車橋的交叉點處，再一次驗證了運動學公式的準確性。

鏟運機運動學模型狀態空間表達式為

3 LEMPC 路徑跟蹤控制

基于鏟運機的運動學方程設計了LEMPC 控制預測模型，MPC 控制具備以下特點：（1）預瞄功能，根據模型預測未來的狀態及參考狀態的偏差；（2）可處理MIMO 狀況；（3）約束功能，對控制量、輸出量、控制增量等進行約束，提高系統穩定性；（4）實時更新功能，每個時刻得到最優輸入。

MPC 控制已廣泛應用于移動機器人及自動駕駛中，而鏟運機由于結構特殊，關于路徑跟蹤的研究還較少。

圖3 為模型預測控制的計算流程。

對參考軌跡的當前參考點進行泰勒展開及雅克比線性化（Jacobian Linearization）：

式（15）、式（17）中下角標ref 代表參考點（ωref即鉸接角的角速度）

處理得新的狀態空間表達式為

輸出量即為狀態變量，輸出方程為

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

為使鏟運機快速平穩地行駛在預定軌跡上，將二次型的標準形式作為目標函數來求解輸入，取[ΔUTε]T作為二次型的求解對象，目標函數矩陣形式表示為

式中：Q，R——輸出量的調整矩陣；ε——松弛因子。Ψξ（k）對應當前時刻k 為已知值，將其定義為e，eT，QQ，e——常數。

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

本文使用MATLAB 嵌入的quadprog 函數對目標函數求解，目標函數輸入量為[ΔUTε]T。控制增量Δu=ΔU+Δuref。

則約束表達為

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

鉸接角約束為

公式中二次型目標函數的約束Ax≤b。

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

4 仿真驗證

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

控制器中各參數設定見表1。

表1 控制器參數Tab.1 Controller parameters

控制器權重參數為

圖5 為仿真結果。

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

5 結語

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