自動駕駛運動規划-Reeds Shepp曲線

本文轉載自查看原文 2021-07-28 19:54 230

自動駕駛運動規划-Reeds Shepp曲線

相比於Dubins Car只允許車輛向前運動，Reeds Shepp Car既允許車輛向前運動，也允許車輛向后運動。

Reeds Shepp Car運動規划

1、車輛模型

車輛運動模型仍然采用Simple Car Model，但增加對車輛運動方向的描述，運動方程如下：

$\dot{x} = u_1 cos{\theta}$

$\dot{y} = u_1 sin{\theta}$

$\dot{\theta} = u_1 u_2$

其中， $u_1 \in \{-1, 1\}$ ， $u_2 \in [-tan \phi_{max}, tan \phi_{max}]$ 。當 $u_1 = 1$ 時，表示車輛向前運動； $u_1=-1$ 時，表示車輛向后運動。

2、Reeds-Shepp Car

J Reeds和L Shepp證明Reeds Shepp Car從起點 $q_I$ 到終點 $q_G$ 的最短路徑一定是下面的word的其中之一。word中的"|"表示車輛運動朝向由正向轉為反向或者由反向轉為正向。

圖片來源：Planning Algorithm，http://planning.cs.uiuc.edu/node822.html

每個word都由 $L^{+}$ ， $L^{-}$ ， $R^{+}$ ， $R^{-}$ ， $S^{+}$ ， $S^{-}$ 這六種primitives組成，其中 $L^{+}$ 表示車輛左轉前進； $L^{-}$ 表示車輛左轉后退； $R^{+}$ 表示車輛右轉前進； $R^{-}$ 表示車輛右轉后退； $S^{+}$ 表示車輛直行前進； $S^{-}$ 表示車輛直行后退。

Reeds and Shepp曲線的word所有組合不超過48種，所有的組合一一枚舉如下：

圖片來源：Planning Algorithm，http://planning.cs.uiuc.edu/node822.html

3、計算優化

3.1 位置姿態統一化

車輛的起點和終點的位置姿態是難以窮舉的，所以一般在計算之前，會將車輛的姿態歸一化：

起始姿態： $q_I = (0, 0, 0)$ ；

目標姿態： $q_G=(x, y, \phi)$ ；其中， $\phi=\theta_2 - \theta_1$

車輛的轉彎半徑： r = 1;

假設車輛的初始姿態為 $q_I = (x_1, y_1, \theta_1)$ ，目標姿態 $q_G=(x_2, y_2, \theta_2)$ ，車輛的轉向半徑為r = $\rho$ ，如何實現姿態的歸一化呢，實際上歸一化的過程就是向量的平移和旋轉過程。歸一化：為了方便起見Reeds-Shepp中最小轉向半徑強制設置為1，如果車輛的實際最小轉向半徑不是1，可也通過適當放縮終點坐標來計算該曲線。比如如果一個車輛的最小轉向半徑為10，終點坐標為x,y，如果我們在計算曲線的時候將終點設為x/10,y/10，計算所得路徑放大10倍曲線的轉向半徑就是10，終點也是x,y，所得曲線就是我們所期望曲線。

首先將向量 $\vec{q_I q_G}$ 平移到坐標原點(0,0)。平移 $q_I$ 到O(0, 0)，平移向量為 $(-x_1, -y_1)$ ；對 $q_G$ 應用同樣的平移向量: $q_G = [x_2 - x_1, y_2 - y_1]$ ，最后得到平移后的向量：

$\vec{q_I q_G} = \begin{bmatrix} x_2 - x_1 \\ y_2 - y_1 \\ \end{bmatrix} = \begin{bmatrix} d_x \\ d_y \\ \end{bmatrix} \\$

應用旋轉矩陣，將車輛的起點朝向轉到x軸正向：

$\begin{bmatrix} cos \theta_1 & sin \theta_1 \\ -sin \theta_1 & cos \theta_1 \\ \end{bmatrix} \begin{bmatrix} d_x \\ d_y \\ \end{bmatrix} = \begin{bmatrix} d_x cos \theta_1 + d_y sin \theta_1 \\ -d_x sin \theta_1 + d_y cos \theta_1 \\ \end{bmatrix} \\$

旋轉之后，目標位置朝向更新為 $\phi = \theta_2 - \theta_1$ 。

將車輛轉向半徑縮放到1，於是最終得到車輛運動的起始姿態：

$I = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} \\$

目標姿態：

$G= \begin{bmatrix} x \\ y \\ \phi \end{bmatrix} = \begin{bmatrix} (d_x cos \theta_1 + d_y sin \theta_1) / \rho \\ (-d_x sin \theta_1 + d_y cos \theta_1 ] / \rho \\ \theta2 - \theta_1 \end{bmatrix} \\$

代碼如下:

double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw(); double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw(); double dx = x2 - x1, dy = y2 - y1, c = cos(th1), s = sin(th1); double x = c * dx + s * dy, y = -s * dx + c * dy, phi = th2 - th1; return ::reedsShepp(x / rho_, y / rho_, phi)

3.2 利用對稱關系降低求解復雜度

Reeds Shepp曲線有48種組合，編程時一一編碼計算比較麻煩，因此可以利用其對稱性降低求解工作量。

以轉向不同的CSC類型為例，它包含4種曲線類型： $L^{+}S^{+}R^{+}$ 、 $L^{-}S^{-}R^{-}$ 、 $R^{+}S^{+}L^{+}$ 、 $R^{-}S^{-}R^{-}$ ，我們只需要編碼推導得到 $L^{+}S^{+}R^{+}$ 的計算過程，其它幾種直接可以通過對稱性關系得到車輛運動路徑。

給定車輛起始姿態 $q_I=(160, 160, 0)$ ，目標姿態 $q_G=(190, 180, 30)$ ，可以得到 $L^{+}S^{+}R^{+}$ 的運動路徑如下：

{ Steering: left        Gear: forward    distance: 0.63 } { Steering: straight Gear: forward distance: 4.02 } { Steering: right Gear: forward distance: 0.11 }

對應的效果如下：

L^{+}S^{+}R^{+}

下面看看利用對稱性求解其它幾種路徑的方法。

3.2.1 timefilp對稱性

假設我們推導出從起始姿態 $q_I(x_1,y_1, \theta_1)$ 達到目標姿態 $(x_2, y_2, \theta_2)$ 的路徑計算方法：

path = calc_path( $x_1$ , $y_1$ , $\theta_1$ , $x_2$ , $y_2$ , $\theta_2$ )

利用對稱性，將目標Pose修改為 $(-x_2, y_2, -\theta_2)$ ，代入同樣的Path計算函數：

path = calc_path( $x_1$ , $y_1$ , $\theta_1$ , - $x_2$ , $y_2$ , - $\theta_2$ )

就得到從 $(x_1, y_1, \theta_1)$ 到 $(x_2, y_2, \theta_2)$ 的 $L^{-}S^{-}R^{-}$ 類型的運動路徑。

計算出的 $L^{-}S^{-}R^{-}$ 的車輛運動路徑如下：

{ Steering: left        Gear: backward    distance: -2.85 } { Steering: straight Gear: backward distance: 4.02 } { Steering: right Gear: backward distance: -2.32 }

下面是車輛的運動效果，一路倒車進入另一個車位。

3.2.2 reflect對稱性

將目標姿態修改為 $(x_2, -y_2, -\theta_2)$ ，代入同樣的Path計算函數：

path = calc_path( $x_1$ , $y_1$ , $\theta_1$ , $x_2$ , - $y_2$ , - $\theta_2$ )

就得到從 $(x_1, y_1, \theta_1)$ 到 $(x_2, y_2, \theta_2)$ 的 $R^{+}S^{+}L^{+}$ 類型的運動路徑。

計算出的 $R^{+}S^{+}L^{+}$ 的車輛運動路徑如下：

{ Steering: right        Gear: forward    distance: -0.56 } { Steering: straight Gear: forward distance: 5.28 } { Steering: left Gear: forward distance: -0.03 }

下面是車輛先右轉、再直行、再左轉從一個車位進入另一個車位的運動效果。

R^{+}S^{+}L^{+}類型曲線

3.2.3 timeflip + reflect

結合timeflip對稱性和reflect對稱性，將目標姿態修改為 $(-x_2, -y_2, \theta_2)$ ，代入同樣的Path計算函數：

path = calc_path( $x_1$ , $y_1$ , $\theta_1$ , - $x_2$ , - $y_2$ , $\theta_2$ )

就得到從 $(x_1, y_1, \theta_1)$ 到 $(x_2, y_2, \theta_2)$ 的 $R^{-}S^{-}L^{-}$ 類型的運動路徑。

計算出的 $R^{-}S^{-}L^{-}$ 的車輛運動路徑如下：

{ Steering: right    Gear: backward    distance: -1.86 } { Steering: straight Gear: backward distance: 5.28 } { Steering: left Gear: backward distance: -2.38 }

下面是車輛先右轉、再直行、再左轉，全程倒車從一個車位進入另一個車位的運動效果。

R^{-}S^{-}L^{-}類型曲線

通過對稱性，48種不同的Reeds Shepp曲線通過不超過12個函數就可以得到全部運動路徑。

參考鏈接

1、Optimal paths for a car that goes both forwards and backwards, J Reeds, L Shepp - Pacific journal of mathematics, 1990

2、OMPL的Reeds Sheep實現代碼。(https://ompl.kavrakilab.org/ReedsSheppStateSpace_8cpp_source.html)

3、Reeds Sheep的Python代碼實現。(https://github.com/nathanlct/reeds-shepp-curves/blob/master/reeds_shepp.py)

轉自：https://zhuanlan.zhihu.com/p/122544884

免責聲明！

本站轉載的文章為個人學習借鑒使用，本站對版權不負任何法律責任。如果侵犯了您的隱私權益，請聯系本站郵箱yoyou2525@163.com刪除。

猜您在找 自動駕駛綜述|定位、感知、規划常見算法匯總 ROS 自動駕駛自動駕駛OS Mobileye 自動駕駛策略（一）一種新穎魯棒的自動駕駛車輛換道軌跡規划方法自動駕駛中的三大坐標系自動駕駛感知系統盤點自動駕駛數據集量產自動駕駛算法入門高精地圖與自動駕駛（下）