"""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}