迁移到dhxdl
This commit is contained in:
王闯
@@ -0,0 +1,607 @@
|
||||
"""Reeds-Shepp 曲线计算。
|
||||
|
||||
参照 Reeds & Shepp (1990) 论文公式 8.1-8.11 实现的纯 Python 版本。
|
||||
与常见实现（只返回最短路径）不同，这里保留全部 48 条候选 word，
|
||||
每条都带端点校验：只有正向积分能命中终点位姿的才标记为有效。
|
||||
|
||||
另外提供 include_dubins 选项：RS 的公式把弧长归一到 [-pi, pi) 并要求
|
||||
各段非负，结构上不可能输出超过 pi 的弧（RS 引理：最优路径不含 > pi 的
|
||||
弧，因为总能换一个带 cusp 的走法把它变短）。代价是绕远的纯前进解会
|
||||
直接从候选里消失。开启后用 [0, 2pi) 把这 6 条 Dubins 解重解出来，
|
||||
其中 L+R+L+ / R+L+R+ 不在标准 48 word 内。
|
||||
|
||||
约定（归一化坐标，转弯半径 = 1）：
|
||||
steering: +1 左转(L) / 0 直行(S) / -1 右转(R)
|
||||
gear: +1 前进(+) / -1 倒车(-)
|
||||
length: 段长度，>= 0（曲线段为转过的弧度，直线段为距离）
|
||||
"""
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from typing import Callable
|
||||
|
||||
PI = math.pi
|
||||
|
||||
|
||||
@dataclass
|
||||
class Segment:
|
||||
steering: int # +1 L, 0 S, -1 R
|
||||
gear: int # +1 forward, -1 backward
|
||||
length: float # >= 0
|
||||
|
||||
|
||||
def _make(length: float, steering: int, gear: int) -> Segment:
|
||||
"""构造段，若 length 为负则翻转 gear 并取绝对值。"""
|
||||
if length < 0:
|
||||
return Segment(steering, -gear, -length)
|
||||
return Segment(steering, gear, length)
|
||||
|
||||
|
||||
def _polar(x: float, y: float):
|
||||
return math.hypot(x, y), math.atan2(y, x)
|
||||
|
||||
|
||||
def _mod2pi(theta: float) -> float:
|
||||
"""归一化到 [-pi, pi)。"""
|
||||
v = theta % (2 * PI)
|
||||
if v >= PI:
|
||||
v -= 2 * PI
|
||||
return v
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 基础公式：每个函数尝试用某一类 word 连接 (0,0,0) 到 (x,y,phi)。
|
||||
# phi 为弧度。成功返回段列表，失败返回 None。
|
||||
# 这些是论文 8.1-8.11 的标准解析解。
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def _LpSpLp(x, y, phi):
|
||||
"""CSC，曲线同向：L+ S+ L+ (公式 8.1)。"""
|
||||
u, t = _polar(x - math.sin(phi), y - 1 + math.cos(phi))
|
||||
if t >= 0:
|
||||
v = _mod2pi(phi - t)
|
||||
if v >= 0:
|
||||
return [Segment(+1, +1, t), Segment(0, +1, u), Segment(+1, +1, v)]
|
||||
return None
|
||||
|
||||
|
||||
def _LpSpRp(x, y, phi):
|
||||
"""CSC，曲线反向：L+ S+ R+ (公式 8.2)。"""
|
||||
u1, t1 = _polar(x + math.sin(phi), y - 1 - math.cos(phi))
|
||||
if u1 ** 2 < 4:
|
||||
return None
|
||||
u = math.sqrt(u1 ** 2 - 4)
|
||||
_, theta = _polar(u, 2.0)
|
||||
t = _mod2pi(t1 + theta)
|
||||
v = _mod2pi(t - phi)
|
||||
if t >= 0 and v >= 0:
|
||||
return [Segment(+1, +1, t), Segment(0, +1, u), Segment(-1, +1, v)]
|
||||
return None
|
||||
|
||||
|
||||
def _LpRnLp(x, y, phi):
|
||||
"""CCC：L+ R- L+ (公式 8.3)。"""
|
||||
xi = x - math.sin(phi)
|
||||
eta = y - 1 + math.cos(phi)
|
||||
u1, theta = _polar(xi, eta)
|
||||
if u1 > 4:
|
||||
return None
|
||||
A = math.acos(u1 / 4.0)
|
||||
t = _mod2pi(theta + PI / 2 + A)
|
||||
u = _mod2pi(PI - 2 * A)
|
||||
v = _mod2pi(phi - t - u)
|
||||
if t >= 0 and u >= 0 and v >= 0:
|
||||
return [Segment(+1, +1, t), Segment(-1, -1, u), Segment(+1, +1, v)]
|
||||
return None
|
||||
|
||||
|
||||
def _LpRnLn(x, y, phi):
|
||||
"""CCC：L+ R- L- (公式 8.4 变体)。"""
|
||||
xi = x - math.sin(phi)
|
||||
eta = y - 1 + math.cos(phi)
|
||||
u1, theta = _polar(xi, eta)
|
||||
if u1 > 4:
|
||||
return None
|
||||
A = math.acos(u1 / 4.0)
|
||||
t = _mod2pi(theta + PI / 2 + A)
|
||||
u = _mod2pi(PI - 2 * A)
|
||||
v = _mod2pi(t + u - phi)
|
||||
if t >= 0 and u >= 0 and v >= 0:
|
||||
return [Segment(+1, +1, t), Segment(-1, -1, u), Segment(+1, -1, v)]
|
||||
return None
|
||||
|
||||
|
||||
def _LpRnSnLn(x, y, phi):
|
||||
"""CCSC：L+ R- S- L- (公式 8.9)。OMPL 修正形式，长度带符号。"""
|
||||
xi = x - math.sin(phi)
|
||||
eta = y - 1 + math.cos(phi)
|
||||
rho, theta = _polar(xi, eta)
|
||||
if rho < 2:
|
||||
return None
|
||||
r = math.sqrt(rho ** 2 - 4)
|
||||
u = 2 - r
|
||||
t = _mod2pi(theta + math.atan2(r, -2))
|
||||
v = _mod2pi(phi - PI / 2 - t)
|
||||
return [_make(t, +1, +1), _make(-PI / 2, -1, +1),
|
||||
_make(u, 0, +1), _make(v, +1, +1)]
|
||||
|
||||
|
||||
def _LpRnSnRn(x, y, phi):
|
||||
"""CCSC：L+ R- S- R- (公式 8.10)。OMPL 修正形式，长度带符号。"""
|
||||
xi = x + math.sin(phi)
|
||||
eta = y - 1 - math.cos(phi)
|
||||
rho, theta = _polar(-eta, xi)
|
||||
if rho < 2:
|
||||
return None
|
||||
t = theta
|
||||
u = 2 - rho
|
||||
v = _mod2pi(t + PI / 2 - phi)
|
||||
return [_make(t, +1, +1), _make(-PI / 2, -1, +1),
|
||||
_make(u, 0, +1), _make(v, -1, +1)]
|
||||
|
||||
|
||||
def _LpRnSnLnRp(x, y, phi):
|
||||
"""CCSCC：L+ R- S- L- R+ (公式 8.11)。OMPL 修正形式，长度带符号。"""
|
||||
xi = x + math.sin(phi)
|
||||
eta = y - 1 - math.cos(phi)
|
||||
rho, _ = _polar(xi, eta)
|
||||
if rho < 2:
|
||||
return None
|
||||
u = 4 - math.sqrt(rho ** 2 - 4)
|
||||
if u > 0:
|
||||
return None
|
||||
t = _mod2pi(math.atan2((4 - u) * xi - 2 * eta,
|
||||
-2 * xi + (u - 4) * eta))
|
||||
v = _mod2pi(t - phi)
|
||||
return [_make(t, +1, +1), _make(-PI / 2, -1, +1),
|
||||
_make(u, 0, +1), _make(-PI / 2, +1, +1),
|
||||
_make(v, -1, +1)]
|
||||
|
||||
|
||||
def _LpRupLunRn(x, y, phi):
|
||||
"""CCCC：L+ R+u L-u R- (公式 8.7)，两中段弧度相等。"""
|
||||
xi = x + math.sin(phi)
|
||||
eta = y - 1 - math.cos(phi)
|
||||
rho = (2 + math.hypot(xi, eta)) / 4.0
|
||||
if rho < 0 or rho > 1:
|
||||
return None
|
||||
u = math.acos(rho)
|
||||
t_ok, t, v = _calc_tauOmega(u, -u, xi, eta, phi)
|
||||
if t >= 0 and v <= 0:
|
||||
return [Segment(+1, +1, t), Segment(-1, +1, u),
|
||||
Segment(+1, -1, u), Segment(-1, -1, -v)]
|
||||
return None
|
||||
|
||||
|
||||
def _LpRunLunRp(x, y, phi):
|
||||
"""CCCC：L+ R-u L-u R+ (公式 8.8)。"""
|
||||
xi = x + math.sin(phi)
|
||||
eta = y - 1 - math.cos(phi)
|
||||
rho = (20 - xi ** 2 - eta ** 2) / 16.0
|
||||
if rho < 0 or rho > 1:
|
||||
return None
|
||||
u = -math.acos(rho)
|
||||
t_ok, t, v = _calc_tauOmega(u, u, xi, eta, phi)
|
||||
if t >= 0 and v >= 0:
|
||||
return [Segment(+1, +1, t), Segment(-1, -1, -u),
|
||||
Segment(+1, -1, -u), Segment(-1, +1, v)]
|
||||
return None
|
||||
|
||||
|
||||
def _calc_tauOmega(u, v, xi, eta, phi):
|
||||
"""CCCC 族的 tau/omega 辅助计算（OMPL 同名函数）。"""
|
||||
delta = _mod2pi(u - v)
|
||||
A = math.sin(u) - math.sin(delta)
|
||||
B = math.cos(u) - math.cos(delta) - 1.0
|
||||
t1 = math.atan2(eta * A - xi * B, xi * A + eta * B)
|
||||
t2 = 2 * (math.cos(delta) - math.cos(v) - math.cos(u)) + 3.0
|
||||
tau = _mod2pi(t1 + PI) if t2 < 0 else _mod2pi(t1)
|
||||
omega = _mod2pi(tau - u + v - phi)
|
||||
return True, tau, omega
|
||||
|
||||
|
||||
# 基础公式集合（含 word 标签，便于显示/调试）
|
||||
BASE_FORMULAS: list[tuple[str, Callable]] = [
|
||||
("LpSpLp", _LpSpLp),
|
||||
("LpSpRp", _LpSpRp),
|
||||
("LpRnLp", _LpRnLp),
|
||||
("LpRnLn", _LpRnLn),
|
||||
("LpRupLunRn", _LpRupLunRn),
|
||||
("LpRunLunRp", _LpRunLunRp),
|
||||
("LpRnSnLn", _LpRnSnLn),
|
||||
("LpRnSnRn", _LpRnSnRn),
|
||||
("LpRnSnLnRp", _LpRnSnLnRp),
|
||||
]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 对称变换：把段列表做镜像。
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def _timeflip(path: list[Segment]) -> list[Segment]:
|
||||
"""时间翻转：前进 <-> 倒车。"""
|
||||
return [Segment(s.steering, -s.gear, s.length) for s in path]
|
||||
|
||||
|
||||
def _reflect(path: list[Segment]) -> list[Segment]:
|
||||
"""左右镜像：L <-> R。"""
|
||||
return [Segment(-s.steering, s.gear, s.length) for s in path]
|
||||
|
||||
|
||||
def _backwards(path: list[Segment]) -> list[Segment]:
|
||||
"""路径反向：倒着走（段顺序翻转）。"""
|
||||
return [Segment(s.steering, s.gear, s.length) for s in reversed(path)]
|
||||
|
||||
|
||||
_STEER_CHAR = {+1: "L", 0: "S", -1: "R"}
|
||||
_GEAR_CHAR = {+1: "+", -1: "-"}
|
||||
|
||||
|
||||
def word_label(path: list[Segment]) -> str:
|
||||
"""把段列表转成可读 word，如 'L+S+R-'。"""
|
||||
return "".join(_STEER_CHAR[s.steering] + _GEAR_CHAR[s.gear] for s in path)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 正向积分：从一个位姿出发，沿各段前进，得到采样点和终点位姿。
|
||||
# 归一化坐标（转弯半径 = 1）。
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def integrate(path: list[Segment], start=(0.0, 0.0, 0.0), step=0.05):
|
||||
"""返回 (xs, ys, end_pose)。xs/ys 为采样点，end_pose=(x,y,theta)。"""
|
||||
x, y, theta = start
|
||||
xs, ys = [x], [y]
|
||||
for seg in path:
|
||||
n = max(1, int(math.ceil(seg.length / step)))
|
||||
ds = seg.length / n
|
||||
for _ in range(n):
|
||||
if seg.steering == 0:
|
||||
x += seg.gear * ds * math.cos(theta)
|
||||
y += seg.gear * ds * math.sin(theta)
|
||||
else:
|
||||
nt = theta + seg.gear * seg.steering * ds
|
||||
x += seg.steering * (math.sin(nt) - math.sin(theta))
|
||||
y -= seg.steering * (math.cos(nt) - math.cos(theta))
|
||||
theta = nt
|
||||
xs.append(x)
|
||||
ys.append(y)
|
||||
return xs, ys, (x, y, theta)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 候选路径 + 公开 API
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
@dataclass
|
||||
class Candidate:
|
||||
word: str # 如 "L+S+R-"
|
||||
segments: list[Segment] # 归一化坐标下的段
|
||||
length: float # 总段长（归一化）
|
||||
valid: bool # 正向积分是否命中目标位姿
|
||||
kind: str = "RS" # "RS" 标准 48 word / "Dubins" 纯前进绕远解
|
||||
max_arc: float = 0.0 # 最长曲线段弧度（归一化，与半径无关）
|
||||
|
||||
@property
|
||||
def key(self) -> str:
|
||||
"""GUI 用的唯一键。Dubins 解可能与 RS 同名，故加前缀区分。"""
|
||||
return (DUBINS_KEY_PREFIX + self.word if self.kind == "Dubins"
|
||||
else self.word)
|
||||
|
||||
@property
|
||||
def is_detour(self) -> bool:
|
||||
"""是否含超过 pi 的弧，即 RS 会主动丢弃的「绕远」。"""
|
||||
return self.max_arc > PI + 1e-6
|
||||
|
||||
|
||||
def _path_length(path):
|
||||
return sum(s.length for s in path)
|
||||
|
||||
|
||||
def _solve_all(x, y, phi):
|
||||
"""对归一化目标位姿 (x,y,phi)，用全部基础公式 × 4 对称求解。
|
||||
|
||||
返回 {word: segments}，每个 word 取最短解。
|
||||
4 种对称：identity / timeflip / reflect / timeflip+reflect。
|
||||
"""
|
||||
results: dict[str, list[Segment]] = {}
|
||||
|
||||
def add(path):
|
||||
if path is None:
|
||||
return
|
||||
w = word_label(path)
|
||||
if w not in results or _path_length(path) < _path_length(results[w]):
|
||||
results[w] = path
|
||||
|
||||
for _, f in BASE_FORMULAS:
|
||||
# identity
|
||||
add(f(x, y, phi))
|
||||
# timeflip: 解 f(-x, y, -phi)，再翻转 gear
|
||||
p = f(-x, y, -phi)
|
||||
add(_timeflip(p) if p else None)
|
||||
# reflect: 解 f(x, -y, -phi)，再 L<->R
|
||||
p = f(x, -y, -phi)
|
||||
add(_reflect(p) if p else None)
|
||||
# timeflip + reflect
|
||||
p = f(-x, -y, phi)
|
||||
add(_timeflip(_reflect(p)) if p else None)
|
||||
|
||||
# backwards：在目标坐标系中表达起点，求解后反转段顺序。
|
||||
# 这扩展出 CSCC 等以倒车段起步的 word。
|
||||
xb = x * math.cos(phi) + y * math.sin(phi)
|
||||
yb = x * math.sin(phi) - y * math.cos(phi)
|
||||
for _, f in BASE_FORMULAS:
|
||||
p = f(xb, yb, phi)
|
||||
add(_backwards(p) if p else None)
|
||||
p = f(-xb, yb, -phi)
|
||||
add(_backwards(_timeflip(p)) if p else None)
|
||||
p = f(xb, -yb, -phi)
|
||||
add(_backwards(_reflect(p)) if p else None)
|
||||
p = f(-xb, -yb, phi)
|
||||
add(_backwards(_timeflip(_reflect(p))) if p else None)
|
||||
|
||||
return results
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Dubins（纯前进）解：把 RS 主动丢弃的「绕远」路径找回来。
|
||||
#
|
||||
# 上面每个 RS 公式都把弧长过 _mod2pi 归一到 [-pi, pi) 再要求各段 >= 0，
|
||||
# 所以结构上不可能输出弧长 > pi 的段。这正是 RS 定理的引理：最优路径
|
||||
# 不含超过 pi 的弧——因为总能换一个带 cusp 的走法把它变短。代价是：
|
||||
# 一旦出现绕远的苗头，那条绕远的纯前进解就直接从候选里消失，换挡解顶上。
|
||||
#
|
||||
# 纯前进的绕远解恰好就是 Dubins 的 6 个 word，所以这里把同样的相切几何
|
||||
# 用 _mod2pi_pos（[0, 2pi)）重解一遍，即可把它们保留下来。
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def _mod2pi_pos(theta: float) -> float:
|
||||
"""归一化到 [0, 2pi)。与 _mod2pi 的唯一区别就是「允许绕远」。"""
|
||||
return theta % (2 * PI)
|
||||
|
||||
|
||||
def _dubins_LSL(x, y, phi):
|
||||
"""L+ S+ L+，弧长可到 2pi。两个左转圆的外公切线，恒有解。"""
|
||||
u, theta = _polar(x - math.sin(phi), y - 1 + math.cos(phi))
|
||||
# 直线段方向即两圆心连线方向 theta，故首尾弧把朝向从 0 转到 theta、再到 phi
|
||||
return [Segment(+1, +1, _mod2pi_pos(theta)),
|
||||
Segment(0, +1, u),
|
||||
Segment(+1, +1, _mod2pi_pos(phi - theta))]
|
||||
|
||||
|
||||
def _dubins_LSR(x, y, phi):
|
||||
"""L+ S+ R+，弧长可到 2pi。圆心距 < 2 时无内公切线。"""
|
||||
d, theta = _polar(x + math.sin(phi), y - 1 - math.cos(phi))
|
||||
if d < 2:
|
||||
return None
|
||||
u = math.sqrt(d * d - 4)
|
||||
t = _mod2pi_pos(theta + math.atan2(2.0, u)) # 内公切线方向
|
||||
return [Segment(+1, +1, t),
|
||||
Segment(0, +1, u),
|
||||
Segment(-1, +1, _mod2pi_pos(t - phi))]
|
||||
|
||||
|
||||
def _dubins_LRL(x, y, phi):
|
||||
"""L+ R+ L+，中段绕远弧（>= pi）。返回候选列表（两个相切分支）。
|
||||
|
||||
与 _LpRnLp 完全相同的三圆几何：起点左转圆 C1、终点左转圆 C3，
|
||||
中间右转圆 C2 与两者相切（|C1C2| = |C2C3| = 2，故需 |C1C3| <= 4）。
|
||||
C2 在 C1C3 两侧各有一个，A = acos(u1/4) 为 C1 处的半张角。
|
||||
|
||||
C2 上两切点之间有两段弧：短的 pi - 2A、长的 pi + 2A。倒着走中段
|
||||
(R-) 取短弧，即 RS 的 L+R-L+；前进走中段 (R+) 只能取长弧，即
|
||||
Dubins 的 L+R+L+。同样三个圆，两种走法——这就是「绕远」的来源。
|
||||
"""
|
||||
xi = x - math.sin(phi)
|
||||
eta = y - 1 + math.cos(phi)
|
||||
u1, theta = _polar(xi, eta)
|
||||
if u1 > 4:
|
||||
return []
|
||||
A = math.acos(min(1.0, u1 / 4.0))
|
||||
out = []
|
||||
# 两个相切分支：C2 方向为 theta + A 或 theta - A
|
||||
for branch in (+1, -1):
|
||||
t = _mod2pi_pos(theta + branch * A + PI / 2)
|
||||
# 长短弧都试，由 compute_paths 的端点校验筛出几何自洽的那个
|
||||
for u in (PI + 2 * A, PI - 2 * A):
|
||||
v = _mod2pi_pos(phi - t + u)
|
||||
out.append([Segment(+1, +1, t), Segment(-1, +1, u),
|
||||
Segment(+1, +1, v)])
|
||||
return out
|
||||
|
||||
|
||||
# Dubins 基础公式。前两个返回单个解，LRL 返回候选列表，统一成列表处理。
|
||||
_DUBINS_FORMULAS: list[Callable] = [
|
||||
lambda x, y, p: [r] if (r := _dubins_LSL(x, y, p)) else [],
|
||||
lambda x, y, p: [r] if (r := _dubins_LSR(x, y, p)) else [],
|
||||
_dubins_LRL,
|
||||
]
|
||||
|
||||
# Dubins 的 6 个 word。前 4 个与 RS 的 CSC 同名（同一 word，RS 只在
|
||||
# 各段弧长都 <= pi 时才给解，绕远时返回 None）；后 2 个不在 RS 的 48 里。
|
||||
DUBINS_WORDS: list[str] = [
|
||||
"L+S+L+", "L+S+R+", "R+S+L+", "R+S+R+", "L+R+L+", "R+L+R+",
|
||||
]
|
||||
|
||||
# 只有这两个 word 是 RS 48 个 word 之外的，专属于 Dubins。
|
||||
DUBINS_ONLY_WORDS: list[str] = ["L+R+L+", "R+L+R+"]
|
||||
|
||||
# Dubins 候选在 word_map 里的键前缀（同名 word 与 RS 解共存时用于区分）
|
||||
DUBINS_KEY_PREFIX = "D:"
|
||||
|
||||
|
||||
def _solve_dubins(x, y, phi, pos_tol=1e-2, ang_tol=1e-2):
|
||||
"""纯前进（Dubins）解，允许弧长 > pi。返回 {word: segments}，每 word 取最短。
|
||||
|
||||
只用 reflect 对称（L<->R），不用 timeflip——翻转挡位就不是纯前进了。
|
||||
|
||||
注意：_dubins_LRL 会投机地给出 4 个分支（2 个相切圆 × 长/短中段弧），
|
||||
只有部分几何自洽。必须先做端点校验再比长度，否则「取最短」可能留下
|
||||
一个不可达的分支、把真解挤掉。
|
||||
"""
|
||||
results: dict[str, list[Segment]] = {}
|
||||
|
||||
def add(path):
|
||||
if path is None:
|
||||
return
|
||||
if any(s.length < -1e-9 for s in path):
|
||||
return
|
||||
_, _, (ex, ey, eth) = integrate(path, start=(0.0, 0.0, 0.0))
|
||||
if (math.hypot(ex - x, ey - y) > pos_tol
|
||||
or abs(_mod2pi(eth - phi)) > ang_tol):
|
||||
return
|
||||
w = word_label(path)
|
||||
if w not in results or _path_length(path) < _path_length(results[w]):
|
||||
results[w] = path
|
||||
|
||||
for f in _DUBINS_FORMULAS:
|
||||
for p in f(x, y, phi):
|
||||
add(p)
|
||||
# reflect: 解 f(x, -y, -phi) 再 L<->R，得到 RSR / RSL / R+L+R+
|
||||
for p in f(x, -y, -phi):
|
||||
add(_reflect(p))
|
||||
return results
|
||||
|
||||
|
||||
def _max_arc(segments) -> float:
|
||||
"""路径中最长的曲线段弧度（直线段不计）。用于判定是否「绕远」。"""
|
||||
arcs = [s.length for s in segments if s.steering != 0]
|
||||
return max(arcs) if arcs else 0.0
|
||||
|
||||
|
||||
def compute_paths(start, goal, turning_radius=1.0, pos_tol=1e-2, ang_tol=1e-2,
|
||||
include_dubins=False):
|
||||
"""计算从 start 到 goal 的全部 Reeds-Shepp 候选路径。
|
||||
|
||||
start, goal: (x, y, theta_rad)，世界坐标。
|
||||
返回 Candidate 列表，按总长度升序；带端点校验的 valid 标记。
|
||||
长度单位与输入坐标一致（已乘回 turning_radius）。
|
||||
"""
|
||||
sx, sy, sth = start
|
||||
gx, gy, gth = goal
|
||||
|
||||
# 变换到以 start 为原点、朝向为 +x、半径归一化的局部坐标
|
||||
dx, dy = gx - sx, gy - sy
|
||||
c, s = math.cos(sth), math.sin(sth)
|
||||
lx = (c * dx + s * dy) / turning_radius
|
||||
ly = (-s * dx + c * dy) / turning_radius
|
||||
lphi = _mod2pi(gth - sth)
|
||||
|
||||
solutions = _solve_all(lx, ly, lphi)
|
||||
|
||||
canonical = set(ALL_WORDS)
|
||||
candidates = []
|
||||
for word, segs in solutions.items():
|
||||
# 只保留标准 48 word。RS 定理保证最优解必在其中；
|
||||
# 对称展开偶尔会产出几何正确但非标准（恒次优）的 word，在此剔除。
|
||||
if word not in canonical:
|
||||
continue
|
||||
# 端点校验：在局部归一化坐标下正向积分，须命中 (lx, ly, lphi)
|
||||
_, _, (ex, ey, eth) = integrate(segs, start=(0.0, 0.0, 0.0))
|
||||
ok = (math.hypot(ex - lx, ey - ly) < pos_tol
|
||||
and abs(_mod2pi(eth - lphi)) < ang_tol)
|
||||
scaled = [Segment(s_.steering, s_.gear, s_.length * turning_radius)
|
||||
for s_ in segs]
|
||||
candidates.append(Candidate(word, scaled,
|
||||
_path_length(segs) * turning_radius, ok,
|
||||
kind="RS", max_arc=_max_arc(segs)))
|
||||
|
||||
if include_dubins:
|
||||
for word, segs in _solve_dubins(lx, ly, lphi, pos_tol, ang_tol).items():
|
||||
scaled = [Segment(s_.steering, s_.gear, s_.length * turning_radius)
|
||||
for s_ in segs]
|
||||
# _solve_dubins 内部已做端点校验，能出来的都是 valid
|
||||
candidates.append(
|
||||
Candidate(word, scaled,
|
||||
_path_length(segs) * turning_radius, True,
|
||||
kind="Dubins", max_arc=_max_arc(segs)))
|
||||
|
||||
candidates.sort(key=lambda cc: (not cc.valid, cc.length))
|
||||
return candidates
|
||||
|
||||
|
||||
def sample_path(candidate: "Candidate", start, turning_radius=1.0, step=0.05):
|
||||
"""把候选路径在世界坐标下采样为 (xs, ys)，供绘图。"""
|
||||
norm_segs = [Segment(s.steering, s.gear, s.length / turning_radius)
|
||||
for s in candidate.segments]
|
||||
xs, ys, _ = integrate(norm_segs, start=(0.0, 0.0, 0.0), step=step)
|
||||
sx, sy, sth = start
|
||||
c, s = math.cos(sth), math.sin(sth)
|
||||
wx = [sx + turning_radius * (c * x - s * y) for x, y in zip(xs, ys)]
|
||||
wy = [sy + turning_radius * (s * x + c * y) for x, y in zip(xs, ys)]
|
||||
return wx, wy
|
||||
|
||||
|
||||
def sample_path_segments(candidate: "Candidate", start,
|
||||
turning_radius=1.0, step=0.05):
|
||||
"""逐段采样，返回 [(gear, xs, ys), ...]。
|
||||
|
||||
gear=+1 前进 / -1 倒车。每段在世界坐标下，相邻段共享端点以保证连续。
|
||||
供 GUI 按前进/倒车分色绘制。
|
||||
"""
|
||||
sx, sy, sth = start
|
||||
c, s = math.cos(sth), math.sin(sth)
|
||||
|
||||
def to_world(lx, ly):
|
||||
return (sx + turning_radius * (c * lx - s * ly),
|
||||
sy + turning_radius * (s * lx + c * ly))
|
||||
|
||||
out = []
|
||||
pose = (0.0, 0.0, 0.0) # 归一化局部坐标
|
||||
for seg in candidate.segments:
|
||||
norm = Segment(seg.steering, seg.gear, seg.length / turning_radius)
|
||||
lxs, lys, pose = integrate([norm], start=pose, step=step)
|
||||
wx, wy = zip(*(to_world(x, y) for x, y in zip(lxs, lys)))
|
||||
out.append((seg.gear, list(wx), list(wy)))
|
||||
return out
|
||||
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 标准 48 个 word，按 Reeds-Shepp 路径族分组。
|
||||
# 顺序固定，供 GUI 的 48 个勾选框稳定布局使用。
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
WORD_GROUPS: list[tuple[str, list[str]]] = [
|
||||
("CSC", [
|
||||
"L+S+L+", "L+S+R+", "L-S-L-", "L-S-R-",
|
||||
"R+S+L+", "R+S+R+", "R-S-L-", "R-S-R-",
|
||||
]),
|
||||
("CCC", [
|
||||
"L+R+L-", "L+R-L+", "L+R-L-", "L-R+L+", "L-R+L-", "L-R-L+",
|
||||
"R+L+R-", "R+L-R+", "R+L-R-", "R-L+R+", "R-L+R-", "R-L-R+",
|
||||
]),
|
||||
("CCCC", [
|
||||
"L+R+L-R-", "L+R-L-R+", "L-R+L+R-", "L-R-L+R+",
|
||||
"R+L+R-L-", "R+L-R-L+", "R-L+R+L-", "R-L-R+L+",
|
||||
]),
|
||||
("CCSC", [
|
||||
"L+R-S-L-", "L+R-S-R-", "L+S+L+R-", "L+S+R+L-",
|
||||
"L-R+S+L+", "L-R+S+R+", "L-S-L-R+", "L-S-R-L+",
|
||||
"R+L-S-L-", "R+L-S-R-", "R+S+L+R-", "R+S+R+L-",
|
||||
"R-L+S+L+", "R-L+S+R+", "R-S-L-R+", "R-S-R-L+",
|
||||
]),
|
||||
("CCSCC", [
|
||||
"L+R-S-L-R+", "L-R+S+L+R-", "R+L-S-R-L+", "R-L+S+R+L-",
|
||||
]),
|
||||
]
|
||||
|
||||
# 扁平化的 48 个 word（保持分组顺序）
|
||||
ALL_WORDS: list[str] = [w for _, group in WORD_GROUPS for w in group]
|
||||
|
||||
# Dubins（纯前进，允许绕远）分组。作为第 6 组附加在 GUI 里，键带 "D:" 前缀。
|
||||
DUBINS_GROUP: tuple[str, list[str]] = ("Dubins（纯前进/可绕远）", DUBINS_WORDS)
|
||||
|
||||
# GUI 用的完整槽位键列表：48 个 RS word + 6 个 Dubins word
|
||||
ALL_KEYS: list[str] = ALL_WORDS + [DUBINS_KEY_PREFIX + w for w in DUBINS_WORDS]
|
||||
|
||||
|
||||
def compute_word_map(start, goal, turning_radius=1.0, include_dubins=False):
|
||||
"""计算所有候选，返回 {key: Candidate}，仅含有效路径。
|
||||
|
||||
key 对 RS 解就是 word，对 Dubins 解带 "D:" 前缀（同名 word 可共存）。
|
||||
GUI 可用 ALL_KEYS 遍历槽位：在此映射中的为可达，否则置灰。
|
||||
"""
|
||||
cands = compute_paths(start, goal, turning_radius=turning_radius,
|
||||
include_dubins=include_dubins)
|
||||
return {c.key: c for c in cands if c.valid}
|
||||
|
||||
Reference in New Issue
Block a user