Jacobian, singularity và resolved-rate control

Trong bài mô hình robot, ta đã có forward kinematics: đưa vector khớp q vào và nhận pose TCP T_0e. Trong bài inverse kinematics, ta dùng sai số pose để tìm lại q. Bài này nối hai phần đó bằng một đối tượng rất thực dụng: Jacobian.

Nếu FK trả lời câu hỏi "với góc khớp này, TCP ở đâu?", Jacobian trả lời câu hỏi tức thời hơn: "nếu mỗi khớp quay một chút với vận tốc qdot, TCP đang đi theo hướng nào?". Đây là nền tảng của jogging Cartesian, servoing, singularity avoidance, force control và nhiều planner công nghiệp. Sau bài này, bạn sẽ tự derive cột Jacobian cho UR5, viết Python/C++ tính J(q), vẽ manipulability dọc quỹ đạo và biến lệnh vận tốc TCP thành vận tốc khớp an toàn.

1. Jacobian đang map cái gì?

Với robot nối tiếp n khớp, ta viết quan hệ vi phân:

twist_e = J(q) qdot

Trong đó qdot là vector vận tốc khớp n x 1. Với arm 6 bậc tự do như UR5, J(q) thường là ma trận 6 x 6. Ba hàng đầu là vận tốc tịnh tiến của TCP, ba hàng sau là vận tốc góc:

[vx vy vz wx wy wz]^T = J(q) [q1dot ... q6dot]^T

Tài liệu Modern Robotics gọi đây là Jacobian liên hệ vận tốc khớp với twist end-effector trong frame space hoặc body. Robotics Toolbox for Python ghi rõ robot.jacob0(q) là geometric Jacobian trong world/base frame và map joint velocity sang end-effector spatial velocity. Pinocchio cũng dùng cùng ý tưởng: getFrameJacobian trả Jacobian của một frame và vận tốc frame được cho bởi J * v.

Điểm cần phân biệt:

Hai loại này giống nhau ở phần tịnh tiến nhưng khác ở phần orientation. Analytic Jacobian phụ thuộc representation, nên nó có thể singular ngay cả khi cơ khí chưa singular, ví dụ Euler angle gimbal lock. Trong controller vận tốc thực tế, hãy ưu tiên geometric Jacobian cho twist [v, omega], rồi chỉ chuyển sang RPY/Euler khi UI hoặc planner yêu cầu.

2. Derive bằng tay cột Jacobian revolute cho UR5

Ta dùng quy ước DH cổ điển cho UR5 CB-series:

Universal Robots công bố bảng DH cho các dòng UR trên trang hỗ trợ chính thức; nếu bạn dùng UR5e, thay bộ d và a bằng bảng UR5e tương ứng, còn công thức Jacobian không đổi.

Với mỗi transform tích lũy:

T_0i = A_1(q1) A_2(q2) ... A_i(qi)

ta lấy:

p_i = T_0i[0:3, 3]
z_i = T_0i[0:3, 2]

Với khớp revolute thứ i, trục quay của nó là z_{i-1} và gốc khớp là p_{i-1}. Nếu p_e là vị trí TCP, cột Jacobian là:

Jv_i = z_{i-1} x (p_e - p_{i-1})
Jw_i = z_{i-1}
J_i  = [Jv_i; Jw_i]

Đây là công thức quan trọng nhất của bài. Nó nói rằng vận tốc tịnh tiến do một khớp quay tạo ra bằng tích có hướng giữa trục quay và vector từ khớp tới TCP. Vận tốc góc thì chính là trục quay. Nếu là prismatic joint, công thức đổi thành Jv_i = z_{i-1}, Jw_i = 0, nhưng UR5 có 6 revolute joints nên ta chỉ cần công thức trên.

Một cách kiểm tra trực giác: khớp 1 quay quanh trục z_0 của base. Nếu TCP nằm xa trục base, z_0 x (p_e - p_0) lớn, TCP đi nhanh theo tiếp tuyến. Nếu TCP gần như nằm trên trục base, phần đóng góp tịnh tiến của khớp 1 nhỏ; đó là mầm của shoulder singularity.

3. Singularity: khi Jacobian mất rank

Singularity xảy ra khi J(q) không còn full rank. Với UR5 6 x 6, điều này nghĩa là det(J) = 0, hoặc ổn định hơn trong code: singular value nhỏ nhất sigma_min tiến về 0.

Ba loại singularity thường gặp:

Không nên dùng riêng det(J) để điều khiển online, vì nó nhạy với đơn vị và scale. Cách bền hơn là SVD:

J = U diag(sigma_1 ... sigma_m) V^T
sigma_min = min(sigma_i)
condition_number = sigma_max / sigma_min
manipulability = sqrt(det(J J^T))

condition_number càng gần 1 thì robot càng isotropic: vận tốc khớp có thể sinh vận tốc TCP tương đối đều theo mọi hướng. Khi condition number tăng rất lớn, robot vẫn có thể di chuyển, nhưng có một hướng TCP "đắt": muốn có vận tốc nhỏ ở hướng đó thì cần vận tốc khớp rất lớn. Yoshikawa manipulability sqrt(det(JJ^T)) tỷ lệ với thể tích ellipsoid vận tốc; về gần 0 nghĩa là ellipsoid xẹp lại.

Lưu ý đơn vị: phần tịnh tiến dùng m/s, phần xoay dùng rad/s, nên manipulability toàn phần 6D có scale phụ thuộc đơn vị. Trong jogging TCP x/y/z, bạn có thể theo dõi Jv = J[0:3, :] riêng. Với điều khiển pose 6D, có thể scale phần xoay theo một chiều dài đặc trưng, ví dụ 0.2 m, trước khi so condition number.

4. Resolved-rate control

Resolved-rate motion control nhận lệnh vận tốc Cartesian rồi giải vận tốc khớp:

qdot = J^+ v_des

Nếu J vuông và xa singularity, J^+ gần như J^{-1}. Nếu robot redundant hoặc task ít chiều hơn, J^+ là Moore-Penrose pseudoinverse. Với jogging tịnh tiến, ta thường dùng Jv kích thước 3 x 6:

qdot = Jv^+ [vx vy vz]^T

Gần singularity, pseudoinverse thường thổi vận tốc khớp lên rất lớn vì nó chia cho singular value nhỏ. Damped least squares (DLS) thay thế bằng:

J_dls^+ = J^T (J J^T + lambda^2 I)^-1
qdot    = J_dls^+ v_des

lambda càng lớn thì nghiệm càng mượt và ít bùng vận tốc, nhưng TCP sẽ tracking kém hơn. Một rule thực dụng:

if sigma_min > sigma_safe: lambda = 0
else lambda = lambda_max * (1 - sigma_min / sigma_safe)^2

Trong controller thật, luôn thêm clamp vận tốc khớp, clamp gia tốc, joint limits và stop nếu sigma_min quá thấp mà operator vẫn cố jog vào hướng nguy hiểm.

5. Python từ đầu: analytic J, numerical J và jogging TCP

Đoạn code dưới đây đủ để bạn chạy notebook, so analytic Jacobian với numerical differentiation, vẽ manipulability dọc một quỹ đạo đơn giản và thử jog TCP theo x/y/z.

import numpy as np
import matplotlib.pyplot as plt

UR5_A = np.array([0.0, -0.425, -0.39225, 0.0, 0.0, 0.0])
UR5_D = np.array([0.089159, 0.0, 0.0, 0.10915, 0.09465, 0.0823])
UR5_ALPHA = np.array([np.pi / 2, 0.0, 0.0, np.pi / 2, -np.pi / 2, 0.0])

def dh(a, d, alpha, theta):
    c, s = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)
    return np.array([
        [c, -s * ca,  s * sa, a * c],
        [s,  c * ca, -c * sa, a * s],
        [0,      sa,      ca,     d],
        [0,       0,       0,     1],
    ], dtype=float)

def fk_all(q):
    T = np.eye(4)
    frames = [T.copy()]
    for a, d, alpha, theta in zip(UR5_A, UR5_D, UR5_ALPHA, q):
        T = T @ dh(a, d, alpha, theta)
        frames.append(T.copy())
    return frames

def fk(q):
    return fk_all(q)[-1]

def jacobian_analytic(q):
    frames = fk_all(q)
    pe = frames[-1][:3, 3]
    J = np.zeros((6, 6))
    for i in range(6):
        zi = frames[i][:3, 2]      # z_{i}
        pi = frames[i][:3, 3]      # p_{i}
        J[:3, i] = np.cross(zi, pe - pi)
        J[3:, i] = zi
    return J

def vee(S):
    return np.array([S[2, 1], S[0, 2], S[1, 0]])

def jacobian_numeric(q, eps=1e-6):
    T0 = fk(q)
    R0 = T0[:3, :3]
    J = np.zeros((6, 6))
    for i in range(6):
        dq = np.zeros(6)
        dq[i] = eps
        Tp = fk(q + dq)
        Tm = fk(q - dq)
        J[:3, i] = (Tp[:3, 3] - Tm[:3, 3]) / (2 * eps)

        Rdot = (Tp[:3, :3] - Tm[:3, :3]) / (2 * eps)
        omega_hat = Rdot @ R0.T          # spatial angular velocity
        J[3:, i] = vee(omega_hat)
    return J

def manipulability(J, rows=slice(0, 3)):
    A = J[rows, :]
    return np.sqrt(max(0.0, np.linalg.det(A @ A.T)))

def damped_pinv(J, lam):
    m = J.shape[0]
    return J.T @ np.linalg.inv(J @ J.T + (lam * lam) * np.eye(m))

def resolved_rate(q, twist, sigma_safe=0.05, lambda_max=0.2, qdot_limit=1.0):
    J = jacobian_analytic(q)
    s = np.linalg.svd(J, compute_uv=False)
    sigma_min = s[-1]
    lam = 0.0 if sigma_min >= sigma_safe else lambda_max * (1 - sigma_min / sigma_safe) ** 2
    qdot = damped_pinv(J, lam) @ twist
    qdot = np.clip(qdot, -qdot_limit, qdot_limit)
    return qdot, sigma_min, lam

q = np.deg2rad([0, -70, 90, -110, -90, 0])
Ja = jacobian_analytic(q)
Jn = jacobian_numeric(q)
print("max |Ja - Jn| =", np.max(np.abs(Ja - Jn)))

# Vẽ manipulability khi nội suy từ pose A sang pose B
q0 = np.deg2rad([0, -90, 90, -90, -90, 0])
q1 = np.deg2rad([60, -60, 40, -110, -60, 30])
ts = np.linspace(0, 1, 200)
mu = []
kappa = []
for t in ts:
    q = (1 - t) * q0 + t * q1
    J = jacobian_analytic(q)
    s = np.linalg.svd(J, compute_uv=False)
    mu.append(manipulability(J, rows=slice(0, 3)))
    kappa.append(s[0] / max(s[-1], 1e-9))

plt.figure()
plt.plot(ts, mu, label="translational manipulability")
plt.twinx()
plt.plot(ts, kappa, "r", label="condition number")
plt.show()

# Jog TCP theo +X trong 100 bước servo
q = q0.copy()
dt = 0.008
for _ in range(100):
    twist = np.array([0.03, 0.0, 0.0, 0.0, 0.0, 0.0])  # 3 cm/s theo base X
    qdot, sigma_min, lam = resolved_rate(q, twist)
    q = q + qdot * dt
print("q final deg =", np.rad2deg(q))

Nếu max |Ja - Jn| lớn, lỗi thường nằm ở thứ tự nhân DH, nhầm z_i với z_{i-1}, hoặc tính angular numerical theo body frame thay vì spatial frame. Hãy debug bằng từng cột: chỉ perturb một khớp và xem TCP dịch chuyển đúng hướng tiếp tuyến chưa.

6. C++/Eigen: jacobian(q), SVD và resolvedRate(twist)

Trong app realtime, Python là công cụ kiểm chứng tốt nhưng controller thường nằm trong C++ hoặc ROS 2 node. Đây là skeleton độc lập dùng Eigen:

// jacobian_demo.cpp
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <algorithm>
#include <array>
#include <cmath>
#include <iostream>

using Vec6 = Eigen::Matrix<double, 6, 1>;
using Mat6 = Eigen::Matrix<double, 6, 6>;

const std::array<double, 6> A = {0.0, -0.425, -0.39225, 0.0, 0.0, 0.0};
const std::array<double, 6> D = {0.089159, 0.0, 0.0, 0.10915, 0.09465, 0.0823};
constexpr double PI = 3.14159265358979323846;
const std::array<double, 6> ALPHA = {PI / 2, 0.0, 0.0, PI / 2, -PI / 2, 0.0};

Eigen::Matrix4d dh(double a, double d, double alpha, double theta) {
  double c = std::cos(theta), s = std::sin(theta);
  double ca = std::cos(alpha), sa = std::sin(alpha);
  Eigen::Matrix4d T;
  T << c, -s * ca,  s * sa, a * c,
       s,  c * ca, -c * sa, a * s,
       0,      sa,      ca,     d,
       0,       0,       0,     1;
  return T;
}

std::array<Eigen::Matrix4d, 7> fkAll(const Vec6& q) {
  std::array<Eigen::Matrix4d, 7> frames;
  frames[0].setIdentity();
  for (int i = 0; i < 6; ++i) {
    frames[i + 1] = frames[i] * dh(A[i], D[i], ALPHA[i], q(i));
  }
  return frames;
}

Mat6 jacobian(const Vec6& q) {
  auto frames = fkAll(q);
  Eigen::Vector3d pe = frames[6].block<3, 1>(0, 3);
  Mat6 J;
  J.setZero();
  for (int i = 0; i < 6; ++i) {
    Eigen::Vector3d z = frames[i].block<3, 1>(0, 2);
    Eigen::Vector3d p = frames[i].block<3, 1>(0, 3);
    J.block<3, 1>(0, i) = z.cross(pe - p);
    J.block<3, 1>(3, i) = z;
  }
  return J;
}

Mat6 dampedPinv(const Mat6& J, double lambda) {
  Mat6 I = Mat6::Identity();
  return J.transpose() * (J * J.transpose() + lambda * lambda * I).inverse();
}

Vec6 resolvedRate(const Vec6& q, const Vec6& twist) {
  Mat6 J = jacobian(q);
  Eigen::JacobiSVD<Mat6> svd(J);
  auto s = svd.singularValues();
  double sigma_min = s(5);
  double sigma_safe = 0.05;
  double lambda_max = 0.2;
  double lambda = sigma_min >= sigma_safe ? 0.0
      : lambda_max * std::pow(1.0 - sigma_min / sigma_safe, 2.0);

  Vec6 qdot = dampedPinv(J, lambda) * twist;
  for (int i = 0; i < 6; ++i) {
    qdot(i) = std::clamp(qdot(i), -1.0, 1.0);
  }

  std::cout << "sigma_min=" << sigma_min << " lambda=" << lambda << "\n";
  return qdot;
}

int main() {
  Vec6 q;
  q << 0, -PI / 2, PI / 2, -PI / 2, -PI / 2, 0;
  Vec6 twist;
  twist << 0.03, 0, 0, 0, 0, 0;
  std::cout << resolvedRate(q, twist).transpose() << "\n";
}

cmake_minimum_required(VERSION 3.16)
project(ur5_jacobian_demo LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

find_package(Eigen3 REQUIRED)
add_executable(jacobian_demo jacobian_demo.cpp)
target_link_libraries(jacobian_demo Eigen3::Eigen)

Build:

mkdir -p build
cd build
cmake ..
cmake --build .
./jacobian_demo

Trong production, đừng gọi .inverse() mù quáng ở tần số cao nếu không kiểm soát điều kiện số. Với ma trận vuông 6 x 6, skeleton trên dễ đọc để học. Khi đưa vào controller thật, bạn có thể dùng SVD để tự dựng DLS theo singular values, thêm low-pass cho qdot, và log sigma_min vào telemetry.

7. Validate bằng thư viện tới 6/2026

Hai thư viện đáng dùng để kiểm chứng code from scratch:

Pinocchio 3.x

import pinocchio as pin

model = pin.buildModelFromUrdf("ur5.urdf")
data = model.createData()
q = pin.neutral(model)
frame_id = model.getFrameId("tool0")

pin.computeJointJacobians(model, data, q)
pin.updateFramePlacements(model, data)
J_pin = pin.getFrameJacobian(model, data, frame_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)

LOCAL_WORLD_ALIGNED thường tiện cho controller: gốc vận tốc đặt tại frame TCP nhưng trục biểu diễn song song world/base. Pinocchio docs nhắc rằng getFrameJacobian cần computeJointJacobians trước; nếu đổi q, hãy recompute.

Robotics Toolbox for Python

import roboticstoolbox as rtb
import numpy as np

robot = rtb.models.UR5()
q = np.zeros(6)
J0 = robot.jacob0(q)              # geometric Jacobian trong base frame
Ja = robot.jacob0_analytical(q, "rpy/xyz")

Khi so với code DH của bạn, hãy thống nhất: UR5 hay UR5e, base/tool transform, đơn vị radian, frame biểu diễn base/world/local, và thứ tự hàng [vx, vy, vz, wx, wy, wz]. 90% sai lệch khi validate Jacobian đến từ khác frame hoặc khác tool offset, không phải từ công thức z x (p_e - p_i).

8. Checklist an toàn cho jogging Cartesian

Trước khi nối resolved-rate controller vào robot thật:

1. Kiểm chứng J_analytic với J_numeric tại ít nhất 20 pose ngẫu nhiên.
2. Log sigma_min, condition number và max(abs(qdot)) trong mỗi chu kỳ servo.
3. Dùng DLS khi sigma_min thấp; stop khi thấp hơn ngưỡng hard stop.
4. Clamp qdot, clamp qdd, và tôn trọng joint limit margin.
5. Scale phần orientation nếu trộn m/s và rad/s trong cùng một norm.
6. Không jog mù qua elbow/shoulder/wrist singularity; giảm tốc trước vùng nguy hiểm.
7. Test trong simulator trước, rồi chạy teach mode/tốc độ thấp trên robot thật.

Nếu bạn đã đọc bài frames và jogging, hãy chú ý frame của twist: jog theo base X khác jog theo tool X. J chỉ đúng nếu twist và Jacobian cùng frame biểu diễn.

Khi đưa controller này vào hệ lớn hơn, bạn sẽ cần nối nó với vòng lặp servo, topic command và simulator. Phần kiến trúc ROS 2 control có thể đọc thêm ở ROS 2 control cho robot, còn cách dựng môi trường kiểm thử trước khi chạm robot thật nằm trong tổng quan simulation robotics.

Tài liệu tham khảo

• Modern Robotics: Space Jacobian và Singularities
• Universal Robots: DH parameters
• Robotics Toolbox for Python: jacob0() và analytic Jacobian
• Pinocchio frame Jacobian docs
• Resolved-rate motion control overview

Bài viết liên quan

• Inverse Kinematics cho robot arm
• Frames, TCP và jogging
• MoveJ và MoveL trong controller cổ điển