Kinematics và Dynamics cơ bản cho điều khiển Robot Arm

Trước khi robot arm có thể thực hiện bất kỳ thao tác nào — từ gắp vật đơn giản đến hàn chính xác — bạn cần hiểu hai trụ cột cốt lõi: Kinematics (mô tả chuyển động hình học) và Dynamics (mô tả lực và mô-men). Đây là bài đầu tiên trong series Traditional Manipulation Control, nơi chúng ta xây dựng từ nền tảng toán học đến triển khai thực tế trên robot thật.

Tại sao Kinematics và Dynamics quan trọng?

Hãy tưởng tượng bạn đang chỉ huy một cánh tay robot 6 bậc tự do (6-DOF) để gắp một vật tại vị trí [x, y, z] trong không gian. Câu hỏi đầu tiên: các khớp robot phải quay bao nhiêu độ? Đó là bài toán Inverse Kinematics (IK). Câu hỏi tiếp theo: cần bao nhiêu mô-men tại mỗi khớp để di chuyển? Đó là Dynamics.

Không nắm vững hai khái niệm này, bạn không thể:

• Lập trình quỹ đạo (trajectory) chính xác
• Tránh singularity (điểm kỳ dị)
• Điều khiển lực tiếp xúc khi gắp vật
• Tối ưu tốc độ và năng lượng

DH Parameters — Ngôn ngữ chung mô tả robot

Denavit-Hartenberg (DH) parameters là cách chuẩn hóa mô tả hình học của robot arm. Mỗi khớp được đặc trưng bởi 4 tham số:

Ma trận biến đổi từ frame $i-1$ sang frame $i$:

T_i = Rot_z(θ_i) × Trans_z(d_i) × Trans_x(a_i) × Rot_x(α_i)

So sánh DH Parameters: UR5e vs Panda

UR5e là robot 6-DOF (kiểu cổ tay spherical — 3 trục giao nhau tại 1 điểm), trong khi Panda là 7-DOF (redundant — thừa 1 bậc tự do, linh hoạt hơn nhưng IK phức tạp hơn).

Forward Kinematics (FK) — Từ khớp đến vị trí

Forward Kinematics trả lời: "Nếu tôi biết góc của tất cả các khớp, vị trí và hướng của end-effector ở đâu?"

Công thức đơn giản — nhân chuỗi ma trận biến đổi:

T_0^n = T_1 × T_2 × ... × T_n

Ma trận $T_0^n$ (4×4) chứa:

• Rotation matrix (3×3): hướng của end-effector
• Translation vector (3×1): vị trí [x, y, z]

Code: Forward Kinematics với roboticstoolbox-python

import roboticstoolbox as rtb
import numpy as np

# Tạo robot UR5e từ thư viện
ur5e = rtb.models.UR5()

# Đặt góc khớp (radians)
q = np.array([0, -np.pi/4, np.pi/4, -np.pi/2, np.pi/2, 0])

# Forward Kinematics — tính vị trí end-effector
T = ur5e.fkine(q)
print("End-effector pose:")
print(T)
print(f"\nPosition: x={T.t[0]:.4f}, y={T.t[1]:.4f}, z={T.t[2]:.4f}")

# Visualize robot
ur5e.plot(q, block=True)

FK luôn có một nghiệm duy nhất — đây là điểm khác biệt lớn so với IK.

Inverse Kinematics (IK) — Từ vị trí đến khớp

Inverse Kinematics là bài toán ngược: "Cho vị trí và hướng mong muốn của end-effector, tìm góc các khớp."

Đây là bài toán khó hơn nhiều vì:

1. Nhiều nghiệm: Robot 6-DOF có thể có tới 16 nghiệm IK
2. Không có nghiệm: Vị trí nằm ngoài workspace
3. Vô số nghiệm: Robot 7-DOF (redundant)

Hai phương pháp giải IK

Analytical IK cho robot có Spherical Wrist

Robot UR5e có cổ tay spherical (3 trục cuối giao nhau), cho phép tách bài toán:

1. Position IK: Tìm $\theta_1, \theta_2, \theta_3$ từ vị trí wrist center
2. Orientation IK: Tìm $\theta_4, \theta_5, \theta_6$ từ hướng end-effector

Thuật toán IKFast (từ OpenRAVE) có thể tự động sinh code analytical IK cho bất kỳ robot nào có cấu trúc phù hợp.

Numerical IK

import roboticstoolbox as rtb
import numpy as np
from spatialmath import SE3

ur5e = rtb.models.UR5()

# Vị trí mong muốn: [x=0.5, y=0.2, z=0.3], hướng thẳng xuống
T_desired = SE3(0.5, 0.2, 0.3) * SE3.Rx(np.pi)

# Phương pháp Levenberg-Marquardt (numerical)
sol = ur5e.ikine_LM(T_desired)
print(f"IK solution found: {sol.success}")
print(f"Joint angles (deg): {np.degrees(sol.q)}")

# Verify: FK lại để kiểm tra
T_actual = ur5e.fkine(sol.q)
pos_error = np.linalg.norm(T_desired.t - T_actual.t)
print(f"Position error: {pos_error*1000:.2f} mm")

Jacobian Matrix — Cầu nối giữa vận tốc khớp và end-effector

Jacobian ($J$) là ma trận 6×n liên hệ vận tốc khớp ($\dot{q}$) với vận tốc end-effector ($v$):

v = J(q) × q̇

Trong đó $v = [v_x, v_y, v_z, \omega_x, \omega_y, \omega_z]^T$ (vận tốc tuyến tính + vận tốc góc).

Jacobian dùng để làm gì?

1. Điều khiển vận tốc Cartesian: $\dot{q} = J^{-1} v$
2. Phát hiện singularity: Khi $\det(J) \approx 0$, robot mất bậc tự do
3. Tính lực tại end-effector: $F = J^T \tau$ (ánh xạ mô-men khớp sang lực Cartesian)
4. Resolved-rate control: Điều khiển robot theo quỹ đạo Cartesian

Singularity — "Điểm chết" của robot

Singularity xảy ra khi Jacobian mất hạng (rank-deficient). Tại singularity:

• Robot mất khả năng di chuyển theo một hoặc nhiều hướng
• Vận tốc khớp phát tán vô cùng ($J^{-1}$ không tồn tại)

Ba loại singularity phổ biến của robot 6-DOF:

1. Shoulder singularity: Wrist center nằm trên trục khớp 1
2. Elbow singularity: Cánh tay duỗi thẳng hoàn toàn
3. Wrist singularity: Hai trục cổ tay thẳng hàng ($\theta_5 = 0$)

# Tính Jacobian và kiểm tra singularity
J = ur5e.jacob0(q)
manipulability = np.sqrt(np.linalg.det(J @ J.T))
print(f"Manipulability index: {manipulability:.4f}")

if manipulability < 0.01:
    print("WARNING: Near singularity!")

Dynamics — Mô hình lực và mô-men

Trong khi kinematics chỉ quan tâm hình học chuyển động, dynamics quan tâm lực gây ra chuyển động đó. Phương trình dynamics tổng quát:

τ = M(q)q̈ + C(q, q̇)q̇ + g(q) + J^T F_ext

Hai phương pháp tính Dynamics

Lagrangian (Energy-based):

• Tính năng lượng động ($T$) và thế ($V$): $L = T - V$
• Áp dụng phương trình Euler-Lagrange
• Ưu điểm: Cho dạng symbolic (biểu thức giải tích)
• Nhược điểm: Tính toán phức tạp với robot nhiều bậc

Newton-Euler (Force-based):

• Quét tiến (forward): Tính vận tốc, gia tốc từ base đến end-effector
• Quét ngược (backward): Tính lực, mô-men từ end-effector về base
• Ưu điểm: Hiệu quả tính toán $O(n)$, phù hợp real-time
• Nhược điểm: Khó thu được dạng symbolic

import roboticstoolbox as rtb
import numpy as np

ur5e = rtb.models.UR5()
q = np.array([0, -np.pi/4, np.pi/4, -np.pi/2, np.pi/2, 0])
qd = np.zeros(6)  # Vận tốc khớp = 0

# Gravity torque — mô-men cần để giữ robot đứng yên
tau_g = ur5e.gravload(q)
print(f"Gravity torques (Nm): {np.round(tau_g, 2)}")

# Inertia matrix
M = ur5e.inertia(q)
print(f"\nInertia matrix shape: {M.shape}")
print(f"Condition number: {np.linalg.cond(M):.1f}")

# Inverse dynamics — tính mô-men cần cho quỹ đạo mong muốn
qdd = np.array([0.1, 0.2, -0.1, 0.05, -0.05, 0.1])  # Gia tốc mong muốn
tau = ur5e.rne(q, qd, qdd)
print(f"\nRequired torques (Nm): {np.round(tau, 2)}")

Workspace Analysis — Robot với được đến đâu?

Workspace là tập hợp tất cả các vị trí mà end-effector có thể đạt tới. Phân biệt:

• Reachable workspace: Tập vị trí end-effector có thể đạt với ít nhất 1 hướng
• Dexterous workspace: Tập vị trí end-effector có thể đạt với mọi hướng

import roboticstoolbox as rtb
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

ur5e = rtb.models.UR5()

# Monte Carlo sampling workspace
n_samples = 10000
positions = []

for _ in range(n_samples):
    q_random = ur5e.random_q()
    T = ur5e.fkine(q_random)
    positions.append(T.t)

positions = np.array(positions)

# Visualize workspace (top view)
fig, axes = plt.subplots(1, 2, figsize=(14, 6))

# XY plane
axes[0].scatter(positions[:, 0], positions[:, 1], s=1, alpha=0.3, c='blue')
axes[0].set_xlabel('X (m)')
axes[0].set_ylabel('Y (m)')
axes[0].set_title('Workspace — Top View (XY)')
axes[0].set_aspect('equal')
axes[0].grid(True)

# XZ plane
axes[1].scatter(positions[:, 0], positions[:, 2], s=1, alpha=0.3, c='red')
axes[1].set_xlabel('X (m)')
axes[1].set_ylabel('Z (m)')
axes[1].set_title('Workspace — Side View (XZ)')
axes[1].set_aspect('equal')
axes[1].grid(True)

plt.tight_layout()
plt.savefig('ur5e_workspace.png', dpi=150)
plt.show()

Ứng dụng thực tế: FK/IK trong Pick-and-Place

Quy trình pick-and-place cơ bản sử dụng FK và IK:

1. Camera detect vật → Tọa độ Cartesian [x, y, z]
2. IK → Tính góc khớp để đưa gripper đến vị trí gắp
3. FK kiểm tra → Verify vị trí thực tế sau khi di chuyển
4. Jacobian → Điều khiển tiếp cận chậm dần (resolved-rate)

# Workflow pick-and-place đơn giản
import roboticstoolbox as rtb
import numpy as np
from spatialmath import SE3

ur5e = rtb.models.UR5()

# Vị trí gắp (từ camera)
pick_pos = SE3(0.4, 0.3, 0.1) * SE3.Rx(np.pi)

# Vị trí approach (cao hơn 10cm)
approach_pos = SE3(0.4, 0.3, 0.2) * SE3.Rx(np.pi)

# Vị trí đặt
place_pos = SE3(-0.3, 0.4, 0.15) * SE3.Rx(np.pi)

# Giải IK cho từng vị trí
q_approach = ur5e.ikine_LM(approach_pos).q
q_pick = ur5e.ikine_LM(pick_pos).q
q_place = ur5e.ikine_LM(place_pos).q

# Kiểm tra manipulability tại mỗi điểm
for name, q in [("Approach", q_approach), ("Pick", q_pick), ("Place", q_place)]:
    J = ur5e.jacob0(q)
    m = np.sqrt(np.linalg.det(J @ J.T))
    print(f"{name}: manipulability = {m:.4f}")

Tài liệu tham khảo

• Craig, J.J. Introduction to Robotics: Mechanics and Control (4th ed.). Pearson, 2017. DOI: 10.1109/MRA.2005.1577021
• Siciliano, B., Sciavicco, L., Villani, L., Oriolo, G. Robotics: Modelling, Planning and Control. Springer, 2009. DOI: 10.1007/978-1-84628-642-1
• Diankov, R. Automated Construction of Robotic Manipulation Programs (IKFast). PhD Thesis, CMU, 2010. Link
• Corke, P. Robotics, Vision and Control (3rd ed.). Springer, 2023. DOI: 10.1007/978-3-031-07262-8

Kết luận

Kinematics và dynamics là nền tảng không thể bỏ qua khi làm việc với robot arm. Trong bài này, chúng ta đã đi qua:

• DH parameters — cách mô tả hình học chuẩn hóa
• FK và IK — chuyển đổi giữa không gian khớp và Cartesian
• Jacobian — cầu nối vận tốc và phát hiện singularity
• Dynamics — mô hình lực với Lagrangian và Newton-Euler

Trong bài tiếp theo, chúng ta sẽ sử dụng kiến thức này để xây dựng trajectory planning — lập kế hoạch quỹ đạo trong Joint Space và Cartesian Space.

Bài viết liên quan

• Inverse Kinematics cho robot 6-DOF — Đi sâu hơn vào các phương pháp giải IK
• MoveIt2 Motion Planning — Framework planning phổ biến nhất cho ROS 2
• Mô phỏng robot với MuJoCo — Simulate dynamics chính xác trong MuJoCo