自主行駛鏟運機路徑跟蹤控制

朱 銘，呂 瀟，張元生，李佳夢，劉 鵬，孫 昊

(1.礦冶科技集團有限公司，北京 100160； 2.北京北礦智能科技有限公司，北京 102628； 3.金屬礦山智能開采技術北京市重點實驗室，北京 102628； 4.礦冶過程自動控制技術國家重點實驗室，北京 102628)

隨著我國對礦產資源的逐步勘探與開采，淺部礦產資源已開發殆盡。我國作為礦藏資源豐富的國家，更多的礦產資源深埋在千米之下。當前我國近三分之一的礦山開采已達到1 000 m深度。深部采礦面臨著許多挑戰，如礦山建設難度大、開采作業困難、環境惡劣、安全風險較高等；井下青年從業人員越來越少，采礦領域人口老齡化現象嚴重[1]；疫情對密集型勞動產業影響較大，嚴重影響了礦山的生產效率。發展智能化、自動化的采礦裝備，革新礦山開采模式，減少井下作業人員，提高生產流程的自動化與智能化，精確化管控生產計劃，是我國資源開發尤其是深部資源開采的必然選擇和必經之路[2]。

本文以Atlas Copco ST3.5鏟運機為研究對象，展開了對鉸接式車輛的運動學建模與自動駕駛控制策略的研究。推導適用于鉸接車控制的基于偏差的線性模型預測控制器，并進行了仿真對比分析與實車驗證。

1 模型預測控制

1.1 鏟運機預測模型

當前針對鉸接式車輛存在兩種較為主流的建模方式，即基于運動學建模和依據動力學建模。在算法中使用車輛模型，計算的實時性是一個非常重要的算法評價指標，越復雜的車輛模型意味著要占用更多的計算資源和更長的求解時間，對于一個實時性要求較高的算法，過于復雜的模型研究并非重點，一定的簡化和近似必不可少[3]。由于運動學模型具有簡單的結構和尚可的模型精確度，故本文控制部分采用鉸接車的運動學模型[4]。

忽略鉸接式車輛在運動過程中的輪胎力學特性，可搭建如圖1所示的運動學特征圖[5]。

圖1 鉸接車運動學分析Fig.1 Kinematic analysis of articulated vehicle

當鉸接式車輛行駛時，分析前車軸中心處的速度與前車身航向角關系。位姿狀態Pf=[xf，yf，θf，γ]與前車身速度、鉸接角速度寫為如下狀態空間方程[6]：

(1)

上述狀態空間為非線性，采用近似線性化方法泰勒展開進行處理，得到：

(2)

式中，Jf(ξ)為f相對于ξ的雅克比矩陣，Jf(u)為f相對于u的雅克比矩陣。處理后可以得到：

(3)

將得到的新狀態方程采用前向歐拉法離散化處理后整理得到一個新的狀態空間表達式：

(4)

(5)

設定控制系統的預測時域為NP，控制時域為NC，則預測時域內的狀態量偏差和系統控制增量可以表示為：

(6)

(7)

將上式計算系統的預測輸出簡化為矩陣形式如下：

Y(t)=ψtξ(t|t)+ΘtΔU(t)

(8)

每一項矩陣如下：

通過上式可以求出控制器未來NP次循環的x向、y向、航向角、鉸接角角度與預測的偏差和參考控制量的偏差。

1.2 優化求解

優化求解是模型預測控制中求解系統的控制增量部分，并將控制增量用于實際輸出和預測模型。將控制增量作為目標函數的狀態量，得到目標函數：

(9)

優化后的目標函數中第一項求和累加了預測時域內狀態量與參考狀態間的偏差，考慮到了未來的跟蹤效果。第二項起到了調節控制量平穩性的作用。

控制量約束：

umin(t+k)≤u(t+k)≤umax(t+k)，k=0，1，…，NC-1

控制增量約束：

Δumin(t+k)≤Δu(t+k)≤Δumax(t+k)，k=0，1，…，NC-1

(10)

車輛狀態約束為：

ymin(t+k)≤y(t+k)≤ymax(t+k)，k=0，1，…，NC-1

(11)

引入松弛因子，如下所示：

(12)

式中：ρ為權重系數；ε為松弛因子。

通過求解以上帶約束的目標函數，可以得到一個控制序列，控制量個數為NC。

1.3 反饋控制

模型預測控制在每個控制周期根據環境反饋的車輛狀態，更新狀態空間方程內的矩陣參數，進行一輪預測控制求解可以得到LMPC對未來目標的控制序列，將控制序列的第一個控制量輸出至系統。如式(13)所示：

u(t)=u(t-1)+Δu×t

(13)

系統接收到此控制指令后作出相應的動作，得到下一時刻環境反饋的狀態量，輸入至控制器，控制器重復之前動作，循環往復完成控制過程。此過程可以一定程度上避免由于系統干擾或是模型不精確導致的控制失敗問題。

2 仿真試驗

采用MapleSim搭建鏟運機保真模型，如圖2所示。根據鏟運機外形尺寸，搭建車輛的左右轉向液壓缸及限位約束、前后車架、添加等效質心等。輪胎使用了“Tire Force and Moments”元件庫中的“Fiala Tire SubSys”動力學模型。

圖2 鏟運機車輛模型Fig.2 LHD vehicle model

仿真平臺選擇MATLAB R2018a版本，將MapleSim車輛模型和模型預測控制器導入Simulink中，根據實車情況設置約束條件，搭建整車控制框架進行仿真。仿真框架如圖3所示。

圖3 仿真框架Fig.3 Simulation framework

跟蹤效果與控制偏差如圖4～7所示。

圖4 跟蹤控制效果Fig.4 Path following control performance

圖6 航向角控制偏差Fig.6 Heading angle deviation

圖7 鉸接角控制偏差Fig.7 Articulation angle deviation

考慮到實際工業應用時采集數據與控制指令下發均為離散型數據，因此在仿真中模擬這一特點使用離散的參考路徑點。本文采用兼顧左右轉向、變曲率的全局路徑，將其進行離散插值得到一系列離散點，之后以車輛根據當前位置尋找最近參考路徑點進行迭代運算。

從軌跡上觀察四狀態偏差的LMPC控制策略可以較準確地跟蹤參考路徑，車輛行駛的路徑未出現明顯的偏差。根據當前車輛所處位置自動尋找該全局路徑上最近的參考點進行跟蹤，這樣得到的參考點能較好地匹配當前車輛的位置信息，并且避免了由于模型誤差推算導致的誤差累計問題。全程中本文提出的四狀態偏差LMPC控制策略在y向的最大偏差約為0.06 m，行駛中航向角偏差不超過0.3°，跟蹤全過程中最大鉸接角控制偏差小于0.8°。在變曲率的路徑下依然具有不錯的控制精度。

3 實車試驗

于三山島金礦-645中段開展井下自動駕駛試驗，選用車型為Atlas Copco ST3.5鏟運機(見圖8)。

圖8 ST3.5鏟運機在-645中段Fig.8 ST3.5 scraper at -645 middle section

-645中段為礦上生產中段，礦車需從主斜坡道下至該中段，將礦車運載的礦石物料運至溜井口破碎(見圖9)。該中段巷道俯視圖如圖10所示。圖中起點處即為該中段入口，終點為溜井口位置。

圖9 鏟運機在巷道中自動行駛Fig.9 Scraper auto driving in underground roadway

圖10 -645中段作業路線Fig.10 -645 middle section operation route

采用多線激光雷達SLAM定位得到車輛在全局地圖的坐標，預設不同路徑得到自動駕駛跟蹤效果如圖11所示。

圖11 ST3.5鏟運機路徑跟蹤效果Fig.11 Path following control performance of ST 3.5 scraper

圖11中藍色ref代表預設路徑，紅色real為鏟運機實際跟蹤軌跡，自動駕駛鏟運機可較好地跟隨全局預設路徑，完成運輸任務。各狀態偏差控制效果如圖12～17所示。

圖12 ST3.5鏟運機鉸接角控制偏差Fig.12 Articulation angle deviation of ST 3.5 scraper

圖13 ST3.5鏟運機y向控制偏差Fig.13 Y-direction deviation of ST 3.5 scraper

圖14 ST3.5鏟運機x向控制偏差Fig.14 X-direction deviation of ST 3.5 scraper

圖15 ST3.5鏟運機航向角控制偏差Fig.15 Heading angle deviation of ST 3.5 scraper

圖16 ST3.5鏟運機航向角控制效果Fig.16 Heading angle control performance of ST 3.5 scraper

圖17 ST3.5鏟運機鉸接角控制效果Fig.17 Articulation angle control performance of ST 3.5 scraper

除去起步時初始偏差外，行駛途中y向偏差正向保持在0.2 m以內，負向偏差小于0.15 m，x向偏差絕對值小于0.3 m，整體較好地跟蹤了預設路徑內最近點的坐標位置。同時航向角控制偏差與鉸接角控制偏差也分別保持在0.042 rad(約2.4°)與0.075 rad(約4.3°)以內，控制誤差均保持在合理的范圍內。

4 結論

本文提出的四狀態偏差MPC控制算法，經仿真驗證y向最大偏差約為0.06 m，行駛中航向角偏差不超過0.3°，跟蹤全過程中最大鉸接角控制偏差小于0.8°。實車試驗中y向最大偏差約為0.2 m，x向最大偏差約為0.3 m，行駛中航向角偏差不超過0.042 rad(約2.4°)，最大鉸接角控制偏差小于0.075 rad(約4.3°)，在約90°轉向的彎道依舊保持良好的跟蹤效果，經現場試驗達到了實際鏟運機自主行駛作業的路徑跟蹤精度要求，證明了本文提出的模型預測控制算法具有較好的實際應用效果，具備工業應用推廣價值。