Hệ tọa độ robot: Base, Tool, User Frame và TCP

Nếu phải chọn một nguồn bug xuất hiện nhiều nhất khi đưa robot arm từ mô phỏng ra máy thật, tôi sẽ không chọn IK, PID hay motion planning. Tôi sẽ chọn nhầm hệ tọa độ. Robot không "đi sai" một cách ngẫu nhiên; thường là ta gửi đúng con số nhưng sai frame. Target được đo theo jig, code lại hiểu là base. TCP được set ở mặt flange, nhưng công cụ thật dài thêm 180 mm. Camera trả pose của vật theo camera frame, còn controller lại dùng base frame. Sai số kiểu này không chỉ làm pick hụt vài cm, mà còn có thể biến một lệnh jog rất chậm thành va chạm.

Trong bài FK/IK và Jacobian, ta đã xem robot như một chuỗi khớp tạo ra pose end-effector. Bài này đứng giữa kinematics và motion command: ta học cách đặt tên, nhân, đảo và kiểm tra các frame trước khi gọi MoveJ, MoveL hay jogging servo. Phần jogging chi tiết sẽ quay lại ở bài servo jogging; ở đây ta chỉ xây nền toán học để khi jog theo base, tool hoặc user frame, ta biết chính xác vector vận tốc đang được hiểu trong frame nào.

Mục tiêu cuối bài: bạn có thể chuyển target từ user frame sang base frame, tính pose TCP từ flange và TCP offset, đổi pose TCP về user frame để debug, prototype bằng Python spatialmath, rồi port sang C++ bằng Eigen::Isometry3d.

Tư duy đúng: pose là quan hệ giữa hai frame

Một pose 6-DOF không chỉ là [x, y, z, rx, ry, rz]. Nó là quan hệ giữa hai hệ tọa độ. Trong bài này ta dùng ký hiệu:

T_base_user

đọc là "transform của frame user biểu diễn trong frame base", hoặc "base_from_user". Nếu ta dùng vector cột, một điểm p_user trong user frame được đổi sang base bằng:

p_base = T_base_user * p_user

Vì vậy thứ tự nhân rất quan trọng. Với ba frame base, user, target:

T_base_target = T_base_user * T_user_target

Nghĩa là: đi từ target về user, rồi từ user về base. Khi viết code, tên biến phải giữ được ý nghĩa này. Đừng đặt biến chung chung như T1, pose2, matrixA; sau hai tuần bạn sẽ không còn biết ma trận đó đổi từ đâu sang đâu.

Một ma trận đồng nhất trong SE(3) có dạng:

T = [ R  t ]
    [ 0  1 ]

Trong đó R là ma trận xoay 3x3, t là vector tịnh tiến 3x1. Pose 6-DOF của robot là cách compact để nhập/xuất cùng thông tin này: ba thành phần vị trí và ba thành phần orientation. Vendor khác nhau có thể dùng roll-pitch-yaw, rotation vector, quaternion hoặc Euler convention khác nhau, nhưng khi vào controller nội bộ, ta nên chuyển về một kiểu SE(3) rõ ràng.

Bốn frame cần thuộc trước khi chạy robot thật

base là frame ổn định nhất trong cell. Trên nhiều robot công nghiệp, pose Cartesian mặc định của waypoint được hiểu trong base nếu không chọn frame khác. user frame thì nên gắn vào đồ gá hoặc pallet. Khi đồ gá bị dời, bạn chỉ cần dạy lại T_base_user, còn toàn bộ target trong user frame vẫn giữ nguyên.

flange không phải lúc nào cũng là nơi ta muốn điều khiển. Nếu robot cầm gripper dài, mỏ hàn, đầu hút chân không hoặc bút đo, điểm quan trọng là TCP ở đầu công cụ. Tài liệu Universal Robots mô tả TCP là một điểm trên tool, có translation và rotation tương đối so với tâm tool output flange; khi TCP bằng zero, TCP trùng với flange. Điều này khớp trực tiếp với công thức:

T_base_tcp = T_base_flange * T_flange_tcp

Nếu cần nhìn TCP trong hệ tọa độ đồ gá để debug, ta đảo transform user:

T_user_tcp = inv(T_base_user) * T_base_tcp

Đây là một công thức nhỏ nhưng cứu rất nhiều giờ debug. Nếu T_user_tcp.z() bất ngờ âm 50 mm khi robot đang ở phía trên pallet, có thể bạn nhập sai chiều trục Z của user frame. Nếu T_user_tcp lệch đúng bằng chiều dài gripper, có thể TCP offset đang thiếu hoặc bị nhân ngược.

Base, user, object: lập trình theo đồ gá thay vì theo sàn

Giả sử một jig được đặt trên bàn, gốc user frame nằm ở góc trái trước của jig, trục X chạy dọc theo hàng lỗ, trục Y chạy ngang theo cột, trục Z hướng lên. Bạn muốn đưa TCP tới lỗ thứ ba:

T_user_target = translate(0.120, 0.040, 0.030) * rotate(...)

Robot lại chỉ nhận target trong base:

T_base_target = T_base_user * T_user_target

Nếu sau này jig bị dời 200 mm hoặc xoay 10 độ trên bàn, bạn không sửa tất cả waypoint. Bạn đo lại T_base_user. Đây là lý do user frame/workobject frame tồn tại trong gần như mọi pendant công nghiệp.

Object frame nằm thêm một lớp bên trong. Camera hoặc fixture có thể trả pose vật thể theo user frame:

T_base_grasp = T_base_user * T_user_object * T_object_grasp

T_object_grasp thường là offset cố định từ tâm vật tới pose gắp mong muốn. Cách tách này rất sạch: perception chịu trách nhiệm T_user_object, process engineer chỉnh T_object_grasp, robot controller chỉ compose transform.

ROS tf2 cũng dùng tư duy cây frame: mỗi frame có parent, hệ thống lookup transform giữa các frame qua cây. Dù bài này không phụ thuộc ROS, quy tắc thực hành giống nhau: mỗi transform phải có parent, child, timestamp nếu chạy real-time, và tên frame rõ ràng.

Prototype Python bằng SpatialMath

Cài gói:

python3 -m venv .venv
source .venv/bin/activate
pip install spatialmath-python matplotlib numpy

Script dưới đây tạo một user frame lệch so với base, một target trong user frame, một pose flange trong base, rồi tính TCP và đổi TCP về user frame.

# python/frame_demo.py
import numpy as np
from spatialmath import SE3
from spatialmath.base import trprint
import matplotlib.pyplot as plt

def rpy_deg(roll, pitch, yaw):
    return SE3.RPY([roll, pitch, yaw], unit="deg", order="xyz")

T_base_user = SE3(0.45, -0.20, 0.08) * rpy_deg(0, 0, 30)
T_user_target = SE3(0.12, 0.04, 0.03) * rpy_deg(180, 0, 90)

T_base_target = T_base_user * T_user_target

T_base_flange = SE3(0.52, -0.08, 0.36) * rpy_deg(180, 0, 45)
T_flange_tcp = SE3(0.0, 0.0, 0.18) * rpy_deg(0, 0, 0)

T_base_tcp = T_base_flange * T_flange_tcp
T_user_tcp = T_base_user.inv() * T_base_tcp

print("T_base_user")
trprint(T_base_user.A, label="base_from_user")
print("\nT_user_target")
trprint(T_user_target.A, label="user_from_target")
print("\nT_base_target = T_base_user * T_user_target")
trprint(T_base_target.A, label="base_from_target")
print("\nT_user_tcp = inv(T_base_user) * T_base_tcp")
trprint(T_user_tcp.A, label="user_from_tcp")

fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
T_base_user.plot(frame="user", color="blue", length=0.08, ax=ax)
T_base_target.plot(frame="target", color="green", length=0.08, ax=ax)
T_base_flange.plot(frame="flange", color="orange", length=0.08, ax=ax)
T_base_tcp.plot(frame="tcp", color="red", length=0.08, ax=ax)
ax.set_title("Robot frames in base coordinates")
plt.show()

Điểm cần kiểm tra:

• T_base_target phải thay đổi nếu bạn xoay T_base_user; target không còn chỉ tịnh tiến theo X/Y base.
• T_base_tcp khác T_base_flange đúng bằng offset tool 180 mm theo trục Z của flange, không phải Z của base.
• T_user_tcp là pose TCP nhìn từ jig. Đây là giá trị rất hữu ích khi debug pallet, bin picking hoặc inspection.

Một thói quen tốt: khi prototype, in cả ma trận và giá trị compact [x, y, z, r, p, y]. Khi port sang C++, test unit nên so sánh ma trận hoặc translation/orientation với tolerance, không so sánh string log.

Implement C++ bằng Eigen::Isometry3d

Trong bài setup C++/CMake, ta đã dùng Eigen làm nền. Với rigid transform 3D, Eigen::Isometry3d phù hợp hơn Matrix4d trần vì nó giữ ý nghĩa "rotation + translation" và có API inverse(), translation(), linear().

// include/arm_controller/frame_manager.hpp
#pragma once

#include <Eigen/Geometry>
#include <cmath>
#include <string>
#include <unordered_map>

namespace arm_controller {

using Pose = Eigen::Isometry3d;

inline double deg2rad(double deg) {
    constexpr double pi = 3.14159265358979323846;
    return deg * pi / 180.0;
}

inline Pose makePose(double x, double y, double z,
                     double roll_deg, double pitch_deg, double yaw_deg) {
    const Eigen::AngleAxisd roll(deg2rad(roll_deg), Eigen::Vector3d::UnitX());
    const Eigen::AngleAxisd pitch(deg2rad(pitch_deg), Eigen::Vector3d::UnitY());
    const Eigen::AngleAxisd yaw(deg2rad(yaw_deg), Eigen::Vector3d::UnitZ());

    Pose pose = Pose::Identity();
    pose.linear() = (yaw * pitch * roll).toRotationMatrix();
    pose.translation() = Eigen::Vector3d(x, y, z);
    return pose;
}

inline Pose compose(const Pose& a_from_b, const Pose& b_from_c) {
    return a_from_b * b_from_c;
}

inline Pose inverse(const Pose& a_from_b) {
    return a_from_b.inverse();
}

class FrameManager {
public:
    void setBaseToUser(const std::string& name, const Pose& base_from_user) {
        users_[name] = base_from_user;
    }

    void setTcp(const Pose& flange_from_tcp) {
        flange_from_tcp_ = flange_from_tcp;
    }

    Pose baseTargetFromUserTarget(const std::string& user,
                                  const Pose& user_from_target) const {
        return userFrame(user) * user_from_target;
    }

    Pose baseTcpFromBaseFlange(const Pose& base_from_flange) const {
        return base_from_flange * flange_from_tcp_;
    }

    Pose userTcpFromBaseFlange(const std::string& user,
                               const Pose& base_from_flange) const {
        const Pose base_from_tcp = baseTcpFromBaseFlange(base_from_flange);
        return userFrame(user).inverse() * base_from_tcp;
    }

private:
    const Pose& userFrame(const std::string& name) const {
        return users_.at(name);
    }

    std::unordered_map<std::string, Pose> users_;
    Pose flange_from_tcp_ = Pose::Identity();
};

}  // namespace arm_controller

Một file demo:

// src/frame_demo.cpp
#include "arm_controller/frame_manager.hpp"

#include <iostream>

int main() {
    using arm_controller::FrameManager;
    using arm_controller::makePose;

    FrameManager frames;
    frames.setBaseToUser("jig_a", makePose(0.45, -0.20, 0.08, 0, 0, 30));
    frames.setTcp(makePose(0.0, 0.0, 0.18, 0, 0, 0));

    const auto user_from_target = makePose(0.12, 0.04, 0.03, 180, 0, 90);
    const auto base_from_target =
        frames.baseTargetFromUserTarget("jig_a", user_from_target);

    const auto base_from_flange = makePose(0.52, -0.08, 0.36, 180, 0, 45);
    const auto user_from_tcp =
        frames.userTcpFromBaseFlange("jig_a", base_from_flange);

    std::cout << "base_from_target:\n" << base_from_target.matrix() << "\n\n";
    std::cout << "user_from_tcp:\n" << user_from_tcp.matrix() << "\n";
    return 0;
}

CMake tối thiểu:

cmake_minimum_required(VERSION 3.16)
project(arm_controller_frames LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

find_package(Eigen3 REQUIRED)

add_executable(frame_demo src/frame_demo.cpp)
target_include_directories(frame_demo PRIVATE include)
target_link_libraries(frame_demo PRIVATE Eigen3::Eigen)

Build:

cmake -S . -B build
cmake --build build
./build/frame_demo

Điểm quan trọng trong code C++ là setTcp() lưu T_flange_tcp, không lưu T_base_tcp. TCP là thuộc tính của tool gắn trên flange, nên nó phải độc lập với joint hiện tại. baseTcpFromBaseFlange() mới compose nó với pose flange hiện tại.

Checklist chống bug frame trên robot thật

Trước khi chạy motion thật, hãy kiểm tra theo thứ tự:

1. Đơn vị: translation là mét hay milimét? Orientation là degree hay radian?
2. Convention orientation: RPY theo thứ tự nào? Rotation vector hay Euler?
3. Tên biến có nói rõ parent/child không: base_from_user, flange_from_tcp?
4. TCP offset có đúng chiều trục của flange không? Đừng cộng 180 mm vào Z base nếu tool nghiêng.
5. User frame có được dạy lại sau khi jig bị dời không?
6. Target perception có đang ở camera/object frame không? Nếu có, đã compose qua camera calibration chưa?
7. Trước MoveL, plot hoặc log T_base_target và kiểm tra nó nằm trong workspace hợp lý.

Nếu bạn đang làm với ROS hoặc ros2_control, hãy xem thêm bài ROS 2 control hardware interface: controller thật cần thống nhất frame giữa driver, state publisher, planning scene và motion command. Với fleet hoặc cell lớn hơn, bài Open-RMF fleet management cũng cho thấy vì sao frame/world model phải nhất quán ở cấp hệ thống.

Kết luận

Frame transform là phần nhỏ nhưng quyết định độ tin cậy của toàn bộ robot arm controller. Công thức cần nhớ chỉ có ba dòng:

T_base_target = T_base_user * T_user_target
T_base_tcp    = T_base_flange * T_flange_tcp
T_user_tcp    = inv(T_base_user) * T_base_tcp

Khi ba dòng này được đặt tên đúng, test đúng và plot được, những bài sau về MoveJ, MoveL, blending, jogging và motion planning sẽ dễ debug hơn rất nhiều. Trong bài MoveJ/MoveL, target Cartesian cuối cùng vẫn phải được đưa về base frame trước khi planner hoặc controller thực thi.

Tài liệu tham khảo

• Universal Robots TCP Configuration
• Pickit: How to define the TCP on Universal Robots
• Eigen Geometry: Space transformations
• Spatial Math Toolbox overview
• ROS 2 tf2 tutorials

Bài viết liên quan

• Bài mở đầu: nền tảng robot arm controller cổ điển
• Dựng project C++ cho robot controller: CMake & Eigen
• MoveJ và MoveL: hai primitive quan trọng nhất