【动手学运动规划】2.6 Reeds Shepp曲线

我出来打工，我不惦记钱，我惦记什么？ — 武林外传 黄豆豆

代码及环境配置：请参考 环境配置和代码运行!

---

Reeds Shepp，通常简称为RS曲线，是一种用于路径规划的算法，由J.A.Reeds和L.A.Shepp在1990年的论文《Optimal Paths for a Car That Goes Both Forwards and Backwards》中提出。该算法主要用于描述机器人或车辆在平面上的运动轨迹，特别是在需要考虑车辆前进和后退的情况下，寻找从起点到终点的最短路径。

2.4.1 Reeds Shepp曲线定义

简单的说, Reeds Shepp曲线在Dubins曲线的基础上，允许切换车辆行驶方向，这在某些情况下能够找到比Dubins曲线更短的路径。

Dubins曲线有6种组合方式, 以LSL为例, Reeds Shepp曲线为每一个运动基元都增加了前进($+$)和后退($-$), 就有了以下8种组合:

$$\begin{gathered} L^{+}{S}^{+}{L}^{+},L^{+}{S}^{+}{L}^{-},L^{+}{S}^{-}{L}^{+},L^{+}{S}^{-}{L}^{-}, \\ {L}^{-}{S}^{-}{L}^{+},L^{-}{S}^{+}{L}^{+},L^{-}{S}^{+}{L}^{-},L^{-}{S}^{-}{L}^{-}, \end{gathered}$$

如果用$|$表示车辆运动朝向反转, 可以简写成下表表示所有的组合:

$$\begin{array}{lllll}\text{ Base }& \alpha & \beta & \gamma & d\\ C_{\alpha }\left|C_{\beta }\right|C_{\gamma }& [0,\pi ]& [0,\pi ]& [0,\pi ]& -\\ C_{\alpha }\mid C_{\beta }{C}_{\gamma }& [0,\beta ]& [0,\pi /2]& [0,\beta ]& -\\ C_{\alpha }{C}_{\beta }\mid C_{\gamma }& [0,\beta ]& [0,\pi /2]& [0,\beta ]& -\\ C_{\alpha }{S}_d{C}_{\gamma }& [0,\pi /2]& -& [0,\pi /2]& (0,\infty )\\ C_{\alpha }{C}_{\beta }\mid C_{\beta }{C}_{\gamma }& [0,\beta ]& [0,\pi /2]& [0,\beta ]& -\\ C_{\alpha }\left|C_{\beta }{C}_{\beta }\right|C_{\gamma }& [0,\beta ]& [0,\pi /2]& [0,\beta ]& -\\ C_{\alpha }\left|C_{\pi /2}{S}_d{C}_{\pi /2}\right|C_{\gamma }& [0,\pi /2]& -& [0,\pi /2]& (0,\infty )\\ C_{\alpha }\mid C_{\pi /2}{S}_d{C}_{\gamma }& [0,\pi /2]& -& [0,\pi /2]& (0,\infty )\\ C_{\alpha }{S}_d{C}_{\pi /2}{C}_{\gamma }& [0,\pi /2]& -& [0,\pi /2]& (0,\infty )\end{array}$$

弧线的下标代表旋转的角度, 直线S的下标代表距离.

将$C$替换为$L$,或者$R$ ，通过简单的变换，则共有 48 种字段组合

$$\begin{array}{ll}\text{ Base word }& \text{ Sequences of motion primitives }\\ C|C|C& \left(L^{+}{R}^{-}{L}^{+}\right)\left(L^{-}{R}^{+}{L}^{-}\right)\left(R^{+}{L}^{-}{R}^{+}\right)\left(R^{-}{L}^{+}{R}^{-}\right)\\ CC\mid C& \left(L^{+}{R}^{+}{L}^{-}\right)\left(L^{-}{R}^{-}{L}^{+}\right)\left(R^{+}{L}^{+}{R}^{-}\right)\left(R^{-}{L}^{-}{R}^{+}\right)\\ C\mid CC& \left(L^{+}{R}^{-}{L}^{-}\right)\left(L^{-}{R}^{+}{L}^{+}\right)\left(R^{+}{L}^{-}{R}^{-}\right)\left(R^{-}{L}^{+}{R}^{+}\right)\\ CSC& \left(L^{+}{S}^{+}{L}^{+}\right)\left(L^{-}{S}^{-}{L}^{-}\right)\left(R^{+}{S}^{+}{R}^{+}\right)\left(R^{-}{S}^{-}{R}^{-}\right)\\ & \left(L^{+}{S}^{+}{R}^{+}\right)\left(L^{-}{S}^{-}{R}^{-}\right)\left(R^{+}{S}^{+}{L}^{+}\right)\left(R^{-}{S}^{-}{L}^{-}\right)\\ C{C}_{\beta }\mid C_{\beta }C& \left(L^{+}{R}_{\beta }^{+}{L}_{\beta }^{-}{R}^{-}\right)\left(L^{-}{R}_{\beta }^{-}{L}_{\beta }^{+}{R}^{+}\right)\left(R^{+}{L}_{\beta }^{+}{R}_{\beta }^{-}{L}^{-}\right)\left(R^{-}{L}_{\beta }^{-}{R}_{\beta }^{+}{L}^{+}\right)\\ C\left|C_{\beta }{C}_{\beta }\right|C& \left(L^{+}{R}_{\beta }^{-}{L}_{\beta }^{-}{R}^{+}\right)\left(L^{-}{R}_{\beta }^{+}{L}_{\beta }^{+}{R}^{-}\right)\left(R^{+}{L}_{\beta }^{-}{R}_{\beta }^{-}{L}^{+}\right)\left(R^{-}{L}_{\beta }^{+}{R}_{\beta }^{+}{L}^{-}\right)\\ C\mid C_{\pi /2}SC& \left(L^{+}{R}_{\pi /2}^{-}{S}^{-}{R}^{-}\right)\left(L^{-}{R}_{\pi /2}^{+}{S}^{+}{R}^{+}\right)\left(R^{+}{L}_{\pi /2}^{-}{S}^{-}{L}^{-}\right)\left(R^{-}{L}_{\pi /2}^{+}{S}^{+}{L}^{+}\right)\\ & \left(L^{+}{R}_{\pi /2}^{-}{S}^{-}{L}^{-}\right)\left(L^{-}{R}_{\pi /2}^{+}{S}^{+}{L}^{+}\right)\left(R^{+}{L}_{\pi /2}^{-}{S}^{-}{R}^{-}\right)\left(R^{-}{L}_{\pi /2}^{+}{S}^{+}{R}^{+}\right)\\ CS{C}_{\pi /2}\mid C& \left(L^{+}{S}^{+}{L}_{\pi /2}^{+}{R}^{-}\right)\left(L^{-}{S}^{-}{L}_{\pi /2}^{-}{R}^{+}\right)\left(R^{+}{S}^{+}{R}_{\pi /2}^{+}{L}^{-}\right)\left(R^{-}{S}^{-}{R}_{\pi /2}^{-}{L}^{+}\right)\\ & \left(R^{+}{S}^{+}{L}_{\pi /2}^{+}{R}^{-}\right)\left(R^{-}{S}^{-}{L}_{\pi /2}^{-}{R}^{+}\right)\left(L^{+}{S}^{+}{R}_{\pi /2}^{+}{L}^{-}\right)\left(L^{-}{S}^{-}{R}_{\pi /2}^{-}{L}^{+}\right)\\ C\left|C_{\pi /2}S{C}_{\pi /2}\right|C& \left(L^{+}{R}_{\pi /2}^{-}{S}^{-}{L}_{\pi /2}^{-}{R}^{+}\right)\left(L^{-}{R}_{\pi /2}^{+}{S}^{+}{L}_{\pi /2}^{+}{R}^{-}\right)\\ & \left(R^{+}{L}_{\pi /2}^{-}{S}^{-}{R}_{\pi /2}^{-}{L}^{+}\right)\left(R^{-}{L}_{\pi /2}^{+}{S}^{+}{R}_{\pi /2}^{+}{L}^{-}\right)\end{array}$$

2.4.2 Reeds Shepp曲线公式

由于公式推导与Dubins曲线大体一致, 不再赘述. 由于Reeds Shepp曲线的不同组合曲线之间存在镜像, 翻转的关系, 并不需要对每一种组合都一一计算. 可以只求解12种组合, 通过时间变换、反射变换和逆向变换 快速计算剩下的情况. 这里大体介绍一下, 具体公式见参考链接.

2.4.2.1 计算技巧

以下技巧都需要先进行上一节提到的, 转换坐标系和正则化.

(1) 时间翻转(timeflip)

将计算出的曲线按照其运动方向进行取反，得到的新的曲线为原曲线相反的曲线。

以$L^{+}{S}^{+}{L}^{+}$为例, 如果对其曲线沿着y轴翻转, 也就是x坐标取反, 则会得到$L^{-}{S}^{-}{L}^{-}$曲线.

(2) 反射(reflect)

将计算的曲线按照其沿圆周运动方向取反, 在转换后的坐标系下, 沿着x轴翻转, 也就是y坐标取反.

得到的结果是运动基元取反的组合, L不变.

以$L^{+}{S}^{+}{L}^{+}$为例, 如果对其曲线沿着y轴翻转, 也就是x坐标取反, 则会得到$R^{+}{S}^{+}{R}^{+}$曲线.

(3) 向后变换(backwards)

将原曲线的路径逆序转换，得到的曲线与原来的曲线长度相同的新曲线。

得到的结果是运动基元顺序颠倒的组合.

以$L^{+}{S}^{+}{R}^{+}$为例, 如果对其曲线沿着y轴翻转, 也就是x坐标取反, 则会得到$R^{+}{S}^{+}{L}^{+}$曲线.

2.4.2.2 不同技巧的组合方式

(1) $C|C|C$

即三段圆弧且每段圆弧两两圆弧方向相反, 基于此, 可以扩展出四种曲线形式:

$$L^{+}{R}^{-}{L}^{+}、L^{-}{R}^{+}{L}^{-}、R^{+}{L}^{-}{R}^{+}、R^{-}{L}^{+}{R}^{-}\text{。 }$$

(2) $CC\mid C$

即三段圆弧且后两段圆弧的圆弧方向相反, 基于此, 可以扩展出四种曲线形式:

$$L^{+}{R}^{+}{L}^{-}、L^{-}{R}^{-}{L}^{+}、R^{+}{L}^{+}{R}^{-}、R^{-}{L}^{-}{R}^{+}\text{。 }$$

(3) $C\mid CC$,

即三段圆弧且前两段圆弧的圆弧方向相反, 基于此, 可以扩展出四种曲线形式:

$$L^{+}{R}^{-}{L}^{-}、L^{-}{R}^{+}{L}^{+}、R^{+}{L}^{-}{R}^{-}、R^{-}{L}^{+}{R}^{+}\text{。 }$$

2.6.c Reeds Shepp曲线代码解析

本节提供了Reeds Shepp曲线的代码测试

python3 tests/curves/reeds_shepp_path_test.py

2.6.c.1 Reeds Shepp曲线代码实现

Reeds Shepp实现代码框架与上一节Dubins曲线整体类似, 仅做简要描述.

path_functions 中存储了完整的曲线组合接口:

  path_functions = [
      left_straight_left,
      left_straight_right,  # CSC
      left_x_right_x_left,
      left_x_right_left,
      left_right_x_left,  # CCC
      left_right_x_left_right,
      left_x_right_left_x_right,  # CCCC
      left_x_right90_straight_left,
      left_x_right90_straight_right,  # CCSC
      left_straight_right90_x_left,
      left_straight_left90_x_right,  # CSCC
      left_x_right90_straight_left90_x_right,
  ]  # CCSCC

以left_straight_left为例, 返回该组合是否生成成功, 旋转角度1:t, 直线距离:u, 旋转角度2:v

def left_straight_left(x, y, phi):
    u, t = polar(x - math.sin(phi), y - 1.0 + math.cos(phi))
    if 0.0 <= t <= math.pi:
        v = mod2pi(phi - t)
        if 0.0 <= v <= math.pi:
            return True, [t, u, v], ["L", "S", "L"]

    return False, [], []

在generate_path 中会将生成的有效的RS曲线放到paths 中, 最终在reeds_shepp_path_planning 中挑选总长度最短的组合.

2.6.c.2 Reeds Shepp曲线代码测试

在main() 函数中, 生成了一组始末状态的随机数, 调用接口, 并可视化.

  for i in range(0, 5):
      start_x = random.uniform(-5, 5)  # [m]
      start_y = random.uniform(-5, 5)  # [m]
      start_yaw = np.deg2rad(random.uniform(-45, 45))  # [rad]

      end_x = random.uniform(-15, 15)  # [m]
      end_y = random.uniform(-15, 15)  # [m]
      end_yaw = np.deg2rad(random.uniform(145, 225))  # [rad]

      curvature = random.uniform(0.1, 0.3)
      step_size = 0.05

      xs, ys, yaws, modes, lengths = reeds_shepp_path_planning(
          start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size
      )

参考链接

• https://blog.csdn.net/weixin_42301220/article/details/125382518

️自动驾驶小白说官网：https://www.helloxiaobai.cn