Forward Kinematics: từ góc khớp/tốc độ bánh ra vị trí thực tế

Bài Differential Drive và Odometry đã giải một bài toán forward kinematics cụ thể: biết tốc độ hai bánh, tính ra vận tốc dài/góc của robot. Forward Kinematics (động học thuận) là tên gọi chung cho dạng bài toán này, áp dụng cho mọi loại robot có khớp/bánh xe — không chỉ robot di động, còn cả tay máy công nghiệp.

Khái niệm chính

Forward Kinematics:  trạng thái khớp (joint space)  →  vị trí thực tế (Cartesian space)

• Robot di động (wheeled): "khớp" là tốc độ quay từng bánh, "vị trí thực tế" là vận tốc dài/góc của cả robot
• Tay máy (manipulator): "khớp" là góc xoay từng khớp nối (joint angle), "vị trí thực tế" là toạ độ (x, y, z) của đầu công tác (end-effector)

Cả hai đều cùng một dạng bài toán trừu tượng: biết cấu hình từng bộ phận chuyển động riêng lẻ, suy ra vị trí/hướng tổng thể của toàn hệ thống.

Nguyên lý hoạt động

Ví dụ kinh điển nhất ngoài robot di động: tay máy 2 đoạn, 2 khớp xoay, hoạt động trong mặt phẳng (2-link planar arm):

θ1 — góc khớp gốc (so với trục x)
θ2 — góc khớp thứ 2 (so với đoạn tay thứ nhất)
L1, L2 — chiều dài hai đoạn tay

Vị trí đầu tay máy (x, y) tính trực tiếp bằng lượng giác — ghép nối tiếp hai phép quay (bài Rotation) và tịnh tiến theo từng đoạn:

x = L1 cos(θ1) + L2 cos(θ1 + θ2)
y = L1 sin(θ1) + L2 sin(θ1 + θ2)

Tóm lại: Forward kinematics luôn đi một chiều dễ tính: biết góc khớp/tốc độ bánh, tính thẳng ra vị trí bằng công thức lượng giác trực tiếp. Chiều ngược lại — đó là bài toán Inverse Kinematics (bài tiếp theo).

Với cùng một bộ giá trị góc khớp/tốc độ bánh đầu vào, vị trí đầu ra luôn tính ra duy nhất một kết quả — không có gì mơ hồ, chỉ là thay số vào công thức có sẵn.

Kiến thức kỹ thuật

Công thức

Công thức 2-link ở trên thực chất là trường hợp riêng của quy tắc tổng quát hơn: mỗi khớp đóng góp đúng 1 homogeneous transform (đã học ở bài Transform), và forward kinematics của toàn bộ chuỗi khớp là nhân liên tiếp các transform đó — giống hệt cách tf2 nhân chuỗi map→odom→base_link→...:

T_end_effector = T1(θ1) × T2(θ2) × ... × Tn(θn)

Với tay máy N khớp, vị trí cuối cùng luôn tính được bằng cách nhân
đúng N ma trận homogeneous liên tiếp — không quan trọng N là 2 hay 20

Thông số

Số bậc tự do (DOF — Degree of Freedom, số khớp độc lập) khác nhau nhiều tuỳ loại tay máy:

Loại tay máy	Số DOF điển hình
Tay máy phẳng minh hoạ trong bài	2 DOF
Tay máy công nghiệp tiêu chuẩn (6 trục)	6 DOF — đủ đặt cả vị trí (x,y,z) lẫn hướng (roll,pitch,yaw) bất kỳ
Tay máy dư dẫn động (redundant, VD: 7 trục)	7 DOF trở lên — dư 1 bậc tự do so với mức cần thiết, tăng tính linh hoạt né vật cản

Ứng dụng thực tế

Forward kinematics không chỉ là lý thuyết trừu tượng — đây chính xác là cơ chế mà robot_state_publisher trong ROS2 dùng để tự động tính và publish toàn bộ cây TF của robot: file URDF khai báo mỗi khớp (vị trí, trục xoay, giới hạn góc), node này đọc góc khớp hiện tại (từ /joint_states) và tự tính forward kinematics để ra transform của từng link, publish liên tục lên /tf. Không cần tự viết công thức lượng giác thủ công như ví dụ 2-link ở trên — với robot thật có khai báo URDF đầy đủ, forward kinematics của toàn bộ chuỗi khớp được tính tự động.

Ví dụ thực tế

import math

L1, L2 = 0.3, 0.25  # chiều dài 2 đoạn tay (m)
theta1 = math.radians(45)
theta2 = math.radians(30)

x = L1 * math.cos(theta1) + L2 * math.cos(theta1 + theta2)
y = L1 * math.sin(theta1) + L2 * math.sin(theta1 + theta2)

print(f"Vi tri dau tay may: x={x:.3f}, y={y:.3f}")

Output:
Vi tri dau tay may: x=0.279, y=0.443

Chỉ với 2 góc khớp và 2 chiều dài đoạn tay, forward kinematics cho ra ngay đúng 1 kết quả vị trí — đúng như tính chất "một lời giải duy nhất" đã nêu ở phần Nguyên lý hoạt động, không cần thử-sai hay giải phương trình.

Lỗi thường gặp

• Dùng θ2 thay vì θ1+θ2 cho góc tuyệt đối của đoạn tay thứ 2 — góc θ2 trong công thức là góc tương đối so với đoạn tay thứ nhất, không phải góc tuyệt đối so với trục x — quên cộng dồn θ1 gây sai vị trí hoàn toàn.
• Đổi thứ tự nhân các transform trong chuỗi khớp — giống tính không giao hoán của matrix đã học, nhân sai thứ tự T1×T2 thành T2×T1 cho ra vị trí hình học khác hẳn.
• Trộn đơn vị chiều dài giữa các đoạn tay — L1 tính bằng mét, L2 tính bằng milimét trong cùng công thức cho kết quả sai lệch hàng nghìn lần, lỗi rất dễ xảy ra khi ghép thông số từ nhiều nguồn tài liệu khác nhau.
• Nhầm góc độ và radian khi đọc từ /joint_states — dữ liệu góc khớp trong ROS2 luôn là radian, hiển thị/nhập liệu bằng độ mà quên đổi đơn vị gây sai kết quả tính toán.

Kết luận

Forward kinematics là bài toán "một chiều dễ" trong động học robot — biết cấu hình khớp, tính thẳng ra vị trí bằng công thức lượng giác hoặc chuỗi transform, luôn cho đúng 1 kết quả duy nhất. Đây chính là cơ chế tự động đứng sau robot_state_publisher của ROS2. Bài tiếp theo trong chuyên mục nói về Inverse Kinematics — bài toán ngược lại khó hơn nhiều: biết vị trí đích mong muốn, cần tìm góc khớp nào để đạt được nó.