Các loại Trajectory: Joint Space vs Cartesian Space Planning

Sau khi nắm vững kinematics và dynamics ở bài trước, câu hỏi tiếp theo là: Robot di chuyển từ điểm A đến điểm B như thế nào? Đây là bài toán trajectory planning — và cách bạn lập kế hoạch quỹ đạo sẽ quyết định robot di chuyển nhanh hay chậm, mượt hay giật, an toàn hay nguy hiểm.

Có hai không gian cơ bản để lập kế hoạch: Joint Space và Cartesian Space. Mỗi loại có ưu nhược điểm riêng, và việc chọn sai có thể dẫn đến va chạm, singularity, hoặc quỹ đạo kém hiệu quả.

Joint Space vs Cartesian Space — Khác biệt cốt lõi

Joint Space Planning

Trong Joint Space, quỹ đạo được lập kế hoạch trực tiếp trên góc khớp $q(t)$. Robot nội suy từ cấu hình khớp ban đầu $q_0$ đến cấu hình đích $q_f$.

Đặc điểm:

• Đường đi TCP (Tool Center Point) trong Cartesian không dự đoán được — có thể cong bất kỳ
• Không có singularity — vì không cần tính IK trong quá trình di chuyển
• Nhanh nhất — mỗi khớp di chuyển ngắn nhất có thể
• Đảm bảo joint limits — dễ kiểm tra vì làm việc trực tiếp với góc khớp

Cartesian Space Planning

Trong Cartesian Space, quỹ đạo được lập kế hoạch trên vị trí và hướng của end-effector trong không gian 3D. Robot đi theo đường thẳng (hoặc cung tròn) trong Cartesian.

Đặc điểm:

• Đường đi TCP có thể dự đoán — đường thẳng hoặc cung tròn chính xác
• Có thể gặp singularity — cần tính IK tại mỗi điểm trên quỹ đạo
• Chậm hơn — bị ràng buộc bởi đường đi Cartesian
• Cần thiết cho nhiều ứng dụng — hàn, sơn, lắp ráp đều cần đường thẳng

Joint Space Trajectory — Point-to-Point

Nội suy tuyến tính (Linear Interpolation)

Cách đơn giản nhất — nội suy tuyến tính giữa $q_0$ và $q_f$:

q(s) = q_0 + s × (q_f - q_0),  s ∈ [0, 1]

Nhưng nội suy tuyến tính không mượt — vận tốc nhảy đột ngột tại điểm bắt đầu và kết thúc.

Nội suy đa thức bậc 3 (Cubic Polynomial)

Thêm ràng buộc vận tốc đầu và cuối bằng 0:

q(t) = a₀ + a₁t + a₂t² + a₃t³

Với 4 ràng buộc:

• $q(0) = q_0$, $q(T) = q_f$
• $\dot{q}(0) = 0$, $\dot{q}(T) = 0$

import numpy as np
import matplotlib.pyplot as plt

def cubic_trajectory(q0, qf, T, dt=0.01):
    """Joint space cubic polynomial trajectory."""
    # Coefficients cho cubic polynomial
    a0 = q0
    a1 = 0  # Zero initial velocity
    a2 = 3 * (qf - q0) / T**2
    a3 = -2 * (qf - q0) / T**3

    t = np.arange(0, T + dt, dt)
    q = a0 + a1*t + a2*t**2 + a3*t**3
    qd = a1 + 2*a2*t + 3*a3*t**2
    qdd = 2*a2 + 6*a3*t

    return t, q, qd, qdd

# Quỹ đạo cho 1 khớp: từ 0° đến 90° trong 2 giây
T = 2.0
t, q, qd, qdd = cubic_trajectory(0, np.pi/2, T)

fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
axes[0].plot(t, np.degrees(q), 'b-', linewidth=2)
axes[0].set_ylabel('Position (deg)')
axes[0].set_title('Cubic Polynomial Joint Trajectory')
axes[0].grid(True)

axes[1].plot(t, np.degrees(qd), 'r-', linewidth=2)
axes[1].set_ylabel('Velocity (deg/s)')
axes[1].grid(True)

axes[2].plot(t, np.degrees(qdd), 'g-', linewidth=2)
axes[2].set_ylabel('Acceleration (deg/s²)')
axes[2].set_xlabel('Time (s)')
axes[2].grid(True)

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

Nội suy đa thức bậc 5 (Quintic Polynomial)

Thêm ràng buộc gia tốc đầu và cuối bằng 0 — mượt hơn cubic:

def quintic_trajectory(q0, qf, T, dt=0.01):
    """Joint space quintic polynomial trajectory."""
    a0 = q0
    a1 = 0
    a2 = 0
    a3 = 10 * (qf - q0) / T**3
    a4 = -15 * (qf - q0) / T**4
    a5 = 6 * (qf - q0) / T**5

    t = np.arange(0, T + dt, dt)
    q = a0 + a1*t + a2*t**2 + a3*t**3 + a4*t**4 + a5*t**5
    qd = a1 + 2*a2*t + 3*a3*t**2 + 4*a4*t**3 + 5*a5*t**4
    qdd = 2*a2 + 6*a3*t + 12*a4*t**2 + 20*a5*t**3

    return t, q, qd, qdd

Cartesian Space Trajectory — Đường thẳng trong 3D

Position Interpolation

Nội suy vị trí tuyến tính trong Cartesian đơn giản:

p(s) = p_0 + s × (p_f - p_0),  s ∈ [0, 1]

Nhưng vấn đề phức tạp hơn: hướng (orientation) không thể nội suy tuyến tính bằng ma trận rotation.

SLERP — Spherical Linear Interpolation cho Orientation

SLERP nội suy giữa hai quaternion trên bề mặt hypersphere — đảm bảo:

• Quay ngắn nhất giữa hai hướng
• Tốc độ quay không đổi
• Kết quả luôn là quaternion hợp lệ (unit quaternion)

q(s) = q_0 × sin((1-s)Ω) / sin(Ω) + q_f × sin(sΩ) / sin(Ω)

Trong đó $\Omega = \arccos(q_0 \cdot q_f)$.

import numpy as np
from spatialmath import SE3, UnitQuaternion
import roboticstoolbox as rtb
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def cartesian_trajectory(T_start, T_end, n_points=100):
    """Linear Cartesian trajectory with SLERP orientation."""
    positions = []
    orientations = []

    q_start = UnitQuaternion(T_start.R)
    q_end = UnitQuaternion(T_end.R)

    for i in range(n_points):
        s = i / (n_points - 1)

        # Linear position interpolation
        pos = T_start.t + s * (T_end.t - T_start.t)

        # SLERP orientation interpolation
        q = q_start.interp(q_end, s)

        positions.append(pos)
        orientations.append(q)

    return np.array(positions), orientations

# Định nghĩa start và end poses
T_start = SE3(0.4, -0.2, 0.3) * SE3.Rz(0)
T_end = SE3(0.4, 0.3, 0.5) * SE3.Rz(np.pi/2)

positions, orientations = cartesian_trajectory(T_start, T_end)

# Visualize đường đi Cartesian
fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
ax.plot(positions[:, 0], positions[:, 1], positions[:, 2],
        'b-', linewidth=2, label='Cartesian path')
ax.scatter(*T_start.t, c='green', s=100, label='Start')
ax.scatter(*T_end.t, c='red', s=100, label='End')
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('Cartesian Linear Trajectory')
ax.legend()
plt.savefig('cartesian_trajectory.png', dpi=150)
plt.show()

So sánh trực quan: Joint vs Cartesian TCP Path

Đây là sự khác biệt rõ ràng nhất — cùng điểm đầu và điểm cuối, nhưng đường TCP hoàn toàn khác:

import roboticstoolbox as rtb
import numpy as np
import matplotlib.pyplot as plt
from spatialmath import SE3

ur5e = rtb.models.UR5()

# Start và end configurations
q_start = np.array([0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0])
q_end = np.array([np.pi/2, -np.pi/4, np.pi/4, -np.pi/3, np.pi/3, np.pi/4])

n_points = 100

# === JOINT SPACE trajectory ===
joint_positions = []
for i in range(n_points):
    s = i / (n_points - 1)
    q = q_start + s * (q_end - q_start)  # Linear joint interpolation
    T = ur5e.fkine(q)
    joint_positions.append(T.t)
joint_positions = np.array(joint_positions)

# === CARTESIAN SPACE trajectory ===
T_start = ur5e.fkine(q_start)
T_end = ur5e.fkine(q_end)

cart_positions = []
for i in range(n_points):
    s = i / (n_points - 1)
    pos = T_start.t + s * (T_end.t - T_start.t)
    cart_positions.append(pos)
cart_positions = np.array(cart_positions)

# Plot comparison
fig = plt.figure(figsize=(14, 6))

ax1 = fig.add_subplot(121, projection='3d')
ax1.plot(joint_positions[:, 0], joint_positions[:, 1], joint_positions[:, 2],
         'b-', linewidth=2, label='Joint Space')
ax1.scatter(*joint_positions[0], c='green', s=100)
ax1.scatter(*joint_positions[-1], c='red', s=100)
ax1.set_title('Joint Space — TCP Path (curved)')
ax1.set_xlabel('X'); ax1.set_ylabel('Y'); ax1.set_zlabel('Z')

ax2 = fig.add_subplot(122, projection='3d')
ax2.plot(cart_positions[:, 0], cart_positions[:, 1], cart_positions[:, 2],
         'r-', linewidth=2, label='Cartesian Space')
ax2.scatter(*cart_positions[0], c='green', s=100)
ax2.scatter(*cart_positions[-1], c='red', s=100)
ax2.set_title('Cartesian Space — TCP Path (straight)')
ax2.set_xlabel('X'); ax2.set_ylabel('Y'); ax2.set_zlabel('Z')

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

Time Parameterization — Điều khiển tốc độ dọc quỹ đạo

Quỹ đạo hình học (geometric path) chỉ mô tả hình dạng đường đi. Để thực thi được, cần thêm time parameterization — ánh xạ $s \in [0,1]$ thành $t \in [0, T]$.

Các phương pháp time parameterization phổ biến:

def trapezoidal_time_scaling(T, v_max, dt=0.01):
    """Trapezoidal velocity time scaling."""
    # Thời gian tăng/giảm tốc
    t_acc = T - 1.0 / v_max  # Assuming s goes from 0 to 1
    if t_acc < 0:
        t_acc = T / 3  # Fallback: 1/3 accel, 1/3 cruise, 1/3 decel

    a = v_max / t_acc  # Acceleration

    t = np.arange(0, T + dt, dt)
    s = np.zeros_like(t)
    sd = np.zeros_like(t)

    for i, ti in enumerate(t):
        if ti < t_acc:
            # Acceleration phase
            s[i] = 0.5 * a * ti**2
            sd[i] = a * ti
        elif ti < T - t_acc:
            # Constant velocity phase
            s[i] = 0.5 * a * t_acc**2 + v_max * (ti - t_acc)
            sd[i] = v_max
        else:
            # Deceleration phase
            t_rem = T - ti
            s[i] = 1.0 - 0.5 * a * t_rem**2
            sd[i] = a * t_rem

    return t, s, sd

Waypoint Planning — Nhiều điểm trung gian

Trong thực tế, robot hiếm khi di chuyển chỉ giữa 2 điểm. Waypoint planning lập kế hoạch qua nhiều điểm trung gian:

import roboticstoolbox as rtb
import numpy as np

ur5e = rtb.models.UR5()

# Định nghĩa 5 waypoints (joint configurations)
waypoints = np.array([
    [0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0],
    [np.pi/4, -np.pi/4, np.pi/4, -np.pi/3, np.pi/3, np.pi/6],
    [np.pi/2, -np.pi/6, np.pi/6, -np.pi/4, np.pi/4, np.pi/4],
    [np.pi/4, -np.pi/3, np.pi/2, -np.pi/2, np.pi/2, np.pi/6],
    [0, -np.pi/4, np.pi/3, -np.pi/3, np.pi/3, 0],
])

# Tạo trajectory qua tất cả waypoints
traj = rtb.mstraj(waypoints, dt=0.05, tacc=0.5,
                   qdmax=[2.0]*6)  # Max velocity per joint

print(f"Trajectory duration: {traj.t[-1]:.2f}s")
print(f"Number of points: {len(traj.t)}")

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

Khi nào dùng loại nào?

Xử lý Singularity trong Cartesian Planning

Khi lập kế hoạch Cartesian, singularity là vấn đề lớn nhất. Giải pháp phổ biến:

Damped Least Squares (DLS)

Thay vì $\dot{q} = J^{-1}v$, dùng:

q̇ = J^T (JJ^T + λ²I)^{-1} v

Tham số $\lambda$ (damping factor) ngăn vận tốc khớp phát tán gần singularity.

def damped_least_squares(J, v, damping=0.01):
    """Compute joint velocities using Damped Least Squares."""
    n = J.shape[1]
    JJT = J @ J.T
    identity = np.eye(JJT.shape[0])
    q_dot = J.T @ np.linalg.solve(JJT + damping**2 * identity, v)
    return q_dot

Tài liệu tham khảo

• Gasparetto, A., Boscariol, P., Lanzutti, A., Vidoni, R. "Path Planning and Trajectory Planning Algorithms: A General Overview." Motion and Operation Planning of Robotic Systems, Springer, 2015. DOI: 10.1007/978-3-319-14705-5_1
• Siciliano, B. et al. Robotics: Modelling, Planning and Control. Springer, 2009. DOI: 10.1007/978-1-84628-642-1
• Corke, P. Robotics, Vision and Control (3rd ed.). Springer, 2023. DOI: 10.1007/978-3-031-07262-8

Kết luận

Trong bài này, chúng ta đã phân tích chi tiết hai loại trajectory planning cơ bản:

• Joint Space — nhanh, an toàn, nhưng không kiểm soát được đường TCP
• Cartesian Space — kiểm soát đường TCP nhưng phải đối mặt với singularity
• SLERP — nội suy hướng chính xác trên quaternion
• Time parameterization — biến đường hình học thành quỹ đạo thực thi được

Trong bài tiếp theo, chúng ta sẽ đi vào chi tiết MoveJ và MoveL — hai lệnh cơ bản nhất trong lập trình robot công nghiệp, chính là hiện thực hóa của Joint Space và Cartesian Space planning.

Bài viết liên quan

• Kinematics và Dynamics cơ bản — Nền tảng toán học cho trajectory planning
• PID Control cho Robot — Điều khiển vị trí và vận tốc khớp
• Top 5 Cobot so sánh — So sánh các robot hỗ trợ phổ biến