我出来打工，我不惦记钱，我惦记什么？ — 武林外传 黄豆豆
🏰代码及环境配置：请参考 环境配置和代码运行!
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种组合:
如果用表示车辆运动朝向反转, 可以简写成下表表示所有的组合:
弧线的下标代表旋转的角度, 直线S的下标代表距离.
将替换为,或者 ，通过简单的变换，则共有 48 种字段组合
2.4.2 Reeds Shepp曲线公式
由于公式推导与Dubins曲线大体一致, 不再赘述. 由于Reeds Shepp曲线的不同组合曲线之间存在镜像, 翻转的关系, 并不需要对每一种组合都一一计算. 可以只求解12种组合, 通过时间变换、反射变换和逆向变换 快速计算剩下的情况. 这里大体介绍一下, 具体公式见参考链接.
2.4.2.1 计算技巧
以下技巧都需要先进行上一节提到的, 转换坐标系和正则化.
(1) 时间翻转(timeflip)
将计算出的曲线按照其运动方向进行取反，得到的新的曲线为原曲线相反的曲线。
以为例, 如果对其曲线沿着y轴翻转, 也就是x坐标取反, 则会得到曲线.
(2) 反射(reflect)
将计算的曲线按照其沿圆周运动方向取反, 在转换后的坐标系下, 沿着x轴翻转, 也就是y坐标取反.
得到的结果是运动基元取反的组合, L不变.
以为例, 如果对其曲线沿着y轴翻转, 也就是x坐标取反, 则会得到曲线.
(3) 向后变换(backwards)
将原曲线的路径逆序转换，得到的曲线与原来的曲线长度相同的新曲线。
得到的结果是运动基元顺序颠倒的组合.
以为例, 如果对其曲线沿着y轴翻转, 也就是x坐标取反, 则会得到曲线.
2.4.2.2 不同技巧的组合方式
(1)
即三段圆弧且每段圆弧两两圆弧方向相反, 基于此, 可以扩展出四种曲线形式:
(2)
即三段圆弧且后两段圆弧的圆弧方向相反, 基于此, 可以扩展出四种曲线形式:
(3) ,
即三段圆弧且前两段圆弧的圆弧方向相反, 基于此, 可以扩展出四种曲线形式:
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
