自動駕駛運動規劃-Dubins曲線

如下圖所示，Simple Car模型是一個表達車輛運動的簡易模型。Simple Car模型将車輛看做平面上的剛體運動，剛體的原點位于車輛後輪的中心；x軸沿着車輛主軸方向，與車輛運動方向相同；車輛在任意一個時刻的姿态可以表述為(x, y, 

)。車輛的運動速度為s；方向盤的轉角為

，它與前輪的轉角相同；前輪和後輪中心的距離為L；如果方向角的轉角固定，車輛會在原地轉圈，轉圈的半徑為

。

在一個很短的時間

内，可以認為車輛沿着後輪指向的方向前進，當

趨于0時，有：

根據數學定義:

将2) 和3)代入1)中，得到：

顯然，

和

是5)式的一個解，兩側乘以速度s等式仍然滿足。是以有:

用

表示車輛前進的距離，則有：

根據三角幾何，有:

将9)式代入8)式，得到：

8)式兩側同除以dt， 并根據

，得到：

至此得到了車輛的運動模型(Motion Model)。

然後引入Action變量，假設車輛運動速度s和方向盤轉角

由Action變量

指定，得到：

假設車輛按照常量速度運作: 

，最大轉向角度為

，最小轉彎半徑

，起點為

， 終點為

，我們目标是求解從起

點到終點

的最短行駛距離。求解最短距離的過程就是優化如下Cost的過程。

是到達

所需的時間，

，當

不可達時，

由于速度

是恒定的，根據前面提到的車輛的運動模型：

其中：

。将13)和14)代入12)，可看到，最短行駛距離隻與時間

有關。

令S為車輛直行的Motion Primitive，L和R分别為車輛左轉和右轉的Motion Primitive，可以證明，任意起點到終點的Dubins最短路徑可以由不超過三個Motion Primitives構成。由三個Motion Primitives構成的序列稱為一個Word。由于兩個連續的、相同的Motion Primitive可以合并為一個Motion Primitive，是以所有可能的Word有10中組合，Dubins證明最優的Word組合隻能是如下6個組合之一：

其中，

，

，這裡注意，

大于

，如果小于

，一定有其它的序列優于該序列。

假設兩個最小轉彎半徑構成的Circle為 

，半徑分别為

，圓心分别為

1）首先構造C1和C2的圓心

到

的向量

2）構造C1和C2的外切線切點構成的向量

3）構造垂直于

的機關法向量n，修改

的使其平行于

根據法向量的定義：

根據機關向量的定義：

，代入上式得到：

16）式兩側同除以D，得到：

注意，這裡

實際是将向量

機關化。

根據向量點乘的數學定義：

是以：

等于向量

與法向量n的夾角的餘弦。為了友善書寫，定義一個常量

等式17）中隻有n是未知數。

5）将向量

旋轉角度C就得到向量n。假設

，根據向量旋轉的數學定義：

6）計算出n之後，就可以很友善的計算出外切線的切點

。從C1的圓心出發，沿着向量n的方向，距離為

的位置即為切點

亦然。

RSR、LSL、RSL、LSR是CSC類型的行駛曲線，該類型曲線首先計算兩個圓的切點，然後車輛沿着最小轉彎半徑構成的圓周行駛到第一個圓的切點，然後直行到第二個圓的切點，再沿着最小轉彎半徑構成的圓周行駛到目的地。下面我們以RSR軌迹為例看看如何計算行駛曲線。

假設起點

和終點

，最小轉彎半徑為

。然後我們計算起點和終點的圓心。

起點的圓心為:

終點的圓心為:

得到起點和終點的圓心之後，可以利用3.1小節的切點計算方法，得到切點

。然後就可以得到車輛的行駛軌迹，該軌迹分為三段：start到

的圓周弧；

的直線距離；

到Goal的圓周弧。至此我們得到了RSR的行駛曲線。

如下圖所示，

的圓心為

是與

相切的圓，圓心為

根據數學關系，可得到：

記

為

與

的夾角，已知三角形的三個邊的長度，根據餘弦定理，有：

最終可得到：

注意此處為LRL模式時，

需要加上

；為RLR模式時，

需要減去

然後計算

和計算

就變得很容易。定義向量

，将向量縮放到

最後可以得到交點

。按照同樣的過程可以計算得到

。然後就可以得到start到

到Goal的圓周弧的三段軌迹組成的行駛曲線。

我們向往遠方，卻忽略了此刻的美麗