基于NMPC的地下無(wú)人鏟運(yùn)機(jī)反應(yīng)式導(dǎo)航系統(tǒng)

羅維東，馬寶全，孟 宇,2，劉 立，顧 青，白國(guó)星，甘 鑫

(1.北京科技大學(xué) 機(jī)械工程學(xué)院，北京 100083; 2.北京科技大學(xué) 人工智能研究院，北京 100083)

隨著礦產(chǎn)資源的開(kāi)采深度逐漸加大，地下采礦工作環(huán)境變得愈加惡劣。利用礦車自主導(dǎo)航來(lái)代替人工駕駛完成地下采礦與運(yùn)輸工作，對(duì)于地下采礦工作的安全性和效率性都會(huì)有明顯的提高[1-3]。鉸接式地下礦用車如鏟運(yùn)機(jī)(Load-Haul-Dump，LHD)具有良好的驅(qū)動(dòng)能力、通過(guò)能力，成為礦產(chǎn)運(yùn)輸?shù)闹饕O(shè)備，開(kāi)發(fā)智能礦用鏟運(yùn)機(jī)更是未來(lái)的重要發(fā)展方向[4-5]。

鏟運(yùn)機(jī)的自主導(dǎo)航的工作機(jī)制與移動(dòng)機(jī)器人相似，所以在移動(dòng)機(jī)器人的研究基礎(chǔ)上，鏟運(yùn)機(jī)自主導(dǎo)航依據(jù)發(fā)展歷程可分為兩代。其中第1代鏟運(yùn)機(jī)自主導(dǎo)航通過(guò)追蹤地下巷道中類似軌道的導(dǎo)引設(shè)施實(shí)現(xiàn)自主導(dǎo)航;第2代鏟運(yùn)機(jī)自主導(dǎo)航利用信息融合進(jìn)行定位判定，實(shí)現(xiàn)自主導(dǎo)航[6]。第2代自主導(dǎo)航又可分為絕對(duì)式導(dǎo)航和相對(duì)式導(dǎo)航。絕對(duì)式導(dǎo)航需要精確的地圖信息，對(duì)定位的要求比較高，車輛對(duì)預(yù)先生成的軌跡進(jìn)行路徑跟蹤。相對(duì)式導(dǎo)航又稱反應(yīng)式導(dǎo)航，主要通過(guò)感知周圍環(huán)境做出合理的預(yù)判，從而做出相應(yīng)的決策，并不依賴精確的全局地圖信息[6-8]。地下采礦巷道狹長(zhǎng)且環(huán)境惡劣，測(cè)繪及鋪設(shè)基礎(chǔ)通訊設(shè)備難度較大，定位與建立精確地圖耗費(fèi)人力物力較大，因此無(wú)需精確地圖信息的反應(yīng)式導(dǎo)航在地下礦車自主導(dǎo)航中有很大的優(yōu)勢(shì)。

反應(yīng)式導(dǎo)航通常分為環(huán)境感知、局部路徑?jīng)Q策以及運(yùn)動(dòng)控制三大部分。在當(dāng)前的研究中通常將研究重點(diǎn)放在環(huán)境感知系統(tǒng)上[8-10]，但關(guān)于局部路徑?jīng)Q策和運(yùn)動(dòng)控制的具體研究較少，由于反應(yīng)式導(dǎo)航的決策與控制技術(shù)的研究不足，地下無(wú)人鏟運(yùn)機(jī)在巷道中的跟蹤與行駛效果不佳而影響到整體自主導(dǎo)航的工作效果。鏟運(yùn)機(jī)的運(yùn)動(dòng)控制是多變量多約束的非線性優(yōu)化問(wèn)題，因此求解包含復(fù)雜多約束的控制問(wèn)題，對(duì)于鏟運(yùn)機(jī)的反應(yīng)式導(dǎo)航的進(jìn)一步研究有很大幫助。當(dāng)前有研究人員將滑模控制(Sliding Mode Control，SMC)運(yùn)用到自主導(dǎo)航的運(yùn)動(dòng)控制上，滑?？刂启敯粜暂^好，但控制器的輸出易出現(xiàn)抖振[11-13]。文獻(xiàn)[14]闡述了一種“沿墻壁”的地下無(wú)人鏟運(yùn)機(jī)的反應(yīng)式導(dǎo)航方法，采用了比例積分微分(Proportional Integral Differentiation，PID)控制，他將巷道視為障礙，使得地下礦車在向前行駛中保證不與巷道壁發(fā)生碰撞，能夠?qū)崿F(xiàn)地下無(wú)人鏟運(yùn)機(jī)的簡(jiǎn)單路況行駛，但控制算法較為簡(jiǎn)單，未考慮鏟運(yùn)機(jī)的模型和約束條件。由于鏟運(yùn)機(jī)存在多約束條件，主流控制算法如PID控制、滑模控制等無(wú)法考慮車輛的多約束情況。近年來(lái)，模型預(yù)測(cè)控制算法(Model Predictive Control,MPC)逐漸被運(yùn)用于地下礦車的自動(dòng)化，其中Nayl和白國(guó)星等將MPC用到了鉸接式車輛的自主導(dǎo)航中[15-16]，這些研究表明，MPC可以有效地解決多約束問(wèn)題，對(duì)于約束條件較多的地下鉸接式鏟運(yùn)機(jī)更加適用。文獻(xiàn)[16]將線性時(shí)變模型預(yù)測(cè)控制(Linear Time-varying Model Predictive Control，LTV-MPC)運(yùn)用到地下無(wú)人鏟運(yùn)機(jī)的反應(yīng)式導(dǎo)航上來(lái)，其按“角平分線”策略計(jì)算當(dāng)前時(shí)刻航線誤差，并與PID控制算法的效果進(jìn)行了對(duì)比，證明了MPC算法在轉(zhuǎn)彎時(shí)相對(duì)于PID控制算法的優(yōu)越性。LTV-MPC算法雖然采用了預(yù)測(cè)控制，但其線性化后的預(yù)測(cè)模型是根據(jù)當(dāng)前誤差預(yù)測(cè)未來(lái)誤差，在直角轉(zhuǎn)彎等航向變化較大的情況下跟蹤誤差較大，容易出現(xiàn)轉(zhuǎn)向和回正滯后的情況。文獻(xiàn)[16]通過(guò)切換航向角和鉸接角等控制量解決這一問(wèn)題，但這種方法對(duì)于寬度不定的地下巷道很難自動(dòng)適應(yīng)。如在巷道變寬時(shí)，運(yùn)動(dòng)控制器可能無(wú)法及時(shí)識(shí)別彎道導(dǎo)致轉(zhuǎn)向失敗;當(dāng)巷道變窄時(shí)，則容易轉(zhuǎn)向過(guò)早難以調(diào)整姿態(tài)，最終導(dǎo)致碰壁。

本文主要工作如下:① 針對(duì)LTV-MPC控制算法在轉(zhuǎn)彎時(shí)較差的彎道通過(guò)能力和對(duì)寬度變化的巷道的適應(yīng)性，筆者提出了一種基于非線性模型預(yù)測(cè)控制(Nonlinear Model Predictive Control,NMPC)的運(yùn)動(dòng)控制器，通過(guò)歐拉法將鏟運(yùn)機(jī)運(yùn)動(dòng)學(xué)模型離散化后推導(dǎo)非線性預(yù)測(cè)模型，根據(jù)當(dāng)前時(shí)刻的狀態(tài)信息和控制序列預(yù)測(cè)未來(lái)時(shí)刻的狀態(tài)信息，對(duì)預(yù)測(cè)時(shí)域內(nèi)的狀態(tài)信息與參考路徑進(jìn)行代價(jià)計(jì)算，求出最優(yōu)控制序列。由于狀態(tài)量與控制量采用的是精確的位姿信息而非線性化后的偏差量，在預(yù)測(cè)算法中不會(huì)因丟失實(shí)際位姿信息而造成轉(zhuǎn)向過(guò)度的情況;② 針對(duì)于NMPC控制算法所需要的局部參考路徑，提出一種基于巷道環(huán)境信息的分段式局部路徑?jīng)Q策策略，由于LTV-MPC使用的角平分線策略僅提供當(dāng)前時(shí)刻的航向誤差量，通過(guò)當(dāng)前誤差量預(yù)測(cè)未來(lái)誤差量，但無(wú)法提供NMPC所需的參考路徑，所以與NMPC控制器并不搭配，故針對(duì)NMPC控制器提出基于巷道環(huán)境信息的分段式局部路徑?jīng)Q策策略，局部參考路徑根據(jù)鏟運(yùn)機(jī)在預(yù)測(cè)時(shí)域內(nèi)的巷道環(huán)境信息動(dòng)態(tài)調(diào)整，為NMPC控制器提供參考路徑輸入信息;③ 通過(guò)仿真實(shí)驗(yàn)對(duì)本文提出的反應(yīng)式導(dǎo)航系統(tǒng)的效果進(jìn)行驗(yàn)證，并與基于LTV-MPC的反應(yīng)式導(dǎo)航系統(tǒng)進(jìn)行對(duì)比，對(duì)兩者的反應(yīng)式導(dǎo)航行駛效果進(jìn)行比較分析。

1 地下無(wú)人鏟運(yùn)機(jī)反應(yīng)式導(dǎo)航系統(tǒng)

地下鏟運(yùn)機(jī)為鉸接式車體，分為前車體與后車體兩部分并由剛性鉸接體連接，由液壓缸驅(qū)動(dòng)車體轉(zhuǎn)向，如圖1所示。該結(jié)構(gòu)可以減小轉(zhuǎn)向半徑，具有良好的驅(qū)動(dòng)能力、通過(guò)能力以及遠(yuǎn)低于剛性工程車輛的運(yùn)營(yíng)成本，在復(fù)雜的地下采礦環(huán)境機(jī)動(dòng)能力較好，成為了礦產(chǎn)運(yùn)輸?shù)闹饕b備[11-14]。

圖1 地下礦用鉸接式鏟運(yùn)機(jī)Fig.1 Articulated LHD unit for underground mining

地下無(wú)人鏟運(yùn)機(jī)的反應(yīng)式導(dǎo)航系統(tǒng)由環(huán)境感知系統(tǒng)、局部路徑?jīng)Q策系統(tǒng)和運(yùn)動(dòng)控制系統(tǒng)組成，如圖2所示，其中，v為前車體速度;ω為鉸接角速度。本文的反應(yīng)式導(dǎo)航系統(tǒng)的環(huán)境感知系統(tǒng)由工業(yè)激光雷達(dá)和射頻識(shí)別(Radio Frequency Identification，RFID)組成[14-16]，為車輛提供巷道的環(huán)境信息。局部路徑?jīng)Q策系統(tǒng)根據(jù)識(shí)別的環(huán)境信息和任務(wù)目標(biāo)進(jìn)行預(yù)測(cè)時(shí)域內(nèi)局部路徑的規(guī)劃。運(yùn)動(dòng)控制器控制鏟運(yùn)機(jī)對(duì)路徑?jīng)Q策系統(tǒng)給出的局部路徑進(jìn)行跟蹤，輸出實(shí)時(shí)速度與航向控制量。

圖2 反應(yīng)式導(dǎo)航系統(tǒng)架構(gòu)Fig.2 Reactive navigation system architecture

在環(huán)境感知系統(tǒng)中，激光雷達(dá)負(fù)責(zé)采集巷道的環(huán)境信息，同時(shí)RFID射頻識(shí)別系統(tǒng)對(duì)關(guān)鍵路口信息進(jìn)行比對(duì)。本文側(cè)重于對(duì)局部路徑?jīng)Q策系統(tǒng)與運(yùn)動(dòng)控制器的改進(jìn)與設(shè)計(jì)，以上環(huán)境感知系統(tǒng)與文獻(xiàn)[14,16]中提出的相同，不再展開(kāi)介紹。環(huán)境感知系統(tǒng)獲得的巷道環(huán)境信息為局部路徑?jīng)Q策策略奠定基礎(chǔ)。接下來(lái)將對(duì)分段式局部路徑?jīng)Q策策略進(jìn)行介紹。

2 分段式局部路徑?jīng)Q策策略

對(duì)于局部參考路徑，文獻(xiàn)[16]中的沿“角平分線”策略能夠?yàn)長(zhǎng)TV-MPC控制器提供當(dāng)前時(shí)刻航向偏差量，但NMPC控制器需要鏟運(yùn)機(jī)前方一定范圍內(nèi)的巷道參考路徑來(lái)進(jìn)行最優(yōu)控制序列求解，所以“角平分線”策略已不再適用，因此對(duì)局部路徑?jīng)Q策策略進(jìn)行重新設(shè)計(jì)。本文在文獻(xiàn)[14]中的“沿墻走”策略基礎(chǔ)上，提出一種基于巷道環(huán)境信息的分段式局部路徑?jīng)Q策策略，并根據(jù)實(shí)際巷道情況將參考單側(cè)巷道壁改成兩側(cè)巷道壁，在直巷道中沿中心線行走，并在彎道行駛中根據(jù)地下巷道環(huán)境信息進(jìn)行動(dòng)態(tài)調(diào)整，從而確定鏟運(yùn)機(jī)需要跟蹤的局部參考路徑。

反應(yīng)式導(dǎo)航不需要全局定位與地圖信息，所以地下巷道不存在全局坐標(biāo)系。如圖3，4所示，激光雷達(dá)P的水平掃描范圍為190°，在地下實(shí)際巷道中可用激光雷達(dá)將巷道壁掃描點(diǎn)濾波、擬合為多段直線，車輛左側(cè)巷道壁擬合直線為l1，車輛右側(cè)巷道壁擬合直線為l2，并且可以得到l1，l2擬合直線相對(duì)于車身坐標(biāo)系的方程，并隨車身運(yùn)動(dòng)實(shí)時(shí)改變。如圖4,5所示，彎道前設(shè)置的RFID射頻識(shí)別系統(tǒng)可以將彎道信息傳遞給車輛，其中包括轉(zhuǎn)彎后的左巷道擬合直線l3、右巷道擬合直線l4與轉(zhuǎn)彎前的巷道壁l1,l2的幾何關(guān)系，為局部參考路徑的預(yù)測(cè)奠定基礎(chǔ)。圖3～5中，lr為后車體長(zhǎng)度;lf為前車體長(zhǎng)度;a1為局部參考路徑;a2為彎道前的參考路徑;a3為彎道后的參考路徑;a4為轉(zhuǎn)向結(jié)束后的參考路徑;b1為巷道寬度;l3為左側(cè)巷道壁擬合直線;l4為右側(cè)巷道壁擬合直線。

圖3 直巷道反應(yīng)式導(dǎo)航控制策略Fig.3 Reactive navigation control strategy for straight laneway

圖4 左轉(zhuǎn)時(shí)反應(yīng)式導(dǎo)航控制策略Fig.4 Reactive navigation control strategy for left turn

圖5 右轉(zhuǎn)時(shí)反應(yīng)式導(dǎo)航控制策略Fig.5 Reactive navigation control strategy for right turn

根據(jù)鏟運(yùn)機(jī)在地下巷道的行駛路徑，可依據(jù)激光雷達(dá)與RFID系統(tǒng)擬合的基于車身坐標(biāo)系的巷道壁直線方程將局部路徑策略分為4個(gè)階段，其中包括:

(1)直巷道行走階段。在曲率沒(méi)有發(fā)生較大變化的直線巷道中，如圖3，6所示，其中l(wèi)1，l2兩擬合直線平行，可根據(jù)左側(cè)巷道壁擬合直線l1，右側(cè)巷道壁擬合直線l2相對(duì)于車身坐標(biāo)系的直線方程來(lái)求解巷道中心線a1方程。其中

(1)

式中，k1，k2分別為直線l1，l2的斜率;c1，c2分別為直線l1，l2在車身坐標(biāo)系y軸上的截距。

圖6 車身坐標(biāo)系上的巷道壁擬合直線Fig.6 Roadway wall fitting line in vehicle Coordinate

已知k1=k2，根據(jù)兩組擬合直線方程可求出巷道中心線a1基于車身坐標(biāo)系方程

(2)

(2)進(jìn)入彎道前階段。在識(shí)別到RFID給出的彎道信息即進(jìn)入彎道前，如圖4，5所示，為了提高鏟運(yùn)機(jī)轉(zhuǎn)彎時(shí)的安全裕度，可將反應(yīng)式導(dǎo)航策略在“中分線”基礎(chǔ)上進(jìn)行優(yōu)化，在臨近彎道處實(shí)行偏離“可能發(fā)生碰撞的一側(cè)”的策略，同時(shí)考慮到大多地下鏟運(yùn)機(jī)寬度小于4 m,將局部參考路徑設(shè)置為外側(cè)巷道與車身中心的安全距離為2 m的直線a2。根據(jù)實(shí)際作業(yè)情況，可將本階段分為兩種情況，分別為圖4所示的左轉(zhuǎn)彎道和圖5所示的右轉(zhuǎn)彎道。在左轉(zhuǎn)時(shí)，可將參考路徑a2設(shè)置為遠(yuǎn)離右側(cè)巷道壁2 m的平行直線以防止轉(zhuǎn)向與內(nèi)側(cè)巷道刮蹭，如圖6所示，局部參考路徑a2可根據(jù)式(3)得到

(3)

式中，θ1為直線k1的反正切值。

如圖5所示，在車輛右轉(zhuǎn)時(shí)，可將參考路徑設(shè)置為遠(yuǎn)離左側(cè)巷道壁2 m的平行直線a2，同理局部參考路徑直線a2方程為

(4)

(3)進(jìn)入彎道后階段。由于激光雷達(dá)識(shí)別能力有限，此時(shí)RFID會(huì)將轉(zhuǎn)彎后的巷道信息如左側(cè)巷道擬合直線l3、右側(cè)巷道擬合直線l4相對(duì)于轉(zhuǎn)彎前巷道直線l1,l2的幾何關(guān)系進(jìn)行補(bǔ)充，從而可得到

(5)

式中，k3，k4分別為直線l3，l4在車身坐標(biāo)系的斜率;c3，c4分別為直線l3，l4在車身坐標(biāo)系y軸上的截距。

若擬合直線l3，l4與車身坐標(biāo)系x軸垂直，則

(6)

式中，c5，c6分別為直線l3，l4在車身坐標(biāo)系x軸上的截距。

如圖4，5所示，k3=k4，且l1與l3垂直、l2與l4垂直，即k1k3=-1,k2k4=-1，為防止轉(zhuǎn)彎后與外側(cè)巷道壁刮蹭，可對(duì)局部參考路徑進(jìn)行調(diào)整，本階段也分為左轉(zhuǎn)和右轉(zhuǎn)兩種情況。左轉(zhuǎn)時(shí)，可將轉(zhuǎn)彎后參考路徑a3設(shè)置為遠(yuǎn)離左側(cè)巷道壁2 m的平行直線，已知

(7)

由于鏟運(yùn)機(jī)行走時(shí)巷道壁基于車身坐標(biāo)系的方程是不斷變化的，擬合直線相對(duì)于車身坐標(biāo)系的位置不同，如圖7所示。因此可根據(jù)l3，l4兩直線在y軸上的截距大小關(guān)系求取參考路徑直線a3:

(8)

圖7 不同位置時(shí)在y軸上的截距關(guān)系Fig.7 Intercept relationship on y axis at different positions

若l3，l4與車身坐標(biāo)系x軸垂直，則參考路徑a3為

x=c5+2

(9)

如圖5所示，右轉(zhuǎn)時(shí)，可將轉(zhuǎn)彎后的參考路徑a3設(shè)置為遠(yuǎn)離右側(cè)巷道壁2 m的平行直線，與左轉(zhuǎn)同理可得參考路徑a3為

(10)

若l3,l4與車身坐標(biāo)系x軸垂直，則參考路徑a3為

x=c6+2

(11)

在得到參考路徑直線a2，a3后，可得到其交點(diǎn)B，如圖4，5所示，B點(diǎn)即為參考路徑轉(zhuǎn)折點(diǎn)。

(4)轉(zhuǎn)彎完成階段。如圖4，5所示，在完成上述階段后，到達(dá)RFID識(shí)別區(qū)，根據(jù)RFID及激光雷達(dá)掃描信息獲知轉(zhuǎn)彎完成，再次進(jìn)入直巷道行走階段，參考路徑為巷道中心線，即

(12)

若l3，l4與車身坐標(biāo)系x軸垂直，則巷道中心線為

(13)

在第4階段完成后，鏟運(yùn)機(jī)再次進(jìn)入直巷道階段，如此循環(huán)，以完成反應(yīng)式導(dǎo)航過(guò)程中的局部參考路徑規(guī)劃。得到的參考路徑為基于車身坐標(biāo)系的分段直線，在此基礎(chǔ)下可將參考路徑直線根據(jù)控制器預(yù)測(cè)步長(zhǎng)離散為連續(xù)的點(diǎn)，從而得到由點(diǎn)序列組成的局部參考路徑。在得到鏟運(yùn)機(jī)在巷道的局部參考路徑后，其參考信息作為實(shí)時(shí)輸入信息傳輸?shù)絅MPC運(yùn)動(dòng)控制器。接下來(lái)將對(duì)基于NMPC算法的運(yùn)動(dòng)控制器設(shè)計(jì)進(jìn)行詳細(xì)介紹。

3 基于NMPC算法的運(yùn)動(dòng)控制器設(shè)計(jì)

筆者以MPC原理為基礎(chǔ)，針對(duì)LTV-MPC控制器在反應(yīng)式導(dǎo)航運(yùn)動(dòng)控制中存在的局限性，提出了應(yīng)用于地下無(wú)人鏟運(yùn)機(jī)反應(yīng)式導(dǎo)航的NMPC運(yùn)動(dòng)控制器[17]。NMPC采用歐拉法將鏟運(yùn)機(jī)的運(yùn)動(dòng)學(xué)模型進(jìn)行離散化，該預(yù)測(cè)模型是基于車輛自身狀態(tài)量與控制量去預(yù)測(cè)下一時(shí)刻的位姿狀態(tài)量，保留了位姿信息，能夠?qū)ξ磥?lái)時(shí)刻的狀態(tài)信息與局部參考路徑的誤差進(jìn)行代價(jià)評(píng)測(cè)，從而輸出每個(gè)預(yù)測(cè)步長(zhǎng)的最佳控制量，在復(fù)雜巷道能夠進(jìn)行準(zhǔn)確地預(yù)測(cè)與控制[18]。

3.1 非線性預(yù)測(cè)模型

鉸接式鏟運(yùn)機(jī)在局部車身坐標(biāo)系中的運(yùn)動(dòng)特征與在全局坐標(biāo)系中的一致，因此其運(yùn)動(dòng)學(xué)模型可以參考孟宇、Nayl等建立的鉸接車運(yùn)動(dòng)學(xué)模型[19-20]。在當(dāng)前鏟運(yùn)機(jī)的研究中，通常以前車橋中心Of為車輛控制點(diǎn)。如圖8所示，鏟運(yùn)機(jī)的前車身和后車身被認(rèn)為是剛體，輪胎被忽略。表1顯示了鉸接式車輛模型中各參數(shù)的意義。

圖8 地下無(wú)人鉸接式鏟運(yùn)機(jī)模型Fig.8 Underground unmanned articulated LHD unit model

表1 鏟運(yùn)機(jī)各參數(shù)物理意義
Table 1 Physical significance of parameters of articulated vehicle

物理意義前車體后車體車橋中心OfOr長(zhǎng)度LfLr角速度wfwr速度vfvr寬度dfdr航向θf(wàn)θr

其中鉸接角γ是θf(wàn)和θr的差:

γ=θf(wàn)-θr

(14)

根據(jù)鏟運(yùn)機(jī)的數(shù)學(xué)關(guān)系可以求出

(15)

該鏟運(yùn)機(jī)的狀態(tài)空間模型可由式(15)推導(dǎo)

(16)

將運(yùn)動(dòng)學(xué)模型進(jìn)行離散化，即可得到預(yù)測(cè)時(shí)域內(nèi)鏟運(yùn)機(jī)的位姿狀態(tài)，將式(16)改為

(17)

由于采樣間隔較小，可用歐拉法(Euler Method)將式(17)離散化為

(18)

式中，t為時(shí)間;T為采樣周期。

同時(shí)可得在預(yù)測(cè)時(shí)域Np內(nèi)未來(lái)每一采樣時(shí)刻機(jī)器人的位姿狀態(tài):

x(t+Np|t)=x(t+Np-1|t)+

Tf(x(t+Np-1|t),u(t+Nc-1|t))

(19)

其中，Np為預(yù)測(cè)時(shí)域;Nc為控制時(shí)域，此時(shí)預(yù)測(cè)層中未來(lái)的狀態(tài)和輸出可以由式(19)算出，實(shí)現(xiàn)了預(yù)測(cè)未來(lái)狀態(tài)量的功能。

3.2 控制器優(yōu)化目標(biāo)函數(shù)設(shè)計(jì)

在鏟運(yùn)機(jī)的預(yù)測(cè)模型中，系統(tǒng)的未來(lái)控制量是未知的，需要通過(guò)設(shè)定合適的優(yōu)化目標(biāo)，求得反應(yīng)式導(dǎo)航系統(tǒng)下一時(shí)刻的最優(yōu)控制量。

在未來(lái)時(shí)刻，鏟運(yùn)機(jī)的位置與參考軌跡的誤差記為e(t):

e(t+Np|t)=x(t+Np|t)-xref(t+Np|t)

(20)

式中，xref為系統(tǒng)局部參考路徑信息。

為了使控制量變化平穩(wěn)，加入控制變量這一約束

Δu(t|t)=u(t|t)-u(t-1|t)

(21)

式中，u(t-1)為上一控制周期的控制量。

因此將優(yōu)化目標(biāo)函數(shù)設(shè)定為

(22)

式中，u為控制變量約束;J為代價(jià)函數(shù)總值;i=1，2，3,…，N。

在實(shí)際控制系統(tǒng)中，往往需要滿足狀態(tài)量及控制量的一些約束條件，如(vfmin,vfmax)，(ωγmin,ωγmax)，(γmin,γmax)。因此需要在每步長(zhǎng)內(nèi)求解以下的帶約束的有限時(shí)域優(yōu)化問(wèn)題:

(23)

s.t.vfmin≤vf(t+i|t)≤vfmax

ωγmin≤ωγ(t+i|t)≤ωγmax

γmin≤γ(t+i|t)≤γmax

其中，Q和R為權(quán)重矩陣;ρ為權(quán)重系數(shù);ε為松弛因子。第1項(xiàng)反映了系統(tǒng)對(duì)參考軌跡的跟蹤能力，第2項(xiàng)反映了對(duì)控制量平穩(wěn)變化的要求。

在每個(gè)采樣時(shí)刻內(nèi)對(duì)優(yōu)化式(23)進(jìn)行求解，通過(guò)優(yōu)化函數(shù)求解得到最優(yōu)控制序列[21]:

(24)

將第1個(gè)控制時(shí)域內(nèi)的控制量作為受控對(duì)象的實(shí)際控制輸入，即

(25)

在下一采樣時(shí)刻，系統(tǒng)重新以新的采樣時(shí)刻為初始狀態(tài)求解優(yōu)化公式，直至完成整個(gè)鏟運(yùn)機(jī)反應(yīng)式導(dǎo)航控制過(guò)程。

4 仿真驗(yàn)證

筆者通過(guò)MATLAB/Simulink對(duì)地下無(wú)人鏟運(yùn)機(jī)的反應(yīng)式導(dǎo)航系統(tǒng)進(jìn)行了仿真驗(yàn)證。地下鏟運(yùn)機(jī)的反應(yīng)式導(dǎo)航不需要全局坐標(biāo)系，仿真系統(tǒng)采用車身局部坐標(biāo)系。其中仿真環(huán)境為模擬的巷道壁，并設(shè)立虛擬激光雷達(dá)來(lái)識(shí)別巷道信息。仿真對(duì)照組為文獻(xiàn)[16]中的基于LTC-MPC的反應(yīng)式導(dǎo)航系統(tǒng)，簡(jiǎn)寫為L(zhǎng)TV-MPC，本文提出的基于NMPC的反應(yīng)式導(dǎo)航系統(tǒng)簡(jiǎn)寫為NMPC。

仿真分為兩部分：第1部分為直角彎道轉(zhuǎn)向效果的仿真，其中包括兩種不同寬度彎道的仿真對(duì)比，對(duì)兩種反應(yīng)式導(dǎo)航系統(tǒng)在這兩種不同寬度的巷道的轉(zhuǎn)向效果進(jìn)行對(duì)比；第2部分為地下無(wú)人采礦模擬工作環(huán)境的仿真對(duì)比，目的是分析鏟運(yùn)機(jī)在仿真地下作業(yè)巷道的行走效果并對(duì)在此環(huán)境下的兩種反應(yīng)式導(dǎo)航效果進(jìn)行分析。為保證對(duì)比的嚴(yán)謹(jǐn)性，反應(yīng)式導(dǎo)航系統(tǒng)及相對(duì)應(yīng)的鏟運(yùn)機(jī)模型采用相同的參數(shù)，其中，記Θ1為激光雷達(dá)的水平掃描角度;s1為激光雷達(dá)的水平掃描距離;p為激光雷達(dá)的水平分辨率;s2為RFID標(biāo)簽的讀寫距離;f為RFID標(biāo)簽的工作頻率，其余控制參數(shù)上文均已提到,見(jiàn)表2。

4.1 不同寬度彎道仿真效果對(duì)比

第1組仿真設(shè)置了兩種不同寬度的仿真巷道。首先，圖9(a)為寬度為8 m的直角彎道的兩種控制器的對(duì)比效果。這組仿真中，NMPC系統(tǒng)與LTV-MPC系統(tǒng)都很好地完成了轉(zhuǎn)彎，但NMPC系統(tǒng)轉(zhuǎn)向較為平滑且離巷道壁最近距離為1.62 m，相對(duì)彎道內(nèi)側(cè)的安全距離較大，同時(shí)LTV-MPC系統(tǒng)的最小安全距離也為1.51 m，也達(dá)到了很好的控制效果。

表2 仿真系統(tǒng)參數(shù)
Table 2 Parameter table of simulation system

控制參數(shù)取值Θ1/(°)(-5,185)s1/m(0,80)p/(°)0.25s2/m(0,20)f/MHz(860,960)Lf/m1.80Lr/m1.80Np50Nc1d/m2.80vf/(m·s-1)(1.95,2.05)ωγ/(rad·s-1)(-0.14,0.14)γ/rad(-0.698,0.698)

圖9 寬度為8,6 m的直角巷道仿真對(duì)比Fig.9 Simulation comparison of 8 and 6 m right-angle laneway

圖10依次為兩種控制器的速度、鉸接角速度、鉸接角、航向誤差參數(shù)的對(duì)比，從圖10(b)可以看出，LTV-MPC系統(tǒng)由于權(quán)重矩陣的設(shè)置，使得鏟運(yùn)機(jī)達(dá)到臨界范圍時(shí)控制量搖擺不定，鉸接角速度出現(xiàn)振蕩，鏟運(yùn)機(jī)執(zhí)行結(jié)構(gòu)的輸出起伏較大，穩(wěn)定性較差，而由圖10(c)，(d)可以看出，兩種控制器的鉸接角在穩(wěn)定范圍內(nèi)，同時(shí)航向誤差均在5 s內(nèi)收斂到趨近0的穩(wěn)定狀態(tài)，其中LTV-MPC為0.003 9 rad，NMPC為0.001 5 rad，總體效果較好。

圖10 寬度為8 m的直角巷道車輛參數(shù)對(duì)比Fig.10 Contrast of vehicle parameters for 8 m right angle laneway

接下來(lái)對(duì)寬度為6 m的直角巷道的轉(zhuǎn)向效果進(jìn)行對(duì)比，如圖9(b)所示，LTV-MPC系統(tǒng)轉(zhuǎn)彎后回正較為遲緩，造成鏟運(yùn)機(jī)過(guò)度轉(zhuǎn)向，在轉(zhuǎn)向左側(cè)時(shí)車輛中心線離巷道壁最小距離為0.52 m，由于車身寬度為2.80 m，此時(shí)已造成碰壁，在寬度為6 m的巷道轉(zhuǎn)彎失敗;同時(shí)NMPC系統(tǒng)能夠在該較窄彎道中成功完成轉(zhuǎn)彎，車輛中心線距離巷道壁最小距離為2.13 m，因此車體距離巷道壁最小距離為0.73 m，安全距離提高了1.61 m。

圖11 寬度為6 m的直角巷道車輛參數(shù)對(duì)比Fig.11 Contrast of vehicle parameters for 6 m right angle laneway

圖11依次為兩種控制器的速度、鉸接角速度、鉸接角、航向誤差參數(shù)的對(duì)比，圖11(b)中依然存在控制器的振蕩，并且LTV-MPC系統(tǒng)的鉸接角在一段時(shí)間內(nèi)為定值，這是由于該算法采用的線性化后的預(yù)測(cè)模型為偏差模型，狀態(tài)量與控制量均為偏差量，在車輛轉(zhuǎn)彎后激光雷達(dá)識(shí)別為直巷道，鉸接角誤差量為0，故特別設(shè)置權(quán)重矩陣進(jìn)行回正，導(dǎo)致了該算法滯后性較明顯，最終偏差為0.68 rad，轉(zhuǎn)向失敗。與此對(duì)比，NMPC控制器的預(yù)測(cè)模型為實(shí)際狀態(tài)模型，其鉸接角變化較為平順，響應(yīng)速度較快，如圖11(d)所示，鏟運(yùn)機(jī)航向誤差在轉(zhuǎn)向后5 s內(nèi)趨于穩(wěn)定，最終穩(wěn)態(tài)誤差為0.004 2 rad，控制效果較好。

對(duì)于以上兩種不同寬度巷道，筆者對(duì)無(wú)人鏟運(yùn)機(jī)的主要仿真數(shù)據(jù)進(jìn)行了總結(jié)，見(jiàn)表3。

4.2 地下作業(yè)環(huán)境的模擬仿真對(duì)比

在第2組仿真中，對(duì)地下巷道進(jìn)行了模擬，鏟運(yùn)機(jī)從A區(qū)經(jīng)過(guò)兩個(gè)直角彎道行駛到達(dá)C區(qū)，如圖12所示:第1段A區(qū)為6 m×14 m(寬×長(zhǎng))的直巷道，第2段A區(qū)到C區(qū)的巷道為8 m×36 m(寬×長(zhǎng))的巷道，第3段為6 m×23 m(寬×長(zhǎng))的直巷道。從圖13可以看出，兩種反應(yīng)式導(dǎo)航系統(tǒng)都可以很好地完成前兩段巷道的自主導(dǎo)航，但LTV-MPC系統(tǒng)在由8 m寬巷道轉(zhuǎn)向C區(qū)巷道時(shí)未及時(shí)轉(zhuǎn)向，出現(xiàn)碰壁情況，而NMPC系統(tǒng)在第3段成功轉(zhuǎn)向，且在轉(zhuǎn)彎時(shí)的最小安全距離為1.47 m，比較成功地完成了此工況的自主導(dǎo)航。

表3 直角彎道仿真結(jié)果對(duì)比
Table 3 Comparison of simulation results of right- angle bend

參數(shù)LTV-MPCNMPCLTV-MPCNMPC彎道寬度/m8 866最小離墻距離/m1.621.5100.73最終航向誤差/rad0.003 90.001 50.678 00.004 2

圖12 地下巷道工作環(huán)境的模擬仿真Fig.12 Simulation of working environment of underground laneway

圖13依次為兩種控制器的速度、鉸接角速度、鉸接角、航向誤差參數(shù)的對(duì)比，其反應(yīng)式導(dǎo)航失敗原因與第1組對(duì)照組類似，而且從圖13(c)控制器的鉸接角變化可以看出，LTV-MPC系統(tǒng)的鉸接角最大值為0.69 rad，轉(zhuǎn)彎幅度相對(duì)NMPC系統(tǒng)較大,難以回正，從圖13(b)中可以看出，在第2個(gè)彎道LTV-MPC系統(tǒng)的鉸接角速度達(dá)到了0.14 rad/s，較大的鉸接角角速度使得鏟運(yùn)機(jī)動(dòng)作幅度過(guò)大，同時(shí)造成了1.32 rad的最終航向誤差，如圖13(d)所示，這也是轉(zhuǎn)彎失敗的主要原因。

圖13 模擬巷道的車輛參數(shù)對(duì)比Fig.13 Contrast of parameters in simulated laneway

圖14 地下長(zhǎng)巷道工作環(huán)境的模擬仿真Fig.14 Simulation of working environment of long laneway

考慮到仿真鏟運(yùn)機(jī)車身較長(zhǎng)，LTV-MPC在上述模擬巷道中未能在第2個(gè)彎道成功轉(zhuǎn)彎可能由于巷道較短，條件較為苛刻，所以此處將第2段巷道延長(zhǎng)，第2段巷道現(xiàn)為8 m×46 m(寬×長(zhǎng))的直巷道。如圖14所示，LTV-MPC驅(qū)動(dòng)的鏟運(yùn)機(jī)已轉(zhuǎn)入C區(qū)，可見(jiàn)LTV-MPC在具有較長(zhǎng)的調(diào)整時(shí)間后效果會(huì)變好，但在轉(zhuǎn)入C區(qū)后，車身中心離巷道壁最小距離為1.12 m，由于車身寬度為2.80 m，未能及時(shí)回正，造成碰壁。同時(shí)NMPC系在整個(gè)行駛過(guò)程離巷道壁最小距離為2.95 m，具有1.55 m的安全距離，安全距離提高了1.83 m。

圖15依次為2種控制器對(duì)應(yīng)的車輛的速度、鉸接角速度、鉸接角、航向誤差參數(shù)的對(duì)比，結(jié)合圖15(b),(c)來(lái)看，LTV-MPC系統(tǒng)的鉸接角最大值為0.68 rad，最小值為-0.68 rad，轉(zhuǎn)向幅度較大，難以及時(shí)回正。與此對(duì)比，NMPC系統(tǒng)能夠在5 s內(nèi)將航向誤差由0.89 rad逐漸減小至0，最終誤差為0.002 8 rad，反應(yīng)式導(dǎo)航較為成功。

圖15 模擬較長(zhǎng)巷道的車輛參數(shù)對(duì)比Fig.15 Contrast of vehicle parameters in simulated long laneway

同時(shí)，在表4中對(duì)以上主要仿真結(jié)果進(jìn)行總結(jié)，從表4的最終航向誤差與最小離墻距離數(shù)據(jù)的對(duì)比也呼應(yīng)了上述的具體分析結(jié)果，驗(yàn)證了NMPC在反應(yīng)式導(dǎo)航系統(tǒng)中決策與控制的優(yōu)勢(shì)。

表4 模擬巷道仿真結(jié)果對(duì)比
Table 4 Comparison of results in simulated laneway

參數(shù)LTV-MPCNMPCLTV-MPCNMPC第2段巷道長(zhǎng)度/m36364646最小離墻距離/m01.4701.55最終航向誤差/rad1.321 00.001 01.092 00.002 8

5 結(jié) 論

(1)本文提出的基于NMPC的反應(yīng)式導(dǎo)航系統(tǒng)能夠較好地解決反應(yīng)式導(dǎo)航目前存在的轉(zhuǎn)彎性能較差的問(wèn)題，尤其體現(xiàn)在寬度為6 m的直角巷道的轉(zhuǎn)彎問(wèn)題和模擬地下工作巷道的工作情況，距離巷道壁最小距離分別為0.73 m和1.47 m，航向偏差能在5 s內(nèi)最大由0.89 rad收斂至穩(wěn)定狀態(tài)，收斂后最小誤差分別為0.001 0 rad和0.002 8 rad，較為成功地完成反應(yīng)式導(dǎo)航任務(wù)。

(2)由于使用了經(jīng)離散化后的非線性預(yù)測(cè)模型，實(shí)際狀態(tài)信息取代了位姿信息偏差量，對(duì)寬度不同的巷道不需要特別切換控制量進(jìn)行回正，使復(fù)雜巷道下控制更為精確，而且避免了輸出抖振情況，使得控制器的控制量輸出相對(duì)平滑，同時(shí)鏟運(yùn)機(jī)執(zhí)行機(jī)構(gòu)的工作更為平順，提高了地下無(wú)人鏟運(yùn)機(jī)反應(yīng)式導(dǎo)航的行駛性能。