Motion Profiles: Trapezoidal, S-Curve và Polynomial Trajectories

Trong bài trước về blending, chúng ta đã thấy velocity profile ảnh hưởng đến chất lượng chuyển động. Bây giờ đến lúc đi sâu vào 3 loại motion profile quan trọng nhất trong điều khiển robot: Trapezoidal, S-Curve, và Polynomial. Hiểu rõ chúng là chìa khóa để tạo ra chuyển động vừa nhanh, vừa mượt, vừa an toàn cho cơ cấu.

Tổng quan 3 loại Motion Profile

Tiêu chí	Trapezoidal	S-Curve (7-segment)	Polynomial
Vận tốc liên tục	C0	C0	C0 → C∞
Gia tốc liên tục	Không (nhảy)	C0	C0 → C∞
Jerk liên tục	Không	Không (bounded)	Tùy bậc
Số tham số	2 (v_max, a_max)	3 (v_max, a_max, j_max)	Tùy bậc
Tính toán	Đơn giản	Trung bình	Đơn giản
Thời gian tối ưu	Tốt	Tốt	Không
Rung động	Cao	Thấp	Rất thấp (bậc cao)

Trapezoidal Velocity Profile

Nguyên lý 3 pha

Trapezoidal profile gồm 3 pha:

Pha tăng tốc (acceleration): $\dot{q}$ tăng tuyến tính, $\ddot{q}$ = hằng số
Pha vận tốc đều (cruise): $\dot{q}$ = hằng số, $\ddot{q}$ = 0
Pha giảm tốc (deceleration): $\dot{q}$ giảm tuyến tính, $\ddot{q}$ = hằng số (âm)

Vấn đề: Jerk vô hạn

Tại các điểm chuyển pha, gia tốc nhảy đột ngột — jerk ($\dddot{q}$) là vô hạn. Điều này gây:

Rung động cơ khí (structural vibration)
Mòn bánh răng nhanh hơn
Tiếng ồn tại các khớp
Sai lệch vị trí do độ dàn hồi cơ cấu

Implementation từ đầu

import numpy as np
import matplotlib.pyplot as plt

def trapezoidal_profile(q0, qf, v_max, a_max, dt=0.001):
    """
    Generate trapezoidal velocity profile.

    Args:
        q0: start position
        qf: end position
        v_max: maximum velocity
        a_max: maximum acceleration
        dt: time step

    Returns:
        t, q, qd, qdd, qddd (time, position, velocity, acceleration, jerk)
    """
    distance = abs(qf - q0)
    sign = np.sign(qf - q0)

    # Thời gian tăng tốc
    t_acc = v_max / a_max

    # Quãng đường trong pha tăng/giảm tốc
    d_acc = 0.5 * a_max * t_acc**2

    # Kiểm tra: đủ quãng đường cho pha cruise?
    if 2 * d_acc > distance:
        # Triangular profile (không đạt v_max)
        t_acc = np.sqrt(distance / a_max)
        v_peak = a_max * t_acc
        t_cruise = 0
        T = 2 * t_acc
    else:
        # Full trapezoidal
        d_cruise = distance - 2 * d_acc
        t_cruise = d_cruise / v_max
        T = 2 * t_acc + t_cruise
        v_peak = v_max

    # Generate trajectory
    t = np.arange(0, T + dt, dt)
    q = np.zeros_like(t)
    qd = np.zeros_like(t)
    qdd = np.zeros_like(t)
    qddd = np.zeros_like(t)

    for i, ti in enumerate(t):
        if ti <= t_acc:
            # Acceleration phase
            qdd[i] = a_max
            qd[i] = a_max * ti
            q[i] = q0 + sign * 0.5 * a_max * ti**2
        elif ti <= t_acc + t_cruise:
            # Cruise phase
            qdd[i] = 0
            qd[i] = v_peak
            q[i] = q0 + sign * (d_acc + v_peak * (ti - t_acc))
        else:
            # Deceleration phase
            t_dec = ti - t_acc - t_cruise
            qdd[i] = -a_max
            qd[i] = v_peak - a_max * t_dec
            q[i] = q0 + sign * (d_acc + v_peak * t_cruise +
                    v_peak * t_dec - 0.5 * a_max * t_dec**2)

    # Apply sign to velocity
    qd *= sign
    qdd *= sign

    # Compute jerk (numerical differentiation)
    qddd[1:] = np.diff(qdd) / dt

    return t, q, qd, qdd, qddd

# Generate trapezoidal profile
t_trap, q_trap, qd_trap, qdd_trap, qddd_trap = trapezoidal_profile(
    q0=0, qf=np.pi/2, v_max=2.0, a_max=5.0
)

fig, axes = plt.subplots(4, 1, figsize=(12, 10), sharex=True)
axes[0].plot(t_trap, np.degrees(q_trap), 'b-', linewidth=2)
axes[0].set_ylabel('Position (deg)')
axes[0].set_title('Trapezoidal Velocity Profile')
axes[0].grid(True)

axes[1].plot(t_trap, qd_trap, 'r-', linewidth=2)
axes[1].set_ylabel('Velocity (rad/s)')
axes[1].grid(True)

axes[2].plot(t_trap, qdd_trap, 'g-', linewidth=2)
axes[2].set_ylabel('Acceleration (rad/s²)')
axes[2].grid(True)

axes[3].plot(t_trap, qddd_trap, 'm-', linewidth=1)
axes[3].set_ylabel('Jerk (rad/s³)')
axes[3].set_xlabel('Time (s)')
axes[3].set_title('Note: Infinite jerk spikes at phase transitions!')
axes[3].grid(True)

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

S-Curve Profile (7-Segment)

Nguyên lý 7 pha

S-Curve giải quyết vấn đề jerk vô hạn bằng cách thêm pha tăng/giảm jerk:

Jerk tăng — gia tốc tăng dần
Gia tốc không đổi — jerk = 0
Jerk giảm — gia tốc giảm dần về 0
Pha cruise — vận tốc không đổi
Jerk tăng (âm) — bắt đầu giảm tốc
Gia tốc không đổi (âm) — giảm tốc đều
Jerk giảm (dương) — gia tốc về 0

def s_curve_profile(q0, qf, v_max, a_max, j_max, dt=0.001):
    """
    Generate S-curve (7-segment) velocity profile.

    Args:
        q0: start position
        qf: end position
        v_max: maximum velocity
        a_max: maximum acceleration
        j_max: maximum jerk
        dt: time step
    """
    distance = abs(qf - q0)
    sign = np.sign(qf - q0)

    # Thời gian jerk
    t_j = a_max / j_max

    # Thời gian pha gia tốc không đổi
    v_at_jerk = 0.5 * j_max * t_j**2  # Vận tốc sau pha jerk
    t_a = (v_max - 2 * v_at_jerk) / a_max

    if t_a < 0:
        # Không đạt a_max — giảm pha gia tốc
        t_a = 0
        t_j = np.sqrt(v_max / j_max)

    # Quãng đường pha tăng tốc (3 phases: j_up + a_const + j_down)
    d_acc = v_at_jerk * t_j + 0.5 * a_max * t_a**2 + a_max * t_a * t_j + \
            v_at_jerk * t_j + 0.5 * j_max * t_j**3 / 3

    # Simplified: compute numerically
    # Build profile step by step
    phases = []

    # Phase 1: Jerk = +j_max
    phases.append(('j_up', t_j, j_max))
    # Phase 2: Jerk = 0, accel = a_max
    if t_a > 0:
        phases.append(('a_const', t_a, 0))
    # Phase 3: Jerk = -j_max
    phases.append(('j_down', t_j, -j_max))

    # Compute total accel phase
    accel_time = t_j + t_a + t_j
    accel_velocity = a_max * t_a + j_max * t_j**2  # Total velocity gained

    # Phase 4: Cruise
    accel_distance = 0  # Will compute numerically
    # Simplified: compute total time
    T_total = 2 * accel_time + max(0, (distance - 2 * accel_distance) / v_max)

    # Numerical simulation
    t_list = [0]
    q_list = [q0]
    qd_list = [0]
    qdd_list = [0]
    qddd_list = [0]

    jerk = 0
    accel = 0
    vel = 0
    pos = q0

    # Phase 1: Increasing jerk
    t_phase = 0
    while t_phase < t_j:
        jerk = j_max
        accel += jerk * dt
        accel = min(accel, a_max)
        vel += accel * dt
        vel = min(vel, v_max)
        pos += sign * vel * dt
        t_phase += dt
        t_list.append(t_list[-1] + dt)
        q_list.append(pos)
        qd_list.append(sign * vel)
        qdd_list.append(sign * accel)
        qddd_list.append(sign * jerk)

    # Phase 2: Constant acceleration
    t_phase = 0
    while t_phase < t_a and vel < v_max:
        jerk = 0
        vel += accel * dt
        vel = min(vel, v_max)
        pos += sign * vel * dt
        t_phase += dt
        t_list.append(t_list[-1] + dt)
        q_list.append(pos)
        qd_list.append(sign * vel)
        qdd_list.append(sign * accel)
        qddd_list.append(0)

    # Phase 3: Decreasing jerk (accel → 0)
    t_phase = 0
    while t_phase < t_j and accel > 0:
        jerk = -j_max
        accel += jerk * dt
        accel = max(accel, 0)
        vel += accel * dt
        vel = min(vel, v_max)
        pos += sign * vel * dt
        t_phase += dt
        t_list.append(t_list[-1] + dt)
        q_list.append(pos)
        qd_list.append(sign * vel)
        qdd_list.append(sign * accel)
        qddd_list.append(sign * jerk)

    # Phase 4: Cruise
    remaining = distance - 2 * abs(pos - q0)
    if remaining > 0:
        t_cruise = remaining / vel
        t_phase = 0
        while t_phase < t_cruise:
            pos += sign * vel * dt
            t_phase += dt
            t_list.append(t_list[-1] + dt)
            q_list.append(pos)
            qd_list.append(sign * vel)
            qdd_list.append(0)
            qddd_list.append(0)

    # Phase 5-7: Mirror of Phase 1-3 (deceleration)
    t_phase = 0
    while t_phase < t_j:
        jerk = -j_max
        accel += jerk * dt
        vel += accel * dt
        vel = max(vel, 0)
        pos += sign * vel * dt
        t_phase += dt
        t_list.append(t_list[-1] + dt)
        q_list.append(pos)
        qd_list.append(sign * vel)
        qdd_list.append(sign * accel)
        qddd_list.append(sign * jerk)

    t_phase = 0
    while t_phase < t_a and vel > 0:
        vel += accel * dt
        vel = max(vel, 0)
        pos += sign * vel * dt
        t_phase += dt
        t_list.append(t_list[-1] + dt)
        q_list.append(pos)
        qd_list.append(sign * vel)
        qdd_list.append(sign * accel)
        qddd_list.append(0)

    t_phase = 0
    while t_phase < t_j and vel > 0:
        jerk = j_max
        accel += jerk * dt
        accel = min(accel, 0)
        vel += accel * dt
        vel = max(vel, 0)
        pos += sign * vel * dt
        t_phase += dt
        t_list.append(t_list[-1] + dt)
        q_list.append(pos)
        qd_list.append(sign * vel)
        qdd_list.append(sign * accel)
        qddd_list.append(sign * jerk)

    return (np.array(t_list), np.array(q_list),
            np.array(qd_list), np.array(qdd_list), np.array(qddd_list))

# Generate S-curve
t_s, q_s, qd_s, qdd_s, qddd_s = s_curve_profile(
    q0=0, qf=np.pi/2, v_max=2.0, a_max=5.0, j_max=30.0
)

Polynomial Trajectories

Cubic Polynomial (4 ràng buộc)

def cubic_poly(q0, qf, qd0, qdf, T, dt=0.001):
    """Cubic polynomial: matches position and velocity at endpoints."""
    a0 = q0
    a1 = qd0
    a2 = (3*(qf - q0) - (2*qd0 + qdf)*T) / T**2
    a3 = (-2*(qf - q0) + (qd0 + qdf)*T) / 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
    qddd = np.full_like(t, 6*a3)

    return t, q, qd, qdd, qddd

# Quintic (6 ràng buộc)
def quintic_poly(q0, qf, T, dt=0.001):
    """Quintic polynomial: zero velocity and acceleration at endpoints."""
    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
    qddd = 6*a3 + 24*a4*t + 60*a5*t**2

    return t, q, qd, qdd, qddd

# Generate all three for comparison
T = 1.0
t_cub, q_cub, qd_cub, qdd_cub, qddd_cub = cubic_poly(0, np.pi/2, 0, 0, T)
t_qui, q_qui, qd_qui, qdd_qui, qddd_qui = quintic_poly(0, np.pi/2, T)

So sánh trực quan tất cả profiles

import numpy as np
import matplotlib.pyplot as plt

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

# Trapezoidal
t1, q1, qd1, qdd1, qddd1 = trapezoidal_profile(0, np.pi/2, 2.0, 5.0)
axes[0].plot(t1, np.degrees(q1), 'b-', linewidth=2, label='Trapezoidal')
axes[1].plot(t1, qd1, 'b-', linewidth=2, label='Trapezoidal')
axes[2].plot(t1, qdd1, 'b-', linewidth=2, label='Trapezoidal')
axes[3].plot(t1, qddd1, 'b-', linewidth=1, alpha=0.7, label='Trapezoidal')

# S-Curve
axes[0].plot(t_s, np.degrees(q_s), 'r-', linewidth=2, label='S-Curve')
axes[1].plot(t_s, qd_s, 'r-', linewidth=2, label='S-Curve')
axes[2].plot(t_s, qdd_s, 'r-', linewidth=2, label='S-Curve')
axes[3].plot(t_s, qddd_s, 'r-', linewidth=1, alpha=0.7, label='S-Curve')

# Quintic
T_q = t1[-1]  # Same duration
t_qui2, q_qui2, qd_qui2, qdd_qui2, qddd_qui2 = quintic_poly(0, np.pi/2, T_q)
axes[0].plot(t_qui2, np.degrees(q_qui2), 'g-', linewidth=2, label='Quintic')
axes[1].plot(t_qui2, qd_qui2, 'g-', linewidth=2, label='Quintic')
axes[2].plot(t_qui2, qdd_qui2, 'g-', linewidth=2, label='Quintic')
axes[3].plot(t_qui2, qddd_qui2, 'g-', linewidth=1, alpha=0.7, label='Quintic')

axes[0].set_ylabel('Position (deg)')
axes[0].set_title('Motion Profile Comparison')
axes[0].legend()
axes[0].grid(True)

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

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

axes[3].set_ylabel('Jerk (rad/s³)')
axes[3].set_xlabel('Time (s)')
axes[3].grid(True)

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

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

Tình huống	Profile	Lý do
Palletizing, material handling	Trapezoidal	Nhanh, đơn giản, đủ tốt
Semiconductor, precision assembly	S-Curve	Giảm rung, bảo vệ payload nhạy
Smooth teach-and-playback	Quintic	Mượt nhất, dễ tính
CNC machining, contouring	S-Curve + spline	Chất lượng bề mặt cao
Real-time adaptive	Trapezoidal	Tính toán nhanh nhất
Research, simulation	Polynomial bậc 7+	Kiểm soát mọi đạo hàm

S-Curve eliminates Vibration — Demo

import numpy as np
import matplotlib.pyplot as plt

def simulate_vibration(qdd_profile, t, mass=1.0, stiffness=500.0,
                       damping=2.0):
    """Simulate structural vibration from acceleration profile."""
    # Simple mass-spring-damper
    omega_n = np.sqrt(stiffness / mass)
    zeta = damping / (2 * np.sqrt(mass * stiffness))
    omega_d = omega_n * np.sqrt(1 - zeta**2)

    dt = t[1] - t[0]
    x = np.zeros_like(t)  # Vibration displacement
    xd = np.zeros_like(t)

    for i in range(1, len(t)):
        # Force from acceleration
        F = mass * qdd_profile[i]
        # Spring-damper dynamics
        xdd = F/mass - 2*zeta*omega_n*xd[i-1] - omega_n**2*x[i-1]
        xd[i] = xd[i-1] + xdd * dt
        x[i] = x[i-1] + xd[i] * dt

    return x

# Compare vibration
fig, axes = plt.subplots(2, 1, figsize=(12, 8))

# Trapezoidal vibration
vib_trap = simulate_vibration(qdd1, t1)
axes[0].plot(t1, vib_trap * 1000, 'b-', linewidth=1.5)
axes[0].set_title('Vibration from Trapezoidal Profile')
axes[0].set_ylabel('Displacement (mm)')
axes[0].grid(True)

# S-Curve vibration
vib_s = simulate_vibration(qdd_s, t_s)
axes[1].plot(t_s, vib_s * 1000, 'r-', linewidth=1.5)
axes[1].set_title('Vibration from S-Curve Profile')
axes[1].set_ylabel('Displacement (mm)')
axes[1].set_xlabel('Time (s)')
axes[1].grid(True)

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

print(f"Trapezoidal max vibration: {np.max(np.abs(vib_trap))*1000:.3f} mm")
print(f"S-Curve max vibration: {np.max(np.abs(vib_s))*1000:.3f} mm")

Tài liệu tham khảo

Biagiotti, L., Melchiorri, C. Trajectory Planning for Automatic Machines and Robots. Springer, 2008. DOI: 10.1007/978-3-540-85629-0
Haschke, R., Weitnauer, E., Ritter, H. "On-Line Planning of Time-Optimal, Jerk-Limited Trajectories." IROS, 2008. DOI: 10.1109/IROS.2008.4650924
Lynch, K.M., Park, F.C. Modern Robotics: Mechanics, Planning, and Control. Cambridge, 2017. Link

Kết luận

Ba loại motion profile phục vụ các nhu cầu khác nhau:

Trapezoidal — đơn giản, nhanh tính toán, nhưng jerk vô hạn gây rung
S-Curve — bounded jerk, giảm rung đáng kể, phổ biến trong công nghiệp
Polynomial — mượt nhất về mặt toán học, dễ tùy chỉnh ràng buộc

Trong bài tiếp theo, chúng ta sẽ khám phá Spline, B-Spline và Time-Optimal Planning — các kỹ thuật nâng cao cho quỹ đạo phức tạp và tối ưu thời gian.