Profile vận tốc: trapezoid vs S-curve

Trong các bài trước, chúng ta đã tách bài toán điều khiển tay máy thành ba lớp: hình học, động học và thời gian. MoveJ/MoveL quyết định robot đi theo kiểu nào; blending quyết định chuyển qua waypoint có dừng hay không; còn bài này trả lời câu hỏi rất thực tế: đi nhanh bao nhiêu, tăng tốc thế nào, và có làm cơ khí bị giật không?

Đó là bài toán trajectory profile hoặc time-scaling. Ta đã có một đường đi hình học, ví dụ một đoạn thẳng trong không gian Cartesian hoặc một đoạn trong joint space. Profile sẽ gán thời gian cho đường đó: tại thời điểm t, robot đang ở phần trăm nào của đoạn đường, vận tốc bao nhiêu, gia tốc bao nhiêu, và jerk bao nhiêu.

Bài này đi từ profile trapezoid cổ điển đến S-curve giới hạn jerk. Cuối bài, bạn sẽ có prototype Python để vẽ bốn đồ thị position / velocity / acceleration / jerk, class C++ tối giản để đưa vào controller, và biết khi nào nên dùng thư viện production-grade như Ruckig hoặc TOPP-RA.

Time-scaling là gì?

Giả sử robot cần di chuyển từ q0 đến q1 trong một joint:

q(t) = q0 + s(t) * (q1 - q0)

Ở đây s(t) là biến vô hướng chạy từ 0 đến 1. Nếu s(t) tăng tuyến tính, robot đi đều với vận tốc không đổi, nhưng tại điểm bắt đầu vận tốc nhảy từ 0 lên giá trị khác 0. Đó là một xung gia tốc lý tưởng, không thể thực hiện trong cơ khí thật. Vì vậy controller cần profile có các ràng buộc:

Trong robot công nghiệp, giới hạn vận tốc và gia tốc thường đến từ motor, hộp số, driver, payload và an toàn cell. Giới hạn jerk liên quan trực tiếp đến độ rung, tiếng ồn, độ mòn, sai số tracking và chất lượng bề mặt khi hàn, sơn, mài, dispensing hoặc pick-and-place tốc độ cao.

Một cách nhìn đơn giản: velocity làm robot nhanh, acceleration làm robot đạt tốc độ nhanh, jerk làm robot cảm thấy mượt hay giật.

Profile trapezoid: nhanh, dễ hiểu, nhưng jerk không bị chặn

Profile trapezoid là bài học kinh điển trong motion control. Với một đoạn từ nghỉ đến nghỉ:

1. Tăng tốc với gia tốc hằng +a_max.
2. Chạy đều ở v_max.
3. Giảm tốc với gia tốc hằng -a_max.

Đồ thị vận tốc có dạng hình thang, nên gọi là trapezoid. Nếu quãng đường quá ngắn, robot không kịp đạt v_max; profile trở thành tam giác.

Ưu điểm của trapezoid:

• Công thức đơn giản, dễ debug.
• Chạy nhanh và đủ tốt cho nhiều axis nhỏ.
• Dễ đồng bộ nhiều joint bằng cách scale thời gian.
• Phù hợp cho mô phỏng hoặc controller tự học.

Nhược điểm lớn: gia tốc bị nhảy bậc. Tại thời điểm bắt đầu, acceleration nhảy từ 0 lên +a_max. Tại điểm chuyển từ tăng tốc sang chạy đều, acceleration nhảy từ +a_max về 0. Về mặt toán học, jerk tại các điểm đó là xung rất lớn. Cơ khí thật không tạo được xung vô hạn; kết quả là rung, overshoot, tiếng va đập nhẹ, hoặc driver phải tự lọc lệnh.

Điều này không có nghĩa trapezoid sai. Nó chỉ có nghĩa trapezoid là một mô hình tối giản. Nếu bạn đang viết simulator, bài học motion profile đầu tiên, hoặc axis chậm, trapezoid vẫn rất hữu ích. Nhưng nếu bạn điều khiển tay máy thật với payload đáng kể, bạn nên hiểu S-curve.

S-curve: giới hạn jerk để chuyển động mượt hơn

S-curve thêm một ràng buộc nữa: acceleration không được nhảy ngay lập tức. Thay vào đó, acceleration tăng dần từ 0 lên a_max bằng jerk hằng +j_max, giữ acceleration, rồi giảm dần về 0 bằng jerk -j_max.

Một profile S-curve đối xứng từ nghỉ đến nghỉ thường có tối đa 7 pha:

1. jerk +j  : acceleration tăng từ 0 lên +a
2. jerk  0  : acceleration giữ +a
3. jerk -j  : acceleration giảm về 0
4. cruise   : velocity giữ gần hằng
5. jerk -j  : acceleration giảm từ 0 xuống -a
6. jerk  0  : acceleration giữ -a
7. jerk +j  : acceleration tăng về 0

Vì acceleration liên tục, robot không bị "đá" ở điểm chuyển pha như trapezoid. Đồ thị velocity có hình chữ S ở phần tăng/giảm tốc, nên gọi là S-curve.

Lợi ích thực tế:

• Giảm rung ở joint, link, gripper và fixture.
• Giảm tracking error khi servo loop có băng thông hữu hạn.
• Giảm mòn cơ khí và tiếng ồn.
• Cho phép chạy nhanh hơn trong cell thật vì ít kích thích cộng hưởng.
• Mượt hơn khi blend nhiều segment hoặc khi jogging liên tục.

Đổi lại, S-curve phức tạp hơn. Bạn phải xử lý các trường hợp quãng đường ngắn, không đạt a_max, không đạt v_max, trạng thái ban đầu có vận tốc/gia tốc khác 0, và nhiều DoF phải đồng bộ. Đó là lý do production system thường dùng thư viện đã được kiểm thử.

Prototype Python: vẽ trapezoid và S-curve

Đoạn dưới đây tạo hai profile cho một trục, sau đó vẽ bốn đồ thị. Đây là code giáo dục, không phải replacement cho Ruckig trong robot thật. Điểm đáng chú ý là S-curve được tạo bằng các đoạn jerk hằng và chọn peak velocity bằng binary search nếu quãng đường ngắn.

import numpy as np
import matplotlib.pyplot as plt

def trapezoid_profile(distance=1.0, v_max=0.8, a_max=1.5, dt=0.002):
    t_acc = v_max / a_max
    d_acc = 0.5 * a_max * t_acc**2

    if 2 * d_acc > distance:
        t_acc = np.sqrt(distance / a_max)
        v_peak = a_max * t_acc
        t_cruise = 0.0
    else:
        v_peak = v_max
        t_cruise = (distance - 2 * d_acc) / v_max

    total = 2 * t_acc + t_cruise
    ts = np.arange(0.0, total + dt, dt)
    p, v, a = [], [], []

    for t in ts:
        if t < t_acc:
            ai = a_max
            vi = ai * t
            pi = 0.5 * ai * t**2
        elif t < t_acc + t_cruise:
            tau = t - t_acc
            ai = 0.0
            vi = v_peak
            pi = 0.5 * a_max * t_acc**2 + v_peak * tau
        else:
            tau = t - t_acc - t_cruise
            ai = -a_max
            vi = v_peak - a_max * tau
            pi = distance - 0.5 * a_max * max(total - t, 0.0) ** 2
        p.append(pi)
        v.append(max(vi, 0.0))
        a.append(ai)

    jerk = np.gradient(a, dt)
    return ts, np.array(p), np.array(v), np.array(a), jerk

def accel_distance_for_v(v_peak, a_max, j_max):
    if v_peak < a_max**2 / j_max:
        tj = np.sqrt(v_peak / j_max)
        ta = 0.0
        return j_max * tj**3, tj, ta
    tj = a_max / j_max
    ta = v_peak / a_max - tj
    d = a_max * tj**2 + 1.5 * a_max * tj * ta + 0.5 * a_max * ta**2
    return d, tj, ta

def scurve_profile(distance=1.0, v_max=0.8, a_max=1.5, j_max=8.0, dt=0.002):
    d_acc, tj, ta = accel_distance_for_v(v_max, a_max, j_max)
    if 2 * d_acc <= distance:
        v_peak = v_max
        t_cruise = (distance - 2 * d_acc) / v_peak
    else:
        lo, hi = 0.0, v_max
        for _ in range(80):
            mid = 0.5 * (lo + hi)
            d_mid, _, _ = accel_distance_for_v(mid, a_max, j_max)
            if 2 * d_mid > distance:
                hi = mid
            else:
                lo = mid
        v_peak = lo
        d_acc, tj, ta = accel_distance_for_v(v_peak, a_max, j_max)
        t_cruise = 0.0

    segments = [
        (+j_max, tj), (0.0, ta), (-j_max, tj),
        (0.0, t_cruise),
        (-j_max, tj), (0.0, ta), (+j_max, tj),
    ]

    p = [0.0]
    v = [0.0]
    a = [0.0]
    j = [0.0]
    t = [0.0]

    for jerk, duration in segments:
        elapsed = 0.0
        while elapsed < duration - 1e-12:
            h = min(dt, duration - elapsed)
            a_next = a[-1] + jerk * h
            v_next = max(v[-1] + a[-1] * h + 0.5 * jerk * h**2, 0.0)
            p_next = p[-1] + v[-1] * h + 0.5 * a[-1] * h**2 + (1.0 / 6.0) * jerk * h**3
            t.append(t[-1] + h)
            p.append(p_next)
            v.append(v_next)
            a.append(a_next)
            j.append(jerk)
            elapsed += h

    scale = distance / p[-1]
    return np.array(t), np.array(p) * scale, np.array(v) * scale, np.array(a) * scale, np.array(j) * scale

trap = trapezoid_profile()
scur = scurve_profile()

fig, axes = plt.subplots(4, 1, figsize=(10, 9), sharex=False)
labels = ["position", "velocity", "acceleration", "jerk"]
for i, ax in enumerate(axes):
    ax.plot(trap[0], trap[i + 1], label="trapezoid")
    ax.plot(scur[0], scur[i + 1], label="S-curve")
    ax.set_ylabel(labels[i])
    ax.grid(True, alpha=0.3)
    ax.legend()
axes[-1].set_xlabel("time [s]")
plt.tight_layout()
plt.show()

Khi chạy, bạn sẽ thấy trapezoid có jerk rất lớn tại các điểm chuyển pha. Với S-curve, jerk là các đoạn hằng hữu hạn; acceleration không nhảy bậc mà đi lên/xuống có kiểm soát.

C++ class tối giản

Trong controller thật, profile thường được gọi mỗi chu kỳ servo, ví dụ 1 ms hoặc 4 ms. Ta định nghĩa một ProfilePoint và hai class tạo danh sách mẫu. Code dưới đây ưu tiên rõ ràng hơn tối ưu.

#include <algorithm>
#include <cmath>
#include <vector>

struct ProfilePoint {
    double t;
    double position;
    double velocity;
    double acceleration;
    double jerk;
};

class TrapezoidProfile {
public:
    std::vector<ProfilePoint> generate(double distance, double v_max,
                                       double a_max, double dt) const {
        const double t_acc_nominal = v_max / a_max;
        const double d_acc_nominal = 0.5 * a_max * t_acc_nominal * t_acc_nominal;

        double t_acc = t_acc_nominal;
        double v_peak = v_max;
        double t_cruise = 0.0;

        if (2.0 * d_acc_nominal > distance) {
            t_acc = std::sqrt(distance / a_max);
            v_peak = a_max * t_acc;
        } else {
            t_cruise = (distance - 2.0 * d_acc_nominal) / v_max;
        }

        const double total = 2.0 * t_acc + t_cruise;
        std::vector<ProfilePoint> out;

        for (double t = 0.0; t <= total + 1e-9; t += dt) {
            double p = 0.0, v = 0.0, a = 0.0;
            if (t < t_acc) {
                a = a_max;
                v = a * t;
                p = 0.5 * a * t * t;
            } else if (t < t_acc + t_cruise) {
                const double tau = t - t_acc;
                a = 0.0;
                v = v_peak;
                p = 0.5 * a_max * t_acc * t_acc + v_peak * tau;
            } else {
                const double remain = std::max(total - t, 0.0);
                a = -a_max;
                v = a_max * remain;
                p = distance - 0.5 * a_max * remain * remain;
            }
            out.push_back({t, p, v, a, 0.0});
        }
        return out;
    }
};

class SCurveProfile {
public:
    std::vector<ProfilePoint> generate(double distance, double v_max,
                                       double a_max, double j_max,
                                       double dt) const {
        double tj = 0.0, ta = 0.0;
        double d_acc = accelDistance(v_max, a_max, j_max, tj, ta);
        double v_peak = v_max;
        double t_cruise = 0.0;

        if (2.0 * d_acc > distance) {
            double lo = 0.0, hi = v_max;
            for (int i = 0; i < 80; ++i) {
                const double mid = 0.5 * (lo + hi);
                double tj_mid = 0.0, ta_mid = 0.0;
                const double d_mid = accelDistance(mid, a_max, j_max, tj_mid, ta_mid);
                if (2.0 * d_mid > distance) hi = mid;
                else lo = mid;
            }
            v_peak = lo;
            d_acc = accelDistance(v_peak, a_max, j_max, tj, ta);
        } else {
            t_cruise = (distance - 2.0 * d_acc) / v_peak;
        }

        struct Segment { double jerk; double duration; };
        const std::vector<Segment> segments = {
            {+j_max, tj}, {0.0, ta}, {-j_max, tj},
            {0.0, t_cruise},
            {-j_max, tj}, {0.0, ta}, {+j_max, tj},
        };

        std::vector<ProfilePoint> out;
        double t = 0.0, p = 0.0, v = 0.0, a = 0.0;
        out.push_back({t, p, v, a, 0.0});

        for (const auto& s : segments) {
            for (double elapsed = 0.0; elapsed < s.duration - 1e-12; elapsed += dt) {
                const double h = std::min(dt, s.duration - elapsed);
                p += v * h + 0.5 * a * h * h + s.jerk * h * h * h / 6.0;
                v += a * h + 0.5 * s.jerk * h * h;
                a += s.jerk * h;
                t += h;
                out.push_back({t, p, v, a, s.jerk});
            }
        }

        const double scale = distance / out.back().position;
        for (auto& x : out) {
            x.position *= scale;
            x.velocity *= scale;
            x.acceleration *= scale;
            x.jerk *= scale;
        }
        return out;
    }

private:
    static double accelDistance(double v_peak, double a_max, double j_max,
                                double& tj, double& ta) {
        if (v_peak < a_max * a_max / j_max) {
            tj = std::sqrt(v_peak / j_max);
            ta = 0.0;
            return j_max * tj * tj * tj;
        }
        tj = a_max / j_max;
        ta = v_peak / a_max - tj;
        return a_max * tj * tj + 1.5 * a_max * tj * ta + 0.5 * a_max * ta * ta;
    }
};

Nếu dùng cho nhiều joint, bạn không nên tạo profile riêng rồi để mỗi joint kết thúc ở thời điểm khác nhau. Cách đơn giản là tạo profile cho từng joint, lấy thời gian dài nhất, rồi giảm v_max/a_max/j_max của các joint còn lại để cùng kết thúc. Cách tốt hơn là dùng solver hỗ trợ multi-DoF synchronization.

Ruckig: online jerk-limited trajectory generation

Ruckig là thư viện C++ chuyên cho online trajectory generation với giới hạn jerk. Theo tài liệu public hiện tại, Ruckig nhận trạng thái hiện tại gồm position, velocity, acceleration; nhận target position, velocity, acceleration; nhận giới hạn max_velocity, max_acceleration, max_jerk; rồi trả về trạng thái mới ở chu kỳ điều khiển tiếp theo.

Điểm mạnh của Ruckig không chỉ là sinh S-curve. Điểm mạnh là nó xử lý đúng các trường hợp khó mà prototype ở trên chưa xử lý đủ:

• Trạng thái ban đầu không đứng yên.
• Target có vận tốc/gia tốc khác 0.
• Multi-DoF synchronization.
• Online update ở mỗi control cycle.
• Jerk-limited và time-optimal trong các điều kiện ràng buộc.
• API C++ gọn, có Python binding cho thử nghiệm.

Ví dụ C++ tối giản:

#include <ruckig/ruckig.hpp>
#include <array>
#include <iostream>

int main() {
    constexpr size_t dofs = 6;
    constexpr double cycle_time = 0.001;

    ruckig::Ruckig<dofs> otg(cycle_time);
    ruckig::InputParameter<dofs> input;
    ruckig::OutputParameter<dofs> output;

    input.current_position = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    input.current_velocity = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    input.current_acceleration = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

    input.target_position = {1.0, -0.4, 0.6, 0.2, -0.2, 0.5};
    input.target_velocity = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    input.target_acceleration = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

    input.max_velocity = {1.0, 1.0, 1.2, 1.5, 1.5, 2.0};
    input.max_acceleration = {2.0, 2.0, 2.5, 3.0, 3.0, 4.0};
    input.max_jerk = {10.0, 10.0, 12.0, 20.0, 20.0, 30.0};

    while (otg.update(input, output) == ruckig::Result::Working) {
        std::cout << output.time << " " << output.new_position[0] << "\n";
        output.pass_to_input(input);
    }
}

CMake có thể link Ruckig theo nhiều cách. Nếu bạn cài bằng package manager hoặc install từ source:

cmake_minimum_required(VERSION 3.16)
project(profile_demo LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

find_package(ruckig REQUIRED)

add_executable(profile_demo main.cpp)
target_link_libraries(profile_demo PRIVATE ruckig::ruckig)

Lệnh build:

cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
cmake --build build -j
./build/profile_demo

Nếu chưa có Ruckig trong hệ thống, bạn có thể dùng FetchContent hoặc thêm repo làm submodule. Với robot thật, ưu tiên pin version cụ thể để kết quả build tái lập được.

TOPP-RA: time-optimal path parameterization

Ruckig phù hợp khi bạn cần online generator từ state hiện tại đến target. TOPP-RA giải một bài toán hơi khác: bạn đã có geometric path rồi, ví dụ spline đi qua nhiều waypoint, và muốn gán thời gian tối ưu theo các constraint vận tốc/gia tốc.

TOPP-RA dùng ý tưởng reachable analysis để tìm path velocity hợp lệ dọc theo tham số đường s. Nó rất hữu ích khi motion planner đã tạo đường tránh vật cản, còn bạn cần biến đường đó thành trajectory có thời gian:

Motion planner -> geometric path q(s)
TOPP-RA        -> time law s(t)
Controller     -> q(t), dq/dt, ddq/dt

Điểm khác biệt quan trọng:

TOPP-RA bản open-source truyền thống tập trung vào velocity/acceleration constraint hơn là jerk-limited online control. Vì vậy trong robot arm controller hiện đại, một kiến trúc thực dụng là: planner tạo path, TOPP-RA gán thời gian sơ bộ hoặc kiểm tra ràng buộc, Ruckig/servo layer xử lý đoạn cuối với jerk-limited update ở chu kỳ điều khiển.

Gắn profile vào MoveJ, MoveL và jogging

Với MoveJ, time-scaling thường chạy trực tiếp trên joint delta. Nếu joint 2 phải đi xa nhất, joint 2 quyết định duration; các joint khác được scale để đến đích cùng lúc. Đây là cách nhiều controller cổ điển làm vì đơn giản và ổn định.

Với MoveL, bạn có một đường thẳng trong Cartesian. Profile nên chạy trên tham số s(t) của đường thẳng, sau đó mỗi tick tính TCP pose rồi gọi IK để lấy joint command. Cách này giữ TCP đi thẳng, nhưng cần kiểm tra joint velocity/acceleration sau IK. Nếu chỉ profile trong Cartesian mà bỏ qua joint limit, robot có thể vi phạm limit ở gần singularity. Bài FK/IK/Jacobian và frames/jogging là nền để hiểu phần này.

Với jogging, target thay đổi liên tục theo input người vận hành. Trapezoid có thể tạo cảm giác giật khi nhấn/nhả nút. S-curve hoặc Ruckig hợp hơn vì mỗi chu kỳ có thể nhận current p/v/a, target mới và giới hạn jerk rồi trả ra state tiếp theo. Đây là lý do bài jogging servo thường nên dùng jerk-limited profile thay vì ramp vận tốc thủ công.

Ngoài series này, nếu bạn đang đưa controller lên hệ thống ROS 2 thật, nên đọc thêm ROS 2 remote monitoring để hiểu monitoring/logging và AGV vs AMR để thấy vì sao profile mượt cũng quan trọng trong mobile robot.

Checklist khi chọn profile

Nếu bạn đang viết controller đầu tiên:

1. Bắt đầu với trapezoid để kiểm tra pipeline FK/IK, interpolation, command loop.
2. Thêm S-curve để giảm rung và hiểu jerk.
3. Với robot thật, chuyển sang Ruckig cho online control.
4. Với path nhiều waypoint từ planner, học TOPP-RA hoặc công cụ time parameterization tương đương.
5. Luôn log position/velocity/acceleration/jerk và so với giới hạn motor/driver.
6. Test với payload thật ở tốc độ thấp trước khi tăng limit.

Một lỗi phổ biến là chỉ nhìn vị trí cuối cùng và kết luận "đã đến đích". Trong motion control, đường đi đến đích mới là phần nguy hiểm: vận tốc peak có vượt không, gia tốc có làm gripper rung không, jerk có kích thích cộng hưởng không, và các joint có đồng bộ không.

Kết luận

Trapezoid là profile đơn giản nhất để bắt đầu: dễ viết, dễ debug, đủ tốt cho nhiều demo. Nhưng nó có acceleration nhảy bậc, nghĩa là jerk không bị chặn. S-curve thêm giới hạn jerk, giúp chuyển động mượt hơn và gần với yêu cầu của robot arm thật.

Nếu bạn chỉ học nguyên lý, hãy tự viết prototype Python và class C++ như trong bài. Nếu bạn xây controller production, đừng tự tin quá sớm với code tự viết: dùng Ruckig cho online jerk-limited trajectory generation, và dùng TOPP-RA khi bạn cần time-parameterize một geometric path đã có. Khi kết hợp đúng, bạn có một lớp time-scaling đủ mạnh cho MoveJ, MoveL, blending, jogging và motion planning.

Bài viết liên quan

• Velocity Blending: Smooth Transitions giữa các Motion Segments
• MoveJ vs MoveL: Joint Space và Cartesian Space
• Motion Planning cho Robot Arm cổ điển