你的人生刚开始，没有到头。 —狂飙 高启强
🏰代码及环境配置：请参考 环境配置和代码运行!
Dubins曲线是在满足曲率约束, 始末点(x, y, theta)约束, 只能前向 的约束下，一组连接始末点的几何线条的组合.
2.5.1 Dubins曲线定义
Dubins曲线通常由圆弧段, 直线段组合而成, 有如下约束:
前向: 沿着Dubins曲线行驶的车辆只能向前行驶 曲率约束：Dubins曲线的圆弧段满足特定的曲率约束 切线方向：在起始点和终点，Dubins曲线的切线方向与给定的方向一致
Dubins曲线定义了这几种运动基元:右转弧线（Right）、左转弧线（Left）或直线行驶（Straight）. 他们可以组合成六种基本类型：RSR、RSL、LSR、LSL、RLR、LRL。
假设A点是起点, B点是终点. Dubins曲线会在起点和终点画两个圆, 然后寻找中间线连接这两个圆.
RSL
RSR
寻找这两个圆的公切线. 对应了RSR、RSL、LSR、LSL这四种 圆弧-直线-圆弧 的组合
LRL
或者是构造一个新的中间圆来连接这两个圆, 对应了 RLR、LRL 这两种 圆弧-圆弧-圆弧 的组合
起点和终点的圆的半价, 可以简单设定为最小转弯半径. 因为半价越小, 越容易有解. 但是对于车辆来说, 一直采用最大转角显然不太合理. 所以也可以通过对转弯半价采样的方式, 尝试能不能找到曲率较小的结果.
Dubins曲线会在这些组合中, 挑选一个路径总长度最小的组合作为最优解, 当然也可以根据需求自行确定最优解.
了解了Dubins曲线的定义之后, 接下来的问题就是如何计算这几个中间点.
2.5.2 不同运动基元的状态转移
我们首先计算这些不同的运动基元的状态转移方程.
初始状态是, 终点状态是
2.5.2.1 直行S状态转移方程
直线段的长度为p
2.5.2.2 左转L状态转移方程
转过的角度是
2.5.2.3 右转R状态转移方程
2.5.3 组合不同运动基元
我们组合不同的运动基元, 以LSL为例:
(1)第1步: L. 设第一步L的结束状态是, L转过的角度是. 代入初始状态得:
(2)第2步: S. 设第2步S的结束状态是, 代入得:
(3)第3步: L. 第3步L的结束状态就是终点状态, 代入得:
(4)第4步:联立求解上面的式子, 可以计算出未知项: 转过的角度, 直线段长度, 总长度
其中是指将角度转换到的区间内, 其他的组合诸如LSR,RSL可以类似的方式进行推导.
2.5.4 简洁的计算方式
如果我们将坐标系原点放在起始点, 以始末点的连线作为坐标系的x轴方向. 仍然以LSL为例, 如下图:
新的坐标系下, 起始点状态是, 终点状态是. 这里将是为了正则化R，这样每个圆的半径都为1，由角度计算弧长时更方便，弧长即等于角度的弧度.
与2.5.3 类似, 可以组合式的调用运动基元状态状态方程, 最终得到出如下方程
比2.5.3简洁很多, 联立求解可得:
其他的组合也是类似的推导.
2.5.c Dubins曲线代码解析
python3 tests/curves/dubins_path_test.py
2.5.c.1 Dubins曲线代码实现
plan_dubins_path 函数, 是Dubins曲线生成接口. 输入始末点状态和曲率:s_x, s_y, s_yaw, g_x, g_y, g_yaw, curvature , 输出一组曲线点的状态,组合方式和总长度:x_list, y_list, yaw_list, modes, lengths
_PATH_TYPE_MAP 中存储了Dubins曲线的6种组合方式, 之后先把全局坐标系下的始末点做了平移旋转, 转换到了以起始点为原点的坐标系下. 调用了_dubins_path_planning_from_origin 接口.
_PATH_TYPE_MAP = {
"LSL": _LSL,
"RSR": _RSR,
"LSR": _LSR,
"RSL": _RSL,
"RLR": _RLR,
"LRL": _LRL,
}
if selected_types is None:
planning_funcs = _PATH_TYPE_MAP.values()
else:
planning_funcs = [_PATH_TYPE_MAP[ptype] for ptype in selected_types]
# calculate local goal x, y, yaw
l_rot = rot_mat_2d(s_yaw)
le_xy = np.stack([g_x - s_x, g_y - s_y]).T @ l_rot
local_goal_x = le_xy[0]
local_goal_y = le_xy[1]
local_goal_yaw = g_yaw - s_yaw
lp_x, lp_y, lp_yaw, modes, lengths = _dubins_path_planning_from_origin(
local_goal_x, local_goal_y, local_goal_yaw, curvature, step_size, planning_funcs
)
在_dubins_path_planning_from_origin 中, 计算了2.5.4中的几个参数:
def _dubins_path_planning_from_origin(
end_x, end_y, end_yaw, curvature, step_size, planning_funcs
):
dx = end_x
dy = end_y
d = hypot(dx, dy) * curvature
theta = _mod2pi(atan2(dy, dx))
alpha = _mod2pi(-theta)
beta = _mod2pi(end_yaw - theta)
之后遍历所有组合, 生成曲线. d1, d2, d3 是3个运动基元曲线的长度, mode是3个运动基元类型.
在所有曲线中, 挑选总长度最小的作为最佳曲线.
for planner in planning_funcs:
d1, d2, d3, mode = planner(alpha, beta, d)
if d1 is None:
continue
cost = abs(d1) + abs(d2) + abs(d3)
if best_cost > cost: # Select minimum length one.
b_d1, b_d2, b_d3, b_mode, best_cost = d1, d2, d3, mode, cost
具体的运动基元组合planner , 以上一节推导的LSL为例.
d1, d2, d3对应, 对应是2.5.4提到的正则化, 因为R是1, 角度即是弧长.
def _LSL(alpha, beta, d):
sin_a, sin_b, cos_a, cos_b, cos_ab = _calc_trig_funcs(alpha, beta)
mode = ["L", "S", "L"]
p_squared = 2 + d**2 - (2 * cos_ab) + (2 * d * (sin_a - sin_b))
if p_squared < 0: # invalid configuration
return None, None, None, mode
tmp = atan2((cos_b - cos_a), d + sin_a - sin_b)
d1 = _mod2pi(-alpha + tmp)
d2 = sqrt(p_squared)
d3 = _mod2pi(beta - tmp)
return d1, d2, d3, mode
挑出最佳曲线之后, 需要将坐标转回到全局坐标系. 也要将曲率半径重新乘回去, 收放到正则化之前坐标.
lengths = [b_d1, b_d2, b_d3]
x_list, y_list, yaw_list = _generate_local_course(
lengths, b_mode, curvature, step_size
)
lengths = [length / curvature for length in lengths]
2.5.c.2 Dubins曲线代码测试
在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)
path_x, path_y, path_yaw, mode, lengths = plan_dubins_path(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature
)
参考链接
https://zhuanlan.zhihu.com/p/414753861
🏎️自动驾驶小白说官网：https://www.helloxiaobai.cn
