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

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

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

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

1 模型預(yù)測控制

1.1 鏟運機預(yù)測模型

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

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

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

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

(1)

上述狀態(tài)空間為非線性，采用近似線性化方法泰勒展開進(jìn)行處理，得到：

(2)

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

(3)

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

(4)

(5)

設(shè)定控制系統(tǒng)的預(yù)測時域為NP，控制時域為NC，則預(yù)測時域內(nèi)的狀態(tài)量偏差和系統(tǒng)控制增量可以表示為：

(6)

(7)

將上式計算系統(tǒng)的預(yù)測輸出簡化為矩陣形式如下：

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

(8)

每一項矩陣如下：

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

1.2 優(yōu)化求解

優(yōu)化求解是模型預(yù)測控制中求解系統(tǒng)的控制增量部分，并將控制增量用于實際輸出和預(yù)測模型。將控制增量作為目標(biāo)函數(shù)的狀態(tài)量，得到目標(biāo)函數(shù)：

(9)

優(yōu)化后的目標(biāo)函數(shù)中第一項求和累加了預(yù)測時域內(nèi)狀態(tài)量與參考狀態(tài)間的偏差，考慮到了未來的跟蹤效果。第二項起到了調(diào)節(jié)控制量平穩(wěn)性的作用。

控制量約束：

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)

車輛狀態(tài)約束為：

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

(11)

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

(12)

式中：ρ為權(quán)重系數(shù)；ε為松弛因子。

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

1.3 反饋控制

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

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

(13)

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

2 仿真試驗

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

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

仿真平臺選擇MATLAB R2018a版本，將MapleSim車輛模型和模型預(yù)測控制器導(dǎo)入Simulink中，根據(jù)實車情況設(shè)置約束條件，搭建整車控制框架進(jìn)行仿真。仿真框架如圖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

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

從軌跡上觀察四狀態(tài)偏差的LMPC控制策略可以較準(zhǔn)確地跟蹤參考路徑，車輛行駛的路徑未出現(xiàn)明顯的偏差。根據(jù)當(dāng)前車輛所處位置自動尋找該全局路徑上最近的參考點進(jìn)行跟蹤，這樣得到的參考點能較好地匹配當(dāng)前車輛的位置信息，并且避免了由于模型誤差推算導(dǎo)致的誤差累計問題。全程中本文提出的四狀態(tài)偏差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中段為礦上生產(chǎn)中段，礦車需從主斜坡道下至該中段，將礦車運載的礦石物料運至溜井口破碎(見圖9)。該中段巷道俯視圖如圖10所示。圖中起點處即為該中段入口，終點為溜井口位置。

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

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

采用多線激光雷達(dá)SLAM定位得到車輛在全局地圖的坐標(biāo)，預(yù)設(shè)不同路徑得到自動駕駛跟蹤效果如圖11所示。

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

圖11中藍(lán)色ref代表預(yù)設(shè)路徑，紅色real為鏟運機實際跟蹤軌跡，自動駕駛鏟運機可較好地跟隨全局預(yù)設(shè)路徑，完成運輸任務(wù)。各狀態(tài)偏差控制效果如圖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以內(nèi)，負(fù)向偏差小于0.15 m，x向偏差絕對值小于0.3 m，整體較好地跟蹤了預(yù)設(shè)路徑內(nèi)最近點的坐標(biāo)位置。同時航向角控制偏差與鉸接角控制偏差也分別保持在0.042 rad(約2.4°)與0.075 rad(約4.3°)以內(nèi)，控制誤差均保持在合理的范圍內(nèi)。

4 結(jié)論

本文提出的四狀態(tài)偏差MPC控制算法，經(jīng)仿真驗證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°轉(zhuǎn)向的彎道依舊保持良好的跟蹤效果，經(jīng)現(xiàn)場試驗達(dá)到了實際鏟運機自主行駛作業(yè)的路徑跟蹤精度要求，證明了本文提出的模型預(yù)測控制算法具有較好的實際應(yīng)用效果，具備工業(yè)應(yīng)用推廣價值。