基于雙維度搜索的地下自主鏟運機最優(yōu)轉(zhuǎn)彎軌跡規(guī)劃

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

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

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

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

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

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

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

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

文章結(jié)構(gòu)如下，第一章介紹鏟運機運動學(xué)模型、轉(zhuǎn)彎區(qū)域模型及所研究的具體問題，第二章介紹軌跡規(guī)劃方法框架，第三章介紹軌跡規(guī)劃方法數(shù)學(xué)模型，第四章和第五章是算例分析與樣機試驗，驗證本文提出的方法有效性和優(yōu)越性，第五章總結(jié)結(jié)論.

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

1.1 鏟運機運動學(xué)模型

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

圖 1 LHD 結(jié)構(gòu)Fig.1 Structure of an LHD

PfPr和 之間的位置關(guān)系可以表示為

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

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

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

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

1.2 彎道區(qū)域模型

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

1.3 問題描述

當(dāng)鏟運機進入轉(zhuǎn)彎區(qū)域時，根據(jù)鏟運機當(dāng)前速度、位置，實時生成一條平穩(wěn)軌跡，使鏟運機能夠在安全轉(zhuǎn)彎的情況下，平穩(wěn)的行駛出轉(zhuǎn)彎區(qū)域.在軌跡規(guī)劃時，要滿足下列約束條件：

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

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

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

（4）在駛出轉(zhuǎn)彎區(qū)域時，前車體方向與巷道壁平行.

2 軌跡規(guī)劃算法框架

2.1 最優(yōu)軌跡存在性分析

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

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

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

2.2 算法思想及框架

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

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

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

（2）軌跡生成.

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

（3）軌跡鉸接角約束檢查及最優(yōu)軌跡確定.

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

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

3 軌跡規(guī)劃算法

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

本算法中利用二次規(guī)劃構(gòu)建軌跡生成模型，設(shè)計基于雙維度搜索的策略來確定優(yōu)化問題的具體條件. 其中，外循環(huán)調(diào)整軌跡的行駛時間，內(nèi)循環(huán)調(diào)整軌跡終端位置. 在每次循環(huán)中，首先計算縱向軌跡，然后計算橫向軌跡.

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

其中： vin是鏟運機進入轉(zhuǎn)彎區(qū)域的入口速度； Δv為速度遞減間隔.

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

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

3.2 基于二次規(guī)劃的轉(zhuǎn)彎軌跡計算

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

（1）縱向軌跡規(guī)劃模型.

將轉(zhuǎn)彎軌跡離散為 N 段，縱向軌跡規(guī)劃建模為一個標(biāo)準(zhǔn)的二次規(guī)劃問題，如公式（11-1）到（11-9）所示.

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

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

（2）橫向軌跡規(guī)劃模型.

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

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

3.3 基于約束檢查的最優(yōu)軌跡確定

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

同時，對于最優(yōu)軌跡來說，松弛變量應(yīng)該為0，即

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

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

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

4 算例分析與討論

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

表 1 算例參數(shù)表Table 1 Parameters for case studies

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

表 2 第一組算例結(jié)果Table 2 Results of the first group

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

表 3 第二組算例結(jié)果Table 3 Results of the second group

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

表 4 第三組算例結(jié)果Table 4 Results of the third group

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

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

為了更直觀顯示軌跡，以入口坐標(biāo)為（0, 2.5），入口速度為2 m·s-1的情況為例，圖4～6 給出了最優(yōu)軌跡的位置曲線，速度曲線、鉸接角、航向角和相應(yīng)的角速度曲線. 圖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可視為以當(dāng)前初始狀態(tài)及平穩(wěn)目標(biāo)下鏟運機通過彎道的最短時間. 因此，當(dāng)找到最優(yōu)軌跡之后，若將行駛時間繼續(xù)延長，也就是繼續(xù)外循環(huán)，也能找到滿足新的行駛時間的解. 同理，對于一個固定的終端位置，只采用外循環(huán)延長時間，也可得到滿足要求的最優(yōu)軌跡，下面分別舉例.

對于入口位置為（0, 2.5），入口速度為2 m·s-1的情況，得到最優(yōu)軌跡時，i=7，j=4，Tbest=42.84 s，軌跡在出口的終端位置為（33, 35）. 此時，若保持此終端位置，將行駛時間設(shè)置為70 s，通過本文提出的方法得到最優(yōu)轉(zhuǎn)彎軌跡如圖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的情況為例，當(dāng)要求鏟運機行駛出轉(zhuǎn)彎區(qū)域時，位置位于巷道中心線，也就是出口位置為（32.25, 35），采用本文提出的方法，只進行外循環(huán)，得到的最優(yōu)軌跡如圖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. 試驗環(huán)境為樓道內(nèi)的一個直角轉(zhuǎn)彎處，樓道寬度為2.2 m. 由于是樓道試驗，為了保證安全，最高速度設(shè)置為1 m·s-1. 試驗參數(shù)設(shè)置如表5所示. 樣機從轉(zhuǎn)彎區(qū)域外開始啟動并加速到1 m·s-1，保持此速度進入轉(zhuǎn)彎區(qū)域，車輛控制周期為0.2 s.要求行駛出轉(zhuǎn)彎區(qū)域時車輛位于巷道中線，基于本文提出的軌跡規(guī)劃方法，得到最優(yōu)解時，i=1，因此軌跡生成時間在一個控制周期之內(nèi). 所生成的軌跡時間為7.2 s，速度和角度曲線如圖14 所示.路徑采用LTV-MPC 進行控制，速度采用PID 控制，樣機也平穩(wěn)安全的通過了直角彎道，圖15 顯示了速度和航向角誤差，其中， Eθf為前車體航向角跟蹤誤差， Ev為鏟運機速度跟蹤誤差. 可以看出，誤差都保持在一個很小的水平.

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

6 結(jié)論

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

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

（3）本文提出的算法可根據(jù)底層控制器的控制周期靈活調(diào)整迭代關(guān)鍵參數(shù)，進而調(diào)整軌跡生成時間，滿足控制器對目標(biāo)軌跡生成時間的要求.

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

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

表 5 試驗參數(shù)表Table 5 Parameters for experiments

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

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