李勝琴，閆祥偉

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

0 引言

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

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

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

1 基本問題描述

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

圖1 場景描述Fig.1 Scene description

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

圖2 RSL曲線Fig.2 RSL curve

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

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

2 基于Dubins曲線的路徑規(guī)劃方法

2.1 路徑求解

α=γ-θ。

(1)

β=δ-θ。

(2)

(3)

(4)

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

(5)

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

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

(6)

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

(7)

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

(8)

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

(9)

(10)

(11)

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

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

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

上述求解的結(jié)果為路徑集合，其中每一條路徑由圓弧和直線組成。對于點(diǎn)對點(diǎn)可能產(chǎn)生的若干種行駛路徑，設(shè)計(jì)損失函數(shù)如下

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

(12)

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

設(shè)計(jì)圖5的規(guī)劃場景，該場景包含一條真實(shí)的目標(biāo)路徑以及規(guī)劃的起始點(diǎn)，起點(diǎn)的GPS坐標(biāo)為(41.802 924 5，109.994 822 1，337.913)。目標(biāo)路徑上的軌跡點(diǎn)作為規(guī)劃的終點(diǎn)，并設(shè)置4種車輛的轉(zhuǎn)彎半徑作為算法的輸入?yún)?shù)，即轉(zhuǎn)彎半徑r1、r2、r3、r4分別為16、21、26、31，對上述算法進(jìn)行驗(yàn)證。

圖5 規(guī)劃場景Fig.5 Scene of planning

由圖5可以看出，灰色的多邊形表示4個(gè)靜態(tài)障礙物，十字交叉線段的交點(diǎn)表示設(shè)置的規(guī)劃起始點(diǎn)，長邊的指向?yàn)槠鹗键c(diǎn)的航向(與正北方向的夾角)。選取目標(biāo)路徑上一點(diǎn)，坐標(biāo)為(14.393 9， 103.905 0)索引為14。以半徑r3為轉(zhuǎn)彎半徑，將設(shè)置好的參數(shù)輸入到編寫的算法程序中，得到表1中的結(jié)果。

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

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

2.2 軌跡點(diǎn)求解

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

(13)

(14)

將L歸一化，以便于軌跡點(diǎn)的求解。

(15)

當(dāng)l

(16)

當(dāng)t

(17)

當(dāng)t+p

(18)

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

圖6 規(guī)劃結(jié)果Fig.6 Result of planning

3 碰撞檢測

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

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

無人駕駛車可行域模型基于車輛單軌運(yùn)動學(xué)模型[17]建立。無人駕駛車可行域模型如圖7所示。圖7為當(dāng)θ>0時(shí)的可行域模型。

(19)

(20)

(21)

(22)

(23)

(24)

(25)

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

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

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

(26)

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

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

3.2 分離軸(SAT)碰撞檢測

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

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

圖8 碰撞檢測Fig.8 Collision detection

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

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

4 路徑優(yōu)化

4.1 評價(jià)函數(shù)

本研究所提算法的目標(biāo)是在已知起點(diǎn)和目標(biāo)路徑以及周圍環(huán)境信息的條件下，快速地求解一條從起點(diǎn)到目標(biāo)路徑的無碰撞、易跟蹤且能快速到達(dá)目標(biāo)路徑的局部路徑。為了從路徑集中篩選滿足既定約束條件的局部最優(yōu)路徑，設(shè)計(jì)如下評價(jià)函數(shù)H。評價(jià)函數(shù)的設(shè)計(jì)，主要針對3個(gè)因素優(yōu)化：無人車距障礙物的距離、無人車轉(zhuǎn)彎半徑、規(guī)劃路徑結(jié)果中曲線路徑和直線路徑長度。

(27)

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

4.2 軌跡平滑

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

根據(jù)文獻(xiàn)[21]，二次均勻B樣條曲線可表示為

(28)

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

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

(29)

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

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

(30)

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

圖10 B樣條曲線平滑F(xiàn)ig.10 Smoothing trajectory by B spline curve

4.3 算法驗(yàn)證與結(jié)果分析

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

表2 仿真結(jié)果數(shù)據(jù)Table2 The data of simulation result

在目標(biāo)路徑中，采樣28個(gè)軌跡點(diǎn)作為規(guī)劃終點(diǎn)，仿真結(jié)果如圖11和圖12所示。從圖11可以看出，在滿足無人駕駛車行駛安全的前提下，紅色的路徑相對其他路徑路程更短，曲率更小。該路徑的H函數(shù)值為158.7，為計(jì)算結(jié)果中的最小值。

圖11 最優(yōu)規(guī)劃結(jié)果Fig.11 The best result of planning

圖12 評價(jià)函數(shù)H與路徑點(diǎn)索引關(guān)系Fig.12 The relationship between the evaluation function H and the waypoint index

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

表3 算法耗時(shí)數(shù)據(jù)Tab.3 The data of algorithm time-consuming

5 結(jié)論

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

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