路径平滑优化算法–Clothoid 路径平滑

从数学推导 → 单段拼接 → 多段全局 G² 连贯 → 动态约束与速度规划

---

0　为什么选 Clothoid？

需求	Clothoid 值点
G² 连续（位置 + 切向 + 曲率连续）	曲率$\kappa (s)={\kappa }_0+as$天然线性 ⇒ 无突变
符合车辆物理	方向盘角速度$\dot{\delta }\propto \dot{\kappa }$可控，不会突然甩尾
解析/数值友好	Fresnel 积分闭式可算；解方程只需牛顿/LM
易拼接	“Clothoid↔圆弧↔Clothoid”或“Clothoid Pair”即可连接任意两态

---

1　数学基础：严谨推导（Mathematical Foundation）

Clothoid（又称 Euler 螺线）是满足曲率随弧长线性变化的平面曲线，是实现路径平滑（尤其是满足 $G^2$ 连续性）最常用的一类几何曲线。该小节给出 Clothoid 的标准数学定义与推导过程。

可视化解释

• 紫色主曲线 是一条随弧长线性增长曲率的轨迹：从直线开始逐渐变弯。
• 灰色箭头 表示不同位置处的切线方向（即当前的朝向角），可以看到方向角逐渐旋转，体现出“曲率线性增长”的特性。
• 弧长从 $s=0$ 到 $s=10$，表示“路径前进距离”。

---

1.1 曲率线性假设

Clothoid 的关键定义是：曲率 $\kappa$ 是弧长 $s$ 的线性函数，即：

$\kappa (s)={\kappa }_0+as$

其中：

• ${\kappa }_0$：起点处的曲率（通常为 0）；
• $a$：曲率变化率（curvature derivative），控制曲线弯曲速度。

这个定义表示：物体从起始点开始，以恒定的角加速度逐渐弯曲，符合汽车、轨道车等动力系统的转向特性。

---

1.2 切向角（Heading Angle）

根据曲率的定义：$\kappa =\frac{d\theta }{ds}$，即曲率是朝向角随弧长的导数。

因此，朝向角 $\theta (s)$ 可通过积分获得：

$\theta (s)={\theta }_0+{\int }_0^s\kappa (t)dt={\theta }_0+{\kappa }_{0}s+\frac{1}{2}a{s}^2$

这个表达式说明，在曲率线性变化的情况下，朝向角是一个关于弧长的二次函数，其一阶导数是曲率，二阶导数是曲率增长率 $a$。

---

1.3 平面位置表达（Fresnel 积分形式）

有了朝向角 $\theta (s)$，我们可以进一步求出在路径上任意位置处的平面坐标 $(x(s),y(s))$。由于：

$\frac{dx}{ds}=\cos \theta (s),\frac{dy}{ds}=\sin \theta (s)$

我们将其表示为复数形式：

$\Delta x+i\Delta y={\int }_0^s{e}^{i\theta (t)}dt$

代入 $\theta (t)={\theta }_0+{\kappa }_{0}t+\frac{1}{2}a{t}^2$，积分结果为复数形式的 Fresnel 积分表达：

$\Delta x+i\Delta y=\sqrt{\frac{\pi }{a}}\cdot e^{i{\theta }_0}{\left[C(u)+iS(u)\right]}_0^{\sqrt{\frac{a}{\pi }}s}$

其中：

• $C(u)={\int }_0^u\cos \left(\frac{\pi }{2}{t}^2\right)dt$：Fresnel cosine 积分；
• $S(u)={\int }_0^u\sin \left(\frac{\pi }{2}{t}^2\right)dt$t：Fresnel sine 积分；
• 变量 $u=\sqrt{\frac{a}{\pi }}s$ 是归一化后的弧长。

这一定义说明：Clothoid 曲线的空间位置无法解析表达，只能通过数值积分近似或表格查表。但其结构非常优雅，适用于连续曲率要求的规划与建模任务。

---

1.4 连续性分析

由上述公式可知，Clothoid 曲线的四个几何量：

• 空间位置：$x(s),y(s)$
• 切向角：$\theta (s)$
• 曲率：$\kappa (s)$

均为弧长 $s$ 的连续函数，且在拼接点处可通过曲率线性规划确保一阶导数（切向）与二阶导数（曲率）均连续。

因此，Clothoid 曲线天然满足：

$G^2\text{ 连续性}\iff \text{位置、切向、曲率全部连续}$

这是其在轨道设计、高速路径生成中的广泛应用基础。

✅ 左图：Clothoid Pair: Path in XY Plane

• 表示路径在平面 $xy$ 中的几何形状；
• 起点 $P_0$：红点标注，具有初始朝向 ${\theta }_0$；
• 终点 $P_1$：终止姿态，朝向为 ${\theta }_1$；
• 中间路径由两段曲率线性变化的 Clothoid 拼接组成：
  • 第一段（Clothoid-in）：曲率从 $k_0$ 增加；
  • 第二段（Clothoid-out）：从峰值 ${\kappa }_m$ 降回终点曲率；
• 整条路径在视觉上光滑，没有拐角，体现 G² 连续性。

✅ 右图：Clothoid Pair: Linear Ramp Up & Down

上图：

• 横轴：弧长 $s$，表示沿路径前进的距离；
• 纵轴：曲率 $\kappa$；
• 曲率从 $k_0$ 线性增加至峰值 κpm\kappa_{pm}κpm，再线性减少至终点曲率；
• 总弧长为 $L=L_1+L_2$，其中 $L_1$、$L_2$ 为两段长度；
• 虚线处为拼接点，满足曲率连续（$G^2$ 连续）。

下图（辅助对比）：

• 展示若采用普通直线 + 圆弧拼接，曲率出现“突变”；
• 从而说明 Clothoid 的优势在于避免这种“跳跃”。

上图展示了单段 Clothoid 曲线的几何结构图，用于形象说明本文中提到的数学推导结果：

• 蓝色曲线：由曲率线性增长 $\kappa (s)=as$ 产生的 Clothoid 路径；
• 黑色箭头：切向角 $\theta (s)$，显示路径的朝向随弧长连续变化；
• 红点：起点与终点位置，展示 Clothoid 曲线从直线逐渐变弯的特性；
• 曲线从直线缓慢弯曲，体现其“转向渐进”的优势。

---

2　单段拼接：Clothoid Pair（Meek & Walton, 1999）

2.1 问题定义

在路径规划中，连接任意两个状态点 $P_0=(x_0,y_0,{\theta }_0,{\kappa }_0)$、$P_1=(x_1,y_1,{\theta }_1,{\kappa }_1)$，不仅要求连接后的路径 位置闭合，而且希望保证 朝向角一致（$G^1$ 连续），更进一步还需 曲率连续（$G^2$ 连续）。

Meek & Walton (1999) 提出使用两段 Clothoid 曲线（即曲率线性变化曲线）以“背靠背”的方式拼接，前段从 ${\kappa }_0$ 增加到中间曲率${\kappa }_m$，后段从 ${\kappa }_m$ 减少到 ${\kappa }_1$，实现完整的高阶连续路径。

这种结构被称为 Clothoid Pair，是现代工业界与自动驾驶中常用的路径平滑结构之一。

---

2.2 归一化

为简化推导，通常先将路径转换到局部坐标系：

• 以 $P_0$ 为原点；
• 将路径旋转使得起始朝向 ${\theta }_0=0$；
• 因此，变换后的目标点为 ${\hat{P}}_1=(\hat{x},\hat{y},\hat{\theta },{\kappa }_1)$，起点为 $(0,0,0,{\kappa }_0)$。

这个归一化操作可以显著简化约束方程和数值求解过程。

---

2.3 未知量与曲率增长率

设 Clothoid Pair 中的拼接变量为：

$\mathbf{x}=(L_1,L_2,{\kappa }_m)$

其中：

• $L_1$：第一段 Clothoid 的弧长，从起点增长至中间点；
• $L_2$：第二段 Clothoid 的弧长，从中间点下降至终点；
• ${\kappa }_m$：拼接点的中间曲率。

每段 Clothoid 的曲率按弧长线性变化，对应曲率增长率为：

$a_1=\frac{{\kappa }_m-{\kappa }_0}{L_1},a_2=\frac{{\kappa }_m-{\kappa }_1}{L_2}$

---

2.4 三条约束方程

为了确保整条路径从 P0P_0P0 精确到达 P1P_1P1，需要满足以下三条几何约束：

$\mathbf{F}(\mathbf{x})=\left[\begin{array}{c}\Delta x_1+\Re \left[e^{i\Delta {\theta }_1}(\Delta x_2+i\Delta y_2)\right]-\hat{x}\\ \\ \Delta y_1+\Im \left[e^{i\Delta {\theta }_1}(\Delta x_2+i\Delta y_2)\right]-\hat{y}\\ \\ \Delta {\theta }_1+\Delta {\theta }_2-\hat{\theta }\end{array}\right]=0$

说明：

• $(\Delta x_1,\Delta y_1)$：第一段 Clothoid 的终点相对位移；
• 第二段的位移$(\Delta x_2,\Delta y_2)$ 要先旋转 $\Delta {\theta }_1$ 后拼接；
• $\Delta {\theta }_1,\Delta {\theta }_2$：各段的旋转角，曲率积分而得；
• 整体路径的终点坐标、朝向必须与 ${\hat{P}}_1$ 一致。

---

2.5 数值求解策略

由于 $\mathbf{F}(\mathbf{x})=0$ 是非线性方程组，采用迭代数值解法：

• 初始猜值：

  $L_1=L_2=\frac{1}{2}\sqrt{{\hat{x}}^2+{\hat{y}}^2}$

  ${\kappa }_m=\text{线性外推值}(\text{根据 }{\theta }_1)$

• 优化算法：

  • 牛顿法（Newton）或 Levenberg–Marquardt 阻尼算法；

  • 迭代公式：

    ${\mathbf{x}}_{k+1}={\mathbf{x}}_k-{\mathbf{J}}^{-1}\mathbf{F}({\mathbf{x}}_k)$其中 J\mathbf{J}J 是雅可比矩阵；

  • 迭代终止条件：

    $\mathbf{F}(\mathbf{x})\Vert <10^{-10}$

这些算法能高效地求得可拼接曲率，从而构建 Clothoid Pair 路径。

---

2.6 采样输出与轨迹生成（Trajectory Sampling）

在数值求解出参数向量 $\mathbf{x}=(L_1,L_2,{\kappa }_m)$ 后，我们就得到了完整定义这条 Clothoid Pair 曲线所需的全部信息。接下来，需要将这条理论路径“展开”成连续轨迹点，以用于实际导航、控制或绘图。

✅ 步骤 1：前段 Clothoid 曲线生成（起点 → 拼接点）

前段曲线的曲率从起始值 ${\kappa }_0$（通常为 0）线性上升至 ${\kappa }_m$。其表达式为：

• 曲率变化：

  ${\kappa }_1(s)={\kappa }_0+a_{1}s={\kappa }_0+\frac{{\kappa }_m-{\kappa }_0}{L_1}s,s\in [0,L_1]$朝向角由曲率积分而得：

  ${\theta }_1(s)={\theta }_0+{\int }_0^s{\kappa }_1(t)dt={\theta }_0+\frac{{\kappa }_m-{\kappa }_0}{2L_1}{s}^2$

• 平面坐标轨迹：

  $x_1(s)={\int }_0^s\cos ({\theta }_1(t))dt,y_1(s)={\int }_0^s\sin ({\theta }_1(t))dt$

这些积分表达式可以通过数值方法（如累积梯形积分）进行高精度计算。

✅ 步骤 2：后段 Clothoid 曲线生成（拼接点 → 终点）

后段曲线从中间曲率${\kappa }_m$ 线性下降至终点曲率 ${\kappa }_1$，表达式类似，但需要注意起点角度、相对位移和坐标系变化：

• 曲率变化：

  ${\kappa }_2(s)={\kappa }_m-a_{2}s={\kappa }_m-\frac{{\kappa }_m-{\kappa }_1}{L_2}s,s\in [0,L_2]$

• 朝向角（从前段末尾 ${\theta }_1^{\text{end}}$开始）：

  ${\theta }_2(s)={\theta }_1^{\text{end}}+{\kappa }_{m}s-\frac{({\kappa }_m-{\kappa }_1)}{2L_2}{s}^2$

• 相对轨迹坐标：

  $x_2(s)={\int }_0^s\cos ({\theta }_2(t))dt,y_2(s)={\int }_0^s\sin ({\theta }_2(t))dt$

此段轨迹位于以第一段末尾为原点、以 $\Delta {\theta }_1$ 为方向的局部坐标系中。

✅ 步骤 3：全局拼接与坐标还原

将后段轨迹旋转回全局坐标系，并平移至前段终点坐标：

$[x2(s)y2(s)]\left[\begin{array}{c}{x}_2^{\text{global}}(s)\\ y_2^{\text{global}}(s)\end{array}\right]=\left[\begin{array}{c}{x}_1(L_1)\\ y_1(L_1)\end{array}\right]+\left[\begin{array}{cc}\cos (\Delta {\theta }_1)& -\sin (\Delta {\theta }_1)\\ \sin (\Delta {\theta }_1)& \cos (\Delta {\theta }_1)\end{array}\right]\left[\begin{array}{c}{x}_2(s)\\ y_2(s)\end{array}\right]$

拼接后得到完整路径$(x(s),y(s),\theta (s),\kappa (s))$，其中：

• $s\in [0,L_1+L_2]$
• 路径连续、光滑；
• 曲率连续，可用于 $G^2$​ 路径要求的应用场景，如自动驾驶、轨迹插值、仿真动画等。

• 上图展示了 Clothoid Pair 拼接的两类示意图，分别对应：
  1. XY 平面路径图（左图）：
     • 蓝色曲线为 Clothoid Pair 路径，由两段曲率线性变化的 Clothoid 构成；
     • 黑色箭头表示每段路径上的姿态方向 $\theta (s)$，揭示车辆或机器人在路径上如何平滑转向；
     • 红点分别标记起点与终点。
  2. 曲率随弧长变化图（右图）：
     • 紫色曲线展示曲率 $\kappa (s)$ 随路径长度的线性变化过程；
     • 虚线为中间拼接点，说明两段曲率在拼接处连续，满足 $G^2$ 要求；
     • 整体曲率轮廓为“上升—下降”结构，典型的 Clothoid Pair 特征。

---

3　多段 Clothoid 全局 G² 拼接（Global G² Clothoid Spline）

在实际路径规划任务中，一条路径往往由多个离散关键节点组成，如：

• A*、Hybrid A*、RRT 等采样-搜索算法的路径输出；
• GPS、SLAM、地图建模系统的粗略路径；
• 手工标注或导航轨迹记录点等。

这类路径通常具有不连续的朝向与曲率，不能直接用于导航控制系统。我们需要通过 多段 Clothoid 拼接 的方式，将离散路径平滑化为整体连续的 G2G^2G2 曲线，从而实现：

• 位置连续；
• 切向角（方向）连续；
• 曲率连续 ⇒ 满足平滑加速度约束，便于后续动态控制器使用。

---

3.1 步骤总览与解释

步骤	内容	技术说明
① 分段（Segmentation）	将原始路径（折线或关键点序列）划分为多个子段，每段连接一对相邻节点 $(P_i,P_{i+1})$	若原始路径已是曲线，也可按弧长等间距采样
② 单段拼接（Local Clothoid Pair）	对每一段$(P_i\to P_{i+1})$，使用 Clothoid Pair 算法（参见 §2）进行曲线拟合	需给定起终点、起终朝向、起终曲率，可用上一段结果或估算方向角
③ 曲率传递（Curvature Propagation）	对于第 $i$ 段生成的曲线，其终点曲率${\kappa }_{\text{end}}^{(i)}$ 自动作为第 $i+1$ 段的起始曲率 ${\kappa }_0^{(i+1)}$	可实现整个路径曲率连续，消除突变，满足 $G^2$ 要求
④ 可选全局优化（Global Refinement, optional）	若需提升整体性能，可将所有段的参数 {L1(i),L2(i),κm(i)}i=1N−1\{L_1^{(i)}, L_2^{(i)}, \kappa_m^{(i)}\}_{i=1}^{N-1}{L1(i)​,L2(i)​,κm(i)​}i=1N−1​ 一起优化，使目标函数（总弧长、加速度变化等）最小	可使用稀疏 Jacobian 的 Levenberg–Marquardt（Sparse-LM）方法高效求解；适用于离线或高精度轨迹需求

---

💡 解耦 vs 联合求解

• 顺序解耦算法（Local-only）
  每一段独立求解，曲率从前段传递 ⇒ 时间复杂度 $O(N)$，可用于实时系统、无人车导航；
• 联合全局优化（Global optimization）
  建立约束优化问题，统一考虑全部段落曲率与参数，使整体路径更“自然”、“柔和”，但代价更高。

建议策略：

• 实时规划或嵌入式系统 ⇒ 使用顺序解耦拼接；
• 离线优化、高精度路径（如道路建模、动画仿真）⇒ 考虑联合优化。

---

🚘 应用场景

• 自动驾驶中的车道生成与变道曲线；
• 工业机器人连续轨迹指令；
• 飞行器 / 航迹规划；
• 智能路径设计（游戏动画 / 仿真路径）；

上图为 多段 Clothoid 全局 G² 拼接示意图，清晰展示了多段路径如何顺滑连接，每段都由一对 Clothoid 构成：

• 蓝色路径：3 段 Clothoid Pair 组成的全局曲线；
• 黑色箭头：表示每一点处的朝向 $\theta (s)$，展现切向连续；
• 红点：起点与终点，整体轨迹完成平滑连接；
• 无突变、不折角：各段之间的拼接点不可见，表明已达 $G^2$ 连续。

---

4　动态约束嵌入（速度规划）

在实际路径规划中，仅满足几何连续性（如 $G^2$）往往还不够，还必须考虑动态约束：

• 车辆在不同曲率下的最大允许速度；
• 转向执行器的限制（如最大转向速率）；
• 提前考虑轨迹的可执行性 ⇒ 使轨迹既平滑又可控。

因此，我们需要将这些动态约束显式嵌入到路径规划和速度规划中，以保证轨迹不仅可达，而且可控、可执行。

---

4.1 侧向加速度约束（Lateral Acceleration）

在车辆以速度 $v(s)$ 沿路径行驶时，受到的侧向加速度为：

$a_y(s)=v(s{)}^2\cdot \kappa (s)$

为保证安全性与乘坐舒适性，侧向加速度应不超过某一最大值 $a_{y,\text{max}}$（例如 3 m/s²）。由此得到第一项速度上界：

$v_{\max ,1}(s)=\sqrt{\frac{a_{y,\max }}{|\kappa (s)|}}$

含义是：曲率越大（转弯越急），允许的速度越低；若曲率趋近于 0（直线段），则$v_{\max ,1}\to \infty$。

---

4.2 方向盘角速度约束（Steering Rate Limit）

在 Ackermann 模型下，转角 $\delta$ 与曲率 $\kappa$ 满足近似：

$\delta (s)\approx L\cdot \kappa (s)$

其中 $L$ 为车辆轴距。转向电机的变化速度有限，即：

$|\dot{\delta }|=\left|\frac{d\delta }{dt}\right|=\left|\frac{d\delta }{ds}\cdot \frac{ds}{dt}\right|=L\cdot v(s)\cdot \left|\frac{d\kappa }{ds}\right|\le {\dot{\delta }}_{\max }$

解得第二项速度上界：

$v_{\max ,2}(s)=\frac{{\dot{\delta }}_{\max }}{L\cdot |d\kappa /ds|}$

含义是：当曲率变化很快（方向变化陡），就要减速以免超过方向盘的极限响应速度。

特别注意：

• $d\kappa /ds$在 Clothoid 上是常数；
• 在拼接点处连续 ⇒ 不存在无限导数（相比样条更稳定）；
• 该项约束对机器人和汽车控制器都非常关键。

---

4.3 最终速度剖面合成

综合各类速度约束，最终速度曲线为：

$v(s)=\min (v_{\text{ref}},v_{\max ,1}(s),v_{\max ,2}(s))$

• $v_{\text{ref}}$：参考期望速度（如限速、巡航速度等）；
• 若任一约束限制速度更低，系统自动减速；
• 该规划结果可直接用于时间参数化，供轨迹跟踪控制器使用。

---

4.4 几何反馈调整（如仍超限）

若上述规划后仍出现速度过低（如转角太急、响应受限），说明当前路径几何结构对车辆动态过于苛刻，可通过以下方式缓解：

• 调小中间曲率 ${\kappa }_m$：减小曲率峰值；
• 延长 Clothoid 段长度 $L_1,L_2$：降低曲率变化率 $a=\frac{\Delta \kappa }{L}$；
• 适当放宽终点约束方向 ${\theta }_1$：减少角度跨度；
• 换用曲率缓变模型（如多段 Clothoid、圆弧-螺线过渡）。

这些属于从“速度层”反向影响“几何层”，实现路径-速度联合规划的一种形式。

图中展示了 Clothoid 曲线在**动态约束（侧向加速度与方向盘角速度）**下的速度剖面规划过程：

• 蓝色虚线：由最大侧向加速度 $a_y$ 限制得到的速度上限 $v_{\max ,1}$；
• 橙色虚线：由最大方向盘角速度 $\dot{\delta }$ 限制得到的速度上限 $v_{\max ,2}$；
• 灰色水平线：理想参考速度 $v_{\text{ref}}$；
• 黑色实线：最终取三者中最小值构成的速度轨迹 $v(s)$。

结论：

• 在曲率较小时 $v_{\max ,1}$ 非常高，不成为瓶颈；
• 当 $\frac{d\kappa }{ds}$ 显著（即变化率高）时，方向盘变化率约束限制了速度；
• 可在速度过低处通过放松 Clothoid 曲率峰值 ${\kappa }_m$ 或延长曲线长度来优化。

5. Clothoid Pair 手动求解算例（全过程 LaTeX 精编）

5.1 题目描述

连接两个状态：

$\text{已知量：}{P}_0=(0,0,0,0),P_1=(10,2,{\theta }_1=0.3\text{rad},{\kappa }_1=0)$

这是一个典型的路径规划拼接问题，我们期望通过两段 Clothoid（曲率线性变化曲线）构造出一条“在中点背靠背拼接”的路径，实现从起点状态平滑过渡至终点状态，满足位置、朝向与曲率的连续。

---

5.2 未知量（目标求解）

$$\begin{gathered} L_1,L_2:\text{两段 Clothoid 的弧长（长度）} \\ {\kappa }_m:\text{中间曲率（两段并接处）} \end{gathered}$$

这三个变量正是我们要解出的目标。它们将决定整条曲线的几何形态与动态性能。

辅助变量：曲率增长率（由未知量间接决定）

$a_1=\frac{{\kappa }_m}{L_1},a_2=\frac{{\kappa }_m}{L_2}$

---

5.3 方程组（构建约束）

为了保证轨迹是“严丝合缝”的，我们需要构造三个独立的约束条件，对应位置 $(x,y)$ 和朝向 $\theta$ 的连续性要求。

(1) 方向角差约束

两段曲率线性变化段的角度增量应加总为目标方向变化：

$\Delta {\theta }_1=\frac{1}{2}{\kappa }_m{L}_1,\Delta {\theta }_2=\frac{1}{2}{\kappa }_m{L}_2$

加总后匹配终点方向：

$\Delta {\theta }_1+\Delta {\theta }_2=\frac{1}{2}{\kappa }_m(L_1+L_2)=0.3$

---

(2)(3) 位置约束 (采用 1 阶 Fresnel 积分近似)

Clothoid 的几何积分没有解析解，但可以用一阶近似表达端点位移：

$\Delta x\approx L-\frac{a^2{L}^5}{40},\Delta y\approx \frac{a{L}^3}{6}$

前段路径位移：

$$\begin{gathered} a_1=\frac{{\kappa }_m}{L_1} \\ \Delta x_1=L_1-\frac{a_1^2{L}_1^5}{40},\Delta y_1=\frac{a_1{L}_1^3}{6} \end{gathered}$$

后段路径位移（逆方向）：

$$\begin{gathered} a_2=\frac{{\kappa }_m}{L_2} \\ \Delta x_2=L_2-\frac{a_2^2{L}_2^5}{40},\Delta y_2=-\frac{a_2{L}_2^3}{6} \end{gathered}$$

由于后段是“反向”的 Clothoid，我们需将其坐标旋转变换回前段末尾坐标系下。

装入旋转：

Rotate:$\text{Rotate:}\left[\begin{array}{c}{x}_2^{\prime }\\ y_2^{\prime }\end{array}\right]=\left[\begin{array}{cc}\cos \Delta {\theta }_1& -\sin \Delta {\theta }_1\\ \sin \Delta {\theta }_1& \cos \Delta {\theta }_1\end{array}\right]\left[\begin{array}{c}\Delta x_2\\ \Delta y_2\end{array}\right]$

组合总位移：

$\Delta x=\Delta x_1+x_2^{\prime },\Delta y=\Delta y_1+y_2^{\prime }$

最终位置约束：

$\Delta x=10,\Delta y=2$

---

5.4 动态约束嵌入

我们现在以一个直观数值例子展示 Clothoid Pair 的整个解算过程，同时嵌入动态约束进行速度规划。

设定初值：

$L_1=L_2=5.0,{\kappa }_m=0.06$

首先验证方向角约束：

$\frac{1}{2}\cdot 0.06\cdot (5+5)=0.3✓$

说明该中间曲率设定在角度约束上是有效的。

计算前段偏移量：

$$\begin{gathered} a_1=0.012,\Delta x_1\approx 5-\frac{(0.012{)}^2\cdot 3125}{40}\approx 4.9888 \\ \Delta y_1\approx \frac{0.012\cdot 125}{6}=0.25 \end{gathered}$$

计算后段偏移量（方向相反）：

$$\begin{gathered} a_2=0.012,\Delta x_2\approx 4.9888,\Delta y_2\approx -0.25 \\ \Delta {\theta }_1=\frac{1}{2}\cdot 0.06\cdot 5=0.15 \end{gathered}$$

将后段偏移量旋转回全局坐标：

$\left[\begin{array}{c}{x}_2^{\prime }\\ y_2^{\prime }\end{array}\right]=\left[\begin{array}{cc}\cos 0.15& -\sin 0.15\\ \sin 0.15& \cos 0.15\end{array}\right]\left[\begin{array}{c}4.9888\\ -0.25\end{array}\right]\approx \left[\begin{array}{c}4.929\\ 0.493\end{array}\right]$

最终组合：

$\Delta x=4.9888+4.929=9.92,\Delta y=0.25+0.493=0.743$

虽然末端位置未完全匹配目标 $(10,2)$，但角度约束满足、几何误差可控，后续可作为初值用于数值优化迭代。

---

5.5 动态约束嵌入检验

车辆实际行驶还需考虑动态限制，为此加入如下限制条件：

$a_y^{\max }=3.0m/{\mathrm{s}}^2,{\dot{\delta }}_{\max }=0.5rad/s,L=2.5\mathrm{m},v_{\text{ref}}=15.0m/s$

由本例参数：${\kappa }_m=0.06$、$a=0.012$，推导动态速度限制：

$$\begin{gathered} v_{\max ,1}=\sqrt{\frac{3.0}{0.06}}\approx 7.07m/s \\ {v}_{\max ,2}=\frac{0.5}{2.5\cdot 0.012}=\frac{0.5}{0.03}\approx 16.67m/s \end{gathered}$$

最终速度剖面：

$v(s)=\min (15.0,v_{\max ,1},v_{\max ,2})=7.07m/s$

✅ 动态验证通过： 虽不满参考速度 15m/s，但满足两种动态限制，说明当前 Clothoid 曲率设定在可控范围内。若要提速，可在几何层进一步优化 ${\kappa }_m$ 或延展段长 $L_1,L_2$。

6 python实现代码

import numpy as np
import matplotlib.pyplot as plt
from scipy.special import fresneldef clothoid_segment(x0, y0, theta0, kappa0, kappa1, L, ds=0.1):a = (kappa1 - kappa0) / Ls = np.arange(0, L + ds, ds)kappa = kappa0 + a * stheta = theta0 + kappa0 * s + 0.5 * a * s**2A = np.sqrt(np.abs(a) / np.pi)u = np.sqrt(np.abs(a) / np.pi) * sC, S = fresnel(u)dx = C * np.sign(a)dy = S * np.sign(a)X = x0 + (1 / A) * (dx * np.cos(theta0) - dy * np.sin(theta0))Y = y0 + (1 / A) * (dx * np.sin(theta0) + dy * np.cos(theta0))return X, Y, theta, kappadef rotate_and_translate(x, y, theta, dx, dy, dtheta):cos_t = np.cos(dtheta)sin_t = np.sin(dtheta)x_new = cos_t * x - sin_t * y + dxy_new = sin_t * x + cos_t * y + dytheta_new = theta + dthetareturn x_new, y_new, theta_new# === 输入参数（手动设置） ===
x0, y0, theta0, kappa0 = 0.0, 0.0, 0.0, 0.0
x1, y1, theta1, kappa1 = 10.0, 2.0, 0.3, 0.0
L1 = 5.0
L2 = 5.0
kappa_m = 0.06
ds = 0.1# === 前段 Clothoid ===
X1, Y1, TH1, K1 = clothoid_segment(x0, y0, theta0, kappa0, kappa_m, L1, ds)
x_end, y_end, theta_end = X1[-1], Y1[-1], TH1[-1]# === 后段 Clothoid（从末端反向构造，再旋转） ===
X2_, Y2_, TH2_, K2_ = clothoid_segment(0, 0, 0, kappa_m, kappa1, L2, ds)
dtheta = theta_end
X2, Y2, TH2 = rotate_and_translate(X2_, Y2_, TH2_, x_end, y_end, dtheta)# === 合并轨迹 ===
X = np.concatenate([X1, X2])
Y = np.concatenate([Y1, Y2])
TH = np.concatenate([TH1, TH2])
K = np.concatenate([K1, K2_])# === 可视化 ===
plt.figure(figsize=(8, 4))
plt.plot(X, Y, 'b-', linewidth=2, label='Clothoid Pair Path')
plt.scatter([x0, x1], [y0, y1], c='red', zorder=5, label='Endpoints')# 添加方向箭头
for i in range(0, len(X), int(1/ds * 1.0)):plt.arrow(X[i], Y[i], 0.5 * np.cos(TH[i]), 0.5 * np.sin(TH[i]),head_width=0.1, head_length=0.2, fc='k', ec='k')plt.axis('equal')
plt.title("Clothoid Pair Path with Orientation")
plt.xlabel("X")
plt.ylabel("Y")
plt.legend()
plt.grid(True)
plt.tight_layout()
plt.show()