一種基于曲率和B 樣條的礦區裝載車輛連續路徑規劃方法①

禹鑫燚 劉逸哲 歐林林

(浙江工業大學信息工程學院 杭州310023)

礦山工作環境惡劣、危險程度高,礦山智能化是當前采礦行業發展的重要趨勢。目前,國內部分礦山已經建成了自動化、信息化的采礦系統。裝載車輛的自動駕駛是礦業智能化的重要組成部分。采用裝載車輛自動駕駛技術可以有效減少對駕駛人員健康和安全的威脅,并大幅度提高生產效率、降低生產成本,并減少事故發生的概率。

路徑規劃技術是裝載車輛自動駕駛技術研究的關鍵。傳統的路徑規劃算法包括A?算法[1-2]、人工勢場法[3]和快速擴展隨機樹(rapidly-exploring random trees,RRT)算法[4]等。這些算法通常假設車輛是能夠以任意方向運動的質點,并不考慮車輛的運動學模型,因此獲得的路徑通常難以執行。同時這類算法獲得的路徑大多數是離散點,需要和其他方法結合才能生成連續路徑。因此,許多研究人員通過將路徑規劃與車輛運動學模型相結合的方式,優化現有的傳統路徑規劃方法。Dubins[5]基于四輪車輛的運動學模型并假設車輛只能向前行駛,提出了Dubins 曲線,利用圓弧和線段獲得了在滿足曲率約束和規定的始端和末端切線方向條件下的最短路徑。Reeds 和Shepp[6]在此基礎上提出了Reeds-Shepp 路徑,允許車輛在執行路徑規劃的時候執行后退的動作。Fraichard 和Scheuer[7]繼續對該類型的路徑進行優化,在線段和圓弧段之間插入了一段曲線,保證了曲率的連續變化。Dmitri 等人[8]將Reeds-Shepp 路徑和A?算法相結合,提出了混合A?算法?；旌螦?規劃的路徑考慮了車輛的運動學約束,使得A?算法滿足了車輛的最大曲率約束。這些方法保證了生成路徑的連續性,同時也保證了車輛的運動學約束。為了獲得更平滑的路徑,一些路徑規劃方法引入了曲線插值方法,如多項式曲線[9]、回旋曲線[10]、貝塞爾曲線[11]和B 樣條[12]。李玄赫[13]提出回旋曲線-圓弧-直線的路徑規劃方法,解決了車輛在窄車位的情況下自動泊車的問題。錢東海等人[14]利用三次B 樣條曲線確定了滿足自動導向車(automated guided vehicle,AGV)運動學約束的最短路徑,使AGV 能夠從偏移位置回到規劃的連續路徑上。Wu 等人[15]將B 樣條和人工勢場算法相結合,利用B 樣條曲線對路徑進行平滑處理,獲得了較好的結果。Zeng 等人[16]討論了B 樣條控制點夾角對路徑曲率的影響,并將其與RRT 算法相結合,使得車輛變道時可以實時生成連續路徑。Mohamed 等人[17]利用B 樣條實現了一種具有連續轉向運動路徑的算法,保證了生成的路徑符合類汽車移動機器人的運動學約束。礦區裝載車輛,在進入裝載點前,車輛需要在回轉點改變行進方向,以便倒車進入裝載點。鮑久勝等人[18]利用改進A?算法和人工勢場算法對無軌膠輪車在井下執行任務進行路徑規劃,并利用三次樣條插值方法進行路徑平滑。在目前研究中,針對礦區環境下滿足上述特殊任務的路徑規劃問題還未得到很好的解決。

本文針對礦區裝載車輛在礦區工作過程中自動駕駛的路徑規劃問題,提出了一種新的路徑規劃方法。首先根據車輛的運動學模型,提出了一種基于曲率優化的A?算法,獲得符合車輛動態運行特性的離散路徑。為了進一步求取連續的路徑,基于車輛在初始位置和終點的位姿,提出了一種改進的B樣條曲線插值方法,保證了路徑兩端的車輛方向。另外,為了確定車輛的回轉點,利用模擬退火算法,結合初始路徑,獲得合適的回轉點位置。最后,仿真實驗證明了算法的有效性。該方法能夠生成無碰撞的連續曲率路徑,且保證了在路徑兩端的切線符合車輛的起止位姿。同時還可以有效找到一個回轉點,使得車輛可以在回轉點改變行進方向。

1 問題描述

礦場內裝載車輛的基本功能是從礦場入口處駛入工作區域,然后在裝載點進行裝載,裝載完畢后從離去點離開礦場,其工作示意圖如圖1 所示。

圖1 裝載車輛礦區工作示意圖

在一般情況下,裝載車輛需倒車駛入裝載區,裝載車輛必須在進入裝載區之前改變方向。因此,需要找到一個回轉點,使得車輛可以在回轉點使運動方式從前進改為倒車。本文要解決的主要問題是,獲得裝載車輛的連續路徑,使其滿足以下2 個條件:(1)確定車輛出發點和離去點之間的路徑,該路徑需要連續且能避開環境障礙;(2)確定裝載車輛的回轉點,使得車輛可以在回轉點處改變行進方向。

2 基于曲率和B 樣條的連續路徑規劃設計方法

本文提出的裝載車輛路徑規劃方法主要包括基于曲率優化的A?算法設計、兩端導數約束的B 樣條連續路徑獲取以及回轉點確定?；谘b載車輛的運動學模型設計成本函數,對A?算法進行改進,獲取裝載車輛從出發點到裝載點的離散路徑點的運動路徑,進一步,將利用兩端導數約束的B 樣條路徑進行路徑插值,然后利用改進B 樣條插值求出連續路徑,之后在求出連續路徑的情況下利用模擬退火算法求出回轉點。最后利用A?算法和改進B 樣條求出各點之間的連續路徑。

2.1 基于曲率優化的改進A?算法

A?算法是一種啟發式的路徑規劃算法,常用于解決靜態路網求取最短路徑的問題。相對于Dijkstra 算法,A?算法加入了啟發信息進行搜索方向引導,提高了路徑搜索效率。A?算法的成本公式為

其中,f(n) 為A?算法的路徑成本,g(n) 為初始位置到位置n所需的最小路徑成本,h(n) 為位置n到最終位置的估計路徑成本。常見的A?算法中,2點間路徑成本通常定義為2 點間的歐氏距離,即2點之間的直線距離。為了使得裝載車輛能夠準確跟蹤所生成的路徑,本文基于裝載車輛運動路徑的曲率對成本函數式(1)進行了設計。

考慮如圖2 所示的裝載車輛運動模型,車輛只有前輪可以轉彎。其中,(x,y) 為車輛的位置,定義為車輛后輪的中心;θ為車輛相對于x軸的方向;ρ為車輛轉彎半徑;L為車輛的軸距長度;?為車輛的轉向角。

圖2 車輛運動模型

定義曲率κ為車輛轉彎半徑的倒數,可得:

對式(2)進行微分,可得:

當?足夠小的時候,可以認為tan?≈?,cos?≈1,因此式(3)可以近似為

在路徑規劃過程中,希望路徑的曲率不發生過大的變化,因此根據式(4),定義如下的路徑成本函數,其中ω1和ω2為大于0 的常數參量。

其中,θi為路徑中的第i個點所對應的車輛位姿,Δsi為第i段路徑。根據中心差分的前向差分公式和后向差分公式,可求得前向差分公式為

在點n處的后向差分公式為

將式(10)作為路徑曲率成本應用于A?算法中,代替原來的路徑距離成本,可以得到初始位姿到點n的已知路徑曲率成本g(n) 和點n到終點位姿的路徑曲率成本h(n)。假設點k為目標點,則可以得出A?算法中的路徑成本公式需修改為

式(11)顯示,在本文中,路徑成本即為路徑的曲率成本。其中g(n) 為起始點到當前探索節點n的最小路徑曲率成本,h(n) 為當前探索節點n到最終位置k的預估路徑曲率成本。

2.2 兩端導數約束的B 樣條路徑

2.1 節中利用改進A?算法求取了車輛的運動路徑,并獲得了車輛運動路徑的離散點,在本節中,將利用兩端導數約束的B 樣條進行路徑插值,獲得連續路徑。

一般情況下,p次B 樣條通常寫成如下形式:

其中,Pi為對應控制點;Ni,p(u)為p次基樣條;r(u) 為對應值的B 樣條曲線上的插值點,m為生成B 樣條所需的控制點數目;u為B 樣條基樣條求取的插值參數,u∈[0,1)。由于生成的路徑要保持曲率連續,因此需要生成的B 樣條保證C2 連續,即生成曲線的二次導數為連續,故至少要使用三次B 樣條,因此p取值為3。假設所生成的離散路徑點數目為n,且生成的B 樣條函數必須經過兩端點,則根據非均勻有理B 樣條的基本性質,所需控制點數目為m=n+p +1=n +4。

根據式(12),為了獲得B 樣條曲線,需要獲得B 樣條的基樣條。利用De-Boor 遞推式可以獲得任意階數的B 樣條基函數。De-Boor 遞推式為[20]

其中,ui為節點,且ui

假定數據點Qi為根據改進A?算法獲得的路徑點。首先,根據弦長法,將弦長參數τi賦給每一個數據點Qi:

其中,d為整段路徑的距離,n為路徑點數目。在獲得弦長參數τi后,定義出實數序列U中的節點ui。根據NURBS 的定義[20],為了獲得三次B 樣條,需要在B 樣條的起點和終點處重復4 次節點。因此實數序列U可以定義為

其中,m為生成實數序列中的節點數目,τi為弦長參數中第i個弦長。

常規的B 樣條曲線并沒有考慮曲線兩端的導數約束,因此生成的路徑并不能反映車輛的真實運動軌跡。因此需要在B 樣條兩端考慮車輛位姿的約束,使得生成的軌跡符合車輛的運動軌跡。假設車輛在路徑兩端都是無轉向動作的,因此可以在軌跡兩端用導數來表示車輛位姿約束。

將路徑兩端車輛運動方向表示為向量D0和Dn。在2 個端點處的車輛方向向量由B 樣條導數決定。對于實際路徑,車輛在進入點和離開點都為直線運動,不存在轉彎,因此在這2 個點上車輛的曲率為0。對于任意曲線上的一點,其曲率公式被定義為

其中y為曲線上對應點的具體數值。因此在該點上的B 樣條曲線二次導數為0。為此,需要求取B 樣條2 端點處的一次導數和二次導數。B 樣條導數公式為

其中,j為導數次數,m為控制點數目,r(j)(u) 為B樣條對應插值點上的j次導數值,(u) 為基樣條的j次導函數,Pi為B 樣條控制點,u為B 樣條基樣條的求取插值參數且u∈[0,1)?；鶚訔l的j次導函數(u) 的求取公式為

其中p、j、u的含義同式(17)一致,ui為對應的實數序列中第i個節點。通常情況下,LU 分解被用來求取B 樣條的控制點[20],可以將起始點和終點的導數約束寫入求取控制點的公式中,改進B 樣條求取控制點的公式如式(19)所示。在式(19)中,為了獲得最優的B 樣條路徑,需要優化參數α和β。

由于生成B 樣條時需要考慮生成路徑的曲率,同時希望生成路徑的曲率的變化率不要過大,故B樣條的成本公式需要考慮曲率對路徑的積分以及曲率對路徑導數對路徑的積分。因此,定義如下的B樣條成本公式:

其中κ為曲率,ω1和ω2為大于0 的常數參量。根據式(15),可以定義出B 樣條上任意一點的曲率公式和曲率導數公式如式(21)和(22)所示。

利用高斯-勒讓德積分公式對式(20)進行評估,并利用Powell 方法對評估后的公式做最小化處理,可以獲得α和β的值。

在求取完B 樣條后,需要對B 樣條中的插值點進行檢測,以防止車輛路徑和障礙物發生碰撞。首先,對障礙物進行Minkowski 和擴展,得到膨脹區域,定義當路徑點經過膨脹區域內,就認為車輛在該點處和障礙物發生碰撞。隨后從B 樣條初始位置的點開始每隔10 個采樣點就選取1 個采樣點進行檢測,當發現在該點處車輛會和障礙物發生碰撞之時,將該點沿法線方向移動一個車輛軸距,如果仍舊發生碰撞,則繼續向外移動,直至該點不和障礙物發生碰撞。為了簡化計算,定義B 樣條路徑上的采樣點的法線的方向為pi到pi-10的向量與pi到pi+10的向量之和的方向,其中pi為B 樣條上第i個采樣點。之后,將該點作為新的中間點加入離散路徑。在加入離散路徑后對該點進行優化。以中間點為圓心繪制2 個圓,圓半徑分別為3/4 軸距和1/2 軸距。在2 圓上分別以等圓心角取8 個點,根據式(11)計算每個點的成本,并將其移動到成本最小的定位點上。然后重新以其點作為圓心重復上述步驟。B 樣條連續路徑生成的流程圖如圖3 所示。車輛從起始點到終點生成連續路徑的路徑規劃整體算法如算法1所示。

圖3 B 樣條連續路徑生成流程圖

2.3 回轉點的求取

一般情況下,裝載車輛會以后退的方式進入裝載區,因此,它必須在進入裝載區之前在回轉點改變方向。車輛在回轉點前后的路徑如圖4 所示。

圖4 車輛在回轉點前后的路徑

車輛回轉點由3 個參數確定,分別為(x,y,θ),其中x、y為車輛的位置坐標,θ為車輛的姿態角度?；剞D點的成本根據下式進行計算:

其中Es→t為起始點到回轉點的路徑成本函數,Et→l為回轉點到裝載點的路徑成本函數,ω1和ω2為大于0 的常數參量。路徑成本函數由式(11)估算決定。在理想情況下,回轉點應當盡量不偏離原路徑,因此基于2.3 節中所得到的連續路徑尋找裝載點。如圖5 所示,從裝載點處回溯長度為b的距離,然后將這個點沿垂直于曲線凸邊的向量移動長度為a的距離,將移動之后的點確定為回轉點。將回轉點的方向確定為垂直于曲線凸邊的向量的方向。

圖5 回轉點的確定

模擬退火算法模仿了玻璃退火的物理過程,相較于貪心算法,其具有一定概率跳出局部最優解,可以有效求出整體最優解。因此,本文中利用模擬退火算法[21]來獲得最優的外移距離a和回溯距離b。首先確定初始迭代值。本文中,規定初始回溯距離b等于5 倍車輛軸距長度,外移距離a等于車輛軸距長度。隨后對回溯距離b和外移距離a進行隨機擾動,即:

為了防止回轉點偏離裝載點太遠,在本文中,規定amax為5 倍車輛軸距,bmax為10 倍車輛軸距,都要進行距離檢測和碰撞檢測。當發現回溯距離b大于整體線段長度或者在該組數據下生成的點可能發生碰撞,則將該組數據作為無效數據舍棄,該次迭代的數據仍為原數據。

根據模擬退火算法,當在一個溫度下執行固定次數迭代或達到平衡時,在該溫度下的熔化過程停止,降低溫度后繼續進行迭代,直至溫度小于最低溫度,停止迭代。式(25)給出了降溫的過程。

其中Ti為第i次迭代所到達的溫度。

相較于典型的尋找回轉點的方法[17],本文所提出的方法僅需尋找2 個參數,而不是直接尋找回轉點(x,y,θ),使得生成點可以直接插入離散路徑中,不必在每一次進行成本估計的時候先進行路徑搜索再預估成本,有效降低了模擬退火算法循環過程中的單次運行的時間。在單次循環中,三參數模擬退火算法的復雜度為Ο(n3),而本文算法中單次循環復雜度為Ο(n)。

礦區車輛在執行裝載任務過程中的整體路徑規劃的算法如算法2 所示。

3 仿真

為了驗證路徑規劃的有效性,本文對所提出的裝載車輛路徑規劃方法進行了驗證。假設裝載車輛從進入點進入礦場,前往裝載點進行裝載,裝載完畢后從離去點離開礦場。由于車輛進入裝載點進行裝載時需要倒車進入,因此需要在進入裝載點之前改變車輛的行進方向。因此,路徑規劃算法的主要任務是:(1)找到兩點之間的曲率連續的無碰撞路徑;(2)找到一個回轉點使得車輛可以調轉行駛方向。

3.1 路徑規劃算法的對比

實驗1 選取了A?算法和Dubins 算法與本文提出的路徑規劃算法進行對比。這是2 種典型的路徑規劃算法,在路徑規劃過程中有廣泛的應用。將模型導入仿真軟件,并確定車輛的起止位姿。其中地圖尺寸為1 300 ×450,車輛起始位姿為(1 105,206,120 °),3 個參數分別代表車輛位置(x,y)和車輛姿態角θ。車輛的終止位姿為(306,205,-90 °)。隨后分別利用A?算法、Dubins 算法和本文所提出的算法進行對比,生成的路徑如圖6 所示。其中實線為A?算法生成的路徑,點線為本文算法生成的路徑,虛線為Dubins 算法生成的路徑。

圖6 3 種路徑規劃算法的對比

由于無需進行路徑平滑插值,A?算法所花費的時間最少,但是A?算法生成的路徑為離散折線路徑,且生成的路徑并不符合車輛在起始點和終點的位姿。Dubins 算法和本文算法都可以生成連續路徑,且生成的路徑符合車輛在起點和終點的位姿。同時,Dubins 算法生成連續路徑的時間為0.334 19 s,本文算法生成連續路徑的時間為0.379 32 s,可見本文算法和Dubins 算法在計算時間上沒有明顯的差距。為此,本文對比了本文算法和Dubins 算法生成路徑的曲率變化,2 種算法生成的連續路徑的曲率變化圖如圖7 所示。

圖7 Dubins 算法和本文算法生成路徑的曲率對比

由圖7 可見,Dubins 曲線生成的路徑會在圓弧和直線的交接處發生曲率突變的現象,且在起始位姿和終止位姿處曲率皆不為0,而本文所生成的路徑曲率連續變化,不會發生突變現象,且在起始位姿和終止位姿處的曲率皆為0,減少了車輛的輪胎磨損。

3.2 礦區全流程連續路徑獲取

實驗2 利用障礙物較為稀疏的情況來演示算法的全流程。

首先,將礦場數據和障礙物數據導入仿真軟件中,地圖尺寸為1 300 ×450。并設置車輛的進入點位姿,裝載點位姿和離去點位姿。車輛進入點位姿為(1 206,285.5,-120°),車輛裝載點位姿為(306,276,0 °),車輛離去點位姿為(1 056,354,30 °)。礦場地圖如圖8 所示,其中黑色表示礦場邊界和障礙物,虛線邊框為利用Minkowski 和生成的膨脹區域邊界。當點位于膨脹邊界當中時,可以視為車輛和障礙物發生碰撞。

圖8 礦場地圖

隨后,利用2.2 節提出的改進A?算法和2.3節提出的B 樣條1 插值法生成車輛從進入點到裝載點的路徑,如圖9 所示。需要注意的是,在生成該路徑時,為了方便之后回轉點的求取,默認車輛在裝載點的朝向為車輛在裝載點位姿的反方向。

圖9 車輛從進入點到裝載點的B 樣條路徑

然后,利用2.4 節的方法獲得回轉點,并利用改進A?算法和B 樣條獲得裝載車輛全流程的路徑。生成的全流程連續路徑,如圖10 所示?？梢娫撍惴梢陨蔁o碰撞路徑且能找到一個回轉點,能夠較好地完成任務。需要注意的是,由于車輛是從回轉點倒車進入裝載點,因此在求取回轉點到裝載點路徑的時候算法輸入的位姿方向為兩點處的車輛位姿的反向。

圖10 全流程連續路徑

同時進行了2 種回轉點搜索算法搜尋回轉點的對比,2 種算法都可以較好地搜索出如圖10 所示的回轉點,但是文獻[17]的三參數模擬退火算法單次搜索時間明顯長于本文的算法,本文算法單次循環平均時間為0.02 s 左右,而文獻中所示的模擬退火算法單次循環的平均時間在9.8 s 左右。

實驗2 所生成路徑的曲率如圖11 所示。由圖11可見,本文提出的算法所生成的路徑為曲率連續變化的曲線,且在起始點和終點處的曲率為0,而Dubins 曲線在圓弧和直線相交的部位會出現曲率突變的現象,且在生成的路徑兩端曲率為最大。因此本文所提出的路徑可以較好地完成裝載車輛的路徑規劃任務。

圖11 三段連續路徑曲率變化

3.3 密集障礙物情況下的連續路徑規劃

實驗3 考慮障礙物較為密集的情況。將礦區邊界數據和障礙物數據導入仿真軟件,地圖尺寸為700 ×350。其中,車輛初始點位姿為(106,204,-15 °),車輛裝載點位姿為(405,96.3,90 °),車輛離去點位姿為(506,193.6,-30 °)。在障礙物密集的情況下,所獲得的全流程連續路徑如圖12 所示,生成的3 段連續路徑的曲率變化如圖13 所示。仿真結果表明,即使障礙物之間存在間隔較窄的瓶頸區或者裝載點附近較為狹窄的情況下也可以規劃出全流程路徑,且曲率變化連續。

圖12 障礙物較為密集的情況下的全流程路徑

圖13 三段連續路徑的曲率變化

4 結論

本文針對礦區環境下裝載車輛的路徑規劃問題進行了研究,設計了一種基于曲率和B 樣條的路徑規劃算法?；谲囕v的運動學模型改進了A?算法的成本函數,保證了生成的離散路徑更接近車輛可執行的最優離散路徑?；诖穗x散路徑,通過改進三次B 樣條曲線規劃出裝載車輛的連續路徑,保證了生成路徑兩端的切線方向符合車輛的位姿。并考慮裝載車輛的任務特點,基于模擬退火算法確定了裝載車輛在礦場中的回轉點位置。仿真實驗結果表明,本文提出的連續路徑規劃方法可以規劃出質量較高的曲率連續路徑,且能求出合理的回轉點,并且在障礙物較為密集的情況下依然有效,因此本文所提出的連續路徑規劃方法可以有效地為自動駕駛裝載車輛提供曲率連續的無碰撞路徑。