Dubins曲线介绍
Dubins 曲线是在满足曲率约束和规定的始端和末端的切线（进入方向）的条件下，连接两个二维平面的最短路径，而且限制目标只能向前行进。Dubins 曲线的规划的目的是找到一条从起始点到目标点的最短距离，并且需要满足最小转弯半径、前进方向、初始相对位置和速度方向的曲线。将路径分为三段,依据每段路径的方向，Dubins 集合 D = {LSL，RSR，RSL，LSR， RLR， LRL}。L 表示从沿逆时针方向的圆弧运动，R 表示沿顺时针方向圆弧运动，L 表示沿直线运动。
它用于机器人导航时路径长度的计算，一般的做法是遍历评估所有方式的长度和时间，综合选择出最优的一种方式。
Dubins曲线Python实现
import math
import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot
show_animation = True
# 计算角度
def mod2pi(theta):
return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi)
# 转角
def pi_2_pi(angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
# 定义dubins曲线集合里的各个元素,dubins只能直行
# 1.左转-直行-左转
def left_straight_left(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
tmp0 = d + sa - sb
mode = ["L", "S", "L"]
p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sa - sb))
if p_squared < 0:
return None, None, None, mode
tmp1 = math.atan2((cb - ca), tmp0)
t = mod2pi(-alpha + tmp1)
p = math.sqrt(p_squared)
q = mod2pi(beta - tmp1)
return t, p, q, mode
# 2.右转-直行-右转
def right_straight_right(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
tmp0 = d - sa + sb
mode = ["R", "S", "R"]
p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sb - sa))
if p_squared < 0:
return None, None, None, mode
tmp1 = math.atan2((ca - cb), tmp0)
t = mod2pi(alpha - tmp1)
p = math.sqrt(p_squared)
q = mod2pi(-beta + tmp1)
return t, p, q, mode
# 3.左转-直行-右转
def left_straight_right(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
p_squared = -2 + (d * d) + (2 * c_ab) + (2 * d * (sa + sb))
mode = ["L", "S", "R"]
if p_squared < 0:
return None, None, None, mode
p = math.sqrt(p_squared)
tmp2 = math.atan2((-ca - cb), (d + sa + sb)) - math.atan2(-2.0, p)
t = mod2pi(-alpha + tmp2)
q = mod2pi(-mod2pi(beta) + tmp2)
return t, p, q, mode
# 4.右转-直行-左转
def right_straight_left(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
p_squared = (d * d) - 2 + (2 * c_ab) - (2 * d * (sa + sb))
mode = ["R", "S", "L"]
if p_squared < 0:
return None, None, None, mode
p = math.sqrt(p_squared)
tmp2 = math.atan2((ca + cb), (d - sa - sb)) - math.atan2(2.0, p)
t = mod2pi(alpha - tmp2)
q = mod2pi(beta - tmp2)
return t, p, q, mode
# 5.右转-左转-右转
def right_left_right(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
mode = ["R", "L", "R"]
tmp_rlr = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (sa - sb)) / 8.0
if abs(tmp_rlr) > 1.0:
return None, None, None, mode
p = mod2pi(2 * math.pi - math.acos(tmp_rlr))
t = mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0))
q = mod2pi(alpha - beta - t + mod2pi(p))
return t, p, q, mode
# 6.左转-右转-左转
def left_right_left(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
mode = ["L", "R", "L"]
tmp_lrl = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (- sa + sb)) / 8.0
if abs(tmp_lrl) > 1:
return None, None, None, mode
p = mod2pi(2 * math.pi - math.acos(tmp_lrl))
t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.0)
q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p))
return t, p, q, mode
def dubins_path_planning_from_origin(end_x, end_y, end_yaw, curvature,
step_size):
dx = end_x
dy = end_y
D = math.hypot(dx, dy)
d = D * curvature
theta = mod2pi(math.atan2(dy, dx))
alpha = mod2pi(- theta)
beta = mod2pi(end_yaw - theta)
# 遍历，比较路径代价，得到最短路径
planners = [left_straight_left, right_straight_right, left_straight_right,
right_straight_left, right_left_right,
left_right_left]
best_cost = float("inf")
bt, bp, bq, best_mode = None, None, None, None
for planner in planners:
t, p, q, mode = planner(alpha, beta, d)
if t is None:
continue
cost = (abs(t) + abs(p) + abs(q))
if best_cost > cost:
bt, bp, bq, best_mode = t, p, q, mode
best_cost = cost
lengths = [bt, bp, bq]
x_list, y_list, yaw_list, directions = generate_local_course(
sum(lengths), lengths, best_mode, curvature, step_size)
return x_list, y_list, yaw_list, best_mode, best_cost
# 数据处理，得到路径
# 插入并更新数据
def interpolate(ind, length, mode, max_curvature, origin_x, origin_y,
origin_yaw, path_x, path_y, path_yaw, directions):
if mode == "S":
path_x[ind] = origin_x + length / max_curvature * math.cos(origin_yaw)
path_y[ind] = origin_y + length / max_curvature * math.sin(origin_yaw)
path_yaw[ind] = origin_yaw
else: # curve
ldx = math.sin(length) / max_curvature
ldy = 0.0
if mode == "L": # 左转
ldy = (1.0 - math.cos(length)) / max_curvature
elif mode == "R": # 右转
ldy = (1.0 - math.cos(length)) / -max_curvature
gdx = math.cos(-origin_yaw) * ldx + math.sin(-origin_yaw) * ldy
gdy = -math.sin(-origin_yaw) * ldx + math.cos(-origin_yaw) * ldy
path_x[ind] = origin_x + gdx
path_y[ind] = origin_y + gdy
if mode == "L": # 左转
path_yaw[ind] = origin_yaw + length
elif mode == "R": # 右转
path_yaw[ind] = origin_yaw - length
if length > 0.0:
directions[ind] = 1
else:
directions[ind] = -1
return path_x, path_y, path_yaw, directions
def dubins_path_planning(s_x, s_y, s_yaw, g_x, g_y, g_yaw, c, step_size=0.1):
"""
s_x 起始点位置x
s_y 起始点位置y
s_yaw 起始点位姿角度
g_x x 终止点位置x
g_y y 终止点位置y
g_yaw 终止点位姿角度
c 曲率 [1/m]
"""
g_x = g_x - s_x
g_y = g_y - s_y
l_rot = Rot.from_euler('z', s_yaw).as_matrix()[0:2, 0:2]
le_xy = np.stack([g_x, g_y]).T @ l_rot
le_yaw = g_yaw - s_yaw
lp_x, lp_y, lp_yaw, mode, lengths = dubins_path_planning_from_origin(
le_xy[0], le_xy[1], le_yaw, c, step_size)
rot = Rot.from_euler('z', -s_yaw).as_matrix()[0:2, 0:2]
converted_xy = np.stack([lp_x, lp_y]).T @ rot
x_list = converted_xy[:, 0] + s_x
y_list = converted_xy[:, 1] + s_y
yaw_list = [pi_2_pi(i_yaw + s_yaw) for i_yaw in lp_yaw]
return x_list, y_list, yaw_list, mode, lengths
def generate_local_course(total_length, lengths, mode, max_curvature,
step_size):
n_point = math.trunc(total_length / step_size) + len(lengths) + 4
path_x = [0.0 for _ in range(n_point)]
path_y = [0.0 for _ in range(n_point)]
path_yaw = [0.0 for _ in range(n_point)]
directions = [0.0 for _ in range(n_point)]
index = 1
if lengths[0] > 0.0:
directions[0] = 1
else:
directions[0] = -1
ll = 0.0
for (m, l, i) in zip(mode, lengths, range(len(mode))):
if l > 0.0:
d = step_size
else:
d = -step_size
# 设置最初的状态
origin_x, origin_y, origin_yaw = \
path_x[index], path_y[index], path_yaw[index]
index -= 1
if i >= 1 and (lengths[i - 1] * lengths[i]) > 0:
pd = - d - ll
else:
pd = d - ll
while abs(pd) <= abs(l):
index += 1
path_x, path_y, path_yaw, directions = interpolate(
index, pd, m, max_curvature, origin_x, origin_y, origin_yaw,
path_x, path_y, path_yaw, directions)
pd += d
ll = l - pd - d # calc 为长度
index += 1
path_x, path_y, path_yaw, directions = interpolate(
index, l, m, max_curvature, origin_x, origin_y, origin_yaw,
path_x, path_y, path_yaw, directions)
if len(path_x) <= 1:
return [], [], [], []
while len(path_x) >= 1 and path_x[-1] == 0.0:
path_x.pop()
path_y.pop()
path_yaw.pop()
directions.pop()
return path_x, path_y, path_yaw, directions
def plot_arrow(x, y, yaw, length=0.5, width=0.2, fc="r",
ec="k"):
# 绘制箭头
if not isinstance(x, float):
for (i_x, i_y, i_yaw) in zip(x, y, yaw):
plot_arrow(i_x, i_y, i_yaw)
else:
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
fc=fc, ec=ec, head_width=width, head_length=width)
plt.plot(x, y)
def main():
print("Dubins路径规划")
# 输入起始终止点
start_x = 1.0 # 单位[m]
start_y = 1.0 # [m]
start_yaw = np.deg2rad(-45.0) # 转换成弧度[rad]
end_x = 1.0 # [m]
end_y = 2.0 # [m]
end_yaw = np.deg2rad(45.0) # [rad]
curvature = 1.0
path_x, path_y, path_yaw, mode, path_length = dubins_path_planning(start_x, start_y, start_yaw,
end_x, end_y, end_yaw, curvature)
if show_animation:
plt.plot(path_x, path_y, label="Dubins " + "".join(mode))
plot_arrow(start_x, start_y, start_yaw)
plot_arrow(end_x, end_y, end_yaw)
plt.legend()
plt.grid(True)
plt.axis("equal")
plt.show()
if __name__ == '__main__':
main()
学习总结
Dubins 曲线在任意的起止位姿之间都存在这样的曲线，即最短路径规划。但是在最小转弯半径较大的话，可能规划出的路径不合理，假设在路径规划中有障碍物，现实中可能很难实现 Dubins 路径，使其做到不会碰撞到障碍物，还需要添加其他算法来进行优化。而且Dubins曲线的局限性很大，车辆只能前向运动。
扫一扫
380
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
