基于解析幾何實現Dubins 避障路徑

黃金芬

（西華大學計算機與軟件工程學院，成都610039）

0 引言

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

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

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

1 Dubins路徑

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

1.1 初始條件和坐標計算規則

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

圖1 相切圓

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

設計Dubins 路徑時需要以下參數：

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

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

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

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

其中，下標s 表示起始位置的參數，f 表示終止位置的參數，其中θ 表示點與x 正軸的夾角。

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

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

1.2 求解路徑過程

Dubins 路徑求解過程：

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

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

當公切線為內公切線或外公切線時，α 對應不同的計算公式。

內公切線：

外公切線：

β 對應公式如下：

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

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

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

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

圖2 RSL型Dubins路徑

（3）切點得到后，就可以根據以下公式得到從起始點旋轉到切出點的弧所對應的角度和從切入點旋轉到終止點的弧所對應的角度。

①從起始點旋轉到切出點G 的弧所對應角度φs

向右旋轉：

向左旋轉：

②從切入點F 旋轉到終止點的弧所對應角度φf向右旋轉：

向左旋轉：

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

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

2 避障路徑

這里只討論已知靜態障礙物的避障。設計避障路徑的方法有兩種，一種是在規劃路徑時就避開障礙物，另一種是先規劃出起點到終點的路徑，然后對路徑進行優化，使其避開障礙物。本文根據后一種方法來規劃避障路徑。根據前面的設計，已經得到了兩位姿點間的Dubins 路徑。

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

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

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

2.1 與切線段相交

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

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

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

式中的di有兩個解：

這兩個解可以分別寫為：

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

圖3 直線與障礙物相交

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

由表1可知，無水關門工況下單缸啟閉機最大推力為707.1 kN，出現在無水關門初始時；最大拉力為209.1 kN，出現在順風無水關門終了時。

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

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

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

2.2 與弧段相交

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

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

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

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

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

向右旋轉時，若θt-len ＜0，則弧段對應的角度范圍為：

否則為：

否則為：

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

（2）障礙物與弧段相交

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

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

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

當圓向右轉時：

當圓向左轉時：

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

圖7 無障礙物路徑

圖8 切線段避障路徑

圖9 弧段避障路徑

圖10 多障礙物避障路徑

3 仿真驗證

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

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

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

從圖10 可以看出，當障礙物圓與弧段所在的圓相交時，如果交點不在弧段內，那么障礙物對路徑沒有影響，不用進行避障處理。

4 結語

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