基于雙維度搜索的地下自主鏟運機最優轉彎軌跡規劃

顧 青，劉 立，白國星，孟 宇?

1) 北京科技大學機械工程學院，北京 100083 2) 北京科技大學順德研究生院，佛山 528300

?通信作者，E-mail: myu@ustb.edu.cn

地下鏟運機是一種鉸接式結構的地下無軌移動采礦裝備，具有轉彎半徑小，地形適應能力強的特點，廣泛應用于礦產開采和運輸. 隨著采礦行業的發展，采礦深度不斷增加，地下高溫高濕環境對駕駛員身體有極大傷害，因此，自主行駛成為新一代智能移動采礦裝備的必備功能.

為了實現鏟運機的自主行駛，軌跡規劃與控制方法被廣泛的研究[1-11]. 文獻[1]～[8]研究了地下鏟運機的導航控制方法，進而實現鏟運機可以在地下巷道內采用“沿壁法”安全避障行駛. 文獻[8]在無約束小范圍內，可在鏟裝點和卸載點之間生成平滑行駛路徑. 文獻[9]提出了一種基于蟻群算法的路徑規劃方法，在地下巷道環境內，可生成全局最優路徑. 文獻[10]提出一種自主導航控制框架：首先人工示教生成導航地圖和駕駛提示信息，然后在自主行駛時依照駕駛提示信息行駛. 基于這些研究，目前鏟運機自主行駛技術可分為兩類：一類是路徑規劃和跟蹤，此類方法沒有考慮到速度因素，因此對于行鏟運機工作效率無法考慮，同時行駛速度較慢；另一類是先人工示教，然后自主行駛. 此類方法受到駕駛員經驗影響較大，一般在直道中可以以較高的速度行駛，而接近轉彎區域時，為了避免與巷道壁碰撞，駕駛員通常會提前減速，以很低的速度通過轉彎區域，也就是說，通過轉彎區域的行駛時間較長. 因為鏟運機的地下金屬開采工作主要是出礦，通常每個“鏟—運—返”工作循環都要轉彎，所以，轉彎區域通過時間長會影響鏟運機工作效率. 總的來說，目前鏟運機的自主行駛水平還處于初級階段.

對于采用跟蹤目標要來完成自主行駛功能的控制框架來說，除了研究高精度的跟蹤控制算法[11-12]，進行高質量的軌跡規劃也是提高自主行駛水平的有效途徑. 目前針對乘用車軌跡規劃法方法的研究很多，總體可以分為直接法[13-19]、基于解耦的方法[20-32]，較好的解決了乘用車在邊界曲率平緩的公路上的軌跡規劃問題. 文獻[31]研究了鏟運機在地下巷道內典型循環工況的軌跡規劃問題，可視為一種轉彎軌跡規劃問題. 其中，縱向軌跡規劃是一個二次規劃問題，易于求解. 然而橫向軌跡還是一個非線性優化模型，因此求解時間較長（而且有可能得不到最優解或可行解）；此外，研究發現，當轉彎角度過大時，現有的約束并不能約束鉸接角及其角速度. 究其原因，還是沒有將鉸接角及其角速度作為決策變量加入到優化模型之中，無法對其直接約束. 然而，若是直接將鉸接角及其角速度作為決策變量，則根據其運動狀態方程，軌跡優化模型一定是一個復雜的非線性優化模型，能否得到最優解或可行解都沒有保障.

為了解決上述問題，提高自主鏟運機的彎道行駛性能，根據鏟運機及其行駛環境特點，本文提出了一種基于雙維度搜索的地下鏟運機最優轉彎軌跡規劃方法. 本文的貢獻及新穎之處在于設計了一種基于行駛里程和時間的雙維度搜索策略，在軌跡計算階段，建立了一種以平穩行駛為目標的縱橫向解耦二次規劃模型，其中，鉸接角及鉸接角速度約束并不在優化模型中直接考慮，而是通過對生成軌跡進行約束條件檢查來篩選得到滿足約束的最優軌跡. 與現有規劃方法不同的是，使用本文提出的方法可以快速得到在縱向橫向兩個維度都具有最優性保證的軌跡，不需要后備策略，保障了導航控制系統中軌跡規劃層的可靠性，提高了生成軌跡的質量和速度，使鏟運機在轉彎時可以合理調整行駛速度和位置，實現高效轉彎，提高生產效率.

本文提出的方法具有以下特點：能夠生成平穩的轉彎軌跡，模型簡單易于實時求解，所得到的軌跡具有最優性保證，不需要后備策略.

文章結構如下，第一章介紹鏟運機運動學模型、轉彎區域模型及所研究的具體問題，第二章介紹軌跡規劃方法框架，第三章介紹軌跡規劃方法數學模型，第四章和第五章是算例分析與樣機試驗，驗證本文提出的方法有效性和優越性，第五章總結結論.

1 對象、場景模型與問題描述

1.1 鏟運機運動學模型

由于鏟運機（Load haul dump，LHD）具有鉸接結構，對其運動姿態的描述與剛性車輛不同，在運動規劃時需要同時考慮其航向角和鉸接角. LHD的整體結構如圖1 所示，車體由兩部分組成. PA是前后車體的交接點， Pf(xf,yf)是前橋中心點，Pr(xr,yr)是 后橋中心點， θf和 θr分別是前后車體的航向角. Pf和 Pr與 PA之 間的距離分別為 Lf和 Lr. 定義鉸接角 γ 為 θf與 θr之差

圖 1 LHD 結構Fig.1 Structure of an LHD

PfPr和 之間的位置關系可以表示為

鉸接車輛的運動學模型[30]可以描述為

其中， vf為前車體速度， γ˙ 為鉸接角 γ的角速度. 可知，前車體航向角的角速度為

鉸接角及其角速度的物理約束為

其中，γmin為鉸接角最小值，γmax為 鉸接角最大值，γ˙min為鉸接角速度最小值， γ˙max為鉸接角速度最大值.

1.2 彎道區域模型

轉彎區域如圖2 所示. 圖2（a）中，巷道A 與B 相交，鏟運機從巷道B 向巷道A 轉彎. 在路口建立直角坐標系， O為轉彎區域起點位置，位于巷道A，彎道區域直角坐標系Y 軸垂直于巷道B 的邊界；紅色虛線為轉彎區域結束的位置，垂直于巷道A 的邊界. Lsafe是安全距離長度，黑色虛線之間的藍色陰影部分是除去安全邊界的可行區域； α為兩巷道夾角； LA和 LB分別為巷道A 和B 在轉彎區域的長度. (xout,yout)是彎道出口處左邊界點位置坐標. 地下巷道轉彎處都會根據鉸接式采礦裝備的轉彎半徑進行有磨角，目的是為了使這類移動采礦裝備能夠安全通過路口. 磨角后，轉彎處可行區域增大，如圖2（b）所示， L′A和 LB′是磨角操作后巷道A 和巷道B 的剩余長度.

1.3 問題描述

當鏟運機進入轉彎區域時，根據鏟運機當前速度、位置，實時生成一條平穩軌跡，使鏟運機能夠在安全轉彎的情況下，平穩的行駛出轉彎區域.在軌跡規劃時，要滿足下列約束條件：

（1）前后車體都不與巷道壁發生碰撞；

（2）鏟運機速度不超過限速；

（3）鉸接角和鉸接角速度不超過設計約束值；

（4）在駛出轉彎區域時，前車體方向與巷道壁平行.

2 軌跡規劃算法框架

2.1 最優軌跡存在性分析

在礦山建設中，巷道的結構和尺寸與礦產種類、分布、采礦工藝相關. 也就是說，在建設之初，巷道尺寸和擬選用的采礦裝備是相互匹配的. 而且，在地下巷道的路口處，都要根據采礦裝備的轉彎半徑進行磨角，這是一種標準操作，目的就是為了能使鉸接式的移動采礦裝備可以安全無碰撞通過路口. 因此，對于給定鏟運機以及符合其標準的路口，鏟運機一定能夠順利通過.

圖 2 地下巷道轉彎區域. （a）磨角之前的轉彎區域；（b）磨角之后的轉彎區域Fig.2 Roadway tuning area: (a) before grinding; (b) after grinding

軌跡同時包含速度和位置信息，通常來說，可以通過路口的軌跡不止一條. 當鏟運機采用不同的速度，沿不同曲率的路徑曲線，都可以安全通過路口. 例如：對于一條比較平緩的位置曲線，采用不同的速度都可以很好的沿此位置曲線行駛，但是每種速度對應了不同的軌跡. 因此，當對軌跡設置一個的評價指標后，則在可行軌跡之中，存在一個最優指標值最小的最優解，本文的目標正是尋找這一最優軌跡.

2.2 算法思想及框架

本質上，彎道區域中每條可行軌跡可以通過不同行駛里程和行駛時間表征. 因此，為了能夠根據入口速度和位置快速的生成最優軌跡，本文提出一種基于雙維度搜索策略的軌跡優化方法. 此方法核心思想是在彎道行駛區域內，通過調整軌跡的里程和時間，找到最優軌跡. 本方法采用最優化算法進行構建，總體過程是：根據里程和時間迭代策略給出的終端位置和行駛時間，計算軌跡并檢查是否符合鏟運機鉸接角物理約束，最終得到最優軌跡. 因此，算法主要包含三步：

（1）確定行駛里程和行駛時間.

此步驟的作用是確定當前軌跡規劃問題的行駛里程與時間. 設計了一種雙維度搜索策略，每次在軌跡計算前，首先通過搜索策略，從終端位置和行駛時間兩個維度進行調整，確定當前迭代的終端位置和行駛時間.

（2）軌跡生成.

在轉彎區域直角坐標系下，將軌跡分解為縱向和橫向兩個維度，分別采用二次規劃生成最優軌跡（此時并不考慮鉸接角約束）. 本算法中，橫縱向軌跡都建模為二次規劃問題，因此，所得到的軌跡在縱向和橫向兩個維度都具有最優性保證，而且求解速度快.

（3）軌跡鉸接角約束檢查及最優軌跡確定.

對于生成的軌跡，根據運動學方程計算鉸接角和鉸接角速度序列，若滿足約束，且防碰撞檢查通過，則最優軌跡找到；否則，回到第一個步驟進入下一次迭代.

使用本算法，針對一個轉彎路口，基于鏟運機進入轉彎區域的位置和速度，本文提出的方法能夠快速的找到一條縱向和橫向都具有最優性保證的最優軌跡，不需要后備策略. 第三章介紹規劃算法的具體細節.

3 軌跡規劃算法

3.1 基于雙維度搜索的終端位置和行駛時間確定

本算法中利用二次規劃構建軌跡生成模型，設計基于雙維度搜索的策略來確定優化問題的具體條件. 其中，外循環調整軌跡的行駛時間，內循環調整軌跡終端位置. 在每次循環中，首先計算縱向軌跡，然后計算橫向軌跡.

記為i外循環中的循環變量， j為內循環中的循環變量，每次循環中行駛時間 Tij通過方程（8）設置，

其中： vin是鏟運機進入轉彎區域的入口速度； Δv為速度遞減間隔.

每次循環中軌跡終端位置 (xendij,yendij)，通過如下方程設置，

其中： WB為 巷道B 寬度； Δd=(WB-2Lsafe)/m為位置遞增間隔，即將巷道B 的可行寬度進行等間隔離散后的離散長度， m為間隔離散個數.

3.2 基于二次規劃的轉彎軌跡計算

本算法中軌跡計算是基于縱向橫向解耦和二次規劃完成的. 首先生成縱向行駛軌跡，然后生成橫向行駛軌跡.

（1）縱向軌跡規劃模型.

將轉彎軌跡離散為 N 段，縱向軌跡規劃建模為一個標準的二次規劃問題，如公式（11-1）到（11-9）所示.

其中： Jlo是 縱向軌跡目標函數； Δt=Ti/N ， Ti是第i 次外循環時鏟運機行駛時間； k是軌跡點序號；λ1和 λ2為 權重系數， ε1是 松弛變量， xi是鏟運機前橋中心點縱向位置； vxi是前橋中心點縱向速度， axi和Δaxi分別是前橋中心點縱向加速度和加速度變化量， x0=0， vx0=vxin， ax0=axin；公式（11-5）表示的約束是為了令軌跡終端姿態與巷道壁平行. vxin和axin分別是鏟運機進入轉彎區域的入口縱向速度和加速度.

在本算法中，在目標函數是最小化加速度及其變化量. 沒有將目標速度考慮在優化模型之中，這是因為在彎道區域，理想的目標速度很難設定，設置不好會造成實際速度曲線發生突變，難以控制.

（2）橫向軌跡規劃模型.

橫向軌跡規劃也采用二次規劃模型，與文獻[30]中方法不同的是，此時并不考慮鉸接角相關約束，因此可以將橫向軌跡規劃模型也建立為一個標準二次規劃模型，如公式（12-1）到（12-9）所示.

其中： Jla是橫向軌跡目標函數； ρ1和 ρ2為權重系數，ε2是松弛變量， yi是鏟運機前橋中心點橫向位置；vyi是前橋中心點橫向速度， ayi和 Δayi分別是前橋中心點縱向加速度和加速度變化量， y0=yin， vy0=vyin，ay0=ayin；公式（12-5）表示的約束是為了令軌跡終端姿態與巷道壁平行. vyin和 ayin分別是鏟運機進入轉彎區域的入口橫向速度和加速度. 與縱向軌跡規劃模型設計思想相同的是，目標函數是最小化加速度及其變化量.

3.3 基于約束檢查的最優軌跡確定

上面的模型并未考慮鉸接角約束，為了保證鉸接角及其角速度符合約束，本算法設計了約束檢查環節. 根據公式（4）可以計算得到此軌跡對應的鉸接角和鉸接角速度序列. 記本條軌跡中最大鉸接角為 γi-max、最大鉸接角速度為 γ˙i-max，對于可行的軌跡，有

同時，對于最優軌跡來說，松弛變量應該為0，即

此外，還需進行碰撞檢查，本文采用的是最為簡單的膨脹法，此處不贅述.

若公式（13）、（14）和（15）同成立，且鏟運機與邊界沒有碰撞，則迭代停止，當前軌跡為最優軌跡；否則 j= j+1，返回公式（9）和（10）進行內循環；若 j=m 時都沒有獲得最優解，則 i=i+1，返回公式（8）進行外循環. 由于外循環相當于降低行駛速度，所以在延長轉彎行駛時間的情況下，一定能找到符合約束的軌跡，此時迭代停止，當前軌跡為最優軌跡. 具體流程如圖3 所示.

圖 3 雙維度搜索軌跡規劃方法流程圖Fig.3 Flow chart for the two-dimensional search-based trajectory planning method

4 算例分析與討論

為了驗證本文提出的軌跡規劃方法，設計了三組算例. 設置了一種難度較高的轉彎場景，轉彎角度為直角，且鏟運機從寬巷道向窄巷道轉彎行駛，描述了一種鏟運機從運輸巷道向開采巷道內轉彎的場景. 算例參數如表1 所示. 三組算例分別設計為：以相同的入口位置，不同的入口速度進入轉彎區域. 將入口位置分為三個等級，由于鏟運機在地下平直巷道工作時限速一般為15 km·h-1，也就是4 m·s-1左右，因此將入口速度分為四個等級，入口方向都與巷道B 保持平行. 然后，針對每個入口速度，求解其最優軌跡，求解采用Matlab quadprog函數，并將得到最優軌跡時內循環次數i、外循環的次數j 、最優軌跡對應的行駛時間 Tbest，以及此條軌跡中的最大鉸接角 γmax和 最大角速度 γ˙max列出.

表 1 算例參數表Table 1 Parameters for case studies

（1）第一組算例，入口位置為（0,1.5）, 不同入口速度的最優軌跡計算結果列表如表2 所示.

表 2 第一組算例結果Table 2 Results of the first group

（2）第二組算例，入口位置為（0,2.5）, 不同入口速度的最優軌跡計算結果列表如表3 所示.

表 3 第二組算例結果Table 3 Results of the second group

（3）第二組算例，入口位置為（0,3.5）, 不同入口速度的最優軌跡計算結果列表如表4 所示.

表 4 第三組算例結果Table 4 Results of the third group

從上面的算例可以看出，針對不同的入口速度和位置，采用所提出的規劃方法都可以得到最優軌跡.

在相同的位置，入口速度越高，迭代次數越多. 根據本算例中的問題規模，單次軌跡生成時間為0.005 s 左右，若是迭代上百次，則有可能超過控制周期（例如，當入口速度為4 m·s-1時，迭代次數為17×3×2=102 次，則得到軌跡的時間超過了0.5 s），這就會影響軌跡跟跟蹤性能. 因此，可以通過調整速度減小間隔 Δv這一迭代關鍵參數的方法來減少迭代次數. 例如，當 Δv=0.2 m·s-1時，迭代次數減少一半. 因此，本方法可以根據控制器性能，調整間隔，滿足實際控制需求.

為了更直觀顯示軌跡，以入口坐標為（0, 2.5），入口速度為2 m·s-1的情況為例，圖4～6 給出了最優軌跡的位置曲線，速度曲線、鉸接角、航向角和相應的角速度曲線. 圖6 中， vγ、 vθf和 vθr分別是鉸接角和前后車體的航向角速度. 可以看出，沿行駛方向速度、鉸接角和鉸接角速度變化平緩，適合跟蹤控制.

圖 4 位置曲線（y in=2.5,vxin =2）Fig.4 Position trajectory (y in=2.5,vxin =2)

圖 5 行駛方向速度曲線（y in=2.5,vxin =2）Fig.5 Heading velocity trajectory (y in=2.5,vxin =2)

圖 6 鉸接角、前后車體航向角及角速度（y in=2.5,vxin =2） Fig.6 Angle and angular velocity for γ , θf and θr(y in=2.5,vxin =2)

通過本文提出的方法可知， Tbest可視為以當前初始狀態及平穩目標下鏟運機通過彎道的最短時間. 因此，當找到最優軌跡之后，若將行駛時間繼續延長，也就是繼續外循環，也能找到滿足新的行駛時間的解. 同理，對于一個固定的終端位置，只采用外循環延長時間，也可得到滿足要求的最優軌跡，下面分別舉例.

對于入口位置為（0, 2.5），入口速度為2 m·s-1的情況，得到最優軌跡時，i=7，j=4，Tbest=42.84 s，軌跡在出口的終端位置為（33, 35）. 此時，若保持此終端位置，將行駛時間設置為70 s，通過本文提出的方法得到最優轉彎軌跡如圖7～9 所示.

圖 7 位置曲線（T i=70 s）Fig.7 Position trajectory (T i=70 s)

圖 8 行駛方向速度曲線（T i=70 s ）Fig.8 Heading velocity trajectory (T i=70 s )

圖 9 鉸接角、前后車體航向角及角速度（T i=70 s）Fig.9 Angle and angular velocity for γ , θf , and θr(T i=70 s)

還是以入口位置為（0, 2.5），入口速度為2 m·s-1的情況為例，當要求鏟運機行駛出轉彎區域時，位置位于巷道中心線，也就是出口位置為（32.25, 35），采用本文提出的方法，只進行外循環，得到的最優軌跡如圖10～12 所示. 此時i=9，彎道行駛時間為46.15 s.

圖 10 位置曲線（出口位置為（32.25, 35））Fig.10 Position trajectory (exit position is (32.25, 35))

圖 11 行駛方向速度曲線（出口位置為（32.25, 35））Fig.11 Heading velocity trajectory (exit position is (32.25, 35))

圖 12 鉸接角、前后車體航向角及角速度（出口位置為（32.25, 35））Fig.12 Angle and angular velocity for γ , θf , and θr (exit position is( 32.25, 35))

5 樣機試驗

為了表明算法的可行性，進行了樣機試驗. 樣機如圖13 所示. 采用激光雷達和電子地圖進行定位. 工控機為研華ARK-3500，Intel Core i5-3610ME，2.7 GHz. 試驗環境為樓道內的一個直角轉彎處，樓道寬度為2.2 m. 由于是樓道試驗，為了保證安全，最高速度設置為1 m·s-1. 試驗參數設置如表5所示. 樣機從轉彎區域外開始啟動并加速到1 m·s-1，保持此速度進入轉彎區域，車輛控制周期為0.2 s.要求行駛出轉彎區域時車輛位于巷道中線，基于本文提出的軌跡規劃方法，得到最優解時，i=1，因此軌跡生成時間在一個控制周期之內. 所生成的軌跡時間為7.2 s，速度和角度曲線如圖14 所示.路徑采用LTV-MPC 進行控制，速度采用PID 控制，樣機也平穩安全的通過了直角彎道，圖15 顯示了速度和航向角誤差，其中， Eθf為前車體航向角跟蹤誤差， Ev為鏟運機速度跟蹤誤差. 可以看出，誤差都保持在一個很小的水平.

圖 13 鉸接車試驗平臺Fig.13 Articulated vehicle

6 結論

（1）本文提出的方法可以生成縱向和橫向都具有最優性保證的軌跡，理論上保證了生成軌跡的最優性.

（2）本文提出的方法優化得到的軌跡，其縱向速度、鉸接角及角速度變化平緩，易于跟蹤控制.

（3）本文提出的算法可根據底層控制器的控制周期靈活調整迭代關鍵參數，進而調整軌跡生成時間，滿足控制器對目標軌跡生成時間的要求.

（4）本文提出的算法適用于寬窄不同的、角度不同的轉彎路口，不需要復雜的邊界處理與坐標轉換，因此便于實際部署實施.

（5）本文提出的方法既可以根據入口速度和位置生成安全平穩的最優轉彎參考軌跡；也可用于根據具體條件確定彎道最短行駛時間，因此，不僅適用于實時軌跡規劃，也可為多裝備協同運行，甚至未來礦山的智能管控和調度等問題提供系統約束.

表 5 試驗參數表Table 5 Parameters for experiments

圖 14 參考速度和航向角曲線Fig.14 Reference trajectory for velocity and heading

圖 15 速度和航向角跟蹤誤差Fig.15 Tracking Errors trajectory for velocity and heading