Velocity Blending: Smooth Transitions giữa các Motion Segments

Đến thời điểm này trong series, bạn đã biết MoveJ, MoveL và MoveC. Nhưng khi chuỗi nhiều motion segments liên tiếp, robot sẽ dừng lại tại mỗi waypoint — gây lãng phí thời gian cycle và rung động cơ khí. Velocity blending (hay motion blending) là kỹ thuật cho phép robot chuyển đổi mượt giữa các đoạn mà không cần dừng.

Đây là kỹ thuật mà mọi nhà tích hợp robot phải thành thạo — nó có thể giảm 20-40% cycle time so với stop-and-go.

Tại sao Blending quan trọng?

Stop-and-Go vs Blended Motion

Không có blending:

MoveL → DỪNG → MoveL → DỪNG → MoveL → DỪNG
          ↑ Mất 0.1-0.5s mỗi lần dừng

Với blending:

MoveL ——→ blend zone ——→ MoveL ——→ blend zone ——→ MoveL
          ↑ Chuyển mượt, không dừng

Lợi ích của blending:

Tradeoff cơ bản

Blending luôn có tradeoff giữa tốc độ và độ chính xác:

• Blend radius lớn → Nhanh hơn nhưng cắt góc nhiều → sai lệch vị trí lớn
• Blend radius nhỏ → Chính xác hơn nhưng chậm hơn → gần giống stop-and-go
• Blend radius = 0 → Dừng hoàn toàn tại waypoint

Blend Radius — Khái niệm cốt lõi

Blend radius ($r$) là bán kính vùng chuyển tiếp quanh waypoint. Khi TCP đến gần waypoint trong phạm vi $r$, controller bắt đầu chuyển sang segment tiếp theo thay vì đi đến chính xác waypoint.

import numpy as np
import matplotlib.pyplot as plt

def visualize_blend_radius():
    """Visualize effect of different blend radii."""
    # 3 waypoints forming an L-shape
    waypoints = np.array([
        [0.0, 0.0],
        [0.3, 0.0],   # Corner
        [0.3, 0.3],
    ])

    fig, axes = plt.subplots(1, 3, figsize=(15, 5))
    blend_radii = [0, 0.03, 0.08]
    titles = ['r = 0 (Stop)', 'r = 0.03m (Small)', 'r = 0.08m (Large)']

    for ax, r, title in zip(axes, blend_radii, titles):
        # Draw segments
        ax.plot(waypoints[:, 0], waypoints[:, 1], 'b--', alpha=0.3)
        ax.scatter(waypoints[:, 0], waypoints[:, 1], c='red', s=80, zorder=5)

        if r > 0:
            # Draw blend zone
            circle = plt.Circle(waypoints[1], r, color='orange',
                              fill=True, alpha=0.2)
            ax.add_patch(circle)

            # Approximate blended path
            n = 50
            theta = np.linspace(np.pi, np.pi/2, n)
            blend_x = waypoints[1, 0] - r + r * (1 - np.cos(theta - np.pi))
            blend_y = waypoints[1, 1] + r * np.sin(theta - np.pi) + r

            # Before blend
            t = np.linspace(0, 1, 20)
            pre_blend = np.column_stack([
                t * (waypoints[1, 0] - r),
                np.zeros(20)
            ])
            # After blend
            post_blend = np.column_stack([
                np.full(20, waypoints[1, 0]),
                np.linspace(r, waypoints[2, 1], 20)
            ])

            ax.plot(pre_blend[:, 0], pre_blend[:, 1], 'b-', linewidth=2)
            ax.plot(blend_x, blend_y, 'r-', linewidth=2, label='Blend')
            ax.plot(post_blend[:, 0], post_blend[:, 1], 'b-', linewidth=2)
        else:
            ax.plot(waypoints[:, 0], waypoints[:, 1], 'b-', linewidth=2)

        ax.set_title(title)
        ax.set_xlim(-0.05, 0.4)
        ax.set_ylim(-0.05, 0.35)
        ax.set_aspect('equal')
        ax.grid(True, alpha=0.3)

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

visualize_blend_radius()

Parabolic Blending — Phương pháp cổ điển

Nguyên lý

Parabolic blending (linear segments with parabolic blend) là phương pháp phổ biến nhất:

1. Giữ nguyên đoạn tuyến tính giữa các waypoints
2. Tại mỗi waypoint, thay thế đoạn sharp corner bằng đường parabol
3. Parabola đảm bảo vận tốc liên tục (C1 continuity)

Toán học

Trong vùng blend (thời gian $t_b$), quỹ đạo là parabola:

q(t) = q_i + q̇_i × t + 0.5 × q̈ × t²

Trong đó gia tốc $\ddot{q}$ được chọn sao cho vận tốc chuyển từ segment trước sang segment sau.

import numpy as np
import matplotlib.pyplot as plt

def parabolic_blend_trajectory(waypoints, durations, blend_time, dt=0.001):
    """
    Generate trajectory with parabolic blending.

    Args:
        waypoints: list of joint positions [q0, q1, q2, ...]
        durations: time for each segment [T1, T2, ...]
        blend_time: blend duration at each waypoint
        dt: time step
    """
    n_segments = len(waypoints) - 1
    assert len(durations) == n_segments

    # Compute velocities for each linear segment
    velocities = []
    for i in range(n_segments):
        v = (waypoints[i+1] - waypoints[i]) / durations[i]
        velocities.append(v)

    # Build trajectory
    t_list = []
    q_list = []
    qd_list = []
    qdd_list = []
    t_current = 0

    for seg in range(n_segments):
        tb = blend_time

        if seg == 0:
            # First blend: accelerate from zero
            accel = velocities[0] / tb
            t_blend = np.arange(0, tb, dt)
            q_blend = waypoints[0] + 0.5 * accel * t_blend**2
            qd_blend = accel * t_blend
            qdd_blend = np.full_like(t_blend, accel)

            t_list.extend(t_current + t_blend)
            q_list.extend(q_blend)
            qd_list.extend(qd_blend)
            qdd_list.extend(qdd_blend)
            t_current += tb

        # Linear segment
        t_linear_duration = durations[seg] - tb  # Adjusted
        if seg < n_segments - 1:
            t_linear_duration -= tb / 2

        if t_linear_duration > 0:
            t_linear = np.arange(0, t_linear_duration, dt)
            q_at_start = q_list[-1] if q_list else waypoints[seg]
            q_linear = q_at_start + velocities[seg] * t_linear
            qd_linear = np.full_like(t_linear, velocities[seg])
            qdd_linear = np.zeros_like(t_linear)

            t_list.extend(t_current + t_linear)
            q_list.extend(q_linear)
            qd_list.extend(qd_linear)
            qdd_list.extend(qdd_linear)
            t_current += t_linear_duration

        # Interior blend (between segments)
        if seg < n_segments - 1:
            v_prev = velocities[seg]
            v_next = velocities[seg + 1]
            accel = (v_next - v_prev) / tb

            t_blend = np.arange(0, tb, dt)
            q_at_start = q_list[-1] if q_list else waypoints[seg+1]
            q_blend = q_at_start + v_prev * t_blend + 0.5 * accel * t_blend**2
            qd_blend = v_prev + accel * t_blend
            qdd_blend = np.full_like(t_blend, accel)

            t_list.extend(t_current + t_blend)
            q_list.extend(q_blend)
            qd_list.extend(qd_blend)
            qdd_list.extend(qdd_blend)
            t_current += tb

    # Final blend: decelerate to zero
    accel = -velocities[-1] / tb
    t_blend = np.arange(0, tb, dt)
    q_at_start = q_list[-1]
    q_blend = q_at_start + velocities[-1] * t_blend + 0.5 * accel * t_blend**2
    qd_blend = velocities[-1] + accel * t_blend
    qdd_blend = np.full_like(t_blend, accel)

    t_list.extend(t_current + t_blend)
    q_list.extend(q_blend)
    qd_list.extend(qd_blend)
    qdd_list.extend(qdd_blend)

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

# Example: 4 waypoints
waypoints = [0, 30, 60, 90]  # degrees
durations = [1.0, 1.5, 1.0]  # seconds per segment
blend_time = 0.3             # seconds

t, q, qd, qdd = parabolic_blend_trajectory(
    np.array(waypoints, dtype=float),
    durations, blend_time
)

fig, axes = plt.subplots(3, 1, figsize=(12, 8), sharex=True)
axes[0].plot(t, q, 'b-', linewidth=2)
axes[0].set_ylabel('Position (deg)')
axes[0].set_title('Parabolic Blend Trajectory')
axes[0].grid(True)

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

axes[2].plot(t, 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('parabolic_blend.png', dpi=150)
plt.show()

Cách các hãng robot implement Blending

Universal Robots — blend_radius

UR dùng tham số r (blend radius) trong MoveJ/MoveL/MoveC:

# URScript — palletizing với blending
def palletize_row(positions, blend_r=0.05):
    for i, pos in enumerate(positions):
        if i == len(positions) - 1:
            r = 0  # Dừng tại điểm cuối
        else:
            r = blend_r

        movel(pos, a=1.2, v=0.25, r=r)

KUKA — Approximation ($APO)

KUKA dùng khái niệm Advance Run và Approximation ($APO):

; KUKA KRL
PTP P1 C_PTP  ; Approximate (no stop)
PTP P2 C_PTP
LIN P3        ; No approximation (stop)

; Set approximation distance
$APO.CPTP = 50  ; 50% approximation cho PTP
$APO.CVEL = 50  ; 50% velocity approximation
$APO.CDIS = 20  ; 20mm distance approximation cho LIN

ABB — Zone ($zone)

ABB dùng zone data để xác định vùng blend:

! ABB RAPID
MoveL p1, v500, z50, tool0;  ! z50 = 50mm zone
MoveL p2, v500, z20, tool0;  ! z20 = 20mm zone
MoveL p3, v500, fine, tool0; ! fine = stop at point

Multi-Segment Blending — Chuỗi MoveL với Blends

Ứng dụng thực tế: palletizing — robot đặt hàng vào pallet theo grid pattern.

import numpy as np
import matplotlib.pyplot as plt

def generate_pallet_path(origin, rows, cols, spacing_x, spacing_y,
                         approach_height, blend_radius):
    """Generate palletizing path with blending."""
    path = []
    blend_flags = []

    for row in range(rows):
        for col in range(cols):
            x = origin[0] + col * spacing_x
            y = origin[1] + row * spacing_y
            z = origin[2]

            # Approach above
            path.append([x, y, z + approach_height])
            blend_flags.append(blend_radius)

            # Move down to place
            path.append([x, y, z])
            blend_flags.append(0)  # Stop to place

            # Move back up
            path.append([x, y, z + approach_height])
            blend_flags.append(blend_radius)

    # Last point — no blend
    blend_flags[-1] = 0

    return np.array(path), blend_flags

# Pallet 3x4
path, blends = generate_pallet_path(
    origin=[0.2, -0.3, 0.05],
    rows=3, cols=4,
    spacing_x=0.08, spacing_y=0.1,
    approach_height=0.15,
    blend_radius=0.03
)

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

# Color segments based on blend
for i in range(len(path) - 1):
    color = 'green' if blends[i] > 0 else 'red'
    ax.plot(path[i:i+2, 0], path[i:i+2, 1], path[i:i+2, 2],
            color=color, linewidth=1.5, alpha=0.7)

ax.scatter(path[::3, 0], path[::3, 1], path[::3, 2],
           c='blue', s=30, label='Waypoints')
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('Palletizing Path (green=blended, red=stop)')
ax.legend()
plt.tight_layout()
plt.savefig('pallet_blend_path.png', dpi=150)
plt.show()

Velocity Blending vs Position Blending

Velocity Profile Comparison

import numpy as np
import matplotlib.pyplot as plt

def compare_blending_profiles(T=2.0, dt=0.001):
    """Compare velocity profiles: no blend vs blended."""
    t = np.arange(0, T*2, dt)

    # No blending — 2 segments, full stop between
    v_no_blend = np.zeros_like(t)
    v_max = 0.25  # m/s
    t_acc = 0.3   # acceleration time

    for i, ti in enumerate(t):
        # Segment 1
        if ti < t_acc:
            v_no_blend[i] = v_max * ti / t_acc
        elif ti < T - t_acc:
            v_no_blend[i] = v_max
        elif ti < T:
            v_no_blend[i] = v_max * (T - ti) / t_acc
        # Stop, then Segment 2
        elif ti < T + t_acc:
            v_no_blend[i] = v_max * (ti - T) / t_acc
        elif ti < 2*T - t_acc:
            v_no_blend[i] = v_max
        elif ti < 2*T:
            v_no_blend[i] = v_max * (2*T - ti) / t_acc

    # With blending — smooth transition
    v_blended = np.zeros_like(t)
    blend_start = T - 0.3
    blend_end = T + 0.3

    for i, ti in enumerate(t):
        if ti < t_acc:
            v_blended[i] = v_max * ti / t_acc
        elif ti < blend_start:
            v_blended[i] = v_max
        elif ti < blend_end:
            # Smooth blend using cosine
            s = (ti - blend_start) / (blend_end - blend_start)
            v_blended[i] = v_max * (0.5 + 0.5 * np.cos(np.pi * s - np.pi))
        elif ti < 2*T - t_acc:
            v_blended[i] = v_max
        elif ti < 2*T:
            v_blended[i] = v_max * (2*T - ti) / t_acc

    fig, ax = plt.subplots(figsize=(12, 5))
    ax.plot(t, v_no_blend, 'r-', linewidth=2, label='No blending (stop-and-go)')
    ax.plot(t, v_blended, 'b-', linewidth=2, label='With blending')
    ax.axvline(T, color='gray', linestyle='--', alpha=0.5, label='Waypoint')
    ax.fill_between(t, v_no_blend, v_blended, alpha=0.1, color='green')
    ax.set_xlabel('Time (s)')
    ax.set_ylabel('Velocity (m/s)')
    ax.set_title('Velocity Profile: No Blending vs Blended')
    ax.legend()
    ax.grid(True, alpha=0.3)
    plt.tight_layout()
    plt.savefig('velocity_blend_comparison.png', dpi=150)
    plt.show()

compare_blending_profiles()

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
• Gasparetto, A. et al. "Path Planning and Trajectory Planning Algorithms." Springer, 2015. DOI: 10.1007/978-3-319-14705-5_1
• Universal Robots. URScript Manual. Documentation
• Lynch, K.M., Park, F.C. Modern Robotics. Cambridge, 2017. Link

Kết luận

Velocity blending là kỹ thuật thiết yếu để tối ưu cycle time:

• Blend radius quyết định mức "cắt góc" tại waypoints
• Parabolic blending là phương pháp cổ điển nhưng hiệu quả
• Mỗi hãng robot có cách implement riêng (UR: r, KUKA: $APO, ABB: zone)
• Luôn có tradeoff tốc độ vs chính xác

Trong bài tiếp theo, chúng ta sẽ đi sâu vào motion profiles — trapezoidal, S-curve và polynomial — để hiểu cách robot tạo ra vận tốc mượt mà.

Bài viết liên quan

• MoveJ và MoveL chi tiết — Blend radius là tham số quan trọng của MoveJ/MoveL
• PID Control cho Robot — PID controller tracking blended trajectory
• Top 5 Cobot so sánh — So sánh cách các hãng xử lý blending