黃金芬

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

0 引言

無人機(jī)飛行路徑規(guī)劃是無人機(jī)自主控制中重點設(shè)計的一環(huán)，無人機(jī)的飛行路徑需要考慮無人機(jī)的約束條件。Dubins 路徑不僅符合無人機(jī)的飛行特性，而且是兩個位姿點間的最短路徑，因此，結(jié)合Dubins 路徑來規(guī)劃無人機(jī)飛行路徑是有利的。當(dāng)前，Dubins 路徑主要用于無人機(jī)等高飛行路徑規(guī)劃，即類似于二維平面上的飛行，很少用于無人機(jī)變高飛行路徑規(guī)劃，即無人機(jī)在三維空間中的飛行。導(dǎo)致這種現(xiàn)象的一個重要原因就是三維Dubins 路徑很難求解。當(dāng)前常見的求解三維Dubins 路徑的方法是先求出兩個位姿點在二維平面上的Dubins 路徑，然后結(jié)合螺旋線將二維Dubins 路徑轉(zhuǎn)為三維Dubins 路徑，但這種方法存在使用范圍受限的缺陷[1]。

Dubins 路徑可被簡單地定義為，在最大曲率限制下，兩位姿點間的最短可行路徑，可以是CSC 路徑或CCC 路徑，或是它們的子集（CS、CC、SC），其中C 表示圓弧段，S 表示與C 相切的直線段[2]。本文主要討論CSC 型Dubins 路徑。

Shanmugavel 等人做了大量關(guān)于Dubins 路徑的研究[3-5]。文獻(xiàn)[3]介紹了基于微分幾何求解二維Dubins路徑的方法；文獻(xiàn)[4]介紹了基于微分幾何求解帶有Clothoid 曲線的Dubins 路徑的方法；文獻(xiàn)[5]提供了基于微分幾何求解三維Dubins 路徑的思路，但沒有給出具體的求解方法。本文結(jié)合文獻(xiàn)[5]提出的思路對基于微分幾何求解三維Dubins 路徑的方法進(jìn)行詳細(xì)解析。

1 基本知識

1.1 參數(shù)定義

計算Dubins 路徑前需要指定一些參數(shù)信息，下面對涉及的參數(shù)在二維和三維中有區(qū)別的分二維和三維來描述，其余的統(tǒng)一描述。

二維路徑中的參數(shù)定義如下：

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

（2）起始位姿點方向向量：ds，ds=[cosθssinθs]。

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

（4）終止位姿點方向向量：df，df=[cosθfsinθf]。

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

三維路徑中的參數(shù)定義如下：

（1）起始位姿點：ps(xs,ys,zs)。

（2）起始位姿點方向向量：ds(xds,yds,zds)。

（3）終止位姿點：pf(xf,yf,zf)。

（4）終止位姿點方向向量：df(xdf,ydf,zdf)。其余參數(shù)定義如下：

（1）起始轉(zhuǎn)彎圓轉(zhuǎn)向：turns，其值為1 表示向左轉(zhuǎn)，為-1 表示向右轉(zhuǎn)。

（2）起始轉(zhuǎn)彎圓半徑：rs，其中ks為起始轉(zhuǎn)彎圓曲率。

（3）終止轉(zhuǎn)彎圓轉(zhuǎn)向：turnf，其值為1 表示向左轉(zhuǎn)，為-1 表示向右轉(zhuǎn)。

1.2 計算規(guī)則

下面對路徑規(guī)劃中所涉及的一些變量的計算方法進(jìn)行講解。

（1）二維平面

圖1 二維平面上位姿點對應(yīng)的兩個轉(zhuǎn)彎圓

根據(jù)圖1 可知，位姿點的兩個轉(zhuǎn)彎圓圓心c1 和c2都在以位姿點p 為圓心的圓上，并且它們在圓p 上的夾角與θ相差是位姿點方向向量d 和向量d0的夾角。二維平面上圓上點坐標(biāo)的求解方法如式（1）所示，其中ε為點在圓上的夾角，圖1 中p0 是ε等于0時計算出來的點坐標(biāo)，在二維平面上，向量d0 與x 軸正方向平行且同向，因此d 和d0 的夾角就等于參數(shù)中的θ。根據(jù)式（1）可知，當(dāng)ε增大時，計算出來的點在原點的左邊，當(dāng)ε減小時，計算出來的點在原點的右邊，因此，圖1 中c1 在圓p 上的夾角為在圓p 上的夾角為反過來，p 在圓c1 上的夾角為在圓c2 上的夾角為

定義位姿點p 在轉(zhuǎn)彎圓上夾角為ε，當(dāng)點p 向左旋轉(zhuǎn)v 度后對應(yīng)的坐標(biāo)如式（2）所示，當(dāng)點p 向右旋轉(zhuǎn)v 度后對應(yīng)的坐標(biāo)如式（3）所示。

位姿點(x,y,θ)沿當(dāng)前方向直線前進(jìn)長度v 后的坐標(biāo)如式（4）所示。

（2）三維平面

三維平面由于其特殊性，需要定義描述圓轉(zhuǎn)向的視角，若三維平面的法向量指向的是三維坐標(biāo)軸中負(fù)軸的方向，則視線和法向量的指向保持一致，若法向量指向的是三維坐標(biāo)軸中正軸的方向，則視線和法向量的指向相反。

式（5）是三維平面上圓上點坐標(biāo)的求解公式。

其中，j 和k 的計算方法如式（6）所示。

其中，n 是圓所在三維平面的法向量，ε是點在圓上的夾角。根據(jù)式（6）可知，三維平面上圓上點坐標(biāo)的計算公式與三維平面的法向量有關(guān)。設(shè)點的夾角ε遞增，若法向量n 指向負(fù)軸的方向，則根據(jù)式（5）、式（6）得到的圓上點向右轉(zhuǎn)，若法向量n 指向正軸的方向，則根據(jù)式（5）、式（6）得到的圓上點向左轉(zhuǎn)。定義變量dire 來表示法向量n 下根據(jù)式（5）、式（6）得到的圓上點的轉(zhuǎn)向，則dire 的值如式（7）所示，值為1 表示向左轉(zhuǎn)，為-1 表示向右轉(zhuǎn)。

式（7）表示的意思是當(dāng)法向量中z 坐標(biāo)值大于0時得到的圓上點向左轉(zhuǎn)，z 坐標(biāo)值小于0 時得到的圓上點向右轉(zhuǎn)，若z 坐標(biāo)值等于0，則判斷y 坐標(biāo)的值，y 坐標(biāo)值大于0 時得到的圓上點向左轉(zhuǎn)，y 坐標(biāo)值小于0 時得到的圓上點向右轉(zhuǎn)，若y 坐標(biāo)的值也等于0，則再判斷x 坐標(biāo)的值，判斷方法和前面一樣。

圖2 展示了三維平面上位姿點對應(yīng)的兩個轉(zhuǎn)彎圓，可以看到，在三維平面上，位姿點也可以向左轉(zhuǎn)或向右轉(zhuǎn)，因此，三維平面上對應(yīng)的CSC 型Dubins 路徑也有四種子類型，即LSL、LSR、RSL、RSR。

圖2 三維平面上位姿點對應(yīng)的兩個轉(zhuǎn)彎圓

圖2 中p0 是圓p 上的起點，即夾角ε為0 時根據(jù)式（5）、式（6）計算出來的圓上的點，α是位姿點方向向量d 和向量d0 的夾角。轉(zhuǎn)彎圓圓心c1 和c2 在圓p上的夾角與α相差，具體的求解方法如式（8）所示。

式（8）表達(dá)的意思是如果轉(zhuǎn)彎圓轉(zhuǎn)向turn 和dire的值不相同，則圓心在圓p 上夾角為如果轉(zhuǎn)彎圓轉(zhuǎn)向turn 和dire 的值相同，則圓心在圓p 上夾角為與二維平面相比不同的是，這里還要考慮dire 的值。

三維空間中點(x,y,z)沿單位方向向量[m,n,q]前進(jìn)長度v 后的坐標(biāo)如式（9）所示。

2 計算二維Dubins路徑

圖3 給出了一條二維Dubins 路徑。將Dubins 路徑分成三段，并給每一段設(shè)置一個FS 坐標(biāo)系，分別為起始坐標(biāo)系es、連接坐標(biāo)系ec和終止坐標(biāo)系ef。

圖3 二維平面上的Dubins路徑

定義基坐標(biāo)系e[t,n]，則es、ec和ef與e分別有如下關(guān)系：

其中，R(θs)、R(φ)、R(θf)表示對應(yīng)角度下的坐標(biāo)系的逆時針旋轉(zhuǎn)矩陣。φ和θs存在如下關(guān)系。

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

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

其中，φs表示起始弧對應(yīng)的角度。由圖3 可知，二維平面上Dubins 路徑中存在如下的矢量等式：

在基坐標(biāo)系e 下，矢量c、as、af和ac可寫為如下

形式：

其中c 是中心矢量c 的長度，u 是單位矢量，a 是切向量ac的長度。由圖3 可知，中心矢量c 可由兩個轉(zhuǎn)彎圓圓心得到。

式（13）可表示為如下形式：

由于上式是一個旋轉(zhuǎn)等式，因此左右兩邊的矢量幅值應(yīng)相等：

所以，a 可以表達(dá)為：

其中，半徑的正負(fù)號和轉(zhuǎn)彎圓的轉(zhuǎn)向有關(guān)，具體的對照關(guān)系可以查看表1。

表1 與路徑類型對應(yīng)的a 的計算公式

為了計算φ，式（15）可以寫為如下形式：

其中

進(jìn)而解出φ：

其中：

解出φ后就可以根據(jù)起始轉(zhuǎn)彎圓的轉(zhuǎn)向和式（11）、式（12）解出φs，得到φs的值后要進(jìn)行如式（20）所示的取模運算，然后根據(jù)表2 即可求出終止弧對應(yīng)的角度φf，得到φf的值后也要進(jìn)行如式（21）所示的取模運算。

表2 路徑類型與角度變換關(guān)系

到此，Dubins 路徑的三個關(guān)鍵信息—起始弧對應(yīng)的角度φs、切線段的長度a以及終止弧對應(yīng)的角度φf都得到了，根據(jù)所需要的路徑上點的個數(shù)、Dubins 路徑的子類型以及式（2）、式（3）、式（4）便可以求得路徑上點的坐標(biāo)。

3 計算三維Dubins路徑

求解三維Dubins 路徑的主要思路是先將起始位姿點旋轉(zhuǎn)到與終止位姿點共面的位置，然后求解三維平面上的Dubins 路徑，最后將旋轉(zhuǎn)路徑和平面上的Dubins 路徑組合起來構(gòu)成指定兩點之間的三維Dubins路徑。

（1）旋轉(zhuǎn)起始位姿點到與終止位姿點共面的位置。使用平行的向量一定共面這個理論來判斷兩位姿點是否共面以及尋找共面位置。圖4 描述了空間中位姿點的轉(zhuǎn)彎圓，可以看出，在三維空間中，從一個位姿點出發(fā)，可以向各個方向以圓的形式旋轉(zhuǎn)，位姿點最終可以旋轉(zhuǎn)到空間中任意方向。因此，一定存在一個轉(zhuǎn)彎圓上的某個點的方向與終止位姿點平行。

根據(jù)法向量的定理可知，三維平面的法向量一定與平面上的向量垂直。若平面上的向量與終止位姿點的方向向量平行，則法向量也與終止位姿點的方向向量垂直。由圖4 可知，圓上點的方向就是點的切向量，假設(shè)圓pc 是尋找的轉(zhuǎn)彎圓，可以看到圓心pc 在圓ps上的切向量dc 就是圓pc 所在平面的法向量。dc、dv和ds 相互垂直，因此dc 等于dv 叉乘ds。結(jié)合式（5）、式（6）可以把圓心pc 用參數(shù)的形式表達(dá)出來，根據(jù)向量垂直的關(guān)系建立等式可以把參數(shù)解出來，反帶入就得到pc 和dc 的值。

圖4 位姿點的轉(zhuǎn)彎圓

圖5 是一個三維空間中的Dubins 路徑，可以看到，轉(zhuǎn)彎圓pc 上存在一點pt，pt 的切向量dt 與終止位姿點的方向df 平行。已知圓心pc 和圓所在平面的法向量dc，則pt 可以根據(jù)式（5）、式（6）用參數(shù)的形式表達(dá)出來。根據(jù)向量平行的關(guān)系建立等式可以求得參數(shù)的解，反帶入就得到pt 和dt 的值，pt 和dt 就構(gòu)成了和終止位姿點共面的新的起始位姿點。

圖5 三維空間中的Dubins路徑

（2）求解三維平面上Dubins 路徑。三維平面上求解Dubins 路徑的原理和步驟與二維平面上一樣，主要是一些變量的求解方法存在區(qū)別，下面分析如何求解這些變量。

求解單位向量u。u 是二維向量，如果要在三維平面上求u，則要在三維平面上設(shè)計一個類似二維的坐標(biāo)軸。圖6 展示了一種構(gòu)造三維平面上類二維坐標(biāo)軸的方法。

圖6 三維平面上的Dubins路徑

以Dubins 路徑中兩個轉(zhuǎn)彎圓圓心cirs 和cirf 連線的中點為圓心，作過cirs 和cirf 的圓，找到圓上的起點p0，當(dāng)出現(xiàn)如下情況時，要對p0 進(jìn)行一個變換。

式（22）表達(dá)的含義是如果p0 在圓心連線的右邊，則將p0 轉(zhuǎn)換到圓心連線的左邊。連接cirs 和p0 的向量dp 就是類二維坐標(biāo)系中的x 軸，將dp 向左旋轉(zhuǎn)就得到了y 軸。因此，三維平面上u 的求解方法如式（23）所示。

求解對應(yīng)二維平面上的θs和θf。對應(yīng)二維平面上的θs和θf的計算方法如表3 所示。

表3 對應(yīng)二維平面上的θs 和θf 的計算規(guī)則

其中γs和γf分別等于從dp 旋轉(zhuǎn)到dtt、dff 的夾角。這里要注意向量之間旋轉(zhuǎn)的方向，定義從dp 旋轉(zhuǎn)到dtt、dff 的旋轉(zhuǎn)方向為dirc，則dirc 的值如式（24）所示，值為1 表示向左轉(zhuǎn)，為-1 表示向右轉(zhuǎn)。

其中c2a 來自于式（22）。要注意，類似判斷條件下式（24）得到的值和式（7）是不一樣的。

以上就是三維平面上相比二維平面計算方法中主要存在差異的變量。到此，根據(jù)二維Dubins 路徑的計算步驟就可以求得三維平面上Dubins 路徑起始弧對應(yīng)的角度φs、切線段的長度a以及終止弧對應(yīng)的角度φf。起始旋轉(zhuǎn)弧對應(yīng)的角度可以通過求解兩個向量之間夾角的方式得到。結(jié)合起始旋轉(zhuǎn)弧段和三維平面上Dubins 路徑就得到了最終的三維Dubins 路徑。根據(jù)所需要的路徑上點的個數(shù)、Dubins 路徑的子類型以及式（5）、式（6）、式（8）、式（9）便可以求得路徑上點的坐標(biāo)。

4 仿真驗證

使用MATLAB 軟件對本文算法的有效性進(jìn)行仿真驗證。圖7 是二維平面上Dubins 路徑的仿真結(jié)果，圖8 是三維空間中Dubins 路徑的仿真結(jié)果，可以看到，不論是二維平面還是三維空間，本文算法都能成功計算出指定兩點之間的Dubins 路徑。

5 結(jié)語

本文對基于微分幾何實現(xiàn)二維和三維Dubins 路徑的原理和步驟進(jìn)行了細(xì)致的闡述，仿真結(jié)果表明，本文提出的方法能夠準(zhǔn)確計算出二維平面內(nèi)和三維空間中兩位姿點間的CSC 型Dubins 路徑。

圖7 二維Dubins路徑仿真結(jié)果

圖8 三維Dubins路徑仿真結(jié)果