---
# System prepended metadata

title: '路徑規劃系列: B-Spline'
tags: [SLAM Learning]

---

# 路徑規劃系列: 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()
```
![image](https://hackmd.io/_uploads/HkBfn_CW-g.png)



## 程式邏輯完全移除 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()

```

![image](https://hackmd.io/_uploads/S140aeHfWl.png)


-------


我們將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 個控制點（綠色方塊），它沒有足夠的自由度去擬合那些隨機雜訊，只能擬合主要趨勢。

![image](https://hackmd.io/_uploads/H1BOQrIG-e.png)
上圖的程式碼:
```
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)。
![image](https://hackmd.io/_uploads/Hy04wSUf-x.png)


```
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`


