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

黃金芬

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

0 引言

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

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

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

1 Dubins路徑

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

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

如圖1 所示，點(diǎn)P 為起始位姿點(diǎn)Ps或終止位姿點(diǎn)Pf，θ 為對(duì)應(yīng)的θs或θf(wàn)，Center1 和Center2 分別為點(diǎn)P 右轉(zhuǎn)和左轉(zhuǎn)時(shí)對(duì)應(yīng)圓的圓心。在給定位姿點(diǎn)P 下，無(wú)人機(jī)可以向左轉(zhuǎn)和向右轉(zhuǎn)，路徑可以以順時(shí)針?lè)较颍ㄓ遥┗蚰鏁r(shí)針?lè)较颍ㄗ螅╅_始或結(jié)束，因此，一對(duì)位姿點(diǎn)間可以產(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è)計(jì)Dubins 路徑時(shí)需要以下參數(shù)：

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

（2）終止位姿點(diǎn)：Pf(xf,yf,θf(wàn))。

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

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

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

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

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

1.2 求解路徑過(guò)程

Dubins 路徑求解過(guò)程：

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

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

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

內(nèi)公切線：

外公切線：

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

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

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

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

表1 四種路徑類型下切出和切入點(diǎn)夾角計(jì)算

圖2 RSL型Dubins路徑

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

①?gòu)钠鹗键c(diǎn)旋轉(zhuǎn)到切出點(diǎn)G 的弧所對(duì)應(yīng)角度φs

向右旋轉(zhuǎn)：

向左旋轉(zhuǎn)：

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

向左旋轉(zhuǎn)：

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

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

2 避障路徑

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

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

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

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

2.1 與切線段相交

（1）與切線段相交檢測(cè)方法

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

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

式中的di有兩個(gè)解：

這兩個(gè)解可以分別寫為：

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

圖3 直線與障礙物相交

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

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

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

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

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

2.2 與弧段相交

（1）與弧段相交的檢測(cè)方法

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

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

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

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

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

否則為：

否則為：

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

（2）障礙物與弧段相交

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

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

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

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

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

向量AB 表示由點(diǎn)A 指向點(diǎn)B 的單位向量。

圖7 無(wú)障礙物路徑

圖8 切線段避障路徑

圖9 弧段避障路徑

圖10 多障礙物避障路徑

3 仿真驗(yàn)證

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

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

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

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

4 結(jié)語(yǔ)

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