路徑規劃算法(0) -路徑規劃與 Dubins, Reeds-Shepp曲線的python實現

參考資料: http://planning.cs.uiuc.edu/node821.html

一般常見的路徑規劃算法, 如 Dijkstra, A*, BFS算法, 重點關注可達性, 搜索速度和效率上等問題, 這些軌跡或以柵格或以圖的形式給出路徑, 事實上還沒有考慮到,實際機器人的運動特徵, 這裏就要介紹到兩種路徑規劃中常用到的曲線, Dubins曲線和 Reeds-Shepp 曲線, 最大的區別 Dubins曲線適用於只能前向行駛的車, 而Reeds-Shepp曲線適用於既可以向前也可以後退的車

Dubins 曲線

Dubins曲線通常用於機器人和控制理論領域，作爲規劃輪式機器人、飛機和水下車輛路徑的一種方式。Dubins曲線給出了兩個有向點之間的最短路徑, 這樣的路徑是實際上可以去實際跟隨執行的.
簡單的運動模型如下,
$\dot{x}= V cos(\theta)$
$\dot{y}= V sin(\theta)$
$\dot{\theta} = \mu$

其中(x,y) 是汽車的位置, $\theta$ 是航向, $\mu$爲轉彎速度. 這樣的條件下, 對於規劃路徑有幾個限制.

1. 機器人最大的速度 $V_{max}$
2. 最大的轉彎角度$\phi_{max}$, 也對應這最小的轉彎半徑.

Dubins曲線可以用常用的三種運動描述, $S$ 直線行駛, $L$左轉, $R$ 右轉. 所以常用的集中運動可以總結如下:
${ LRL, RLR, LSL, LSR, RSL, RSR}$

這樣的軌跡就被稱爲 Dubins路徑, 兩個有向點的最短路徑可以如下如所示:

觀察上圖, $R_{\alpha} S_{d}L_{\gamma}$ 由兩個相切圓和一段直線組成, 而 $R_{\alpha} L_{\beta}R_{\gamma}$ , 所以現在的問題變成了 那種路徑方式有最小時間消耗., 我們可以嘗試編程解決.
(1) RSL 路徑方程舉例:
最小轉彎半徑爲 $r$, $q_{l}(x_l,y_l,\theta_{1})$, $q_{G}(x_G,y_G,\theta_{2})$

• 添加約束
  • $y軸約束: y_G - y_l $
  • $x軸約束: x_G - x_l $
  • $\theta 約束: \alpha - \beta = \theta_2 -\theta_1$
• $路徑總長度S = r sin(\alpha) + d + r sin(\gamma)$
  具體細節就先不展示了, 方程應該很好列出來, 總之當最小轉彎半徑$r$確定之後, $\alpha$, $\beta$, $d$, 就能解出來了.

Reeds-Shepp 曲線

現在來考慮 Reeds-Shepp 型的車, 與Dubins車型唯一區別就是. 它可以向後走

同理, Reeds-Shepp 曲線加入了後退的邏輯, 可能的路徑類型就擴大很多, 可以規劃爲9類, 一共有48種

應該也是可以根據約束列方程求解; 我將繼續探索, 來日完善…

Python 待完善