基于Dubins曲線的局部路徑規劃方法研究

李勝琴，閆祥偉

(東北林業大學 交通學院，哈爾濱 150040)

0 引言

近幾年，無人駕駛技術逐漸成為研究熱點。無人駕駛技術是系統性問題，主要涉及環境感知、高精度定位、運動預測、車輛控制和路徑規劃等。要使傳統汽車具備全場景的無人駕駛能力，不僅需要保證各個模塊功能的正常，更需要各模塊之間的協調。所以至今全場景的無人駕駛技術仍處在研發測試階段，而針對封閉園區、礦區和港口等特定場景，無人駕駛技術已相對成熟。其中礦山智慧化已成為當前采礦業發展的重要趨勢。在整個智慧礦山系統中，礦區車輛的路徑規劃是關鍵問題之一。路徑規劃分為全局路徑規劃與局部路徑規劃，全局路徑由云平臺下發到車端，而局部路徑的規劃則由車載計算平臺實時計算產生。在無人自動駕駛系統中，路徑規劃是關鍵環節之一，上游承接感知模塊，下游承接控制模塊，其負責在真實的道路環境中，按照既定的約束條件(如無碰撞、路程最短和時間最短等)尋找一條從開始位置到目標位置的最優路徑[1-2]。

目前路徑規劃按不同算法可分為圖搜索類算 法[3-4]、采樣類算法[5-6]和人工勢場類算法[7-9]等，不同的算法適用的范圍都有所不同。馮凱等[10]在基于無人駕駛高精度地圖的基礎上，通過 A*算法進行了全局路徑規劃的設計與實現，但此方法需要在已建立的高精度地圖基礎上實現，對于結構化道路場景，無法滿足實時規劃。李全勇等[11]提出了改進的Dijkstra 算法，引入路徑平滑度，以時間權重為優化目標求解最優路徑，但其并沒有考慮障礙物距離權重對規劃結果的影響。劉冰雁等[12]改進了人工勢場函數,調整勢場作用區域，構建了以遠距點斥力忽略、障礙點引力減弱的綜合勢場模型，解決了傳統人工勢場法容易陷入局部極小值的問題，但是并沒有對生成的軌跡點進行優化處理。孫上杰等[13]針對強化學習算法收斂速度慢，易產生“維度災難”的問題提出一種深度學習與強化學習相結合的算法，用于解決6足森林消防機器人的路徑規劃問題。唐文秀等[14]對于移動機器人立體視覺導航的全過程進行研究，分析立體視覺測量信息到真實三維景物信息的計算、人工勢場法進行路徑規劃這兩項關鍵技術。Shkel等[15]提出基于 Dubins路徑的分類方案，能夠在有方向約束的2點直接找到最短路徑的方法，該方法不需要顯式計算候選路徑，在實時性上可以滿足無人車實時路徑規劃。

本研究針對礦區場景，基于Dubins曲線，提出一種無碰撞、平滑的局部路徑規劃算法。通過選取Dubins集合中的4種曲線類型，計算預選路徑集合，然后建立車輛可行域模型，對預選路徑集合做碰撞檢測，提取無碰撞的路徑，之后基于分離軸理論，對無碰撞的路徑做精確的碰撞檢測。并設計評價函數，在無碰撞的路徑集合中，結合規劃中的影響因素，求解目標路徑。最后利用B樣條曲線對生成的軌跡做平滑處理，從而獲得一條相對最優的局部路徑。

1 基本問題描述

礦區的場景相對公開道路場景，道路信息較為多變。礦區的道路隨著“采”“排”作業的進行會不定期發生變化，尤其是采掘面和排土場變化頻率很大，云平臺進行全局路徑規劃后，礦卡按照路徑任務進行循跡行駛，在采掘面和排土場，或者前方存在障礙物需要繞行時，礦卡根據信息決策模塊輸入的信息，對可行駛區域進行局部路徑規劃[16]。礦卡在采掘面和排土場的局部路徑規劃上的問題，可以描述為在目標路徑與起點之間規劃一條局部最優路徑的問題，其中目標路徑是由云平臺全局路徑規劃產生,由n個等距的點p(x,y,θ)描述,點之間的間隔為0.2 m,如圖1所示,θ為目標路徑上軌跡點的航向。圖1中的無人駕駛車輛路徑規劃問題可以歸納為：在已知周圍障礙物信息、目標路徑信息和車輛參數信息的條件下，求解起點到目標路徑點的路徑，且該路徑須滿足車輛運動約束、無碰撞、路徑平滑的條件。

圖1 場景描述Fig.1 Scene description

Dubins曲線的路徑集合D={LSL，RSR，RSL，LSR，RLR，LRL}，包含平面內2個指定運動方向的點之間的最短路徑[15]。其中，L表示沿逆時針方向的圓弧路徑段；R表示沿順時針方向圓弧路徑段，S表示沿直線運動。圖2為D集合中的一種，pi(x0，y0,α)表示初始位姿，pf(x1，y1,β)表示目標點位姿，兩圓半徑r相等，其中α、β是起點與目標點切線方向與X軸的夾角。

圖2 RSL曲線Fig.2 RSL curve

Dubins曲線有6種類型，如圖3所示。其中RLR、LRL曲線類型，由于不包含直線路段且圓弧路段曲率過大，使得無人車控制難度增大。在軌跡跟隨過程中，應盡量避免大曲率的路徑出現，防止在跟隨路徑的過程中，出現橫向偏差過大情況。所以在計算路徑時，僅需要求解LSL、RSR、RSL、LSR這4種曲線，再從中選取滿足約束的路徑作為最優解。

圖3 Dubins曲線類型Fig.3 Dubins curve

2 基于Dubins曲線的路徑規劃方法

2.1 路徑求解

α=γ-θ。

(1)

β=δ-θ。

(2)

(3)

(4)

式中:γ和δ分別為起點、終點與X軸的夾角。通過公式(4)單位化，將轉彎半徑單位化r= 1，這樣處理可簡化后續的計算工作。為了簡化路徑的求解，引入3個算子[13]Lv(x，y，φ)、Sv(x，y，φ)、Rv(x，y，φ)。

(5)

式中：v為在L上所經過的路徑長度；φ為在曲線上經過的弧度；x、y為在旋轉后的坐標系中路徑點的坐標。建立Rq(Sp(Lt(0， 0，α))) = (d，0，β)等式關系，將上述算子帶入，則有如下關系式。

pcos(α+t)+2sin(α+t)-sinα-sinβ=d。

(6)

psin(α+t)-2cos(α+t)+cosα+cosβ=0 。

(7)

α+t-q=β{mod 2π}。

(8)

聯立式(5) (6) (7)可得到L的隱式解

(9)

(10)

(11)

則L=α-β+2t+p。

圖4 求解RSL型Dubins曲線Fig.4 Calculate RSL Dubins curve

同理可求得LSL、RSR、LSR的路徑長度L，這里不再贅述。將L反歸一化，則可得到目標路徑長度。

上述求解的結果為路徑集合，其中每一條路徑由圓弧和直線組成。對于點對點可能產生的若干種行駛路徑，設計損失函數如下

cost=wc×t+ws×p+wc×q。

(12)

其中wc和ws表示曲線段和直線段的權重。通過對比路徑集合中每一條路徑的損失函數值，篩選出cost值最小的路徑。該路徑作為備選路徑，再進行反求得到路徑點，作為碰撞檢測的輸入。

設計圖5的規劃場景，該場景包含一條真實的目標路徑以及規劃的起始點，起點的GPS坐標為(41.802 924 5，109.994 822 1，337.913)。目標路徑上的軌跡點作為規劃的終點，并設置4種車輛的轉彎半徑作為算法的輸入參數，即轉彎半徑r1、r2、r3、r4分別為16、21、26、31，對上述算法進行驗證。

圖5 規劃場景Fig.5 Scene of planning

由圖5可以看出，灰色的多邊形表示4個靜態障礙物，十字交叉線段的交點表示設置的規劃起始點，長邊的指向為起始點的航向(與正北方向的夾角)。選取目標路徑上一點，坐標為(14.393 9， 103.905 0)索引為14。以半徑r3為轉彎半徑，將設置好的參數輸入到編寫的算法程序中，得到表1中的結果。

表1 成本(cost)值與路徑參數Table1 Value of cost and path parameters

表1的數據表明，當成本即損失函數(cost)值最小時，計算的總路徑長度最短，曲線路徑長度最短。驗證了本研究設計的損失函數可以有效地從Dubins解集中初步篩選符合期望的路徑，減少后續碰撞檢測的數據量，提高算法的運行效率。

2.2 軌跡點求解

軌跡點由p(x，y，ε)表示，其中ε表示每一個路徑點的航向，x和y表示軌跡點在車體坐標系中的坐標。設置起點pi(0，0，α)= (0，0，π/2)。取每個路徑點(xpi，ypi)之間的距離為0.2 m,則L=0.2×i，其中i表示路徑點的索引。在計算路徑軌跡點之前，首先需要計算p段的起點p1(x1，y1，θ1)和終點p2(x2，y2，θ2)。

(13)

(14)

將L歸一化，以便于軌跡點的求解。

(15)

當l

(16)

當t

(17)

當t+p

(18)

將點p(x，y，ε)反歸一化，則可得到路徑上的軌跡點，如圖6所示。

圖6 規劃結果Fig.6 Result of planning

3 碰撞檢測

前文計算的路徑結果是在沒有考慮環境中障礙物的條件下得到的。為了兼顧無人駕駛車的行駛安全性，對規劃的路徑進行碰撞檢測，剔除可能與車輛發生碰撞的路徑，保障智能車輛行駛的安全。出于提高碰撞檢測算法效率和準確性的考慮，將碰撞檢測算法設計為分層檢測。首先將無人駕駛車周圍環境中的障礙物依次輸入到可行域中進行判斷，忽略不在可行域范圍內的障礙物。然后對可行域范圍內的障礙物進行精確地碰撞檢測，如果發生碰撞，則更改路徑，重復上述步驟，直至找到無碰撞的安全路徑。設計的無人駕駛車可行域模型，僅需要障礙物與無人駕駛車之間的距離，即可初步判斷是否會與車輛發生碰撞。對于可能發生碰撞的障礙物，基于分離軸理論對其進行精確碰撞檢測，進一步判斷障礙物是否影響無人駕駛車的行駛安全。

3.1 無人駕駛車可行域模型建立

無人駕駛車可行域模型基于車輛單軌運動學模型[17]建立。無人駕駛車可行域模型如圖7所示。圖7為當θ>0時的可行域模型。

(19)

(20)

(21)

(22)

(23)

(24)

(25)

式中：Rl和Rr分別為車輛左右兩側最小轉彎半徑；lm表示車輛后視鏡的長度；dr為車輛尺寸的膨脹值。

圖7 可行域模型Fig.7 Vehicle feasible region model

圖7中三角形表示障礙物，位置為Q(xp，yp)。O1(xo，yo)為轉向圓心。則車輛的可行駛域為[xl，xr]。

(26)

當xl≥xp+dp/2或xr≤xp-dp/2時，障礙物不在車輛可行域內，不需要考慮該障礙物。dp為障礙物的多邊形包圍盒在坐標系YOX的X軸上的投影。

通過建立無人駕駛車可行域模型，可以僅通過障礙物距離無人駕駛車的縱向距離，快速地排除掉不需要進行精確碰撞檢測的障礙物，提高算法運行效率。篩選出的危險障礙物輸出到分離軸碰撞檢測算法模塊，再進行精確的碰撞檢測。

3.2 分離軸(SAT)碰撞檢測

碰撞檢測的目的是檢測車體的凸多邊形與障礙物的凸多邊形之間是否發生交涉[18]，而分離軸(SAT)定理是一種被廣泛使用于凸多邊形碰撞檢測的技術，該理論僅通過基本的數學向量計算，就可以判斷2個凸多邊形是否相交，計算速度快且準確。將車輛與待檢測障礙物抽象為AABB多邊形包圍盒，進而對車輛可行域內的危險障礙物進行分離軸檢測，判斷與車輛是否發生碰撞。

車輛與障礙物之間的分離軸檢測如圖8所示。以障礙物多邊形ab邊的垂線為分離軸，分別將車輛多邊形與障礙物多邊形投影到分離軸上，得到兩多邊形的投影線段，判斷投影線段是否重合。當兩多邊形所有邊的檢測結果都重合時，表示兩多邊形發生碰撞。碰撞檢測算法的流程如圖9所示。

圖8 碰撞檢測Fig.8 Collision detection

圖9 碰撞檢測算法流程Fig.9 Collision detection process

通過遍歷前文計算的路徑上每一個軌跡點，在每一個軌跡點上對無人駕駛車輛與障礙物進行分離軸檢測，則可快速地判斷所規劃的路徑是否安全可行。

4 路徑優化

4.1 評價函數

本研究所提算法的目標是在已知起點和目標路徑以及周圍環境信息的條件下，快速地求解一條從起點到目標路徑的無碰撞、易跟蹤且能快速到達目標路徑的局部路徑。為了從路徑集中篩選滿足既定約束條件的局部最優路徑，設計如下評價函數H。評價函數的設計，主要針對3個因素優化：無人車距障礙物的距離、無人車轉彎半徑、規劃路徑結果中曲線路徑和直線路徑長度。

(27)

式中：d為軌跡點距障礙物的最近距離，m；r為當前路徑轉彎半徑，m；rmin為最小轉彎半徑，m；rmax為最大轉彎半徑，m；Lt、Lq分別為路徑中曲線路徑段t、q的長度，m；Lp為路徑中直線路徑的長度，m。

4.2 軌跡平滑

Dubins曲線由圓弧和直線組成，應用在路徑規劃時，符合無人駕駛車的運動特性，然而當無人駕駛車的轉彎半徑過小時，各曲線連接點處會出現不平滑的現象。為了使得規劃的軌跡更加平滑，便于無人駕駛車跟蹤軌跡，引入B樣條曲線對軌跡進行平滑處理。B 樣條函數曲線具有計算成本低、能夠保證路徑的連續性，且可以通過節點改變控制點來影響局部曲線等優點。在路徑平滑性上具有明顯優勢[19-20]。

根據文獻[21]，二次均勻B樣條曲線可表示為

(28)

式中：t∈[0,1]；Ni,2(t)為B樣條曲線的基函數；P0、P1、P2為3個軌跡點。

P(t)=(1-t)2P0+2t(1-t)P1+t2P2。

(29)

插值的軌跡點的航向方向與樣條曲線上點的切矢指向相同。切矢由下式給出。

P′(t)=2P0(1-t)+2(P0-2P1+P2)t。

(30)

將2.2中求解的軌跡點帶入公式(30)，則可得到平滑后的軌跡點。通過B樣條曲線平滑，可以消除軌跡點中突變較大、車輛無法跟蹤的點，從而保證路徑的曲率連續性。由圖10可知，B樣條曲線對平滑軌跡中不連續的點，有明顯效果。

圖10 B樣條曲線平滑Fig.10 Smoothing trajectory by B spline curve

4.3 算法驗證與結果分析

使用基于Qt編寫的仿真軟件驗證本研究設計的算法，構建圖5所設計的規劃場景，進行局部路徑規劃,仿真結果如圖11所示，其中紅色路徑即本研究所提算法計算出的相對最優路徑。以目標路徑中索引為16的軌跡點作為規劃終點，使用本研究設計的評價函數H，對比不同轉彎半徑條件下的H函數值，則可從規劃的結果中篩選出一條平滑、無碰撞、曲線路徑長度短的局部最優路徑。仿真結果數據(表2)表明，當H值最小時，總路徑的長度最小，且到障礙物的最小距離以及轉彎半徑都是最大。

表2 仿真結果數據Table2 The data of simulation result

在目標路徑中，采樣28個軌跡點作為規劃終點，仿真結果如圖11和圖12所示。從圖11可以看出，在滿足無人駕駛車行駛安全的前提下，紅色的路徑相對其他路徑路程更短，曲率更小。該路徑的H函數值為158.7，為計算結果中的最小值。

圖11 最優規劃結果Fig.11 The best result of planning

圖12 評價函數H與路徑點索引關系Fig.12 The relationship between the evaluation function H and the waypoint index

統計不同障礙物數量的條件下算法的運行時間，見表3。表3中的數據顯示，算法運行時長基本處于20 ms左右，能夠滿足無人駕駛車的實時性要求。

表3 算法耗時數據Tab.3 The data of algorithm time-consuming

5 結論

針對礦區采掘面和排土場的作業場景，提出基于Dubins曲線的局部路徑規劃方法。在Dubins曲線的理論基礎上，建立車輛可行域模型，結合分離軸理論對預選路徑進行碰撞檢測，同時設計評價函數，從路徑集合中篩選出滿足約束的路徑，并對該路徑進行平滑處理，最終輸出最優路徑。在基于Qt軟件開發的仿真軟件中，針對所提規劃方法的實時性與有效性進行仿真驗證。通過對比場景中不同障礙物數量、算法的運行時間，驗證了所提規劃方法的實時性。統計不同轉彎半徑下評價函數值，證明了本研究所設計評價函數的有效性。由于引入了碰撞檢測算法，克服了Dubins的間隔問題對路徑規劃的影響。

經仿真驗證，表明所提的局部路徑規劃方法對解決礦區采掘面和排土場場景中的局部路徑規劃問題，在實時性與有效性上有顯著效果，可以為該場景下的無人駕駛車局部路徑規劃提供理論和實踐依據。