Forward Kinematics

Khi robot chuẩn bị gắp một chiếc cốc, hệ điều khiển luôn biết khá nhiều thứ về bản thân robot. Encoder ở các khớp cho biết từng khớp đang quay bao nhiêu độ. Mô hình cơ khí cho biết mỗi đoạn tay dài bao nhiêu. Cấu trúc robot cho biết khớp nào nối với đoạn nào. Nhưng từ những con số đó, robot vẫn phải trả lời một câu hỏi rất thực tế: đầu kẹp hiện đang ở đâu?

Đó là bài toán của forward kinematics. Ta đã biết trạng thái các khớp, và muốn tính pose của end-effector trong một frame tham chiếu, thường là base frame. Nếu inverse kinematics bắt đầu từ mục tiêu rồi tìm ngược về khớp, forward kinematics đi theo chiều tự nhiên của cơ thể robot: từ đế, qua từng khớp, tới đầu kẹp.

Điều làm forward kinematics quan trọng không nằm ở việc nó là bài toán “dễ hơn” inverse kinematics. Nó quan trọng vì gần như mọi tầng phía sau đều cần biết robot đang thật sự ở đâu trong không gian. Muốn kiểm tra đầu kẹp có gần cốc chưa, cần forward kinematics. Muốn vẽ robot trong mô phỏng, cần forward kinematics. Muốn so sánh pose hiện tại với pose mục tiêu, cần forward kinematics. Muốn tính Jacobian, điều khiển vận tốc, tránh va chạm hoặc phát hiện sai số, cũng phải đứng trên nền của forward kinematics.

Bài toán

Ta tiếp tục dùng ví dụ tay robot gắp cốc trên bàn. Robot có nhiều khớp nối tiếp nhau. Mỗi khớp có một biến trạng thái: khớp quay dùng góc $\theta$, khớp tịnh tiến dùng độ dịch chuyển $d$. Tập hợp tất cả biến khớp tạo thành vector cấu hình:

$$\mathbf{q} = \begin{bmatrix} q_1 & q_2 & \cdots & q_n \end{bmatrix}^{T}$$

Forward kinematics là hàm ánh xạ từ cấu hình khớp sang pose của end-effector:

$${}^{B}\mathbf{T}_{E} = f(\mathbf{q})$$

Trong đó, $B$ là base frame của robot, $E$ là end-effector frame, còn ${}^{B}\mathbf{T}_{E}$ là transform mô tả pose của end-effector trong base frame. Pose này gồm cả position và orientation, không chỉ một điểm $(x, y, z)$.

Nếu robot chỉ cần biết đầu kẹp nằm ở đâu, position có vẻ đủ. Nhưng khi gắp cốc, orientation cũng quan trọng. Đầu kẹp đến đúng tâm cốc nhưng quay sai hướng vẫn có thể không gắp được. Vì vậy, forward kinematics đúng nghĩa phải trả về pose, không chỉ tọa độ điểm cuối.

Link và joint

Robot nối tiếp có thể được nhìn như một chuỗi link và joint. Link là các đoạn cứng. Joint là nơi hai link nối với nhau và cho phép chuyển động tương đối. Với serial manipulator, chuyển động bắt đầu từ base, đi qua joint 1, link 1, joint 2, link 2, và cứ thế cho tới end-effector.

Điều này tạo ra một đặc điểm rất quan trọng: pose của một link phía sau phụ thuộc vào tất cả các khớp phía trước nó. Nếu khớp gần đế quay, toàn bộ phần robot phía sau bị kéo theo. Nếu khớp gần đầu kẹp quay, nó chủ yếu thay đổi phần cuối robot. Vì vậy, ta không thể tính vị trí đầu kẹp bằng cách xem từng khớp như các chuyển động độc lập trong cùng một frame cố định.

Forward kinematics xử lý vấn đề này bằng cách gắn một coordinate frame vào từng link hoặc từng joint. Thay vì cố viết trực tiếp công thức từ base tới đầu kẹp trong một bước, ta mô tả quan hệ giữa các frame lân cận, rồi nhân chúng lại thành chuỗi.

Ý tưởng này rất tự nhiên nếu nhớ lại bài coordinate transformation. Mỗi khớp tạo ra một transform cục bộ. Mỗi link tạo ra một khoảng lệch hình học. Khi ghép tất cả transform cục bộ theo đúng thứ tự, ta thu được transform từ base tới end-effector.

Frame gắn trên robot

Giả sử robot có các frame:

$$\{0\}, \{1\}, \{2\}, \ldots, \{n\}$$

Frame $\{0\}$ thường là base frame. Frame $\{n\}$ thường gắn gần end-effector. Quan hệ giữa hai frame liên tiếp được biểu diễn bằng transform:

{}^{i-1}\mathbf{T}_{i}\

Transform này nói frame $\{i\}$ nằm ở đâu và quay như thế nào so với frame $\{i-1\}$. Nó có dạng homogeneous transform:

$${}^{i-1}\mathbf{T}_{i} = \begin{bmatrix} {}^{i-1}\mathbf{R}_{i} & {}^{i-1}\mathbf{t}_{i} \\ \mathbf{0}^{T} & 1 \end{bmatrix}$$

Ở đây, ${}^{i-1}\mathbf{R}_{i}$ là rotation từ frame $\{i\}$ sang frame $\{i-1\}$, còn ${}^{i-1}\mathbf{t}_{i}$ là vị trí gốc của frame $\{i\}$ biểu diễn trong frame $\{i-1\}$.

Khi frame được gắn nhất quán, forward kinematics trở thành phép nhân một chuỗi transform:

$${}^{0}\mathbf{T}_{n} = {}^{0}\mathbf{T}_{1} {}^{1}\mathbf{T}_{2} \cdots {}^{n-1}\mathbf{T}_{n}$$

Đây là phần lõi của forward kinematics cho robot nối tiếp. Mỗi transform nhỏ dễ hiểu hơn transform tổng. Cả chuỗi giữ đúng quan hệ hình học giữa base và end-effector.

Tay máy hai khớp phẳng

Trước khi nhìn robot 3D, ta nên bắt đầu với tay máy hai khớp phẳng. Đây là ví dụ đủ đơn giản để thấy công thức, nhưng vẫn chứa đúng bản chất của forward kinematics.

Giả sử robot có hai link dài $l_1$ và $l_2$, hai khớp quay với góc $\theta_1$ và $\theta_2$. End-effector nằm ở cuối link thứ hai. Khi khớp 1 quay một góc $\theta_1$, link 1 tạo ra vector:

$$\mathbf{v}_1 = \begin{bmatrix} l_1 \cos \theta_1 \\ l_1 \sin \theta_1 \end{bmatrix}$$

Link 2 không chỉ quay theo $\theta_2$ trong world frame. Vì nó gắn trên link 1, hướng tuyệt đối của link 2 là $\theta_1 + \theta_2$. Vector của link 2 là:

$$\mathbf{v}_2 = \begin{bmatrix} l_2 \cos(\theta_1 + \theta_2) \\ l_2 \sin(\theta_1 + \theta_2) \end{bmatrix}$$

Vị trí end-effector là tổng hai vector:

$$\mathbf{p} = \mathbf{v}_1 + \mathbf{v}_2$$

Hay viết thành tọa độ:

$$x = l_1 \cos \theta_1 + l_2 \cos(\theta_1 + \theta_2)$$ $$y = l_1 \sin \theta_1 + l_2 \sin(\theta_1 + \theta_2)$$

Hai công thức này cho thấy bản chất của forward kinematics: chuyển động phía sau tích lũy chuyển động phía trước. Khớp 2 không sống trong một thế giới độc lập. Nó sống trên link 1, nên hướng của nó trong base frame đã mang theo ảnh hưởng của $\theta_1$.

Với robot nhiều khớp hơn, công thức trực tiếp sẽ dài ra rất nhanh. Đó là lý do ta dùng homogeneous transform để viết mọi thứ theo một quy tắc lặp lại.

Transform của một khớp

Mỗi khớp đóng góp một chuyển động tương đối giữa hai link. Với khớp quay, biến khớp thường là một góc. Với khớp tịnh tiến, biến khớp thường là một độ dịch chuyển.

Trong mặt phẳng, một khớp quay quanh trục $z$ có thể được viết bằng transform:

$$\mathbf{T}(\theta, l) = \begin{bmatrix} \cos\theta & -\sin\theta & l\cos\theta \\ \sin\theta & \cos\theta & l\sin\theta \\ 0 & 0 & 1 \end{bmatrix}$$

Ma trận này đồng thời mô tả rotation của link và translation tới cuối link. Nếu nhân các transform như vậy cho từng link, ta sẽ nhận được pose cuối cùng của end-effector trong base frame.

Với tay máy hai khớp, có thể viết:

$${}^{0}\mathbf{T}_{2} = \mathbf{T}(\theta_1, l_1) \mathbf{T}(\theta_2, l_2)$$

Khi nhân ra, phần translation của ma trận cuối chính là $(x, y)$ đã tính ở trên. Phần rotation cho biết hướng của end-effector trong mặt phẳng:

$$\phi = \theta_1 + \theta_2$$

Điều này nhấn mạnh một điểm hay bị bỏ qua: forward kinematics trả về cả vị trí và hướng. Với robot phẳng hai khớp, orientation cuối chỉ là tổng các góc. Với robot 3D, orientation sẽ là tích của các rotation matrix.

Homogeneous transform trong 3D

Trong 3D, transform giữa hai frame thường có dạng:

$${}^{i-1}\mathbf{T}_{i} = \begin{bmatrix} {}^{i-1}\mathbf{R}_{i} & {}^{i-1}\mathbf{t}_{i} \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

Với robot nối tiếp, forward kinematics là:

$${}^{0}\mathbf{T}_{n}(\mathbf{q}) = \prod_{i=1}^{n} {}^{i-1}\mathbf{T}_{i}(q_i)$$

Ký hiệu tích ở đây cần được hiểu theo đúng thứ tự từ base tới end-effector:

$${}^{0}\mathbf{T}_{n} = {}^{0}\mathbf{T}_{1} {}^{1}\mathbf{T}_{2} \cdots {}^{n-1}\mathbf{T}_{n}$$

Mỗi ${}^{i-1}\mathbf{T}_{i}(q_i)$ có thể phụ thuộc vào biến khớp $q_i$ và các tham số hình học cố định của robot. Nếu $q_i$ là khớp quay, rotation thay đổi theo góc. Nếu $q_i$ là khớp tịnh tiến, translation thay đổi theo độ trượt.

Khi nhân chuỗi transform này, ta nhận được:

$${}^{0}\mathbf{T}_{n} = \begin{bmatrix} {}^{0}\mathbf{R}_{n} & {}^{0}\mathbf{p}_{n} \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

Trong đó, ${}^{0}\mathbf{p}_{n}$ là position của end-effector trong base frame, còn ${}^{0}\mathbf{R}_{n}$ là orientation của end-effector trong base frame.

Denavit-Hartenberg parameters

Một robot 3D có nhiều cách gắn frame. Nếu mỗi người tự gắn frame theo một kiểu, công thức forward kinematics sẽ khó đọc và khó kiểm tra. Denavit-Hartenberg parameters, thường gọi là DH parameters, là một quy ước kinh điển để mô tả quan hệ giữa hai link liên tiếp bằng bốn tham số.

Với dạng DH tiêu chuẩn, một transform giữa frame $\{i-1\}$ và $\{i\}$ thường được tạo từ bốn đại lượng:

• $\theta_i$: góc quay quanh trục $z_{i-1}$
• $d_i$: độ tịnh tiến dọc trục $z_{i-1}$
• $a_i$: độ dài link theo trục $x_i$
• $\alpha_i$: góc xoắn giữa hai trục $z$

Transform tương ứng có dạng:

$${}^{i-1}\mathbf{T}_{i} = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

Với khớp quay, $\theta_i$ là biến khớp, còn $d_i, a_i, \alpha_i$ thường là hằng số hình học. Với khớp tịnh tiến, $d_i$ là biến khớp, còn $\theta_i, a_i, \alpha_i$ thường cố định.

DH không phải là bản chất duy nhất của forward kinematics. Nó là một quy ước giúp bảng hóa robot nối tiếp. Bản chất vẫn là ghép các transform cục bộ thành transform tổng. Nếu dùng URDF, product of exponentials, hoặc một thư viện robot hiện đại, cách biểu diễn có thể khác, nhưng câu hỏi lõi không đổi: mỗi joint tạo ra transform nào, và chuỗi transform đó đưa end-effector tới đâu.

Pose của end-effector

Sau khi tính ${}^{0}\mathbf{T}_{n}$, ta có pose của end-effector. Pose này thường được dùng theo nhiều cách khác nhau trong hệ robot.

Position ${}^{0}\mathbf{p}_{n}$ cho biết đầu kẹp đang ở đâu trong base frame. Nếu mục tiêu gắp cốc cũng đã được đưa về base frame, ta có thể so sánh vị trí hiện tại với vị trí mục tiêu. Sai số position có thể viết là:

$$\mathbf{e}_{p} = {}^{0}\mathbf{p}_{target} -{}^{0}\mathbf{p}_{n}$$

Orientation ${}^{0}\mathbf{R}_{n}$ cho biết đầu kẹp đang quay như thế nào. Với các thao tác cần hướng tiếp cận chính xác, sai số orientation quan trọng không kém sai số position. Ví dụ, đầu kẹp phải song song với thành cốc, camera gắn trên robot phải nhìn đúng hướng, hoặc mũi hàn phải giữ đúng góc với bề mặt.

Nếu chỉ lấy position mà bỏ orientation, robot có thể “đúng chỗ nhưng sai tư thế”. Với nhiều nhiệm vụ thao tác, đó vẫn là thất bại. Vì vậy, trong pipeline robot, forward kinematics thường được xem như phép tính pose đầy đủ:

$$\mathbf{q} \longmapsto ({}^{0}\mathbf{p}_{n}, {}^{0}\mathbf{R}_{n})$$

Từ pose này, hệ thống mới có thể tính sai số, điều khiển chuyển động, kiểm tra va chạm hoặc ghi nhận trạng thái robot trong world frame.

Thứ tự nhân transform

Một lỗi phổ biến khi học forward kinematics là nhân transform sai thứ tự. Trong chuỗi robot, transform phải được nhân theo đường đi hình học từ frame gốc tới frame đích.

Nếu ta có:

$${}^{0}\mathbf{T}_{1} \quad \text{và} \quad {}^{1}\mathbf{T}_{2}$$

thì transform từ frame $\{2\}$ về frame $\{0\}$ là:

$${}^{0}\mathbf{T}_{2} = {}^{0}\mathbf{T}_{1} {}^{1}\mathbf{T}_{2}$$

Không thể tự ý đổi thành ${}^{1}\mathbf{T}_{2}{}^{0}\mathbf{T}_{1}$. Ma trận transform không giao hoán. Về mặt vật lý, quay rồi tịnh tiến không giống tịnh tiến rồi quay. Với robot nhiều khớp, sai thứ tự nhân có thể làm mô hình end-effector đi tới một vị trí hoàn toàn khác.

Cách đọc chỉ số frame giúp tránh lỗi này. Trong ${}^{0}\mathbf{T}_{1}{}^{1}\mathbf{T}_{2}$, frame $\{1\}$ ở giữa khớp nhau, nên kết quả nối được từ $\{2\}$ về $\{0\}$. Đây là thói quen nhỏ nhưng rất hữu ích khi công thức dài.

Forward kinematics và coordinate transformation

Forward kinematics có thể được nhìn như một trường hợp đặc biệt của coordinate transformation. Mỗi link của robot có một frame riêng. Mỗi joint định nghĩa transform giữa hai frame liên tiếp. Khi robot đổi cấu hình, một số transform trong chuỗi thay đổi theo biến khớp.

Điểm khác với một transform cố định giữa camera và base là transform trong forward kinematics phụ thuộc vào $\mathbf{q}$. Ta có thể viết:

$${}^{0}\mathbf{T}_{n} = {}^{0}\mathbf{T}_{n}(\mathbf{q})$$

Điều này nghĩa là pose end-effector không phải một hằng số. Nó thay đổi mỗi khi encoder báo góc khớp mới. Trong một controller chạy thời gian thực, forward kinematics có thể được tính liên tục ở mỗi chu kỳ điều khiển để biết đầu kẹp hiện đang ở đâu.

Với ví dụ gắp cốc, camera có thể cho ta pose cốc trong base frame. Forward kinematics cho ta pose đầu kẹp trong base frame. Khi cả hai cùng nằm trong một frame, robot mới có thể so sánh chúng một cách có nghĩa. Nếu cốc ở camera frame còn đầu kẹp ở base frame, lấy hiệu hai vector sẽ là một phép tính sai.

Workspace

Forward kinematics cũng cho ta cách hiểu workspace. Nếu cho $\mathbf{q}$ chạy qua toàn bộ miền giá trị hợp lệ của các khớp, tập hợp tất cả position mà end-effector có thể đạt được tạo thành workspace:

$$\mathcal{W} = \left\{ {}^{0}\mathbf{p}_{n}(\mathbf{q}) \mid \mathbf{q} \in \mathcal{Q} \right\}$$

Trong đó, $\mathcal{Q}$ là không gian cấu hình hợp lệ của robot, bao gồm giới hạn góc, giới hạn trượt, và các ràng buộc cơ khí khác.

Điều này nối lại với phần modeling. Khi thiết kế robot, ta chọn chiều dài link và giới hạn khớp. Forward kinematics cho ta biết những lựa chọn đó tạo ra workspace nào. Nếu workspace không phủ tới vùng đặt cốc, robot sẽ không thể gắp cốc dù controller tốt đến đâu.

Workspace cũng không chỉ là “vùng tay với tới”. Với nhiều tác vụ, orientation cũng phải hợp lệ. Một điểm có thể nằm trong workspace position, nhưng robot không thể đạt orientation cần thiết tại điểm đó. Khi đó, điểm này vẫn không hữu ích cho nhiệm vụ gắp cụ thể.

Singular configuration

Forward kinematics trả lời pose từ cấu hình khớp, nên bản thân nó vẫn tính được ở nhiều cấu hình đặc biệt. Tuy nhiên, một số cấu hình làm robot mất khả năng tạo chuyển động theo một hướng nào đó. Những cấu hình này thường được gọi là singular configuration.

Ví dụ, với tay máy hai khớp phẳng, khi hai link duỗi thẳng hàng, end-effector ở rất xa base. Ở gần cấu hình này, một số thay đổi nhỏ trong hướng mong muốn có thể đòi hỏi vận tốc khớp rất lớn. Forward kinematics vẫn cho ra vị trí rõ ràng, nhưng quan hệ giữa vận tốc khớp và vận tốc end-effector trở nên kém điều kiện.

Đây là vùng mà forward kinematics dẫn ta sang Jacobian. Nếu forward kinematics là:

$$\mathbf{x} = f(\mathbf{q})$$

thì ở mức vận tốc, ta quan tâm:

$$\dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}}$$

Jacobian $\mathbf{J}(\mathbf{q})$ là đạo hàm của forward kinematics theo cấu hình khớp. Nó cho biết vận tốc khớp tạo ra vận tốc end-effector như thế nào. Khi Jacobian mất hạng, robot rơi vào singularity. Bài này không đi sâu vào Jacobian, nhưng cần thấy rằng forward kinematics là nền để đi tới phần đó.

Vai trò trong hệ robot

Forward kinematics nằm ở nhiều vị trí trong pipeline robot hơn ta thường nghĩ. Khi robot đang di chuyển tới chiếc cốc, controller cần biết pose hiện tại để tính sai số. Khi planner kiểm tra va chạm, nó cần biết vị trí của từng link, không chỉ end-effector. Khi mô phỏng hiển thị robot, mọi link đều được đặt bằng forward kinematics. Khi một thuật toán học điều khiển nhận observation, pose của end-effector thường đến từ forward kinematics trên encoder.

Trong pipeline gắp cốc, forward kinematics có thể xuất hiện ở các bước sau. Encoder trả về $\mathbf{q}$. Forward kinematics tính ${}^{B}\mathbf{T}_{E}(\mathbf{q})$. Perception và coordinate transformation đưa cốc về ${}^{B}\mathbf{T}_{O}$. Hệ thống so sánh end-effector với object hoặc grasp pose. Planner/controller quyết định robot nên di chuyển tiếp như thế nào.

Nếu bỏ forward kinematics, robot chỉ biết các khớp đang ở trạng thái nào, nhưng không biết điều đó có ý nghĩa gì trong không gian làm việc. Nói cách khác, $\mathbf{q}$ là trạng thái nội bộ của cơ thể robot, còn forward kinematics dịch trạng thái đó thành hình học của nhiệm vụ.

Forward kinematics là bài toán nền tảng, nhưng nó chỉ đi một chiều: từ khớp tới pose. Nó không trực tiếp trả lời các khớp phải xoay thế nào để đạt một pose mong muốn. Đó là inverse kinematics. Nó cũng không nói cần moment bao nhiêu để robot chuyển động. Đó là dynamics và control. Nó không tự tránh vật cản, không tự chọn đường đi, và không tự sửa sai số cảm biến.

Tuy vậy, forward kinematics là phần gần như không thể thiếu trước khi đi tới các lớp đó. Nó cho robot biết cơ thể hiện tại của nó tạo ra hình học nào trong thế giới. Khi hiểu rõ forward kinematics, ta sẽ thấy inverse kinematics không phải một công thức đảo đơn giản, Jacobian không phải một ma trận lạ, và coordinate transformation không phải một chương tách rời. Tất cả đều xoay quanh cùng một việc: nối trạng thái bên trong của robot với vị trí, hướng và chuyển động của nó trong không gian vật lý.