基于解析幾何實現(xiàn)Dubins 避障路徑

黃金芬

（西華大學(xué)計算機(jī)與軟件工程學(xué)院，成都610039）

0 引言

近年來，隨著無人機(jī)的使用日益增多，以及無人機(jī)完成的任務(wù)越來越復(fù)雜，無人機(jī)的設(shè)計越來越智能，最基本的操作之一就是無人機(jī)飛行路徑規(guī)劃。無人機(jī)的飛行路徑規(guī)劃需要滿足無人機(jī)飛行性能約束，以及規(guī)劃出的路徑最短和能躲避障礙物等需求。

目前已經(jīng)有大量的路徑規(guī)劃方法，包括：路圖法、單元分解法、概率法、勢場法等。這些方法一開始是應(yīng)用與地面機(jī)器人，后來人們對其進(jìn)行改進(jìn)和提升，應(yīng)用到無人機(jī)上。這些方法的缺陷是生成的路徑并不能直接運(yùn)用于無人機(jī)，需要進(jìn)行平滑處理得到可飛行的路徑，因此，直接將曲線運(yùn)用于路徑規(guī)劃中顯得更好?；诖?，Shanmugavel 等人做了大量采用曲線進(jìn)行路徑規(guī)劃的研究，文獻(xiàn)[1-2]描述了在二維和三維空間中規(guī)劃多無人機(jī)的等長不相交Dubins 路徑。文獻(xiàn)[3]描述了在三維空間中規(guī)劃多無人機(jī)的畢達(dá)哥拉斯矢端曲線（PH 曲線）路徑，文獻(xiàn)[4]描述了在二維空間中使用具有Clothoid 曲線的Dubins 路徑規(guī)劃多無人機(jī)的協(xié)同路徑。

解析幾何是常用來獲取Dubins 路徑的方法。本文參考了Shanmugavel 所整理的解析幾何設(shè)計方法，詳細(xì)介紹了根據(jù)解析幾何設(shè)計二維Dubins 路徑的原理和過程、檢測已知靜止障礙物是否和Dubins 相交的方法以及避開障礙物的方法。本文提出了原方法中沒有明確給出的條件和變量的計算公式，以及根據(jù)實驗結(jié)果改變了原方法中的一些公式。

1 Dubins路徑

Dubins 路徑可被簡單地定義為，在最大曲率限制下，平面內(nèi)兩個有方向的點間的最短可行路徑是CLC路徑或CCC 路徑，或是它們的子集（CL、CC、LC），其中C 表示圓弧段，L 表示與C 相切的直線段[5]。本文主要討論CLC 路徑。

1.1 初始條件和坐標(biāo)計算規(guī)則

如圖1 所示，點P 為起始位姿點Ps或終止位姿點Pf，θ 為對應(yīng)的θs或θf，Center1 和Center2 分別為點P 右轉(zhuǎn)和左轉(zhuǎn)時對應(yīng)圓的圓心。在給定位姿點P 下，無人機(jī)可以向左轉(zhuǎn)和向右轉(zhuǎn)，路徑可以以順時針方向（右）或逆時針方向（左）開始或結(jié)束，因此，一對位姿點間可以產(chǎn)生四條Dubins 路徑，分別是LSL、LSR、RSL、RSR，其中R 表示向右轉(zhuǎn)，L 表示向左轉(zhuǎn)，S 表示兩圓之間的切線段。

圖1 相切圓

兩圓之間有兩條公切線：內(nèi)公切線和外公切線。在指定圓的旋轉(zhuǎn)方向下，同向旋轉(zhuǎn)的兩圓具有外公切線，反向旋轉(zhuǎn)的兩圓具有內(nèi)公切線。圖2 就屬于有內(nèi)公切線的反向旋轉(zhuǎn)的兩圓。

設(shè)計Dubins 路徑時需要以下參數(shù)：

（1）起始位姿點：Ps(xs,ys,θs)。

（2）終止位姿點：Pf(xf,yf,θf)。

（3）起始曲率：ks≤kmax，其中ks=，rs是曲率半徑。

（4）終點曲率：kf≤kmax，其中kf=，rf是曲率半徑。

其中，下標(biāo)s 表示起始位置的參數(shù)，f 表示終止位置的參數(shù)，其中θ 表示點與x 正軸的夾角。

在單位圓中，設(shè)角度基于x 正軸向左旋轉(zhuǎn)為正，向右旋轉(zhuǎn)為負(fù)，則在單位圓上點(x,y,θ)向左或右旋轉(zhuǎn)v度后的坐標(biāo)有如下計算公式：

點(x,y,θ)沿切線段前進(jìn)長度v 后的坐標(biāo)計算公式如下：

1.2 求解路徑過程

Dubins 路徑求解過程：

（1）通過下式，確定起始圓C1 圓心(xcs,ycs)和終止圓C2 圓心(xcf,ycf)。

（2）計算兩圓的切點坐標(biāo)。圖2 是CSC 型Dubins路徑中的RSL 型，其中G、F 分別是圓C1 和圓C2 上的切出點和切入點，α 是兩圓圓心連線與公切線的夾角，β 是兩圓圓心連線與x 軸的夾角，t 是切線段長度，c 是圓心連線的長度。

當(dāng)公切線為內(nèi)公切線或外公切線時，α 對應(yīng)不同的計算公式。

內(nèi)公切線：

外公切線：

β 對應(yīng)公式如下：

β 的取值范圍為(-π,π)。因此，切出點G(xG,yG)和切入點F(xF,yF)的坐標(biāo)可由下式計算：

式中，θG和θF有如下公式：

其中θG0和θF0可由表1 得到。

表1 四種路徑類型下切出和切入點夾角計算

圖2 RSL型Dubins路徑

（3）切點得到后，就可以根據(jù)以下公式得到從起始點旋轉(zhuǎn)到切出點的弧所對應(yīng)的角度和從切入點旋轉(zhuǎn)到終止點的弧所對應(yīng)的角度。

①從起始點旋轉(zhuǎn)到切出點G 的弧所對應(yīng)角度φs

向右旋轉(zhuǎn)：

向左旋轉(zhuǎn)：

②從切入點F 旋轉(zhuǎn)到終止點的弧所對應(yīng)角度φf向右旋轉(zhuǎn)：

向左旋轉(zhuǎn)：

（4）得到切線段的長度。切線段長度可以由畢達(dá)哥拉斯理論計算得到：

根據(jù)上面的步驟，求解得到了起始弧對應(yīng)角度、切出點坐標(biāo)、切線段長度、切入點坐標(biāo)和終止弧對應(yīng)角度，到此，就可以繪出Dubins 路徑了。

2 避障路徑

這里只討論已知靜態(tài)障礙物的避障。設(shè)計避障路徑的方法有兩種，一種是在規(guī)劃路徑時就避開障礙物，另一種是先規(guī)劃出起點到終點的路徑，然后對路徑進(jìn)行優(yōu)化，使其避開障礙物。本文根據(jù)后一種方法來規(guī)劃避障路徑。根據(jù)前面的設(shè)計，已經(jīng)得到了兩位姿點間的Dubins 路徑。

這里設(shè)障礙物是固定半徑的圓，障礙物與Dubins路徑相撞有兩種情況：

（1）障礙物圓與路徑中的切線段相交。

（2）障礙物圓與路徑中的弧段相交。

2.1 與切線段相交

（1）與切線段相交檢測方法

圖3 給出了障礙物與直線段相交的幾何示意圖。判斷是否與切線段相交，首先需要判斷障礙物是否與切線段所在的直線相交，為此，要計算從障礙物中心點到直線段起始點的距離(do)，同時計算障礙物中心點與該線段的垂直投影線(dn)。當(dāng)直線段與障礙物相交時可以得到：

式中：ro是障礙物的半徑；ko是障礙物的曲率。如果檢測到障礙物與直線段相交，那么可以用余弦定理計算障礙物與直線段的交線段di：

式中的di有兩個解：

這兩個解可以分別寫為：

當(dāng)確認(rèn)障礙物與直線段相交時，若dim的長度大于路徑中切線段(dj)的長度，則切線段不會與障礙物相交，否則相交。圖3 是障礙物與直線段相交但不與切線段相交的情形。

圖3 直線與障礙物相交

（2）障礙物與切線段相交

由表1可知，無水關(guān)門工況下單缸啟閉機(jī)最大推力為707.1 kN，出現(xiàn)在無水關(guān)門初始時；最大拉力為209.1 kN，出現(xiàn)在順風(fēng)無水關(guān)門終了時。

當(dāng)障礙物與切線段相交時，比較簡單的處理就是切線段從障礙物圓上繞過去。如圖4 所示，通過調(diào)整初始運(yùn)動弧和終止運(yùn)動弧使切線段與障礙物圓相切，這有兩種可行解：繞障礙物圓同向旋轉(zhuǎn)或反向旋轉(zhuǎn)。

最終的避障路徑是包括兩個直線段和三個弧段，在設(shè)計時，可以看做是把兩個Dubins 路徑合并而成。求解方法和求解單個Dubins 路徑類似，這里不做介紹。

圖4 直線段與障礙物相交時的避障路徑

2.2 與弧段相交

（1）與弧段相交的檢測方法

圖5 給出了障礙物圓與弧段相交的幾何示意圖。相交的必要條件是弧段圓心到障礙物圓心的距離dc應(yīng)滿足兩圓相交的條件。該條件是：

式中：rn是弧段所在圓半徑，ro是障礙物圓半徑。若此條件滿足，還需考慮相交的交點是否落在圓弧段內(nèi)，如果沒有，則沒有相交，否則相交。

圖5 弧段與障礙物相交的情況

交點是否落在圓弧段內(nèi)可以通過交點的夾角是否落于弧段對應(yīng)角度范圍來判斷，獲取弧段對應(yīng)角度范圍需要考慮兩種情況，弧段向右旋轉(zhuǎn)和向左旋轉(zhuǎn)。

向右旋轉(zhuǎn)時，若θt-len ＜0，則弧段對應(yīng)的角度范圍為：

否則為：

否則為：

其中，θt表示弧段起點與x 正軸的夾角，len 表示弧段對應(yīng)的角度大小。

（2）障礙物與弧段相交

當(dāng)弧段與障礙物相交時，需要通過調(diào)整弧段的曲率來避開障礙物，圖6 描述了相應(yīng)的兩種解，由這兩種解得到的弧段均與障礙物相切，運(yùn)用余弦定理可以計算此時弧段的新半徑rn。即：

其中cos(μ)可以由單位向量t→[xt,yt]和n→[xn,yn]求得，即：

圖6 避開與障礙物相交的2種相切圓

當(dāng)圓向右轉(zhuǎn)時：

當(dāng)圓向左轉(zhuǎn)時：

向量AB 表示由點A 指向點B 的單位向量。

圖7 無障礙物路徑

圖8 切線段避障路徑

圖9 弧段避障路徑

圖10 多障礙物避障路徑

3 仿真驗證

設(shè)定初始條件，對本文設(shè)計的路徑規(guī)劃算法進(jìn)行仿真驗證。圖7 是無障礙物環(huán)境下規(guī)劃出的最短路徑，圖8 是障礙物與切線段相交時的避障路徑，圖9 是障礙物與弧段相交時的避障路徑，圖10 是多個障礙物存在時的避障路徑。圖中虛線圓是弧段所在的圓或弧段避障之前所在的圓，實線圓代表障礙物。

從圖8 可以看出，切線段避障時，可以繞障礙物同向運(yùn)動或者反向運(yùn)動，我們選擇其中更短的路徑作為首選路徑。

當(dāng)障礙物與弧段相交后，對應(yīng)有兩個與障礙物相切的圓，如果選擇半徑較小的那個圓，那么新的切線段又可能會與障礙物相交，此時，需要再進(jìn)行切線段的避障處理。圖9 中我們選擇了半徑較大的圓。

從圖10 可以看出，當(dāng)障礙物圓與弧段所在的圓相交時，如果交點不在弧段內(nèi)，那么障礙物對路徑?jīng)]有影響，不用進(jìn)行避障處理。

4 結(jié)語

本文對基于解析幾何設(shè)計Dubins 路徑的原理和方法進(jìn)行了詳細(xì)描述，并在已有Dubins 路徑的基礎(chǔ)上進(jìn)行避障分析設(shè)計，考慮了直線避障和弧段避障兩種情況。仿真結(jié)果表明，算法能正確設(shè)計出兩位姿點間的Dubins 路徑，并根據(jù)已知障礙物信息優(yōu)化已有路徑，避開路徑上的障礙物。