Advanced Trajectories: Spline, B-Spline và Time-Optimal Planning

Trong các bài trước, chúng ta đã xây dựng từ motion profiles cơ bản đến velocity blending. Nhưng khi robot cần đi qua nhiều waypoints phức tạp — spray painting hình cong, CNC machining bề mặt 3D, hay di chuyển tối ưu thời gian — chúng ta cần các kỹ thuật nâng cao: Spline interpolation và Time-Optimal Planning.

Bài này sẽ đi sâu vào cubic spline, B-spline, NURBS, và đặc biệt là thuật toán TOPPRA — state-of-the-art cho time-optimal trajectory parameterization.

Cubic Spline Interpolation

Vấn đề

Cho $n$ waypoints, chúng ta muốn tạo đường cong mượt (C2 continuous) đi qua chính xác tất cả các điểm. Polynomial bậc cao (bậc $n-1$) có vấn đề Runge phenomenon — dao động lớn giữa các điểm.

Giải pháp: Piecewise Cubic Spline

Thay vì 1 polynomial bậc cao, dùng nhiều polynomial bậc 3, mỗi cái giữa 2 waypoints liên tiếp:

S_i(t) = a_i + b_i(t - t_i) + c_i(t - t_i)² + d_i(t - t_i)³

Với ràng buộc:

• Nội suy: $S_i(t_i) = q_i$, $S_i(t_{i+1}) = q_{i+1}$
• C1: $S_i'(t_{i+1}) = S_{i+1}'(t_{i+1})$ (vận tốc liên tục)
• C2: $S_i''(t_{i+1}) = S_{i+1}''(t_{i+1})$ (gia tốc liên tục)

import numpy as np
from scipy.interpolate import CubicSpline
import matplotlib.pyplot as plt

# 10 waypoints cho robot arm (joint 1 position)
t_waypoints = np.array([0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5])
q_waypoints = np.array([0, 15, 45, 60, 50, 30, 55, 70, 45, 10])  # degrees

# Cubic spline interpolation
cs = CubicSpline(t_waypoints, q_waypoints, bc_type='clamped')

# Dense evaluation
t_dense = np.linspace(0, 4.5, 1000)
q_dense = cs(t_dense)
qd_dense = cs(t_dense, 1)   # 1st derivative (velocity)
qdd_dense = cs(t_dense, 2)  # 2nd derivative (acceleration)
qddd_dense = cs(t_dense, 3) # 3rd derivative (jerk)

fig, axes = plt.subplots(4, 1, figsize=(14, 10), sharex=True)

axes[0].plot(t_dense, q_dense, 'b-', linewidth=2)
axes[0].scatter(t_waypoints, q_waypoints, c='red', s=80, zorder=5,
               label='Waypoints')
axes[0].set_ylabel('Position (deg)')
axes[0].set_title('Cubic Spline Through 10 Waypoints')
axes[0].legend()
axes[0].grid(True)

axes[1].plot(t_dense, qd_dense, 'r-', linewidth=2)
axes[1].set_ylabel('Velocity (deg/s)')
axes[1].grid(True)

axes[2].plot(t_dense, qdd_dense, 'g-', linewidth=2)
axes[2].set_ylabel('Acceleration (deg/s²)')
axes[2].grid(True)

axes[3].plot(t_dense, qddd_dense, 'm-', linewidth=1.5)
axes[3].set_ylabel('Jerk (deg/s³)')
axes[3].set_xlabel('Time (s)')
axes[3].grid(True)

plt.tight_layout()
plt.savefig('cubic_spline.png', dpi=150)
plt.show()

Multi-joint Spline Trajectory

import roboticstoolbox as rtb
import numpy as np
from scipy.interpolate import CubicSpline
import matplotlib.pyplot as plt

ur5e = rtb.models.UR5()

# 6 waypoints (6-DOF joints)
waypoints = np.array([
    [0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0],
    [np.pi/6, -np.pi/4, np.pi/4, -np.pi/3, np.pi/3, np.pi/6],
    [np.pi/3, -np.pi/6, np.pi/6, -np.pi/4, np.pi/4, np.pi/3],
    [np.pi/4, -np.pi/3, np.pi/2, -np.pi/2, np.pi/3, np.pi/6],
    [np.pi/6, -np.pi/4, np.pi/3, -np.pi/3, np.pi/4, np.pi/4],
    [0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0],  # Return home
])

t_way = np.linspace(0, 5, len(waypoints))

# Spline for each joint
t_dense = np.linspace(0, 5, 500)
q_traj = np.zeros((len(t_dense), 6))

for j in range(6):
    cs = CubicSpline(t_way, waypoints[:, j], bc_type='clamped')
    q_traj[:, j] = cs(t_dense)

# Compute TCP positions
tcp_positions = []
for q in q_traj:
    T = ur5e.fkine(q)
    tcp_positions.append(T.t)
tcp_positions = np.array(tcp_positions)

# 3D TCP path
fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
ax.plot(tcp_positions[:, 0], tcp_positions[:, 1], tcp_positions[:, 2],
        'b-', linewidth=2, label='Spline TCP path')

# Mark waypoint TCP positions
for q in waypoints:
    T = ur5e.fkine(q)
    ax.scatter(*T.t, c='red', s=80, zorder=5)

ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('Multi-Joint Cubic Spline — TCP Path')
ax.legend()
plt.tight_layout()
plt.savefig('multi_joint_spline.png', dpi=150)
plt.show()

B-Spline — Đường cong mượt hơn, không nội suy

Khác biệt so với Cubic Spline

B-Spline không đi qua control points mà "bị hút" về phía chúng — tạo đường cong mượt hơn và dễ kiểm soát hơn.

from scipy.interpolate import BSpline, make_interp_spline
import numpy as np
import matplotlib.pyplot as plt

# Control points
control_points = np.array([
    [0.0, 0.0],
    [0.1, 0.3],
    [0.3, 0.5],
    [0.5, 0.2],
    [0.7, 0.6],
    [0.9, 0.4],
    [1.0, 0.0],
])

# Parameter values (uniform)
t_ctrl = np.linspace(0, 1, len(control_points))

# B-Spline interpolation (degree 3)
bspline = make_interp_spline(t_ctrl, control_points, k=3)

# Evaluate
t_dense = np.linspace(0, 1, 500)
path = bspline(t_dense)

# Compare with cubic spline
cs_x = CubicSpline(t_ctrl, control_points[:, 0])
cs_y = CubicSpline(t_ctrl, control_points[:, 1])

fig, ax = plt.subplots(figsize=(10, 6))
ax.plot(path[:, 0], path[:, 1], 'b-', linewidth=2, label='B-Spline')
ax.plot(cs_x(t_dense), cs_y(t_dense), 'r--', linewidth=2, label='Cubic Spline')
ax.scatter(control_points[:, 0], control_points[:, 1],
           c='green', s=100, zorder=5, label='Control Points')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_title('B-Spline vs Cubic Spline')
ax.legend()
ax.grid(True)
ax.set_aspect('equal')
plt.tight_layout()
plt.savefig('bspline_vs_cubic.png', dpi=150)
plt.show()

NURBS — Non-Uniform Rational B-Splines

NURBS mở rộng B-Spline bằng:

• Non-uniform: Knot vector không đều — cho phép tập trung độ phân giải ở vùng cong phức tạp
• Rational: Mỗi control point có weight — cho phép biểu diễn chính xác conic sections (circle, ellipse)

NURBS phổ biến trong CAD/CAM và CNC robotics.

# NURBS implementation đơn giản
def nurbs_evaluate(control_points, weights, knots, degree, t_values):
    """Evaluate NURBS curve at given parameter values."""
    n = len(control_points) - 1

    def basis(i, p, t, knots):
        """Cox-de Boor recursion for B-spline basis."""
        if p == 0:
            return 1.0 if knots[i] <= t < knots[i+1] else 0.0

        d1 = knots[i+p] - knots[i]
        d2 = knots[i+p+1] - knots[i+1]

        c1 = ((t - knots[i]) / d1 * basis(i, p-1, t, knots)) if d1 > 0 else 0
        c2 = ((knots[i+p+1] - t) / d2 * basis(i+1, p-1, t, knots)) if d2 > 0 else 0

        return c1 + c2

    points = []
    for t in t_values:
        # Clamp to avoid boundary issues
        t = min(t, knots[-1] - 1e-10)

        # Weighted sum
        numerator = np.zeros(2)
        denominator = 0

        for i in range(n + 1):
            b = basis(i, degree, t, knots)
            numerator += b * weights[i] * control_points[i]
            denominator += b * weights[i]

        if denominator > 0:
            points.append(numerator / denominator)

    return np.array(points)

Time-Optimal Trajectory Planning — TOPPRA

Bài toán

Cho một geometric path (đường hình học từ spline), tìm time parameterization $s(t)$ sao cho:

• Robot đi dọc path nhanh nhất có thể
• Không vi phạm ràng buộc velocity, acceleration, torque

TOPPRA Algorithm (Pham & Pham, 2018)

TOPPRA (Time-Optimal Path Parameterization based on Reachability Analysis) là state-of-the-art:

• Giải bài toán trong $O(n)$ (tuyến tính với số điểm)
• Xử lý ràng buộc đa dạng: velocity, acceleration, torque, friction
• Luôn tìm được nghiệm tối ưu toàn cục

import numpy as np

# TOPPRA cần cài đặt riêng:
# pip install toppra

import toppra
import toppra.constraint as constraint
import toppra.algorithm as algo
import matplotlib.pyplot as plt
from scipy.interpolate import CubicSpline

# Định nghĩa geometric path (spline qua waypoints)
waypoints = np.array([
    [0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0],
    [np.pi/6, -np.pi/4, np.pi/4, -np.pi/3, np.pi/3, np.pi/6],
    [np.pi/3, -np.pi/6, np.pi/6, -np.pi/4, np.pi/4, np.pi/3],
    [np.pi/4, -np.pi/3, np.pi/2, -np.pi/2, np.pi/3, np.pi/6],
    [np.pi/6, -np.pi/4, np.pi/3, -np.pi/3, np.pi/4, np.pi/4],
    [0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0],
])

# Tạo path parameter
ss = np.linspace(0, 1, len(waypoints))

# Tạo spline path
path = toppra.SplineInterpolator(ss, waypoints)

# Ràng buộc velocity và acceleration
vlim = np.array([
    [-2.0, 2.0],    # Joint 1 velocity limits (rad/s)
    [-2.0, 2.0],
    [-3.0, 3.0],
    [-3.0, 3.0],
    [-3.0, 3.0],
    [-3.0, 3.0],
])

alim = np.array([
    [-5.0, 5.0],    # Joint 1 acceleration limits (rad/s²)
    [-5.0, 5.0],
    [-10.0, 10.0],
    [-10.0, 10.0],
    [-10.0, 10.0],
    [-10.0, 10.0],
])

pc_vel = constraint.JointVelocityConstraint(vlim)
pc_acc = constraint.JointAccelerationConstraint(alim)

# Chạy TOPPRA
instance = algo.TOPPRA([pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel")
trajectory = instance.compute_trajectory()

if trajectory is not None:
    print(f"Time-optimal duration: {trajectory.duration:.3f} s")

    # Evaluate trajectory
    t_grid = np.linspace(0, trajectory.duration, 500)
    q_traj = trajectory(t_grid)
    qd_traj = trajectory(t_grid, 1)
    qdd_traj = trajectory(t_grid, 2)

    # Plot
    fig, axes = plt.subplots(3, 1, figsize=(14, 8), sharex=True)

    for j in range(6):
        axes[0].plot(t_grid, np.degrees(q_traj[:, j]), label=f'J{j+1}')
        axes[1].plot(t_grid, qd_traj[:, j], label=f'J{j+1}')
        axes[2].plot(t_grid, qdd_traj[:, j], label=f'J{j+1}')

    axes[0].set_ylabel('Position (deg)')
    axes[0].set_title(f'TOPPRA Time-Optimal Trajectory (T={trajectory.duration:.2f}s)')
    axes[0].legend(ncol=3)
    axes[0].grid(True)

    axes[1].set_ylabel('Velocity (rad/s)')
    axes[1].grid(True)

    axes[2].set_ylabel('Acceleration (rad/s²)')
    axes[2].set_xlabel('Time (s)')
    axes[2].grid(True)

    plt.tight_layout()
    plt.savefig('toppra_trajectory.png', dpi=150)
    plt.show()
else:
    print("TOPPRA failed — path may be infeasible")

Minimum-Time vs Minimum-Jerk Tradeoff

So sánh thời gian thực thi

# So sánh: TOPPRA time-optimal vs uniform time
import numpy as np

# Uniform timing: mỗi segment cùng thời gian
uniform_duration = 5.0  # seconds
print(f"Uniform timing: {uniform_duration:.2f}s")

# TOPPRA timing
if trajectory is not None:
    speedup = uniform_duration / trajectory.duration
    print(f"TOPPRA optimal: {trajectory.duration:.2f}s")
    print(f"Speedup: {speedup:.1f}x faster")

Ứng dụng: Spray Painting Complex Shapes

import numpy as np
import matplotlib.pyplot as plt
from scipy.interpolate import CubicSpline

def generate_painting_path(surface_func, u_range, v_range,
                           n_passes, standoff=0.05):
    """
    Generate spray painting path over a surface.

    Args:
        surface_func: function(u, v) -> [x, y, z]
        u_range: (u_min, u_max)
        v_range: (v_min, v_max)
        n_passes: number of parallel passes
        standoff: distance from surface
    """
    v_values = np.linspace(v_range[0], v_range[1], n_passes)
    u_dense = np.linspace(u_range[0], u_range[1], 50)

    all_points = []

    for i, v in enumerate(v_values):
        pass_points = []
        u_range_dir = u_dense if i % 2 == 0 else u_dense[::-1]

        for u in u_range_dir:
            point = surface_func(u, v)
            # Add standoff (assume surface normal is +Z)
            point[2] += standoff
            pass_points.append(point)

        all_points.extend(pass_points)

    return np.array(all_points)

# Curved surface
def curved_surface(u, v):
    x = u
    y = v
    z = 0.1 * np.sin(2 * np.pi * u) * np.cos(np.pi * v)
    return np.array([x, y, z])

path = generate_painting_path(
    curved_surface,
    u_range=(0, 0.5),
    v_range=(0, 0.3),
    n_passes=8,
    standoff=0.03
)

fig = plt.figure(figsize=(12, 8))
ax = fig.add_subplot(111, projection='3d')

# Surface
u_grid = np.linspace(0, 0.5, 50)
v_grid = np.linspace(0, 0.3, 50)
U, V = np.meshgrid(u_grid, v_grid)
Z = 0.1 * np.sin(2 * np.pi * U) * np.cos(np.pi * V)
ax.plot_surface(U, V, Z, alpha=0.2, color='gray')

# Path
ax.plot(path[:, 0], path[:, 1], path[:, 2], 'r-', linewidth=1.5,
        label='Painting path')
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('Spray Painting — Spline Path Over Curved Surface')
ax.legend()
plt.tight_layout()
plt.savefig('painting_path.png', dpi=150)
plt.show()

Tài liệu tham khảo

• Pham, H., Pham, Q.C. "A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis." IEEE Transactions on Robotics, 2018. DOI: 10.1109/TRO.2018.2819195 | arXiv:1707.07239
• Biagiotti, L., Melchiorri, C. Trajectory Planning for Automatic Machines and Robots. Springer, 2008. DOI: 10.1007/978-3-540-85629-0
• Piegl, L., Tiller, W. The NURBS Book. Springer, 1997. DOI: 10.1007/978-3-642-59223-2

Kết luận

Kỹ thuật nâng cao cho trajectory planning:

• Cubic Spline — nội suy mượt C2 qua waypoints
• B-Spline — approximating, local control, mượt hơn spline
• NURBS — biểu diễn chính xác curves phức tạp, dùng trong CNC
• TOPPRA — time-optimal parameterization, O(n), state-of-the-art

Trong bài cuối cùng của series, chúng ta sẽ tổng hợp tất cả kiến thức vào MoveIt2, URScript và deploy trên robot thật.

Bài viết liên quan

• Motion Profiles: Trapezoidal, S-Curve, Polynomial — Nền tảng cho time parameterization
• MoveIt2 Motion Planning — Framework tích hợp trajectory planning
• Mô phỏng robot với MuJoCo — Test trajectory trong simulation trước khi deploy