# 路徑規劃系列: B-Spline
B-Spline（基底樣條曲線，Basis Spline）是計算機圖學、CAD（電腦輔助設計）以及機器人路徑規劃中最重要的數學工具之一
B-Spline 的優點：
局部控制： 移動一個控制點，只會影響曲線的一小部分，而不是全部。
階數獨立： 你可以用很多的控制點，但保持低階數（例如 3 階），確保計算快且曲線平滑。
想像一條像橡皮筋一樣的曲線，被一系列**控制點（Control Points）** 吸引，但通常不會經過這些點（除了起點和終點，如果設定正確的話）。
## B-Spline 由三個主要參數定義：
1. 階數 (Degree, $p$)
決定了曲線的平滑程度。常見使用的是 $p=3$ (Cubic B-Spline)，這能保證曲線在連接處保持 $C^2$ 連續（位置、切線、曲率都連續）。
2. 控制點 (Control Points, $P_i$)
定義曲線的大致形狀。如果有 $n+1$ 個控制點，那麼 $i$ 的範圍是 $0$ 到 $n$。
3. 節點向量 (Knot Vector, $U$)
節點向量是一個非遞減的數字序列，例如 $[0, 0, 0, 1, 2, 3, 3, 3]$。
把 $u$ 想像成「時間」或沿著曲線行走的參數。節點決定了每個基底函數（Basis Function）在那裡開始和結束。
Clamped (Clamped Uniform): 節點向量的頭尾重複 $p+1$ 次（例如 [0,0,0,1,2,3,3,3]）。這強迫曲線必須經過第一個和最後一個控制點。
## 數學原理 (The Math):
B-Spline 的數學定義是基於基底函數 (Basis Functions) 的線性組合：
$$C(u) = \sum_{i=0}^{n} N_{i,p}(u) P_i$$
其中 $P_i$ 是控制點，而 $N_{i,p}(u)$ 是 B-Spline 的基底函數。
1. $p=0$ (0 階，階梯函數):
$$N_{i,0}(u) = \begin{cases} 1 & \text{if } u_i \le u < u_{i+1} \\ 0 & \text{otherwise} \end{cases}$$
2. $p > 0$ (遞迴定義):$$N_{i,p}(u) = \frac{u - u_i}{u_{i+p} - u_i} N_{i,p-1}(u) + \frac{u_{i+p+1} - u}{u_{i+p+1} - u_{i+1}} N_{i+1,p-1}(u)$$
每一個高階的基底函數，都是由兩個低一階的基底函數加權平均而來的。
```
import numpy as np
import matplotlib.pyplot as plt
from scipy.interpolate import make_interp_spline, BSpline
# 1. 定義控制點 (Control Points)
x = np.array([0, 1, 3, 4, 5, 7, 12])
y = np.array([0, 2, 1, 5, 3, 4, 16])
plt.plot(x, y, 'o', label='Control Points')
for k in [1,2,3]:
# 2. 建立 B-Spline
# 使用 make_interp_spline 會自動幫你計算節點向量(Knots)並擬合通過這些點
spl = make_interp_spline(x, y, k=k) # # k=3 代表 Cubic B-Spline (3階)
# 3. 產生平滑曲線數據
x_new = np.linspace(x.min(), x.max(), 300)
y_new = spl(x_new)
# 4. 繪圖
plt.plot(x_new, y_new, '-', label='B-Spline Curve:order={}'.format(k))
plt.legend()
plt.show()
```
## 程式邏輯完全移除 Scipy
改用 Pure Numpy 來實現原本 make_interp_spline 的功能。
這裡的核心邏輯是：
1. Cox-de Boor 公式：計算基底函數。
2. 解線性方程式 (Matrix Solver)：因為原本的程式碼是「插值」（曲線必須經過點），所以我們需要建立矩陣 $\mathbf{M}$，並求解 $\mathbf{M} \cdot \mathbf{P} = \mathbf{y}$ 來算出控制點 $\mathbf{P}$。
```
import numpy as np
import matplotlib.pyplot as plt
# ==========================================
# 純 Numpy 實現 B-Spline 插值核心
# ==========================================
def cox_de_boor(u, i, k, knots):
"""
計算基底函數 N_{i,k}(u)
支援向量化計算 (u 可以是 array)
"""
# 遞迴終止條件 (0階)
if k == 0:
'''
公式:
N_i,0(u) = 1 if u_i <= u <= u_i+1
N_i,0(u) = 0 otherwise
knots帶入的是公式內的u_{}的
'''
# 處理 u 是 array 的情況
out = np.zeros_like(u, dtype=float)
# 區間 [knots[i], knots[i+1])
mask = (u >= knots[i]) & (u < knots[i+1])
out[mask] = 1.0
# 處理最後一個節點的邊界條件 (包含 u=knots[-1])
if knots[i+1] == knots[-1]:
out[u == knots[-1]] = 1.0
return out
# 遞迴步驟
'''
公式:
N_i,p =
(u-u_i)/(u_{i+p} - u_i) * N_{i,p-1}(u)
+
(u_{i+p+1}-u)/(u_{i+p+1} - u_{i+1}) * N_{i+1,p-1}(u)
knots帶入的是公式內的u_{}的
p就是k
'''
denom1 = knots[i+k] - knots[i]
term1 = 0.0
if denom1 > 0:
term1 = ((u - knots[i]) / denom1) * cox_de_boor(u, i, k-1, knots)
denom2 = knots[i+k+1] - knots[i+1]
term2 = 0.0
if denom2 > 0:
term2 = ((knots[i+k+1] - u) / denom2) * cox_de_boor(u, i+1, k-1, knots)
return term1 + term2
def generate_knots(x, k):
"""
根據輸入的 x 生成 Clamped Knot Vector
(頭尾重複 k+1 次)
"""
n = len(x)
# 1. 歸一化 x 到 [0, 1] (參數 u)
u = (x - x[0]) / (x[-1] - x[0])
# 2. 生成節點向量 (這決定了基底函數的形狀)
# 注意：這裡為了確保矩陣可逆，節點數量必須精確匹配
# 對於插值：控制點數量 = 數據點數量 = n
# 節點向量長度 = n + k + 1
# 頭部重複 k+1 次 0, 尾部重複 k+1 次 1
knots = np.zeros(n + k + 1)
knots[-(k+1):] = 1.0 # 尾部
# 中間節點 (De Boor's averaging method)
# 使用平均法生成更適合非均勻數據的節點 (中間均勻分佈或者使用 t 的分佈 (這裡為了穩定性使用均勻分佈)))
# 註：為了模擬 make_interp_spline 的效果，這裡我們採用
# "Averaging" 策略來生成 knot vector，或者簡單的 Clamped Uniform
# 對於插值矩陣求解，Clamped Uniform 通常足夠
for j in range(1, n - k):
knots[j+k] = np.mean(u[j : j+k])
return u, knots
def solve_bspline_interpolation(x, y, k):
"""
解線性方程組找出控制點 P
M * P = y
"""
n = len(x)
u, knots = generate_knots(x, k)
u = np.array(u)
# 3. 建立矩陣 M
M = np.zeros((n, n))
for row_i in range(n): # 對於每一個數據點
for col_j in range(n): # 對於每一個控制點(基底函數)
M[row_i, col_j] = cox_de_boor(u[row_i],
col_j,
k,
knots)
# 4. 解 M * P = y (求出控制點 P)
P = np.linalg.solve(M, y)
return P, knots
def evaluate_bspline(x_new, P, knots, k, x_min, x_max):
"""
輸入新的 x，計算對應的 y
"""
# 歸一化 x_new
u_new = (x_new - x_min) / (x_max - x_min)
u_new = np.clip(u_new, 0, 1) # 防止浮點數誤差越界
y_new = np.zeros_like(u_new)
n_control = len(P)
# 疊加所有控制點的貢獻
for i in range(n_control):
basis = cox_de_boor(u_new, i, k, knots)
y_new += P[i] * basis
return y_new
# ==========================================
# 主程式 (修改自您的代碼)
# ==========================================
# 1. 定義數據點 (Data Points)
x = np.array([0, 2, 5, 8, 10])
y = np.array([0, 2, 1, 5, 5])
plt.figure(figsize=(10, 6))
plt.plot(x, y, 'ko', label='Data Points (Knots)', zorder=5)
for k in [1, 2, 3]:
try:
# 2. 建立 B-Spline (純 Numpy 版)
# 解方程找出控制點 P 和對應的 knots
P, knots = solve_bspline_interpolation(x, y, k=k)
# 3. 產生平滑曲線數據
x_new = np.linspace(x.min(), x.max(), 300)
y_new = evaluate_bspline(x_new, P, knots, k, x.min(), x.max())
# 4. 繪圖
plt.plot(x_new,
y_new,
'-',
linewidth=2,
label='B-Spline Curve: order={}'.format(k))
# (選用) 畫出算出來的隱藏控制點，幫助理解
plt.plot(np.linspace(0, 12, len(P)),
P,
'--',
alpha=0.3,
label=f'Control Poly k={k}')
except np.linalg.LinAlgError:
print(f"k={k} 時矩陣奇異，無法求解 (數據點太少或分布不均)")
plt.legend()
plt.grid(True)
plt.title("Pure Numpy B-Spline Interpolation (No Scipy)")
plt.show()
```
-------
我們將B-spline的公式寫成矩陣的型態
$$C = M \cdot P$$
$C$:軌跡點矩陣($N_{sample} \times 2$)
$P$:控制點矩陣($N_{control} \times 2$)
$M$:基底矩陣($N_{sample} \times N_{control}$)
$M_{j,i} = N_{i,p}(u_j)$
**我們的目的就是透過給定的$C$(軌跡點矩陣)來算出$P$(控制點矩陣)**
這邊有一個問題就是$M$(基底矩陣)是什麼，要怎麼設定或是找到$M$。
所以要透過上面得遞迴公式來建立$M$，當有$C$和$M$，解出$P$只是一般的矩陣相乘。
$$
C = M \cdot P \implies M^{-1} \cdot C = M^{-1} \cdot M \cdot P \newline
\implies P = M^{-1} \cdot C
$$
這邊有個重點是基底矩陣($M$)必須要可逆才能求解。
# 當基底矩陣不可逆的情形
但實際上除了方陣的解法外，有可能用逼近的方式來處理大量雜訊
目的: 曲線也許不需要經過所有的點，只要「最接近」這些點就可以，同時保持曲線平滑(由控制點決定)
例如Lidar有1000個點，但我們可能只要20個點控制短來描述即可
所以此時控制點($N_{control}$)<軌跡點($N_{sample}$)，$M$就是一個$1000 \times 20$的矩陣，這是一個超定系統 (Overdetermined System)，方程式比未知數多，無法精確求解。
此時就使用最小平方法(Least Squares)，這跟MLP神經網路一樣，我們就不多寫推導，結論就是透過Pseudo-inverse
$$
C = M \cdot P \implies M^T \cdot C = M^T \cdot M \cdot P \implies P = (M^T M)^{-1} M^T C
$$
此時的
$M$: 基底矩陣($N_{sample} \times N_{control}$)
在求控制點階段(Solver Stage)
- $N_{sample}$其實就是$N_{waypoints}$ (約束點的數量)
- 如果是插值，$N_{waypoints}=N_{conrtol}$就是方陣
- 如果是逼近值，$N_{waypoints}>N_{conrtol}$就是長方陣
如果您要「嚴格經過」每一個 Waypoint：這是一個方陣問題。控制點數量必須等於 Waypoints 數量。
如果您要「平滑化」一堆 Waypoints：這是一個最小二乘問題。您可以指定較少的控制點（例如 Waypoints 有 100 個，您指定 Control Points 為 10 個），算出來的 $\mathbf{P}$ 會讓曲線像一條回歸線一樣穿過數據群。
下面範例就是一個sin波，我們從訊號中產生30個點，然後給他random noise，也就是圖上面的 Noisy Data Points，我們如果設定$N_{conrtol}=N_{sample}$，也就是希望然就透過B-spline得到的解要能通過所有的waypoints(這條線完美穿過了每一個黑點)，結果就是紅色那條線，因為要經過每一個雜訊點，紅線在兩點之間劇烈震盪（Overfitting）。這在機器人控制中是災難性的，因為導數（速度/加速度）會變得非常大且不連續。
如果我們只設定8個控制點，透過逼近法的方式，這時候的解就比較平滑也比較接進原始的sin波，這條線沒有經過黑點，而是穿過它們的中間。它像是一個「低通濾波器」，過濾掉了高頻雜訊，還原了原本的正弦波形狀。這是因為我們只給了它 8 個控制點（綠色方塊），它沒有足夠的自由度去擬合那些隨機雜訊，只能擬合主要趨勢。
上圖的程式碼:
```
import numpy as np
import matplotlib.pyplot as plt
# ==========================================
# 1. B-Spline 核心數學 (Pure Numpy)
# ==========================================
def cox_de_boor(u, i, k, knots):
""" 計算單個基底函數的值 """
if k == 0:
out = np.zeros_like(u)
mask = (u >= knots[i]) & (u < knots[i+1])
out[mask] = 1.0
# 處理最後一個節點的邊界
if knots[i+1] == knots[-1]:
out[u == knots[-1]] = 1.0
return out
denom1 = knots[i+k] - knots[i]
term1 = 0.0
if denom1 > 0:
term1 = ((u - knots[i]) / denom1) * cox_de_boor(u, i, k-1, knots)
denom2 = knots[i+k+1] - knots[i+1]
term2 = 0.0
if denom2 > 0:
term2 = ((knots[i+k+1] - u) / denom2) * cox_de_boor(u, i+1, k-1, knots)
return term1 + term2
def generate_knots(n_control, k):
""" 生成 Clamped Uniform Knots """
# 節點向量長度 = n_control + k + 1
n_inner = n_control - (k + 1)
if n_inner < 0: n_inner = 0
knots = np.concatenate([
np.zeros(k + 1),
np.linspace(0, 1, n_inner + 2)[1:-1],
np.ones(k + 1)
])
return knots
def build_basis_matrix(u_params, n_control, k, knots):
""" 建立矩陣 M (N_samples x N_control) """
n_samples = len(u_params)
M = np.zeros((n_samples, n_control))
for j in range(n_control):
M[:, j] = cox_de_boor(u_params, j, k, knots)
return M
# ==========================================
# 2. 兩種求解策略
# ==========================================
def solve_strict_interpolation(data, k=3):
"""
策略 A: 插值 (Interpolation)
控制點數量 = 數據點數量
解方陣: P = M^-1 * D
"""
n_data = len(data)
n_control = n_data # 強制相等
# 參數化 (使用簡單的 linspace，實際專案可用弦長)
u = np.linspace(0, 1, n_data)
knots = generate_knots(n_control, k)
# 建立方陣 M (n_data x n_data)
M = build_basis_matrix(u, n_control, k, knots)
# 使用 solve (求逆)
try:
P = np.linalg.solve(M, data)
except np.linalg.LinAlgError:
# 簡單處理奇異矩陣 (加上微小擾動)
P = np.linalg.solve(M + np.eye(n_data)*1e-6, data)
return P, knots
def solve_least_squares_approximation(data, n_control, k=3):
"""
策略 B: 逼近 (Approximation)
控制點數量 < 數據點數量
解超定方程: P = (M.T * M)^-1 * M.T * D
"""
n_data = len(data)
# n_control 由外部指定 (例如只給 10 個點來擬合 100 個數據)
u = np.linspace(0, 1, n_data)
knots = generate_knots(n_control, k)
# 建立長方形矩陣 M (n_data x n_control)
M = build_basis_matrix(u, n_control, k, knots)
# 使用 lstsq (最小二乘法)
# rcond=None 讓 numpy 自動處理奇異值截斷
P, residuals, rank, s = np.linalg.lstsq(M, data, rcond=None)
# a = np.linalg.pinv(np.dot(M.transpose(),M))
# b = np.dot(a, M.transpose())
# P1 = np.dot(b, data)
# print(P-P1)
return P, knots
def evaluate_curve(P, knots, k, num_samples=200):
""" 畫圖用的函數: C = M_plot * P """
u_new = np.linspace(0, 1, num_samples)
n_control = len(P)
M_plot = build_basis_matrix(u_new, n_control, k, knots)
curve = M_plot @ P # 矩陣乘法
return curve
# ==========================================
# 3. 主程式：對比實驗
# ==========================================
# A. 生成數據：正弦波 + 隨機雜訊
np.random.seed(42)
N_data = 40
x_data = np.linspace(0, 10, N_data)
y_clean = np.sin(x_data)
noise = np.random.normal(0, 0.5, N_data) # 加入雜訊
y_noisy = y_clean + noise
data_points = np.column_stack([x_data, y_noisy])
# B. 執行插值 (Interpolation) - Control Points = 30 (等於數據量)
P_interp, knots_interp = solve_strict_interpolation(data_points, k=3)
curve_interp = evaluate_curve(P_interp, knots_interp, k=3)
# C. 執行逼近 (Approximation) - Control Points = 8 (遠小於數據量)
# 我們只用 8 個控制點來描述這 30 個點的趨勢
P_approx, knots_approx = solve_least_squares_approximation(data_points, n_control=8, k=3)
curve_approx = evaluate_curve(P_approx, knots_approx, k=3)
# ==========================================
# 4. 畫圖分析
# ==========================================
plt.figure(figsize=(12, 6))
# 1. 原始數據
plt.plot(data_points[:,0], data_points[:,1], 'ko', alpha=0.6, label='Noisy Data Points (N=30)')
# 2. 插值結果 (紅色)
plt.plot(curve_interp[:,0], curve_interp[:,1], 'r-', linewidth=1.5, alpha=0.7,
label='Interpolation (Strict, N_ctrl=30)')
# 3. 逼近結果 (藍色)
plt.plot(curve_approx[:,0], curve_approx[:,1], 'b-', linewidth=3,
label='Approximation (Least Squares, N_ctrl=8)')
# 4. 畫出逼近法的控制點 (綠色)
plt.plot(P_approx[:,0], P_approx[:,1], 'g--s', linewidth=1, markersize=8,
label='Control Points (Approximation)')
plt.ylim(-3,3)
plt.title("B-Spline: Interpolation (Matrix Solve) vs Approximation (Least Squares)")
plt.legend()
plt.grid(True)
plt.show()
```
# 3D可以嗎?
行。在數學本質上，2D 和 3D 的 B-Spline 「完全沒有差別」。
唯一的差別在於數據的維度（Data Dimension），以及您如何解釋計算出來的結果（幾何意義）。
B-Spline 的數學定義：
$$C(u) = \sum_{i=0}^{n} N_{i,p}(u) P_i$$
在
1. 基底函數/權重 $N_{i,p}(u)$:
他只計算參數 $u$ 和節點向量 (Knots)。這邊跟控制點($P_i$)在2D還是3D還是6D關節空間完全無關，他只負責輸一組權重，例如$[0.1,0.6,0.3]$
2. 控制點 $P_i$ ：這是唯一包含維度資料的地方
$2D: P_i=\begin{bmatrix} x_i, y_i\end{bmatrix}$
$3D: P_i=\begin{bmatrix} x_i, y_i, z_i\end{bmatrix}$
計算 B-Spline 時，其實是把 X軸、Y軸、Z軸 拆開來獨立計算的（Decoupled）。
$X(u) = \sum N_i(u) \cdot x_i$
$Y(u) = \sum N_i(u) \cdot y_i$
$Z(u) = \sum N_i(u) \cdot z_i$ （3D 只是多算這一行）
在 NumPy 的實作中，這種差別幾乎是無差的。
我們解方程式 $\mathbf{P} = \mathbf{M}^{-1} \mathbf{D}$ 嗎？
- 矩陣 $\mathbf{M}$ ($N \times N$)： 只跟 $u$ 有關。所以不管你是 2D 還是 3D，$\mathbf{M}$ 矩陣完全一模一樣，算一次就好。
- 數據 $\mathbf{D}$：
** 2D 時，它是 shape (N, 2) 的矩陣。
** 3D 時，它是 shape (N, 3) 的矩陣。
求解： np.linalg.solve(M, D) 會自動處理多維度的 Right-Hand Side (RHS)。
```
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D # 啟用 3D 繪圖
# ==========================================
# 1. 核心數學 (完全不用動，跟 2D 一模一樣)
# ==========================================
def cox_de_boor(u, i, k, knots):
""" 計算基底函數 (遞迴+向量化) """
if k == 0:
out = np.zeros_like(u, dtype=float)
mask = (u >= knots[i]) & (u < knots[i+1])
out[mask] = 1.0
if knots[i+1] == knots[-1]:
out[u == knots[-1]] = 1.0
return out
denom1 = knots[i+k] - knots[i]
term1 = 0.0
if denom1 > 0:
term1 = ((u - knots[i]) / denom1) * cox_de_boor(u, i, k-1, knots)
denom2 = knots[i+k+1] - knots[i+1]
term2 = 0.0
if denom2 > 0:
term2 = ((knots[i+k+1] - u) / denom2) * cox_de_boor(u, i+1, k-1, knots)
return term1 + term2
# ==========================================
# 2. 3D 求解器 (支援 N維 輸入)
# ==========================================
def solve_3d_bspline(points, k=3):
"""
輸入: points (N, 3) 或是 (N, D) 的任意維度陣列
輸出: Control Points (N, D), Knots, u參數
"""
points = np.array(points)
n = len(points)
# --- 1. 參數化 (使用 3D 弦長) ---
# 計算相鄰點的歐幾里得距離
diffs = np.diff(points, axis=0) # (N-1, 3)
dists = np.linalg.norm(diffs, axis=1) # (N-1,)
# 累積距離歸一化成 [0, 1]
u = np.cumsum(dists)
u = np.hstack(([0], u))
u = u / u[-1]
# --- 2. 生成節點向量 (Averaging Method) ---
knots = np.zeros(n + k + 1)
knots[-(k+1):] = 1.0
for j in range(1, n - k):
knots[j+k] = np.mean(u[j : j+k])
# --- 3. 建立矩陣 M ---
# M 的大小是 (N, N)
M = np.zeros((n, n))
for row_i in range(n):
for col_j in range(n):
# 注意：這裡只算基底函數的值，跟 x,y,z 無關
val = cox_de_boor(np.array([u[row_i]]), col_j, k, knots)
M[row_i, col_j] = val[0] # 取純量
# --- 4. 一次解出 x, y, z 三個維度的控制點 ---
# numpy solve 支援 RHS 為多行向量
# M * P = Points
try:
P = np.linalg.solve(M, points)
except np.linalg.LinAlgError:
print("矩陣奇異，嘗試加入微小擾動...")
M += np.eye(n) * 1e-6
P = np.linalg.solve(M, points)
return P, knots, u
def evaluate_3d_bspline(P, knots, k, num_samples=100):
"""
計算插值後的曲線點
"""
u_new = np.linspace(0, 1, num_samples)
n_control = len(P)
dim = P.shape[1] # 偵測維度 (例如 3)
# 初始化輸出 (samples, 3)
curve = np.zeros((num_samples, dim))
# 疊加
for i in range(n_control):
basis = cox_de_boor(u_new, i, k, knots)
# basis: (samples,)
# P[i]: (3,)
# outer product 會變成 (samples, 3)
curve += np.outer(basis, P[i])
return curve
# ==========================================
# 3. 主程式：3D 螺旋線範例
# ==========================================
# 1. 產生 3D 數據點 (螺旋線)
num_points = 10
theta = np.linspace(0, 4 * np.pi, num_points)
z_data = np.linspace(0, 5, num_points)
x_data = np.sin(theta)
y_data = np.cos(theta)
waypoints_3d = np.column_stack([x_data, y_data, z_data])
# 2. 解 B-Spline (Order=3)
k_order = 3
control_points, knots, u_params = solve_3d_bspline(waypoints_3d, k=k_order)
# 3. 生成平滑軌跡
smooth_path = evaluate_3d_bspline(control_points, knots, k=k_order, num_samples=200)
# ==========================================
# 4. 3D 視覺化
# ==========================================
fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
# 畫原始資料點
ax.scatter(waypoints_3d[:,0], waypoints_3d[:,1], waypoints_3d[:,2],
c='r', s=50, label='Waypoints', depthshade=False)
# 畫控制點連線 (為了理解用)
ax.plot(control_points[:,0], control_points[:,1], control_points[:,2],
'g--.', alpha=0.4, label='Control Polygon')
# 畫 B-Spline 曲線
ax.plot(smooth_path[:,0], smooth_path[:,1], smooth_path[:,2],
'b-', linewidth=2, label=f'3D B-Spline (k={k_order})')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title("Pure Numpy 3D B-Spline Interpolation")
ax.legend()
# 設定視角以便觀察
ax.view_init(elev=20, azim=-45)
plt.tight_layout()
plt.show()
```
路徑規劃中的「物理意義」
差別雖然數學一樣，但在**工程應用（路徑規劃）**上，3D 會帶來三個需要額外處理的細節：
A. 參數化 (Parameterization) 的距離計算
我們通常用「弦長（Chord Length）」來決定參數 $u$。
- 2D: $\Delta d = \sqrt{\Delta x^2 + \Delta y^2}$
- 3D: $\Delta d = \sqrt{\Delta x^2 + \Delta y^2 + \mathbf{\Delta z^2}}$
影響： 如果忽略了 Z 軸的變化只看 XY 平面距離，參數 $u$ 的分佈會失真，導致無人機在爬升時速度計算錯誤。
B. 姿態與航向 (Orientation / Yaw)
這是最麻煩的地方。
- 2D: 算出切線向量 $(v_x, v_y)$ 後，航向角 $Yaw = \text{arctan2}(v_y, v_x)$。
- 3D: 算出切線向量 $(v_x, v_y, v_z)$ 後：
- $Yaw$ (水平轉向) 依然主要由 $v_x, v_y$ 決定。
- 但是多了 $Pitch$ (俯仰角)，由 $v_z$ 和水平速度的比例決定。
- 注意： B-Spline 本身是位置曲線 $(x,y,z)$，它無法保證 3D 空間中的 Roll (滾轉角)。如果您需要控制無人機的鏡頭方向，通常需要額外規劃一條 B-Spline 來描述「視線向量」。
C. 避障計算 (Collision Check)
- 2D: 點到圓的距離 $\sqrt{(x-x_o)^2 + (y-y_o)^2} < R$。
- 3D: 點到球的距離 $\sqrt{(x-x_o)^2 + (y-y_o)^2 + (z-z_o)^2} < R$。
- 優化成本： 在 3D 空間做優化（Optimization）時，搜尋空間變大（多了一個維度），Local Minima（局部最佳解，例如被困在凹陷的地形裡）的問題會比 2D 更容易發生。
I. 數學上： 3D B-Spline 只是 2D B-Spline 多了一個 Z 通道。所有的矩陣求解、Cox-de Boor 遞迴公式完全不用改。
II. 實作上： 確保您的輸入 Array 是 (N, 3) 而不是 (N, 2)，NumPy 會搞定剩下的一切。
III. 物理上： 記得在計算參數 $u$ (累積距離) 和 Cost Function (避障距離) 時，把 $Z$ 軸的貢獻加進去即可。
###### tags: `SLAM Learning`
Sign in with Wallet
Connect another wallet