基于NMPC的地下無人鏟運機反應式導航系統

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

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

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

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

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

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

1 地下無人鏟運機反應式導航系統

地下鏟運機為鉸接式車體，分為前車體與后車體兩部分并由剛性鉸接體連接，由液壓缸驅動車體轉向，如圖1所示。該結構可以減小轉向半徑，具有良好的驅動能力、通過能力以及遠低于剛性工程車輛的運營成本，在復雜的地下采礦環境機動能力較好，成為了礦產運輸的主要裝備[11-14]。

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

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

圖2 反應式導航系統架構Fig.2 Reactive navigation system architecture

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

2 分段式局部路徑決策策略

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

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

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

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

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

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

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

(1)

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

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

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

(2)

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

(3)

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

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

(4)

(3)進入彎道后階段。由于激光雷達識別能力有限，此時RFID會將轉彎后的巷道信息如左側巷道擬合直線l3、右側巷道擬合直線l4相對于轉彎前巷道直線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，為防止轉彎后與外側巷道壁刮蹭，可對局部參考路徑進行調整，本階段也分為左轉和右轉兩種情況。左轉時，可將轉彎后參考路徑a3設置為遠離左側巷道壁2 m的平行直線，已知

(7)

由于鏟運機行走時巷道壁基于車身坐標系的方程是不斷變化的，擬合直線相對于車身坐標系的位置不同，如圖7所示。因此可根據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所示，右轉時，可將轉彎后的參考路徑a3設置為遠離右側巷道壁2 m的平行直線，與左轉同理可得參考路徑a3為

(10)

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

x=c6+2

(11)

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

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

(12)

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

(13)

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

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

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

3.1 非線性預測模型

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

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

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

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

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

γ=θf-θr

(14)

根據鏟運機的數學關系可以求出

(15)

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

(16)

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

(17)

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

(18)

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

同時可得在預測時域Np內未來每一采樣時刻機器人的位姿狀態:

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

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

(19)

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

3.2 控制器優化目標函數設計

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

在未來時刻，鏟運機的位置與參考軌跡的誤差記為e(t):

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

(20)

式中，xref為系統局部參考路徑信息。

為了使控制量變化平穩，加入控制變量這一約束

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

(21)

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

因此將優化目標函數設定為

(22)

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

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

(23)

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

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

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

其中，Q和R為權重矩陣;ρ為權重系數;ε為松弛因子。第1項反映了系統對參考軌跡的跟蹤能力，第2項反映了對控制量平穩變化的要求。

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

(24)

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

(25)

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

4 仿真驗證

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

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

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

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

表2 仿真系統參數
Table 2 Parameter table of simulation system

控制參數取值Θ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依次為兩種控制器的速度、鉸接角速度、鉸接角、航向誤差參數的對比，從圖10(b)可以看出，LTV-MPC系統由于權重矩陣的設置，使得鏟運機達到臨界范圍時控制量搖擺不定，鉸接角速度出現振蕩，鏟運機執行結構的輸出起伏較大，穩定性較差，而由圖10(c)，(d)可以看出，兩種控制器的鉸接角在穩定范圍內，同時航向誤差均在5 s內收斂到趨近0的穩定狀態，其中LTV-MPC為0.003 9 rad，NMPC為0.001 5 rad，總體效果較好。

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

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

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

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

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

4.2 地下作業環境的模擬仿真對比

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

表3 直角彎道仿真結果對比
Table 3 Comparison of simulation results of right- angle bend

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

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

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

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

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

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

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

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

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

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

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

5 結 論

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

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