路徑平滑優化算法–Clothoid 路徑平滑

從數學推導 → 單段拼接 → 多段全局 G2 連貫 → 動態約束與速度規劃

---

0　為什么選 Clothoid？

需求	Clothoid 值點
G2 連續（位置 + 切向 + 曲率連續）	曲率$\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}}?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{?連續性}?\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$ 降回終點曲率；
• 整條路徑在視覺上光滑，沒有拐角，體現 G2 連續性。

? 右圖：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+?\left[e^{i\Delta {\theta }_1}(\Delta x_2+i\Delta y_2)\right]?\hat{x}\\ \\ \Delta y_1+?\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 全局 G2 拼接（Global G2 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 全局 G2 拼接示意圖，清晰展示了多段路徑如何順滑連接，每段都由一對 Clothoid 構成：

• 藍色路徑：3 段 Clothoid Pair 組成的全局曲線；
• 黑色箭頭：表示每一點處的朝向 $\theta (s)$，展現切向連續；
• 紅點：起點與終點，整體軌跡完成平滑連接；
• 無突變、不折角：各段之間的拼接點不可見，表明已達 $G^2$ 連續。

---

4　動態約束嵌入（速度規劃）

在實際路徑規劃中，僅滿足幾何連續性（如 $G^2$）往往還不夠，還必須考慮動態約束：

• 車輛在不同曲率下的最大允許速度；
• 轉向執行器的限制（如最大轉向速率）；
• 提前考慮軌跡的可執行性 ? 使軌跡既平滑又可控。

因此，我們需要將這些動態約束顯式嵌入到路徑規劃和速度規劃中，以保證軌跡不僅可達，而且可控、可執行。

---

4.1 側向加速度約束（Lateral Acceleration）

在車輛以速度 $v(s)$ 沿路徑行駛時，受到的側向加速度為：

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

為保證安全性與乘坐舒適性，側向加速度應不超過某一最大值 $a_{y,\text{max}}$（例如 3 m/s2）。由此得到第一項速度上界：

$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?\kappa (s)$

其中 $L$ 為車輛軸距。轉向電機的變化速度有限，即：

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

解得第二項速度上界：

$v_{\max ,2}(s)=\frac{{\dot{\delta }}_{\max }}{L?|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}?0.06?(5+5)=0.3?$

說明該中間曲率設定在角度約束上是有效的。

計算前段偏移量：

$$\begin{gathered} a_1=0.012,\Delta x_1\approx 5?\frac{(0.012{)}^2?3125}{40}\approx 4.9888 \\ \Delta y_1\approx \frac{0.012?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}?0.06?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?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()