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

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

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

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

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

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

1 地下無人鏟運機反應式導航系統(tǒng)

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

圖1 地下礦用鉸接式鏟運機Fig.1 Articulated LHD unit for underground mining

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

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

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

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

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

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

圖3 直巷道反應式導航控制策略Fig.3 Reactive navigation control strategy for straight laneway

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

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

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

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

(1)

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

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

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

(2)

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

(3)

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

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

(4)

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

(5)

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

若擬合直線l3，l4與車身坐標系x軸垂直，則

(6)

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

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

(7)

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

(8)

圖7 不同位置時在y軸上的截距關系Fig.7 Intercept relationship on y axis at different positions

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

x=c5+2

(9)

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

(10)

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

x=c6+2

(11)

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

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

(12)

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

(13)

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

3 基于NMPC算法的運動控制器設計

筆者以MPC原理為基礎，針對LTV-MPC控制器在反應式導航運動控制中存在的局限性，提出了應用于地下無人鏟運機反應式導航的NMPC運動控制器[17]。NMPC采用歐拉法將鏟運機的運動學模型進行離散化，該預測模型是基于車輛自身狀態(tài)量與控制量去預測下一時刻的位姿狀態(tài)量，保留了位姿信息，能夠?qū)ξ磥頃r刻的狀態(tài)信息與局部參考路徑的誤差進行代價評測，從而輸出每個預測步長的最佳控制量，在復雜巷道能夠進行準確地預測與控制[18]。

3.1 非線性預測模型

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

圖8 地下無人鉸接式鏟運機模型Fig.8 Underground unmanned articulated LHD unit model

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

物理意義前車體后車體車橋中心OfOr長度LfLr角速度wfwr速度vfvr寬度dfdr航向θfθr

其中鉸接角γ是θf和θr的差:

γ=θf-θr

(14)

根據(jù)鏟運機的數(shù)學關系可以求出

(15)

該鏟運機的狀態(tài)空間模型可由式(15)推導

(16)

將運動學模型進行離散化，即可得到預測時域內(nèi)鏟運機的位姿狀態(tài)，將式(16)改為

(17)

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

(18)

式中，t為時間;T為采樣周期。

同時可得在預測時域Np內(nèi)未來每一采樣時刻機器人的位姿狀態(tài):

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

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

(19)

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

3.2 控制器優(yōu)化目標函數(shù)設計

在鏟運機的預測模型中，系統(tǒng)的未來控制量是未知的，需要通過設定合適的優(yōu)化目標，求得反應式導航系統(tǒng)下一時刻的最優(yōu)控制量。

在未來時刻，鏟運機的位置與參考軌跡的誤差記為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)化目標函數(shù)設定為

(22)

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

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

(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項反映了系統(tǒng)對參考軌跡的跟蹤能力，第2項反映了對控制量平穩(wěn)變化的要求。

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

(24)

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

(25)

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

4 仿真驗證

筆者通過MATLAB/Simulink對地下無人鏟運機的反應式導航系統(tǒng)進行了仿真驗證。地下鏟運機的反應式導航不需要全局坐標系，仿真系統(tǒng)采用車身局部坐標系。其中仿真環(huán)境為模擬的巷道壁，并設立虛擬激光雷達來識別巷道信息。仿真對照組為文獻[16]中的基于LTC-MPC的反應式導航系統(tǒng)，簡寫為LTV-MPC，本文提出的基于NMPC的反應式導航系統(tǒng)簡寫為NMPC。

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

4.1 不同寬度彎道仿真效果對比

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

表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的直角巷道仿真對比Fig.9 Simulation comparison of 8 and 6 m right-angle laneway

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

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

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

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

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

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

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

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

表3 直角彎道仿真結(jié)果對比
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ù)的對比，其反應式導航失敗原因與第1組對照組類似，而且從圖13(c)控制器的鉸接角變化可以看出，LTV-MPC系統(tǒng)的鉸接角最大值為0.69 rad，轉(zhuǎn)彎幅度相對NMPC系統(tǒng)較大,難以回正，從圖13(b)中可以看出，在第2個彎道LTV-MPC系統(tǒng)的鉸接角速度達到了0.14 rad/s，較大的鉸接角角速度使得鏟運機動作幅度過大，同時造成了1.32 rad的最終航向誤差，如圖13(d)所示，這也是轉(zhuǎn)彎失敗的主要原因。

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

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

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

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

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

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

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

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

5 結(jié) 論

(1)本文提出的基于NMPC的反應式導航系統(tǒng)能夠較好地解決反應式導航目前存在的轉(zhuǎn)彎性能較差的問題，尤其體現(xiàn)在寬度為6 m的直角巷道的轉(zhuǎ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，較為成功地完成反應式導航任務。

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