Inverse Kinematics: analytical vs numerical IK

Trong bài mô hình robot, ta đã có FK: đưa vector khớp q vào và nhận pose T_base_tool. Trong bài Jacobian, ta đã thấy vận tốc TCP liên hệ với vận tốc khớp bằng v = J(q) qdot. Bài này ghép hai mảnh đó thành inverse kinematics: cho trước pose đích, tìm một hoặc nhiều cấu hình khớp đưa robot tới pose đó.

IK là phần làm nhiều người mới robotics bị hụt vì cùng một pose có thể có nhiều nghiệm, không có nghiệm, hoặc có nghiệm nhưng đi qua vùng singularity. Một UR5 6-DOF thường có tối đa 8 nghiệm rời rạc: shoulder trái/phải, elbow up/down, wrist flip/non-flip. Khi thêm joint limits, self-collision, tool offset, vùng cấm của cell, và yêu cầu đường đi mượt, "nghiệm đúng" không còn là nghiệm có residual nhỏ nhất, mà là nghiệm dùng được trong controller thật.

IK cần trả về gì?

Forward kinematics là hàm:

T = FK(q)
q = [q1, q2, q3, q4, q5, q6]^T
T ∈ SE(3)

Inverse kinematics là bài toán ngược:

tìm q sao cho FK(q) ≈ T_target

Chữ ≈ rất quan trọng. Với analytical IK, ta cố giải chính xác bằng lượng giác và đại số. Với numerical IK, ta lặp dần để residual nhỏ hơn tolerance. Trong controller thực tế, residual thường gồm 6 phần: 3 lỗi vị trí và 3 lỗi orientation. Một dạng residual phổ biến:

e(q) = [ p_target - p(q) ]
       [ log(R(q)^T R_target) ]

log(R) là vector góc-trục nhỏ biểu diễn lỗi xoay. Nếu chỉ dạy beginner một điều, hãy dạy điều này: đừng trừ Euler angles trực tiếp nếu bạn chưa kiểm soát wrap-around và singularity của Euler. Với code from scratch, có thể bắt đầu bằng rotation vector hoặc quaternion error.

IK solver tốt nên trả về:

struct IKSolution {
    bool success;
    Eigen::VectorXd q;
    double residual_norm;
    int iterations;
    std::string reason;
};

Analytical solver nên trả về danh sách std::vector<Eigen::VectorXd> vì một target có nhiều nghiệm. Numerical solver thường trả một nghiệm gần q0, nên q0 chính là cách bạn chọn branch ngầm.

Nhiều nghiệm, joint limits và singularity

Với tay máy 6R kiểu công nghiệp, nhiều nghiệm xuất hiện từ các lựa chọn hình học:

Joint limits lọc nghiệm. Một nghiệm FK đúng nhưng q3 vượt giới hạn cơ khí thì không dùng được. Một nghiệm nằm sát giới hạn cũng nguy hiểm vì MoveL hoặc jogging tiếp theo có thể hết biên. Vì vậy numerical IK bên dưới sẽ có hai lớp: clamp cứng sau mỗi bước và null-space term đẩy khớp về giữa khoảng giới hạn.

Singularity là nơi Jacobian mất hạng. Khi đó một hướng vận tốc Cartesian cần vận tốc khớp rất lớn, hoặc có hướng TCP không điều khiển được. UR-style arm thường gặp wrist singularity khi sin(q5) ≈ 0: trục wrist 4 và wrist 6 thẳng hàng, nên q4 và q6 không còn tách biệt. Analytical IK lúc này thường phải nhận q6_des từ cấu hình trước để chọn nghiệm liên tục. Numerical IK dùng damped least squares để không phóng vận tốc khớp lên vô hạn.

Bằng tay: closed-form IK cho UR5

Ta dùng bộ thông số UR5 classic quen thuộc từ ur_kinematics:

d1 = 0.089159
a2 = -0.42500
a3 = -0.39225
d4 = 0.10915
d5 = 0.09465
d6 = 0.08230

Giả sử pose đích là ma trận 4x4:

T = [ n_x  o_x  a_x  p_x ]
    [ n_y  o_y  a_y  p_y ]
    [ n_z  o_z  a_z  p_z ]
    [ 0    0    0    1   ]

Trong implementation ROS ur_kinematics, ma trận được đổi dấu một số cột để khớp convention nội bộ. Bài này giữ ý tưởng giải theo nhánh, không yêu cầu bạn copy từng dấu nếu DH của bạn khác. Khi đưa vào project, hãy đối chiếu bằng round-trip q -> FK(q) -> IK(T) -> FK(q_ik).

Bước 1: giải theta1

Đặt:

A = d6 * T12 - T13
B = d6 * T02 - T03
R = A^2 + B^2

Với convention của ur_kinematics, hai nghiệm shoulder là:

theta1_a = atan2(-B, A) + acos(d4 / sqrt(R))
theta1_b = atan2(-B, A) - acos(d4 / sqrt(R))

Nếu d4^2 > R, wrist center nằm trong vùng không với tới theo hình chiếu ngang, nên không có nghiệm thật. Các nhánh đặc biệt A ≈ 0 hoặc B ≈ 0 nên xử lý bằng asin/acos có clamp để tránh lỗi số học khi giá trị là 1.0000000002.

Bước 2: giải theta5

Với mỗi theta1, ta giải wrist 2:

numer = T03 * sin(theta1) - T13 * cos(theta1) - d4
c5 = numer / d6
theta5_a = acos(clamp(c5, -1, 1))
theta5_b = 2*pi - theta5_a

Đây là branch wrist flip. Nếu sin(theta5) gần 0, wrist bị singular. Khi đó orientation quanh trục tool không xác định duy nhất; controller thực tế nên giữ theta6 gần giá trị hiện tại thay vì để nghiệm nhảy.

Bước 3: giải theta6

Với mỗi cặp theta1, theta5, nếu abs(sin(theta5)) > eps:

theta6 = atan2(
    sign(sin(theta5)) * -(T01 * sin(theta1) - T11 * cos(theta1)),
    sign(sin(theta5)) *  (T00 * sin(theta1) - T10 * cos(theta1))
)

Nếu sin(theta5) ≈ 0, dùng theta6_des từ cấu hình hiện tại. Đây là lý do analytical IK công nghiệp thường nhận thêm seed hoặc current joint state, dù công thức closed-form có vẻ độc lập.

Bước 4: giải theta2, theta3, theta4

Sau khi biết theta1, theta5, theta6, phần còn lại là tay phẳng 3R. Trong ur_kinematics, ta tính hai biến trung gian:

x04x = -s5*(T02*c1 + T12*s1)
       -c5*(s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1))

x04y = c5*(T20*c6 - T21*s6) - T22*s5

p13x = d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1))
       -d6*(T02*c1 + T12*s1) + T03*c1 + T13*s1

p13y = T23 - d1 - d6*T22 + d5*(T21*c6 + T20*s6)

Với p13x, p13y, khớp 2-3 là tam giác hai cạnh a2, a3:

c3 = (p13x^2 + p13y^2 - a2^2 - a3^2) / (2*a2*a3)
theta3_a = acos(clamp(c3, -1, 1))
theta3_b = 2*pi - theta3_a

Nếu abs(c3) > 1, target nằm ngoài vòng với của hai link chính. Với mỗi theta3, đặt:

denom = a2^2 + a3^2 + 2*a2*a3*cos(theta3)
A2 = a2 + a3*cos(theta3)
B2 = a3*sin(theta3)

theta2_a = atan2((A2*p13y - B2*p13x)/denom,
                 (A2*p13x + B2*p13y)/denom)

theta2_b = atan2((A2*p13y + B2*p13x)/denom,
                 (A2*p13x - B2*p13y)/denom)

Cuối cùng, theta4 bù phần orientation còn lại:

c23 = cos(theta2 + theta3)
s23 = sin(theta2 + theta3)

theta4 = atan2(c23*x04y - s23*x04x,
               x04x*c23 + x04y*s23)

Tổng cộng ta có 2 nghiệm shoulder × 2 nghiệm wrist × 2 nghiệm elbow = tối đa 8 nghiệm. Sau đó normalize về [-pi, pi], lọc joint limits, tính lại FK cho từng nghiệm, và chọn nghiệm gần q_current nhất:

score(q) = w_pose * residual(q)
         + w_jump * ||wrap(q - q_current)||^2
         + w_limit * limit_penalty(q)

Một bước Newton/DLS bằng tay

Numerical IK không giải lượng giác toàn cục. Nó tuyến tính hóa FK quanh q_k:

e_k ≈ J(q_k) Δq
q_{k+1} = q_k + Δq

Newton/Gauss-Newton dùng pseudo-inverse:

Δq = J^+ e
J^+ = J^T (J J^T)^-1

Gần singularity, J J^T gần suy biến. Damped least squares thay bằng:

Δq = J^T (J J^T + λ^2 I)^-1 e

Ví dụ một bước lặp nhỏ:

e = [0.010, -0.004, 0.006, 0.020, 0.000, -0.010]^T
||e|| = 0.025 m/rad-mixed
λ = 0.03
solve (J J^T + λ^2 I) y = e
Δq = J^T y
q_next = clamp(q + α Δq, q_min, q_max)

α là step scale, thường 0.2..1.0. Nếu residual tăng, giảm α hoặc tăng λ. Khi robot ở vùng tốt, λ nhỏ cho nghiệm nhanh; khi gần singular, λ lớn làm bước đi thận trọng hơn.

From scratch: Python prototype

Prototype Python nên ngắn, dễ plot residual, và dùng đúng FK/Jacobian của project. Đoạn dưới giả định bạn đã có fk(q) trả R, p và jacobian(q) trả ma trận 6×6 trong base frame.

import numpy as np
import matplotlib.pyplot as plt

def so3_error(R, Rd):
    Rerr = R.T @ Rd
    return 0.5 * np.array([
        Rerr[2, 1] - Rerr[1, 2],
        Rerr[0, 2] - Rerr[2, 0],
        Rerr[1, 0] - Rerr[0, 1],
    ])

def ik_dls(target_R, target_p, q0, q_min, q_max,
           lam=0.03, alpha=0.6, tol=1e-5, max_iter=200):
    q = q0.copy()
    mid = 0.5 * (q_min + q_max)
    span = q_max - q_min
    hist = []

    for it in range(max_iter):
        R, p = fk(q)
        e = np.r_[target_p - p, so3_error(R, target_R)]
        hist.append(np.linalg.norm(e))
        if hist[-1] < tol:
            return q, True, np.array(hist)

        J = jacobian(q)
        A = J @ J.T + (lam * lam) * np.eye(6)
        dq_task = J.T @ np.linalg.solve(A, e)

        # Null-space: push joints gently toward the center of their limits.
        J_pinv = J.T @ np.linalg.solve(A, np.eye(6))
        N = np.eye(len(q)) - J_pinv @ J
        grad_limit = -2.0 * (q - mid) / (span * span)
        dq = dq_task + 0.05 * (N @ grad_limit)

        q = np.clip(q + alpha * dq, q_min, q_max)

    return q, False, np.array(hist)

q_sol, ok, residual = ik_dls(Rd, pd, q0, q_min, q_max)
plt.semilogy(residual)
plt.xlabel("iteration")
plt.ylabel("||residual||")
plt.grid(True)
plt.show()

Lưu ý: so3_error dạng skew ở trên tốt cho lỗi orientation nhỏ. Nếu target cách rất xa, dùng log map chuẩn của SO(3) sẽ ổn định hơn. Bài này cố tình giữ prototype beginner-friendly để bạn thấy vòng lặp IK hoạt động.

From scratch: C++/Eigen numericalIK

Trong C++ runtime, ta không plot mà trả trạng thái rõ ràng. API tối thiểu:

struct IKOptions {
    int max_iter = 100;
    double tolerance = 1e-5;
    double lambda = 0.03;
    double alpha = 0.7;
    double null_gain = 0.05;
};

struct IKResult {
    bool success = false;
    Eigen::VectorXd q;
    double residual_norm = 0.0;
    int iterations = 0;
};

Implementation DLS:

IKResult numericalIK(const Eigen::Isometry3d& target,
                     const Eigen::VectorXd& q0,
                     const Eigen::VectorXd& q_min,
                     const Eigen::VectorXd& q_max,
                     const IKOptions& opt) {
    Eigen::VectorXd q = q0;
    const int n = q.size();
    const Eigen::VectorXd mid = 0.5 * (q_min + q_max);
    const Eigen::VectorXd span = q_max - q_min;

    IKResult out;
    out.q = q;

    for (int iter = 0; iter < opt.max_iter; ++iter) {
        Eigen::Isometry3d T = forwardKinematics(q);
        Eigen::Matrix<double, 6, 1> e;
        e.head<3>() = target.translation() - T.translation();
        Eigen::AngleAxisd aa(T.rotation().transpose() * target.rotation());
        e.tail<3>() = T.rotation() * (aa.axis() * aa.angle());

        const double norm = e.norm();
        if (norm < opt.tolerance) {
            out.success = true;
            out.q = q;
            out.residual_norm = norm;
            out.iterations = iter;
            return out;
        }

        Eigen::MatrixXd J = geometricJacobian(q);  // 6 x n
        Eigen::Matrix<double, 6, 6> A =
            J * J.transpose()
            + opt.lambda * opt.lambda * Eigen::Matrix<double, 6, 6>::Identity();

        Eigen::MatrixXd Jpinv = J.transpose() * A.ldlt().solve(
            Eigen::Matrix<double, 6, 6>::Identity());
        Eigen::VectorXd dq_task = Jpinv * e;

        Eigen::MatrixXd N = Eigen::MatrixXd::Identity(n, n) - Jpinv * J;
        Eigen::VectorXd grad_limit =
            -2.0 * (q - mid).array() / span.array().square();
        Eigen::VectorXd dq = dq_task + opt.null_gain * (N * grad_limit);

        q += opt.alpha * dq;
        q = q.cwiseMax(q_min).cwiseMin(q_max);
        out.residual_norm = norm;
        out.iterations = iter + 1;
    }

    out.q = q;
    return out;
}

Nếu n = 6, null-space chính xác chỉ còn tác dụng khi damping làm pseudo-inverse không hoàn toàn là inverse. Với robot 7-DOF hoặc mobile manipulator, null-space term mạnh hơn nhiều. Dù vậy, giữ cấu trúc này từ đầu giúp project mở rộng sang redundant arm dễ hơn.

CMakeLists.txt tối thiểu:

cmake_minimum_required(VERSION 3.16)
project(arm_controller_ik LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

find_package(Eigen3 REQUIRED)

add_library(arm_controller
    src/robot_model.cpp
    src/kinematics.cpp
    src/numerical_ik.cpp
)

target_include_directories(arm_controller PUBLIC include)
target_link_libraries(arm_controller PUBLIC Eigen3::Eigen)

add_executable(test_ik tests/test_ik_roundtrip.cpp)
target_link_libraries(test_ik PRIVATE arm_controller)

Round-trip test:

int main() {
    Eigen::VectorXd q(6);
    q << 0.2, -1.1, 1.4, -0.6, 0.8, 0.3;

    Eigen::VectorXd q_min(6), q_max(6);
    q_min.setConstant(-M_PI);
    q_max.setConstant(M_PI);

    Eigen::Isometry3d target = forwardKinematics(q);
    IKOptions opt;
    IKResult sol = numericalIK(target, q + Eigen::VectorXd::Constant(6, 0.05),
                               q_min, q_max, opt);

    Eigen::Isometry3d check = forwardKinematics(sol.q);
    double pos_err = (target.translation() - check.translation()).norm();
    Eigen::AngleAxisd aa(check.rotation().transpose() * target.rotation());

    if (!sol.success || pos_err > 1e-4 || std::abs(aa.angle()) > 1e-4) {
        return 1;
    }
    return 0;
}

Build:

cmake -S . -B build -G Ninja
cmake --build build
./build/test_ik

Thư viện nên biết, cập nhật tới 13/06/2026

Robotics Toolbox for Python vẫn là lựa chọn rất dễ học. robot.ikine_LM(...) là Levenberg-Marquardt numerical IK, có q0, tol, joint_limits, mask, ilimit, slimit và các tham số tránh limit/manipulability. PyPI hiện ghi roboticstoolbox-python bản 1.1.1, upload ngày 2024-05-11.

Pinocchio là thư viện C++/Python mạnh cho rigid-body algorithms. Tài liệu CLIK của Pinocchio trình bày vòng lặp kinh điển: đọc model, tính forward kinematics, lấy frame error, tính Jacobian, giải bước vận tốc có damping, rồi integrate(model, q, v*dt). Nếu project của bạn đang pin Pinocchio 3.x, CLIK vẫn là mẫu tốt. Trạng thái mới nhất kiểm tra ngày 13/06/2026: PyPI package pin là 4.0.0, upload ngày 2026-05-21; GitHub release mới nhất cũng là v4.0.0.

Pink là differential IK dựa trên Pinocchio. PyPI pin-pink hiện là 4.2.0, release ngày 2026-04-20. Điểm mạnh của Pink là task/limit có trọng số: frame task, posture task, center-of-mass task, joint limits, barriers. Nếu bạn đi từ single arm sang humanoid hoặc mobile manipulator, mô hình "nhiều task + QP" dễ mở rộng hơn vòng lặp DLS một mục tiêu.

Mink là differential IK dựa trên MuJoCo. PyPI mink hiện là 1.1.1, release ngày 2026-05-15. Mink giải joint velocity từ các task-space objectives và constraints bằng QP, hợp với mô phỏng MuJoCo, teleoperation và retargeting.

NVIDIA cuRobo là thư viện CUDA cho motion generation, gồm forward/inverse kinematics, collision checking, trajectory optimization, geometric planning và whole-body motion generation. GitHub release mới nhất kiểm tra ngày 13/06/2026 là v0.8.0, tên release "cuRoboV2 research and Open Source", phát hành 2026-04-18. Đây không phải thư viện beginner để thay cho solver C++ nhỏ trong bài này, nhưng rất đáng biết nếu bạn cần chạy nhiều seed IK/planning song song trên GPU.

Checklist debug IK

Nếu IK không hội tụ, kiểm tra theo thứ tự này:

1. FK có đúng không? Dùng một q biết trước và so với Robotics Toolbox hoặc Pinocchio.
2. Jacobian có đúng frame không? Nếu residual ở base frame mà Jacobian ở tool frame, solver sẽ đi kỳ lạ.
3. Orientation error có đúng chiều không? Thử chỉ solve position trước.
4. Đơn vị có thống nhất radian, metre không?
5. Target có nằm trong workspace và trong joint limits không?
6. Gần singularity không? In det(JJ^T) hoặc singular values của J.
7. Seed q0 có ở branch mong muốn không?

Analytical IK cho tốc độ và trả được toàn bộ branch, rất hợp cho UR-style arm và MoveJ tới pose rời rạc. Numerical IK linh hoạt hơn, dùng được với URDF bất kỳ, tool offset, robot dư bậc tự do và task phụ. Trong controller cổ điển, cách thực dụng là dùng cả hai: analytical IK để liệt kê nghiệm tốt khi robot có công thức rõ, numerical IK/DLS để refine, xử lý target nhỏ liên tục trong MoveL/jogging, và làm fallback khi model thay đổi.

Nguồn tham khảo

• Kelsey P. Hawkins, "Analytic Inverse Kinematics for the Universal Robots UR-5/UR-10 Arms", Georgia Tech, 2013.
• ROS Industrial ur_kinematics, file ur_kin.cpp, thông số UR5 và thuật toán branch IK.
• Alexander Elias, "UR5 Inverse Kinematics" và repo aelias36/ik-geo-ur5.
• Robotics Toolbox for Python documentation: Robot.ikine_LM.
• Pinocchio documentation: inverse kinematics CLIK example.
• Pink 4.2.0 documentation và PyPI pin-pink.
• Mink documentation và PyPI mink.
• NVIDIA cuRobo GitHub và Isaac Sim cuRobo documentation.

Bài viết liên quan

• Mô hình cánh tay robot: joints, links, URDF, DH, SE(3)
• Forward kinematics, inverse kinematics và Jacobian
• MoveJ và MoveL trong robot controller cổ điển