5. Động lực học (Dynamics / 動力学) của cánh tay Robot

Estimated reading: 45 minutes 62 views

Trong Động lực học (Dynamics / 動力学), có hai dạng bài toán: bài toán động lực học thuận thông thường – tìm chuyển vị, vận tốc và gia tốc của từng khâu khi cho trước điều kiện ban đầu về lực truyền động khớp và chuyển vị khớp của cánh tay Robot; và bài toán động lực học nghịch – tìm lực truyền động khớp khi cho trước chuyển vị, vận tốc và gia tốc của từng khâu. Bài toán trước là bài toán tích phân Phương trình chuyển động (Equation of motion / 運動方程式) của cánh tay, chủ yếu cần thiết cho mô phỏng trên máy tính. Bài toán sau cần thiết cho việc thiết kế đầu vào điều khiển nhằm giúp phần đầu cuối chuyển động bám sát quỹ đạo đã lập kế hoạch với tốc độ và độ chính xác cao, hoặc để điều khiển lực động. Như vậy, động lực học của cánh tay Robot đóng vai trò quan trọng trong giải tích và điều khiển. Chương này sẽ trình bày những vấn đề cơ bản của động lực học.

5.1 Khái quát về động lực học

Là phương pháp dẫn xuất mô hình động lực học (Phương trình chuyển động (Equation of motion / 運動方程式)), Phương pháp Lagrange (Lagrange method / ラグランジュ法) và Phương pháp Newton-Euler (Newton-Euler method / ニュートン・オイラー法) là hai phương pháp tiêu biểu.

Phương pháp Lagrange (Lagrange method / ラグランジュ法) tìm Hàm Lagrange (Lagrange function / ラグランジュ関数) từ động năng và thế năng của toàn bộ khâu, sau đó thế vào phương trình Lagrange để dẫn ra phương trình chuyển động một cách cơ học. Người đầu tiên thiết lập phương trình chuyển động của Robot bằng phương pháp Lagrange là Uicker/Kahn, sau đó một công thức truy hồi hiệu quả đã được dẫn xuất để tính toán mô hình động lực học nghịch. Đặc điểm chính của phương pháp này là không cần xét đến lực ràng buộc nội bộ giữa các khâu, nhưng bù lại có nhiều phép tính dư thừa.

Phương pháp Newton-Euler (Newton-Euler method / ニュートン・オイラー法) xử lý lực ràng buộc và chuyển động tương đối giữa các khâu dưới dạng đại lượng vectơ, và dẫn xuất phương trình chuyển động từ sự cân bằng của lực và mô-men. Đặc điểm chính của phương pháp này là phải xét đến sự cân bằng của lực và mô-men trong không gian 3 chiều, nhưng ít phép tính dư thừa. Đặc biệt, công thức truy hồi do Luh và cộng sự dẫn xuất là một cách thiết lập hiệu quả để tính toán mô hình động lực học nghịch. Khi kết hợp với phương pháp tính toán Ma trận quán tính (Inertia matrix / 慣性行列) cánh tay của Walker/Orin, nó đã được chứng minh là một phương pháp hiệu quả ngay cả trong mô phỏng máy tính sử dụng mô hình động lực học thuận.

5.2 Phương trình chuyển động theo phương pháp Lagrange

≫ 5.2.1 Phương trình chuyển động của Robot

Gọi động năng của toàn bộ cánh tay là $K$, thế năng của toàn bộ cánh tay là $P$, Hàm Lagrange (Lagrange function / ラグランジュ関数) $L$ được cho bởi:

$$L = K – P \tag{5.1}$$

Nếu chọn Tọa độ suy rộng (Generalized coordinate / 一般化座標) là biến khớp $q_i$, thì Lực suy rộng (Generalized force / 一般化力) tương ứng với $q_i$ sẽ là lực truyền động khớp $\tau_i$, khi đó phương trình chuyển động Lagrange (Lagrange’s equation of motion) là:

$$\tau_i = \frac{d}{dt}\left[\frac{\partial L}{\partial \dot{q}_i}\right] – \frac{\partial L}{\partial q_i} \quad (i = 1, 2, \cdots, n) \tag{5.2}$$

(Tham khảo Phụ lục E). Vì $P$ không phụ thuộc vào $\dot{q}$, phương trình trên trở thành phương trình sau:

$$\tau_i = \frac{d}{dt}\left[\frac{\partial K}{\partial \dot{q}_i}\right] – \frac{\partial K}{\partial q_i} + \frac{\partial P}{\partial q_i} \quad (i = 1, 2, \cdots, n) \tag{5.3}$$

Gọi động năng của khâu $i$ là $K_i$, động năng $K$ của toàn bộ cánh tay gồm $n$ khâu là:

$$K = \sum_{i=1}^{n} K_i \tag{5.4}$$

Động năng của khâu $i$ có thể được biểu diễn bằng cách tích phân động năng mà phần tử vi phân $dm$ (thể hiện trong Hình 5.1) sở hữu trên toàn bộ khâu $i$:

$$K_i = \frac{1}{2} m_i \dot{\boldsymbol{s}}_i^T \dot{\boldsymbol{s}}_i + \frac{1}{2} \boldsymbol{\omega}_i^T \hat{\boldsymbol{I}}_i \boldsymbol{\omega}_i \tag{5.5}$$

Trong đó, $m_i$ là khối lượng của khâu $i$, $\hat{\boldsymbol{I}}_i$ là Tensor quán tính (Inertia tensor / 慣性テンソル) tại khối tâm của khâu biểu diễn trong tọa độ cơ sở, $\boldsymbol{s}_i$ là vectơ vị trí khối tâm của khâu $i$ biểu diễn trong tọa độ cơ sở, $\boldsymbol{\omega}_i$ là vectơ vận tốc góc của khâu $i$. Số hạng thứ nhất vế phải của phương trình trên là động năng sinh ra do chuyển động tịnh tiến của khối lượng $m_i$, và số hạng thứ hai là động năng sinh ra do chuyển động quay quanh khối tâm. Vectơ vận tốc khối tâm của khâu $i$ là $\dot{\boldsymbol{r}}_i = [\dot{\boldsymbol{s}}_i^T, \boldsymbol{\omega}_i^T]^T$, nếu định nghĩa $\dot{\boldsymbol{q}} = [\dot{q}_1, \dot{q}_2, \cdots, \dot{q}_n]^T$, thì tương tự phương trình (4.89), ta có thể biểu diễn bằng phương trình sau:

$$\dot{\boldsymbol{s}}_i = \boldsymbol{J}_{P1}^{(i)} \dot{q}_1 + \cdots + \boldsymbol{J}_{Pi}^{(i)} \dot{q}_i = \boldsymbol{J}_P^{(i)} \dot{\boldsymbol{q}} \tag{5.6}$$
$$\boldsymbol{\omega}_i = \boldsymbol{J}_{O1}^{(i)} \dot{q}_1 + \cdots + \boldsymbol{J}_{Oi}^{(i)} \dot{q}_i = \boldsymbol{J}_O^{(i)} \dot{\boldsymbol{q}} \tag{5.7}$$

Tại đây, $\boldsymbol{J}_P^{(i)} \in R^{3 \times n}$ là Ma trận Jacobian (Jacobian matrix / ヤコビ行列) liên quan đến vị trí của khâu $i$, $\boldsymbol{J}_{Pj}^{(i)} \in R^3$ là vectơ cột thứ $j$ của $\boldsymbol{J}_P^{(i)}$; $\boldsymbol{J}_O^{(i)} \in R^{3 \times n}$ là ma trận Jacobian liên quan đến tư thế của khâu $i$, $\boldsymbol{J}_{Oj}^{(i)} \in R^3$ là vectơ cột thứ $j$ của $\boldsymbol{J}_O^{(i)}$. Vì chuyển động của khâu $i$ chỉ phụ thuộc vào các khớp từ khớp 1 đến khớp $i$, nên khi $j > i$, các vectơ cột $\boldsymbol{J}_{Pj}^{(i)}$ và $\boldsymbol{J}_{Oj}^{(i)}$ sẽ là vectơ không. Từ những phương trình quan hệ này, động năng $K_i$ của khâu $i$ được biểu diễn là:

$$K_i = \frac{1}{2} m_i \dot{\boldsymbol{q}}^T \boldsymbol{J}_P^{(i)T} \boldsymbol{J}_P^{(i)} \dot{\boldsymbol{q}} + \frac{1}{2} \dot{\boldsymbol{q}}^T \boldsymbol{J}_O^{(i)T} \hat{\boldsymbol{I}}_i \boldsymbol{J}_O^{(i)} \dot{\boldsymbol{q}} \tag{5.8}$$

và động năng toàn phần $K$ thu được là:

$$K = \frac{1}{2} \dot{\boldsymbol{q}}^T \boldsymbol{M} \dot{\boldsymbol{q}} \tag{5.9}$$

Trong đó, $\boldsymbol{M} \in R^{n \times n}$ được gọi là Ma trận quán tính (Inertia matrix / 慣性行列) của cánh tay Robot, và được cho bởi công thức sau:

$$\boldsymbol{M} = \sum_{i=1}^{n} \left( m_i \boldsymbol{J}_P^{(i)T} \boldsymbol{J}_P^{(i)} + \boldsymbol{J}_O^{(i)T} \hat{\boldsymbol{I}}_i \boldsymbol{J}_O^{(i)} \right) \tag{5.10}$$

Ma trận quán tính của cánh tay là một Ma trận xác định dương (Positive definite matrix / 正定行列) đối xứng, và nó thay đổi cùng với vị trí và tư thế của phần đầu cuối.

Nếu gọi thế năng của từng khâu là $P_i$, thì thế năng toàn phần $P$ của toàn bộ các khâu là:

$$P = \sum_{i=1}^{n} P_i \tag{5.11}$$

với $P_i$ được biểu diễn là:

$$P_i = -m_i \boldsymbol{g}^T \boldsymbol{s}_i \tag{5.12}$$

Trong đó, $\boldsymbol{g}$ là vectơ gia tốc trọng trường.

Phương trình chuyển động được tìm bằng cách thay phương trình (5.9) và (5.11) vào phương trình (5.3). Khi biểu diễn phương trình (5.3) dưới dạng vectơ, ta có:

$$\frac{d}{dt}\left(\frac{\partial K}{\partial \dot{\boldsymbol{q}}}\right) – \frac{\partial K}{\partial \boldsymbol{q}} + \frac{\partial P}{\partial \boldsymbol{q}} = \boldsymbol{\tau} \tag{5.13}$$

Thay phương trình (5.9), (5.11) vào phương trình này, ta thu được:

$$\boldsymbol{M}(\boldsymbol{q})\ddot{\boldsymbol{q}} + \boldsymbol{h}(\boldsymbol{q}, \dot{\boldsymbol{q}}) + \boldsymbol{g}(\boldsymbol{q}) = \boldsymbol{\tau} \tag{5.14}$$

Trong đó:

$$\boldsymbol{h}(\boldsymbol{q}, \dot{\boldsymbol{q}}) = \dot{\boldsymbol{M}}\dot{\boldsymbol{q}} – \frac{\partial K}{\partial \boldsymbol{q}} \tag{5.15}$$
$$\boldsymbol{g}(\boldsymbol{q}) = \frac{\partial P}{\partial \boldsymbol{q}} \tag{5.16}$$

Phương trình (5.14) chỉ ra rằng phương trình chuyển động của Robot là phương trình vi phân bậc hai của biến khớp. Số hạng thứ nhất vế trái là số hạng quán tính, nếu đặt $\boldsymbol{M} = \{M_{ij}\}$, thì $M_{ii}\ddot{q}_i$ là số hạng quán tính do gia tốc của khớp $i$, $M_{ij}\ddot{q}_j \ (i \neq j)$ biểu thị số hạng nhiễu/tác động (interference term) lên khớp $i$ do gia tốc của khớp $j$. Số hạng thứ hai là số hạng bình phương vận tốc, được cấu thành từ Lực ly tâm (Centrifugal force / 遠心力) và Lực Coriolis (Coriolis force / コリオリ力). Số hạng thứ ba là số hạng biểu thị ảnh hưởng của trọng lực.

Ví dụ 5.1 Hãy dùng phương pháp Lagrange để tìm phương trình chuyển động cho Hình 5.2. Chú ý sử dụng các ký hiệu trong hình cho chiều dài khâu và mô-men quán tính tại khối tâm.

Vì chuyển động bị giới hạn trong mặt phẳng nên ta xử lý nó như một chuyển động phẳng. Khối tâm của khâu 1 và khâu 2 là:

$$\boldsymbol{s}_1 = \begin{pmatrix} L_{g1} C\theta_1 \\ L_{g1} S\theta_1 \end{pmatrix}, \quad \boldsymbol{s}_2 = \begin{pmatrix} L_1 C\theta_1 + L_{g2} C\theta_{12} \\ L_1 S\theta_1 + L_{g2} S\theta_{12} \end{pmatrix}$$

Ở đây, $C\theta_{12} = \cos(\theta_1 + \theta_2)$, $S\theta_{12} = \sin(\theta_1 + \theta_2)$. Vận tốc khối tâm được lấy đạo hàm theo thời gian từ phương trình trên:

$$\dot{\boldsymbol{s}}_1 = \begin{pmatrix} -L_{g1} S\theta_1 \\ L_{g1} C\theta_1 \end{pmatrix} \dot{\theta}_1$$
$$\dot{\boldsymbol{s}}_2 = \begin{pmatrix} -L_1 S\theta_1 – L_{g2} S\theta_{12} \\ L_1 C\theta_1 + L_{g2} C\theta_{12} \end{pmatrix} \dot{\theta}_1 + \begin{pmatrix} -L_{g2} S\theta_{12} \\ L_{g2} C\theta_{12} \end{pmatrix} \dot{\theta}_2$$

Vận tốc góc là:

$$\omega_1 = \dot{\theta}_1, \quad \omega_2 = \dot{\theta}_1 + \dot{\theta}_2$$

Động năng của khâu 1 và khâu 2, nếu đặt mô-men quán tính tại khối tâm của khâu $i$ là $\hat{I}_i \ (i=1, 2)$, sẽ là:

$$K_1 = \frac{1}{2} m_1 \dot{\boldsymbol{s}}_1^T \dot{\boldsymbol{s}}_1 + \frac{1}{2} \hat{I}_1 \omega_1^2 = \frac{1}{2} (m_1 L_{g1}^2 \dot{\theta}_1^2 + \hat{I}_1 \dot{\theta}_1^2)$$
$$K_2 = \frac{1}{2} m_2 \dot{\boldsymbol{s}}_2^T \dot{\boldsymbol{s}}_2 + \frac{1}{2} \hat{I}_2 \omega_2^2$$
$$= \frac{1}{2} \Big( m_2 \big( (L_1^2 + L_{g2}^2 + 2L_1 L_{g2} C\theta_2) \dot{\theta}_1^2 + L_{g2}^2 \dot{\theta}_2^2$$
$$+ 2(L_{g2}^2 + L_1 L_{g2} C\theta_2) \dot{\theta}_1 \dot{\theta}_2 \big) + \hat{I}_2 (\dot{\theta}_1 + \dot{\theta}_2)^2 \Big)$$

Thế năng của từng khâu có thể biểu diễn là:

$$P_1 = m_1 L_{g1} S\theta_1 g$$
$$P_2 = m_2 (L_1 S\theta_1 + L_{g2} S\theta_{12}) g$$

Sau khi tìm động năng toàn phần $K = K_1 + K_2$, thế năng toàn phần $P = P_1 + P_2$ và hàm Lagrange $L = K – P$, ta thay vào phương trình Lagrange (5.3) và thu được:

$$\tau_1 = \frac{d}{dt}\left(\frac{\partial K}{\partial \dot{\theta}_1}\right) – \frac{\partial K}{\partial \theta_1} + \frac{\partial P}{\partial \theta_1}$$
$$= (\hat{I}_1 + \hat{I}_2 + 2m_2 L_1 L_{g2} C\theta_2 + m_2 L_1^2 + m_1 L_{g1}^2 + m_2 L_{g2}^2)\ddot{\theta}_1$$
$$+ (\hat{I}_2 + m_2 L_1 L_{g2} C\theta_2 + m_2 L_{g2}^2)\ddot{\theta}_2$$
$$- m_2 L_1 L_{g2} S\theta_2 (2\dot{\theta}_1 \dot{\theta}_2 + \dot{\theta}_2^2)$$
$$+ \{(m_1 L_{g1} + m_2 L_1)C\theta_1 + m_2 L_{g2} C\theta_{12}\} g$$
$$\tau_2 = \frac{d}{dt}\left(\frac{\partial K}{\partial \dot{\theta}_2}\right) – \frac{\partial K}{\partial \theta_2} + \frac{\partial P}{\partial \theta_2}$$
$$= (\hat{I}_2 + m_2 L_1 L_{g2} C\theta_2 + m_2 L_{g2}^2)\ddot{\theta}_1 + (\hat{I}_2 + m_2 L_{g2}^2)\ddot{\theta}_2$$
$$+ m_2 L_1 L_{g2} S\theta_2 \dot{\theta}_1^2 + m_2 L_{g2} C\theta_{12} g$$

Trong đó, mô-men quán tính $I_i$ tại gốc hệ tọa độ khâu $i$, theo định lý trục song song (tham khảo Phụ lục C), có thể biểu diễn là:

$$I_1 = m_1 L_{g1}^2 + \hat{I}_1, \quad I_2 = m_2 L_{g2}^2 + \hat{I}_2$$

Sử dụng nó để rút gọn các phương trình trên, ta được phương trình sau:

$$\begin{bmatrix} \tau_1 \\ \tau_2 \end{bmatrix} = \begin{bmatrix} I_1 + I_2 + 2m_2 L_1 L_{g2} C\theta_2 + m_2 L_1^2 & I_2 + m_2 L_1 L_{g2} C\theta_2 \\ I_2 + m_2 L_1 L_{g2} C\theta_2 & I_2 \end{bmatrix} \begin{bmatrix} \ddot{\theta}_1 \\ \ddot{\theta}_2 \end{bmatrix}$$
$$+ \begin{bmatrix} -m_2 L_1 L_{g2} S\theta_2 (2\dot{\theta}_1 \dot{\theta}_2 + \dot{\theta}_2^2) \\ m_2 L_1 L_{g2} S\theta_2 \dot{\theta}_1^2 \end{bmatrix}$$
$$+ \begin{bmatrix} (m_1 L_{g1} + m_2 L_1)C\theta_1 + m_2 L_{g2} C\theta_{12} \\ m_2 L_{g2} C\theta_{12} \end{bmatrix} g$$

5.3 Phương trình chuyển động theo phương pháp Newton-Euler

Trình tự tìm phương trình chuyển động của Robot bằng phương pháp Newton-Euler như sau:

  1. Cung cấp chuyển động khớp $\boldsymbol{q}$, $\dot{\boldsymbol{q}}$, $\ddot{\boldsymbol{q}}$, khi đó tính toán lần lượt từ phía đế (base) về phía phần đầu cuối các đại lượng: vận tốc quay $\boldsymbol{\omega}_i$ tại khối tâm khâu $i$, gia tốc quay $\dot{\boldsymbol{\omega}}_i$, vận tốc tịnh tiến $\bar{\boldsymbol{v}}_i$ và gia tốc tịnh tiến $\dot{\bar{\boldsymbol{v}}}_i$.

  2. Để mỗi khâu thực hiện chuyển động đã tìm ở 1), tính toán lực $\boldsymbol{F}_i$ và mô-men $\boldsymbol{N}_i$ tác dụng lên khối tâm.

  3. Tính toán lần lượt từ phần đầu cuối về phía đế lực $\boldsymbol{f}_i$ và mô-men $\boldsymbol{n}_i$ tác dụng lên khớp $(i+1)$ để cân bằng với lực và mô-men tác dụng lên khối tâm, từ đó tính toán lực truyền động khớp $\boldsymbol{\tau}$ cần tác dụng lên mỗi khớp.

    Dưới đây, ta sẽ dẫn xuất các phương trình quan hệ cần thiết cho tính toán trên.

≫ 5.3.1 Đạo hàm của vectơ vị trí và ma trận quay

Xét đạo hàm theo thời gian của một vectơ vị trí quay có chuẩn (norm) không đổi $\boldsymbol{p}(t)$. Như thể hiện trong Hình 5.3, vì vectơ vị trí $\boldsymbol{p}(t)$ dịch chuyển đến $\boldsymbol{p}(t + \Delta t)$ sau một khoảng thời gian cực nhỏ $\Delta t$ do phép quay, đạo hàm thời gian của $\boldsymbol{p}(t)$ được định nghĩa là:

$$\dot{\boldsymbol{p}}(t) = \lim_{\Delta t \to 0} \frac{\boldsymbol{p}(t + \Delta t) – \boldsymbol{p}(t)}{\Delta t} \tag{5.17}$$

Đạo hàm thời gian này trực giao với vectơ vận tốc góc $\boldsymbol{\omega} = [\omega_x, \omega_y, \omega_z]^T$ và vectơ vị trí $\boldsymbol{p}$, đồng thời tỷ lệ với độ lớn của chúng nên được biểu diễn bằng:

$$\dot{\boldsymbol{p}}(t) = \boldsymbol{\omega} \times \boldsymbol{p}(t) \tag{5.18}$$

Ngoài ra, tích có hướng của vectơ có thể được biểu thị dưới dạng tích của ma trận và vectơ, từ phương trình (4.112), ta có thể viết thành:

$$\dot{\boldsymbol{p}}(t) = \boldsymbol{S}(\boldsymbol{\omega})\boldsymbol{p}(t) \tag{5.19}$$

Trong đó, $\boldsymbol{S}(\boldsymbol{\omega})$ là Ma trận phản xứng (Skew-symmetric matrix / 歪対称行列)$^\dagger$ và được cho bởi:

$$\boldsymbol{S}(\boldsymbol{\omega}) = \begin{bmatrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix} \tag{5.20}$$

Tiếp theo, xem xét vật thể $\text{A}$ chuyển động quay với vận tốc góc ${}^0\boldsymbol{\omega}_A$. Vì ma trận quay từ tọa độ cơ sở sang hệ tọa độ $\Sigma_A = \{\boldsymbol{x}_A, \boldsymbol{y}_A, \boldsymbol{z}_A\}$ thiết lập trên vật thể là ${}^0\boldsymbol{R}_A = [{}^0\boldsymbol{x}_A, {}^0\boldsymbol{y}_A, {}^0\boldsymbol{z}_A]$, nên đạo hàm của nó, từ quan hệ của phương trình (5.18), sẽ là:

$${}^0\dot{\boldsymbol{R}}_A = [{}^0\dot{\boldsymbol{x}}_A, {}^0\dot{\boldsymbol{y}}_A, {}^0\dot{\boldsymbol{z}}_A] = [{}^0\boldsymbol{\omega}_A \times {}^0\boldsymbol{x}_A, {}^0\boldsymbol{\omega}_A \times {}^0\boldsymbol{y}_A, {}^0\boldsymbol{\omega}_A \times {}^0\boldsymbol{z}_A]$$
$$= \boldsymbol{S}({}^0\boldsymbol{\omega}_A) [{}^0\boldsymbol{x}_A, {}^0\boldsymbol{y}_A, {}^0\boldsymbol{z}_A]$$
$$= \boldsymbol{S}({}^0\boldsymbol{\omega}_A) {}^0\boldsymbol{R}_A \tag{5.21}$$

Phương trình này chỉ ra rằng đạo hàm thời gian của ma trận quay có thể thu được qua phép ánh xạ bằng ma trận phản xứng được tạo từ vectơ vận tốc góc. Ngoài ra, từ phương trình này, ta có:

$$\boldsymbol{S}({}^0\boldsymbol{\omega}_A) = {}^0\dot{\boldsymbol{R}}_A {}^0\boldsymbol{R}_A^T \tag{5.22}$$

Ở đây, về mặt hình học rõ ràng là tích có hướng của hai vectơ $3 \times 1$ bất kỳ $\boldsymbol{a}$, $\boldsymbol{b}$ sau khi bị tác dụng bởi ma trận quay $\boldsymbol{R}$, tức là $\boldsymbol{R}(\boldsymbol{a} \times \boldsymbol{b})$, bằng tích có hướng của các vectơ $\boldsymbol{a}$, $\boldsymbol{b}$ sau khi bị tác dụng riêng lẻ bởi $\boldsymbol{R}$, tức là $\boldsymbol{R}\boldsymbol{a}$, $\boldsymbol{R}\boldsymbol{b}$. Tức là:

$$\boldsymbol{R}(\boldsymbol{a} \times \boldsymbol{b}) = (\boldsymbol{R}\boldsymbol{a}) \times (\boldsymbol{R}\boldsymbol{b}) \tag{5.23}$$

Hơn nữa, từ ${}^0\boldsymbol{R}_A^T = [{}^A\boldsymbol{x}_0 \ {}^A\boldsymbol{y}_0 \ {}^A\boldsymbol{z}_0]$, ta có mối quan hệ sau:

$${}^0\boldsymbol{R}_A \boldsymbol{S}({}^0\boldsymbol{\omega}_A) {}^0\boldsymbol{R}_A^T = {}^0\boldsymbol{R}_A [{}^0\boldsymbol{\omega}_A \times {}^A\boldsymbol{x}_0 \quad {}^0\boldsymbol{\omega}_A \times {}^A\boldsymbol{y}_0 \quad {}^0\boldsymbol{\omega}_A \times {}^A\boldsymbol{z}_0]$$
$$= [({}^0\boldsymbol{R}_A {}^0\boldsymbol{\omega}_A) \times {}^0\boldsymbol{x}_0 \quad ({}^0\boldsymbol{R}_A {}^0\boldsymbol{\omega}_A) \times {}^0\boldsymbol{y}_0 \quad ({}^0\boldsymbol{R}_A {}^0\boldsymbol{\omega}_A) \times {}^0\boldsymbol{z}_0]$$
$$= \boldsymbol{S}({}^0\boldsymbol{R}_A {}^0\boldsymbol{\omega}_A) \tag{5.24}$$

$\dagger$ Ma trận phản xứng: Ma trận $\boldsymbol{A}$ kích thước $n \times n$ thỏa mãn điều kiện $\boldsymbol{A} = -\boldsymbol{A}^T$ được gọi là ma trận phản xứng. Các phần tử của ma trận phản xứng $\boldsymbol{A} = \{a_{ij}\}$ là $a_{ij} = -a_{ji} \ (i \neq j)$ và $a_{ii} = 0$, do đó đối với bất kỳ vectơ $n$ chiều $\boldsymbol{x}$ nào, ta cũng có $\boldsymbol{x}^T \boldsymbol{A} \boldsymbol{x} = 0$.

≫ 5.3.2 Mối quan hệ của các hệ tọa độ chuyển động tịnh tiến / quay

Như thể hiện trong Hình 5.4, xét hệ tọa độ cơ sở $\Sigma_0 = \{\boldsymbol{x}_0, \boldsymbol{y}_0, \boldsymbol{z}_0\}$, hệ tọa độ di chuyển tịnh tiến / quay $\Sigma_A = \{\boldsymbol{x}_A, \boldsymbol{y}_A, \boldsymbol{z}_A\}$ và hệ tọa độ $\Sigma_B$, ta sẽ khảo sát mối quan hệ vận tốc và gia tốc của chúng.

Đạo hàm theo thời gian của vectơ $3 \times 1$ $\boldsymbol{p}_{BA}$ từ gốc $\Sigma_A$ đến gốc $\Sigma_B$ biểu diễn trong $\Sigma_0$ là ${}^0\boldsymbol{p}_{BA}$, từ phương trình (5.21) sẽ là:

$${}^0\dot{\boldsymbol{p}}_{BA} = \frac{d({}^0\boldsymbol{R}_A {}^A\boldsymbol{p}_{BA})}{dt} = {}^0\dot{\boldsymbol{R}}_A {}^A\boldsymbol{p}_{BA} + {}^0\boldsymbol{R}_A {}^A\dot{\boldsymbol{p}}_{BA}$$
$$= {}^0\boldsymbol{R}_A {}^A\dot{\boldsymbol{p}}_{BA} + \boldsymbol{S}({}^0\boldsymbol{\omega}_A) {}^0\boldsymbol{R}_A {}^A\boldsymbol{p}_{BA}$$
$$= {}^0\boldsymbol{R}_A {}^A\dot{\boldsymbol{p}}_{BA} + {}^0\boldsymbol{\omega}_A \times ({}^0\boldsymbol{R}_A {}^A\boldsymbol{p}_{BA}) \tag{5.25}$$

${}^A\dot{\boldsymbol{p}}_{BA}$ là sự thay đổi vận tốc tương đối của $\Sigma_B$ so với $\Sigma_A$. Gọi các vectơ từ gốc $\Sigma_0$ đến gốc $\Sigma_A$ và gốc $\Sigma_B$ lần lượt là $\boldsymbol{p}_A$, $\boldsymbol{p}_B$, ta có quan hệ:

$${}^0\boldsymbol{p}_B = {}^0\boldsymbol{p}_A + {}^0\boldsymbol{p}_{BA} \tag{5.26}$$

Lấy đạo hàm cả hai vế phương trình trên và thế phương trình (5.25) vào, ta được:

$${}^0\dot{\boldsymbol{p}}_B = {}^0\dot{\boldsymbol{p}}_A + {}^0\boldsymbol{R}_A {}^A\dot{\boldsymbol{p}}_{BA} + {}^0\boldsymbol{\omega}_A \times ({}^0\boldsymbol{R}_A {}^A\boldsymbol{p}_{BA}) \tag{5.27}$$

Đồng thời, nếu vận tốc góc của $\Sigma_B$ là ${}^0\boldsymbol{\omega}_B$ và vận tốc góc tương đối của $\Sigma_B$ so với $\Sigma_A$ là ${}^0\boldsymbol{\omega}_{BA}$, thì:

$${}^0\boldsymbol{\omega}_B = {}^0\boldsymbol{\omega}_A + {}^0\boldsymbol{\omega}_{BA}$$
$$= {}^0\boldsymbol{\omega}_A + {}^0\boldsymbol{R}_A {}^A\boldsymbol{\omega}_{BA} \tag{5.28}$$

được thành lập. Điều này mang ý nghĩa là tính cộng của vận tốc góc. Bằng các phương trình (5.27) và (5.28), ta thu được mối quan hệ giữa vận tốc góc và vận tốc tịnh tiến của $\Sigma_A$ và $\Sigma_B$ khi biểu diễn trong $\Sigma_0$.

Lấy đạo hàm hai vế của phương trình (5.27), ta thu được gia tốc tịnh tiến:

$${}^0\ddot{\boldsymbol{p}}_B = {}^0\ddot{\boldsymbol{p}}_A + {}^0\boldsymbol{R}_A {}^A\ddot{\boldsymbol{p}}_{BA} + 2 {}^0\boldsymbol{\omega}_A \times ({}^0\boldsymbol{R}_A {}^A\dot{\boldsymbol{p}}_{BA})$$
$$+ {}^0\dot{\boldsymbol{\omega}}_A \times ({}^0\boldsymbol{R}_A {}^A\boldsymbol{p}_{BA}) + {}^0\boldsymbol{\omega}_A \times [{}^0\boldsymbol{\omega}_A \times ({}^0\boldsymbol{R}_A {}^A\boldsymbol{p}_{BA})] \tag{5.29}$$

Số hạng thứ 3 bên vế phải của phương trình trên là Gia tốc Coriolis (Coriolis acceleration / コリオリの加速度), số hạng thứ 5 được gọi là Gia tốc hướng tâm (Centripetal acceleration / 求心加速度).

Lấy đạo hàm hai vế của phương trình (5.28), ta thu được gia tốc góc:

$${}^0\dot{\boldsymbol{\omega}}_B = {}^0\dot{\boldsymbol{\omega}}_A + {}^0\boldsymbol{R}_A {}^A\dot{\boldsymbol{\omega}}_{BA} + {}^0\boldsymbol{\omega}_A \times ({}^0\boldsymbol{R}_A {}^A\boldsymbol{\omega}_{BA}) \tag{5.30}$$

Thông qua các bước trên, các phương trình quan hệ giữa các hệ tọa độ di chuyển tịnh tiến / quay đã được thiết lập.

≫ 5.3.3 Chuyển động tương đối của hệ tọa độ khâu

Thiết lập hệ tọa độ $\Sigma_i$ tại khâu $i$ bằng phương pháp D-H. Như thể hiện trong Hình 5.5, gọi vectơ từ gốc $\Sigma_0$ đến gốc $\Sigma_i$ là $\boldsymbol{p}_i$, vectơ từ gốc $\Sigma_0$ đến khối tâm của khâu $i$ là $\boldsymbol{s}_i$, vectơ từ gốc $\Sigma_{i-1}$ đến gốc $\Sigma_i$ là $\hat{\boldsymbol{p}}_i$, và vectơ từ gốc $\Sigma_i$ đến khối tâm của khâu $i$ là $\hat{\boldsymbol{s}}_i$. Vectơ vận tốc góc tương đối của $\Sigma_i$ đối với $\Sigma_{i-1}$, khi khớp $i$ là khớp quay, sẽ có hướng theo trục $\boldsymbol{z}_i$ với độ lớn là $\dot{q}_i$, và khi là khớp tịnh tiến thì là $\boldsymbol{0}$. Ở đây, hãy thiết lập sự tương ứng với hệ tọa độ ở phần trước như $\Sigma_A \to \Sigma_{i-1}$, $\Sigma_B \to \Sigma_i$, và tìm mối quan hệ tương đối của hệ tọa độ khâu. Lúc này, từ ${}^i\boldsymbol{z}_i = \boldsymbol{z}_0 = [0 \ 0 \ 1]^T$, do ${}^{i-1}\boldsymbol{z}_i = {}^{i-1}\boldsymbol{R}_i {}^i\boldsymbol{z}_i = {}^{i-1}\boldsymbol{R}_i \boldsymbol{z}_0$, nên:

$${}^A\boldsymbol{\omega}_{BA} \to \begin{cases} {}^{i-1}\boldsymbol{R}_i \boldsymbol{z}_0 \dot{q}_i & (\text{Khi khớp } i \text{ là khớp quay: IF R}) \\ \boldsymbol{0} & (\text{Khi khớp } i \text{ là khớp tịnh tiến: IF T}) \end{cases} \tag{5.31}$$
$${}^A\dot{\boldsymbol{\omega}}_{BA} \to \begin{cases} {}^{i-1}\boldsymbol{R}_i \boldsymbol{z}_0 \ddot{q}_i & (\text{IF R}) \\ \boldsymbol{0} & (\text{IF T}) \end{cases} \tag{5.32}$$

tương ứng với nhau. Ngoài ra, khi khớp $i$ là khớp quay, vectơ vận tốc tịnh tiến tương đối của $\Sigma_i$ đối với $\Sigma_{i-1}$ là $\boldsymbol{0}$, và khi là khớp tịnh tiến, nó có hướng theo trục $\boldsymbol{z}_i$ với độ lớn là $\dot{q}_i$, do đó:

$${}^A\dot{\boldsymbol{p}}_{BA} \to \begin{cases} \boldsymbol{0} & (\text{IF R}) \\ {}^{i-1}\boldsymbol{R}_i \boldsymbol{z}_0 \dot{q}_i & (\text{IF T}) \end{cases} \tag{5.33}$$
$${}^A\ddot{\boldsymbol{p}}_{BA} \to \begin{cases} \boldsymbol{0} & (\text{IF R}) \\ {}^{i-1}\boldsymbol{R}_i \boldsymbol{z}_0 \ddot{q}_i & (\text{IF T}) \end{cases} \tag{5.34}$$

tương ứng với nhau. Vì vậy, từ phương trình (5.28), vận tốc góc của khâu là:

$${}^0\boldsymbol{\omega}_i = \begin{cases} {}^0\boldsymbol{\omega}_{i-1} + {}^0\boldsymbol{R}_i \boldsymbol{z}_0 \dot{q}_i & (\text{IF R}) \\ {}^0\boldsymbol{\omega}_{i-1} & (\text{IF T}) \end{cases} \tag{5.35}$$

Vận tốc tịnh tiến của khâu, nếu đặt ${}^0\dot{\boldsymbol{p}}_i = {}^0\boldsymbol{v}_i$, từ phương trình (5.27) sẽ là:

$${}^0\boldsymbol{v}_i = \begin{cases} {}^0\boldsymbol{v}_{i-1} + {}^0\boldsymbol{\omega}_{i-1} \times ({}^0\boldsymbol{R}_{i-1} {}^{i-1}\hat{\boldsymbol{p}}_i) & (\text{IF R}) \\ {}^0\boldsymbol{v}_{i-1} + {}^0\boldsymbol{R}_i \boldsymbol{z}_0 \dot{q}_i + {}^0\boldsymbol{\omega}_{i-1} \times ({}^0\boldsymbol{R}_{i-1} {}^{i-1}\hat{\boldsymbol{p}}_i) & (\text{IF T}) \end{cases} \tag{5.36}$$

Tương tự, gia tốc góc và gia tốc tịnh tiến của khâu, từ phương trình (5.29) và (5.30) sẽ là:

$${}^0\dot{\boldsymbol{\omega}}_i = \begin{cases} {}^0\dot{\boldsymbol{\omega}}_{i-1} + {}^0\boldsymbol{R}_i \boldsymbol{z}_0 \ddot{q}_i + {}^0\boldsymbol{\omega}_{i-1} \times ({}^0\boldsymbol{R}_i \boldsymbol{z}_0 \dot{q}_i) & (\text{IF R}) \\ {}^0\dot{\boldsymbol{\omega}}_{i-1} & (\text{IF T}) \end{cases} \tag{5.37}$$
$${}^0\dot{\boldsymbol{v}}_i = \begin{cases} {}^0\dot{\boldsymbol{v}}_{i-1} + {}^0\dot{\boldsymbol{\omega}}_{i-1} \times ({}^0\boldsymbol{R}_{i-1} {}^{i-1}\hat{\boldsymbol{p}}_i) \\ \quad + {}^0\boldsymbol{\omega}_{i-1} \times [{}^0\boldsymbol{\omega}_{i-1} \times ({}^0\boldsymbol{R}_{i-1} {}^{i-1}\hat{\boldsymbol{p}}_i)] & (\text{IF R}) \\ {}^0\dot{\boldsymbol{v}}_{i-1} + {}^0\boldsymbol{R}_i \boldsymbol{z}_0 \ddot{q}_i + 2 {}^0\boldsymbol{\omega}_{i-1} \times ({}^0\boldsymbol{R}_i \boldsymbol{z}_0 \dot{q}_i) \\ \quad + {}^0\dot{\boldsymbol{\omega}}_{i-1} \times ({}^0\boldsymbol{R}_{i-1} {}^{i-1}\hat{\boldsymbol{p}}_i) \\ \quad + {}^0\boldsymbol{\omega}_{i-1} \times [{}^0\boldsymbol{\omega}_{i-1} \times ({}^0\boldsymbol{R}_{i-1} {}^{i-1}\hat{\boldsymbol{p}}_i)] & (\text{IF T}) \end{cases} \tag{5.38}$$

≫ 5.3.4 Phương trình Newton-Euler (Newton-Euler equation / ニュートン・オイラー式)

Phương trình Newton và phương trình Euler liên quan đến Khâu (Link / リンク) $i$ được biểu diễn bằng các công thức sau:

$${}^0\boldsymbol{F}_i = m_i {}^0\dot{\hat{\boldsymbol{v}}}_i \tag{5.39}$$
$${}^0\boldsymbol{N}_i = \frac{d}{dt}({}^0\hat{\boldsymbol{I}}_i {}^0\boldsymbol{\omega}_i) = {}^0\hat{\boldsymbol{I}}_i {}^0\dot{\boldsymbol{\omega}}_i + {}^0\boldsymbol{\omega}_i \times ({}^0\hat{\boldsymbol{I}}_i {}^0\boldsymbol{\omega}_i) \tag{5.40}$$

Trong đó,

${}^0\boldsymbol{F}_i$: Tổng ngoại lực tác dụng lên Khâu $i$ biểu diễn trong $\Sigma_0$

${}^0\boldsymbol{N}_i$: Tổng mô-men tác dụng lên Khâu $i$ biểu diễn trong $\Sigma_0$

$m_i$: Khối lượng (Mass / 質量) của Khâu $i$

${}^0\dot{\hat{\boldsymbol{v}}}_i$: Gia tốc tịnh tiến (Translational acceleration / 並進加速度) tại khối tâm của Khâu $i$ biểu diễn trong $\Sigma_0$ $[={}^0\ddot{\boldsymbol{s}}_i]$

${}^0\hat{\boldsymbol{I}}_i$: Tensor quán tính (Inertia tensor / 慣性テンソル) quanh khối tâm của Khâu $i$ biểu diễn trong $\Sigma_0$

Phương trình (5.39) thể hiện Định luật 2 Newton (Newton’s second law / ニュートンの第2法則), phát biểu rằng “Tốc độ biến thiên theo thời gian của Động lượng (Momentum / 運動量) của một vật bằng với lực tác dụng lên vật đó”. Phương trình (5.40) là Phương trình chuyển động Euler (Euler’s equation of motion / オイラーの運動方程式), phát biểu rằng “Sự biến thiên theo thời gian của Mô-men động lượng (Angular momentum / 角運動量) của một vật bằng với mô-men tác dụng lên vật đó”.

Gia tốc tịnh tiến tại khối tâm của Khâu $i$, bằng cách cho tương ứng $\Sigma_A \rightarrow \Sigma_i$ trong công thức (5.29) và cho gốc $\Sigma_B$ tương ứng với khối tâm của Khâu $i$, ta thu được:

$${}^0\dot{\hat{\boldsymbol{v}}}_i = {}^0\dot{\boldsymbol{v}}_i + {}^0\dot{\boldsymbol{\omega}}_i \times ({}^0\boldsymbol{R}_i {}^i\hat{\boldsymbol{s}}_i) + {}^0\boldsymbol{\omega}_i \times \left[ {}^0\boldsymbol{\omega}_i \times ({}^0\boldsymbol{R}_i {}^i\hat{\boldsymbol{s}}_i) \right] \tag{5.41}$$

Như được thể hiện trong Hình 5.6, nếu lực tác dụng từ Khâu $i-1$ lên Khâu $i$ biểu diễn trong hệ toạ độ cơ sở là ${}^0\boldsymbol{f}_i$ và mô-men là ${}^0\boldsymbol{n}_i$, thì lực và mô-men tác dụng lên Khâu $i$ có mối quan hệ như sau:

$${}^0\boldsymbol{F}_i = {}^0\boldsymbol{f}_i – {}^0\boldsymbol{f}_{i+1} \tag{5.42}$$

(Hình 5.6 Lực và mô-men tác dụng lên Khâu $i$)

$${}^0\boldsymbol{N}_i = {}^0\boldsymbol{n}_i – {}^0\boldsymbol{n}_{i+1} – {}^0\hat{\boldsymbol{s}}_i \times {}^0\boldsymbol{f}_i + ({}^0\hat{\boldsymbol{s}}_i – {}^0\hat{\boldsymbol{p}}_{i+1}) \times {}^0\boldsymbol{f}_{i+1} \tag{5.43}$$

Từ công thức trên, ta có:

$${}^0\boldsymbol{f}_i = {}^0\boldsymbol{F}_i + {}^0\boldsymbol{f}_{i+1} \tag{5.44}$$
$${}^0\boldsymbol{n}_i = {}^0\boldsymbol{n}_{i+1} + ({}^0\boldsymbol{R}_i {}^i\hat{\boldsymbol{s}}_i) \times {}^0\boldsymbol{F}_i + ({}^0\boldsymbol{R}_i {}^i\hat{\boldsymbol{p}}_{i+1}) \times {}^0\boldsymbol{f}_{i+1} + {}^0\boldsymbol{N}_i \tag{5.45}$$

Lực dẫn động khớp (Joint driving force / 関節駆動力) $\tau_i$ sẽ cân bằng với thành phần theo trục $\boldsymbol{z}_i$ của $\boldsymbol{n}_i$ khi khớp $i$ là khớp quay, và cân bằng với thành phần trục $\boldsymbol{z}_i$ của $\boldsymbol{f}_i$ khi khớp $i$ là khớp tịnh tiến. Do đó, nó được cho bởi:

$$\tau_i = \begin{cases} ({}^0\boldsymbol{z}_i)^T {}^0\boldsymbol{n}_i \quad (IF \quad R) \\ ({}^0\boldsymbol{z}_i)^T {}^0\boldsymbol{f}_i \quad (IF \quad T) \end{cases} \tag{5.46}$$

Lực cân bằng với các thành phần ngoài phương trục $\boldsymbol{z}_i$ của $\boldsymbol{n}_i$, $\boldsymbol{f}_i$ sẽ phát sinh tự nhiên như lực liên kết. Từ những điều trên, Lực dẫn động khớp $\tau_i$ có thể được tính toán, nhưng vì ${}^0\hat{\boldsymbol{I}}_i$ biến thiên theo chuyển động của Khâu $i$, việc chuyển đổi sang công thức biểu diễn trong $\Sigma_i$ sẽ có lợi hơn về mặt tính toán. Nhằm mục đích đơn giản hóa cách biểu diễn công thức, ta định nghĩa:

$$\rho_i = \begin{cases} 1 \quad (IF \quad R) \\ 0 \quad (IF \quad T) \end{cases} \tag{5.47}$$

và sử dụng hệ thức (xem Phụ lục C):

$${}^i\hat{\boldsymbol{I}}_i = ({}^0\boldsymbol{R}_i)^T {}^0\hat{\boldsymbol{I}}_i {}^0\boldsymbol{R}_i \tag{5.48}$$

từ các công thức (5.35)~(5.46), ta rút ra được các hệ thức sau biểu diễn trong $\Sigma_i$:

$${}^i\boldsymbol{\omega}_i = {}^i\boldsymbol{R}_{i-1} {}^{i-1}\boldsymbol{\omega}_{i-1} + \rho_i \boldsymbol{z}_0 \dot{q}_i \tag{5.49}$$
$${}^i\dot{\boldsymbol{\omega}}_i = {}^i\boldsymbol{R}_{i-1} {}^{i-1}\dot{\boldsymbol{\omega}}_{i-1} + \rho_i \{ \boldsymbol{z}_0 \ddot{q}_i + {}^i\boldsymbol{R}_{i-1} {}^{i-1}\boldsymbol{\omega}_{i-1} \times (\boldsymbol{z}_0 \dot{q}_i) \} \tag{5.50}$$
$${}^i\dot{\boldsymbol{v}}_i = {}^i\boldsymbol{R}_{i-1} [ {}^{i-1}\dot{\boldsymbol{v}}_{i-1} + {}^{i-1}\dot{\boldsymbol{\omega}}_{i-1} \times {}^{i-1}\hat{\boldsymbol{p}}_i + {}^{i-1}\boldsymbol{\omega}_{i-1} \times ({}^{i-1}\boldsymbol{\omega}_{i-1} \times {}^{i-1}\hat{\boldsymbol{p}}_i) ]$$
$$+ (1-\rho_i) \left[ \boldsymbol{z}_0 \ddot{q}_i + 2({}^i\boldsymbol{R}_{i-1} {}^{i-1}\boldsymbol{\omega}_{i-1}) \times (\boldsymbol{z}_0 \dot{q}_i) \right] \tag{5.51}$$
$${}^i\dot{\hat{\boldsymbol{v}}}_i = {}^i\dot{\boldsymbol{v}}_i + {}^i\dot{\boldsymbol{\omega}}_i \times {}^i\hat{\boldsymbol{s}}_i + {}^i\boldsymbol{\omega}_i \times ({}^i\boldsymbol{\omega}_i \times {}^i\hat{\boldsymbol{s}}_i) \tag{5.52}$$
$${}^i\boldsymbol{f}_i = m_i {}^i\dot{\hat{\boldsymbol{v}}}_i + {}^i\boldsymbol{R}_{i+1} {}^{i+1}\boldsymbol{f}_{i+1} \tag{5.53}$$
$${}^i\boldsymbol{n}_i = {}^i\hat{\boldsymbol{I}}_i {}^i\dot{\boldsymbol{\omega}}_i + {}^i\boldsymbol{\omega}_i \times ({}^i\hat{\boldsymbol{I}}_i {}^i\boldsymbol{\omega}_i) + m_i {}^i\hat{\boldsymbol{s}}_i \times {}^i\dot{\hat{\boldsymbol{v}}}_i$$
$$+ {}^i\hat{\boldsymbol{p}}_{i+1} \times {}^i\boldsymbol{R}_{i+1} {}^{i+1}\boldsymbol{f}_{i+1} + {}^i\boldsymbol{R}_{i+1} {}^{i+1}\boldsymbol{n}_{i+1} \tag{5.54}$$
$$\tau_i = \begin{cases} \boldsymbol{z}_0^T {}^i\boldsymbol{n}_i \quad (IF \quad R) \\ \boldsymbol{z}_0^T {}^i\boldsymbol{f}_i \quad (IF \quad T) \end{cases} \tag{5.55}$$

Ở đây, ${}^{n+1}\boldsymbol{f}_{n+1}$ và ${}^{n+1}\boldsymbol{n}_{n+1}$ là lực và mô-men mà Khâu $n$ tác dụng ra bên ngoài, và ${}^{i-1}\hat{\boldsymbol{p}}_i$ như được trình bày trong công thức (4.44):

$${}^{i-1}\hat{\boldsymbol{p}}_i = [ a_i, \, -d_i S\alpha_i, \, d_i C\alpha_i ]^T \tag{5.56}$$

Tensor quán tính ${}^i\hat{\boldsymbol{I}}_i = \{ \hat{I}_{ijk} \} (= {}^i\boldsymbol{R}_0 {}^0\hat{\boldsymbol{I}}_i {}^0\boldsymbol{R}_i)$ là một ma trận hằng số. Thông qua các hệ thức trên, có thể tính toán được Lực dẫn động khớp, nhưng bằng cách điều chỉnh thêm như dưới đây, khối lượng tính toán sẽ giảm bớt.

Giả sử Tensor quán tính của Khâu $i$ tại gốc $\Sigma_i$ là ${}^i\boldsymbol{I}_i = \{ I_{ijk} \}$, thì theo Định lý trục song song (Parallel axis theorem / 平行軸の定理, xem Phụ lục C), ta có hệ thức:

$${}^i\boldsymbol{I}_i = {}^i\hat{\boldsymbol{I}}_i + m_i ( {}^i\hat{\boldsymbol{s}}_i^T {}^i\hat{\boldsymbol{s}}_i \boldsymbol{I}_3 – {}^i\hat{\boldsymbol{s}}_i {}^i\hat{\boldsymbol{s}}_i^T ) \tag{5.57}$$

${}^i\boldsymbol{I}_i$ này có thể được biểu diễn bằng công thức:

$${}^i\boldsymbol{I}_i = \begin{bmatrix} I_{ixx} & -H_{ixy} & -H_{ixz} \\ -H_{ixy} & I_{iyy} & -H_{iyz} \\ -H_{ixz} & -H_{iyz} & I_{izz} \end{bmatrix} \tag{5.58}$$
Trong đó, $I_{ixx}$, $I_{iyy}$, $I_{izz}$ được gọi là Mô-men quán tính (Moment of inertia / 慣性モーメント), $H_{ixy}$, $H_{ixz}$, $H_{iyz}$ được gọi là Tích quán tính (Product of inertia / 慣性乗積). Thế công thức (5.52) vào công thức (5.53) ta được:$${}^i\boldsymbol{f}_i = m_i {}^i\dot{\boldsymbol{v}}_i + {}^i\dot{\boldsymbol{\omega}}_i \times m_i {}^i\hat{\boldsymbol{s}}_i + {}^i\boldsymbol{\omega}_i \times ({}^i\boldsymbol{\omega}_i \times m_i {}^i\hat{\boldsymbol{s}}_i) + {}^i\boldsymbol{R}_{i+1} {}^{i+1}\boldsymbol{f}_{i+1} \tag{5.59}$$và thế công thức (5.52), (5.57) vào công thức (5.54) ta có:$${}^i\boldsymbol{n}_i = {}^i\boldsymbol{I}_i {}^i\dot{\boldsymbol{\omega}}_i + {}^i\boldsymbol{\omega}_i \times ({}^i\boldsymbol{I}_i {}^i\boldsymbol{\omega}_i) + m_i {}^i\hat{\boldsymbol{s}}_i \times {}^i\dot{\boldsymbol{v}}_i$$$$+ {}^i\boldsymbol{R}_{i+1} ({}^{i+1}\hat{\boldsymbol{p}}_{i+1} \times {}^{i+1}\boldsymbol{f}_{i+1} + {}^{i+1}\boldsymbol{n}_{i+1}) \tag{5.60}$$Công thức trên là hệ thức khi sử dụng ${}^i\boldsymbol{I}_i$ thay cho ${}^i\hat{\boldsymbol{I}}_i$. Chú ý rằng, vì ${}^i\hat{\boldsymbol{p}}_i = {}^i\boldsymbol{R}_{i-1} {}^{i-1}\hat{\boldsymbol{p}}_i$ nên:$${}^i\hat{\boldsymbol{p}}_i = [ a_i \cos \theta_i, \, -a_i \sin \theta_i, \, d_i ]^T \tag{5.61}$$

Sử dụng các hệ thức trên, khi cho trước biến khớp $\boldsymbol{q}_i$, vận tốc khớp $\dot{\boldsymbol{q}}_i$, gia tốc khớp $\ddot{\boldsymbol{q}}_i$, ta có thể thực hiện tính toán động lực học ngược để tìm ra mô-men khớp $\boldsymbol{\tau}$ cân bằng với chúng. Lúc này, ngoài các tham số Khâu (Link parameters), ta cần thêm các tham số của từng Khâu là $m_i$, ${}^i\hat{\boldsymbol{s}}_i$, ${}^i\boldsymbol{I}_i$. Những tham số này được gọi là Tham số động lực học (Dynamics parameters / 動力学パラメータ). Thuật toán tính toán thực tế như sau.

Phương pháp tính theo phương pháp Newton-Euler

  • STEP 1 Cho trước giá trị khởi tạo, tham số Khâu và tham số động lực học.

    ${}^0\boldsymbol{\omega}_0 = {}^0\dot{\boldsymbol{\omega}}_0 = \boldsymbol{0}$, ${}^0\dot{\boldsymbol{v}}_0 = -\boldsymbol{g}$ ($\boldsymbol{g}$ là vector gia tốc trọng trường), $m_i$, ${}^i\hat{\boldsymbol{s}}_i$, ${}^i\boldsymbol{I}_i$, ${}^{i-1}\hat{\boldsymbol{p}}_i$, ${}^{i-1}\boldsymbol{R}_i$, $\rho_i$, ${}^{n+1}\boldsymbol{f}_{n+1}$, ${}^{n+1}\boldsymbol{n}_{n+1}$ ($i=1, 2, \cdots, n$)

  • STEP 2 Tính toán thuận ($i=1, 2, \cdots, n$)

    Tính ${}^i\boldsymbol{\omega}_i$, ${}^i\dot{\boldsymbol{\omega}}_i$, ${}^i\dot{\boldsymbol{v}}_i$ bằng các công thức (5.49)~(5.51).

  • STEP 3 Tính toán ngược ($i=n, n-1, \cdots, 1$)

    Tính ${}^i\hat{\boldsymbol{p}}_i$, ${}^i\boldsymbol{f}_i$, ${}^i\boldsymbol{n}_i$, $\tau_i$ bằng các công thức (5.55), (5.59)~(5.61).

Khối lượng tính toán bằng phương pháp Newton-Euler được trình bày ở đây, đối với Robot có tất cả các khớp là khớp quay, sẽ là ($126n-99$) phép nhân và ($106n-92$) phép cộng (khi $n=6$ thì có 657 phép nhân và 544 phép cộng), có thể nói là đạt hiệu suất tính toán cao hơn so với phương pháp Lagrange.

Khi khớp có Lực ma sát (Friction force / 摩擦力) Coulomb và ma sát nhớt tác dụng, công thức (5.55) trở thành:

$$\tau_i = \begin{cases} \boldsymbol{z}_0^T {}^i\boldsymbol{n}_i + D_i \dot{q}_i + fr_i \text{sgn}(\dot{q}_i) \quad (IF \quad R) \\ \boldsymbol{z}_0^T {}^i\boldsymbol{f}_i + D_i \dot{q}_i + fr_i \text{sgn}(\dot{q}_i) \quad (IF \quad T) \end{cases} \tag{5.62}$$

Trong đó, $D_i$ là hệ số ma sát nhớt của khớp $i$, $fr_i$ là lực ma sát Coulomb của khớp $i$. $\text{sgn}$ là hàm dấu được định nghĩa như sau:

$$\text{sgn}(x) = \begin{cases} 1 : x > 0 \\ 0 : x = 0 \\ -1 : x < 0 \end{cases} \tag{5.63}$$

【Bài toán mẫu 5.2】 Hãy tìm phương trình chuyển động của cánh tay Robot 2 khớp trong Bài toán mẫu 5.1 bằng phương pháp Newton-Euler. Lưu ý, dấu $*$ trong các công thức dưới đây biểu thị những phần tử không liên quan đến việc tính mô-men nên không được ghi rõ.

STEP 1

Từ điều kiện cơ cấu:

$${}^0\boldsymbol{R}_1 = \begin{bmatrix} C_1 & -S_1 & 0 \\ S_1 & C_1 & 0 \\ 0 & 0 & 1 \end{bmatrix}, \quad {}^1\boldsymbol{R}_2 = \begin{bmatrix} C_2 & -S_2 & 0 \\ S_2 & C_2 & 0 \\ 0 & 0 & 1 \end{bmatrix},$$
$${}^0\hat{\boldsymbol{p}}_1 = [0, \; 0, \; 0]^T, \quad {}^1\hat{\boldsymbol{p}}_2 = [L_1, \; 0, \; 0]^T,$$
$${}^1\hat{\boldsymbol{s}}_1 = [L_{g1}, \; 0, \; 0]^T, \quad {}^2\hat{\boldsymbol{s}}_2 = [L_{g2}, \; 0, \; 0]^T,$$
$${}^1\boldsymbol{I}_1 = \begin{bmatrix} * & * & * \\ * & * & * \\ * & * & I_1 \end{bmatrix}, \quad {}^2\boldsymbol{I}_2 = \begin{bmatrix} * & * & * \\ * & * & * \\ * & * & I_2 \end{bmatrix}$$

Hơn nữa, vì không có ngoại lực tác dụng lên khâu công tác (End-effector), nên:

$${}^3\boldsymbol{f}_3 = \boldsymbol{0}, \quad {}^3\boldsymbol{n}_3 = \boldsymbol{0}$$

Điều kiện ban đầu được cho bởi hệ thức sau từ mối quan hệ $\boldsymbol{g} = [0 \; -g \; 0]^T$.

$${}^0\boldsymbol{\omega}_0 = {}^0\dot{\boldsymbol{\omega}}_0 = \boldsymbol{0}, \quad {}^0\dot{\boldsymbol{v}}_0 = [0, \; g, \; 0]^T$$

STEP 2

Với $i=1$, từ các công thức (5.49) đến (5.51) ta có:

$${}^1\boldsymbol{\omega}_1 = {}^1\boldsymbol{R}_0 {}^0\boldsymbol{\omega}_0 + \boldsymbol{z}_0 \dot{\theta}_1 = [0, \; 0, \; \dot{\theta}_1]^T$$
$${}^1\dot{\boldsymbol{\omega}}_1 = {}^1\boldsymbol{R}_0 {}^0\dot{\boldsymbol{\omega}}_0 + \boldsymbol{z}_0 \ddot{\theta}_1 = [0, \; 0, \; \ddot{\theta}_1]^T$$
$${}^1\dot{\boldsymbol{v}}_1 = {}^1\boldsymbol{R}_0 \left[ {}^0\dot{\boldsymbol{v}}_0 + {}^0\dot{\boldsymbol{\omega}}_0 \times {}^0\hat{\boldsymbol{p}}_1 + {}^0\boldsymbol{\omega}_0 \times ({}^0\boldsymbol{\omega}_0 \times {}^0\hat{\boldsymbol{p}}_1) \right]$$
$$= [gS_1, \; gC_1, \; 0]^T$$

Tương tự với $i=2$, ta được:

$${}^2\boldsymbol{\omega}_2 = [0, \; 0, \; \dot{\theta}_1 + \dot{\theta}_2]^T$$
$${}^2\dot{\boldsymbol{\omega}}_2 = [0, \; 0, \; \ddot{\theta}_1 + \ddot{\theta}_2]^T$$
$${}^2\dot{\boldsymbol{v}}_2 = \begin{bmatrix} gS_{12} + L_1(-\dot{\theta}_1^2 C_2 + \ddot{\theta}_1 S_2) \\ gC_{12} + L_1(\dot{\theta}_1^2 S_2 + \ddot{\theta}_1 C_2) \\ 0 \end{bmatrix}$$

STEP 3

Với $i=2$, từ các công thức (5.55), (5.59), (5.60) ta có:

$${}^2\boldsymbol{f}_2 = m_2 {}^2\dot{\boldsymbol{v}}_2 + {}^2\dot{\boldsymbol{\omega}}_2 \times (m_2 {}^2\hat{\boldsymbol{s}}_2) + {}^2\boldsymbol{\omega}_2 \times ({}^2\boldsymbol{\omega}_2 \times m_2 {}^2\hat{\boldsymbol{s}}_2) + {}^2\boldsymbol{R}_3 {}^3\boldsymbol{f}_3$$
$$= m_2 \begin{bmatrix} gS_{12} + L_1(-\dot{\theta}_1^2 C_2 + \ddot{\theta}_1 S_2) – L_{g2}(\dot{\theta}_1 + \dot{\theta}_2)^2 \\ gC_{12} + L_1(\dot{\theta}_1^2 S_2 + \ddot{\theta}_1 C_2) + L_{g2}(\ddot{\theta}_1 + \ddot{\theta}_2) \\ 0 \end{bmatrix}$$
$${}^2\boldsymbol{n}_2 = {}^2\boldsymbol{I}_2 {}^2\dot{\boldsymbol{\omega}}_2 + {}^2\boldsymbol{\omega}_2 \times ({}^2\boldsymbol{I}_2 {}^2\boldsymbol{\omega}_2) + m_2 {}^2\hat{\boldsymbol{s}}_2 \times {}^2\dot{\boldsymbol{v}}_2 + {}^2\boldsymbol{R}_3 ({}^3\hat{\boldsymbol{p}}_3 \times {}^3\boldsymbol{f}_3 + {}^3\boldsymbol{n}_3)$$
$$= [*, *, I_2(\ddot{\theta}_1 + \ddot{\theta}_2) + m_2 L_{g2} \{ gC_{12} + L_1(\dot{\theta}_1^2 S_2 + \ddot{\theta}_1 C_2) \} ]^T$$
$$\tau_2 = [0, 0, 1] {}^2\boldsymbol{n}_2$$

Tương tự, với $i=1$:

$${}^2\hat{\boldsymbol{p}}_2 = [L_1 C_2, \; -L_1 S_2, \; 0]^T$$
$${}^1\boldsymbol{n}_1 = [*, *, (I_1 + m_2 L_1^2 + 2m_2 L_1 L_{g2} C_2 + I_2)\ddot{\theta}_1 + (m_2 L_1 L_{g2} C_2 + I_2)\ddot{\theta}_2 – m_2 L_1 L_{g2} S_2 (2\dot{\theta}_1 \dot{\theta}_2 + \dot{\theta}_2^2) + g\{ (m_1 L_{g1} + m_2 L_1)C_1 + m_2 L_{g2} C_{12} \} ]^T$$
$$\tau_1 = [0, \; 0, \; 1] {}^1\boldsymbol{n}_1$$

Rút gọn $\tau_2$, $\tau_1$, ta thấy chúng trùng khớp với phương trình chuyển động thu được bằng phương pháp Lagrange trong Bài toán mẫu 5.1.

≫ 5.4 Bài toán động lực học thuận (Forward dynamics problem / 順動力学問題)

Bài toán động lực học thuận là bài toán tìm xem Robot sẽ thực hiện chuyển động như thế nào khi biết trạng thái ban đầu $[\boldsymbol{q}(0), \dot{\boldsymbol{q}}(0)]$ và mô-men truyền động $\boldsymbol{\tau}(t)$, đây là bài toán thường gặp trong mô phỏng máy tính. Bài toán này biến đổi phương trình chuyển động của Robot trong công thức (5.14):

$$\boldsymbol{M}(\boldsymbol{q})\ddot{\boldsymbol{q}} + \boldsymbol{h}(\boldsymbol{q}, \dot{\boldsymbol{q}}) + \boldsymbol{g}(\boldsymbol{q}) = \boldsymbol{\tau} \tag{5.64}$$

thành

$$\frac{d\boldsymbol{q}(t)}{dt} = \dot{\boldsymbol{q}}(t) \tag{5.65a}$$
$$\frac{d\dot{\boldsymbol{q}}(t)}{dt} = \boldsymbol{M}(\boldsymbol{q})^{-1}(\boldsymbol{\tau} – \boldsymbol{h}(\boldsymbol{q}, \dot{\boldsymbol{q}}) – \boldsymbol{g}(\boldsymbol{q})) \tag{5.65b}$$

và chỉ cần cung cấp điều kiện ban đầu cùng với đầu vào, rồi thực hiện giải tích số bằng Phương pháp Runge-Kutta (Runge-Kutta method / ルンゲ・クッタ法). Tuy nhiên, lúc này đòi hỏi phải tính toán $\boldsymbol{M}(\boldsymbol{q})$ và $\boldsymbol{h}(\boldsymbol{q}, \dot{\boldsymbol{q}}) + \boldsymbol{g}(\boldsymbol{q})$ một cách hiệu quả. Trong việc tính toán này, có thể áp dụng phương pháp Newton-Euler ở phần trước.

Đưa phương pháp tính toán Lực dẫn động khớp bằng phương pháp Newton-Euler vào thành một chương trình con (subroutine) và đặt là $\boldsymbol{\tau} = \text{JOTR}(\boldsymbol{q}, \dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}})$. Ở đây, đặt:

$$\boldsymbol{A}(t) = \boldsymbol{M}(\boldsymbol{q}) \tag{5.66a}$$
$$\boldsymbol{b}(t) = \boldsymbol{h}(\boldsymbol{q}, \dot{\boldsymbol{q}}) + \boldsymbol{g}(\boldsymbol{q}) \tag{5.66b}$$

thì từ công thức (5.64) ta có:

$$\boldsymbol{A}(t)\ddot{\boldsymbol{q}} + \boldsymbol{b}(t) = \boldsymbol{\tau}(t) \tag{5.67}$$

do đó, $\boldsymbol{A}(t) = [\boldsymbol{a}_1, \boldsymbol{a}_2, \cdots, \boldsymbol{a}_n]$ và $\boldsymbol{b}(t)$ được tìm bằng các bước tính toán sau. Trong đó, $\boldsymbol{a}_i$ là vector cột thứ $i$ của $\boldsymbol{A}(t)$.

Phương pháp tính $\boldsymbol{A}$, $\boldsymbol{b}$

  • STEP 1 Đặt $\ddot{\boldsymbol{q}} = \boldsymbol{0}$, tính $\boldsymbol{b}$ bằng $\boldsymbol{b} = \text{JOTR}(\boldsymbol{q}, \dot{\boldsymbol{q}}, \boldsymbol{0})$.

  • STEP 2 Đặt $i = 1$.

  • STEP 3 Đặt $\ddot{\boldsymbol{q}} = \boldsymbol{e}_i$, tính $\boldsymbol{a}_i$ bằng $\boldsymbol{a}_i = \text{JOTR}(\boldsymbol{q}, \dot{\boldsymbol{q}}, \boldsymbol{e}_i) – \boldsymbol{b}$. Trong đó, $\boldsymbol{e}_i$ là vector đơn vị có kích thước $n \times 1$ với phần tử thứ $i$ bằng 1 và các phần tử khác bằng 0.

  • STEP 4 Nếu $i \ge n$ thì kết thúc, nếu không đặt $i = i + 1$ và quay lại Bước 3.

Trong phương pháp tính này, bằng việc gọi JOTR $(n+1)$ lần, ta có thể tìm được $\boldsymbol{A}$ và $\boldsymbol{b}$.

【Bài tập thực hành】

5.1 Tìm phương trình chuyển động của cánh tay 2 khớp dạng quay – tịnh tiến như trong Hình 5.7 bằng phương pháp Lagrange. Giả sử vector trọng trường hướng theo chiều âm trục $\boldsymbol{y}_0$.

(Hình 5.7 Cánh tay 2 khớp quay – tịnh tiến)

5.2 Tìm phương trình chuyển động của cánh tay 2 khớp dạng quay – tịnh tiến như trong Hình 5.7 bằng phương pháp Newton-Euler. Giả sử vector trọng trường hướng theo chiều âm trục $\boldsymbol{y}_0$.

5.3 Hãy chứng minh rằng Động năng của một vật chuyển động trong không gian gồm tịnh tiến và quay được cho bởi

$$K = \frac{1}{2}m\boldsymbol{v}^T\boldsymbol{v} + \frac{1}{2}\boldsymbol{\omega}^T\hat{\boldsymbol{I}}\boldsymbol{\omega}$$

Trong đó, $m$ là Khối lượng của vật, $\boldsymbol{v}$ là vector vận tốc của khối tâm, $\boldsymbol{\omega}$ là vector vận tốc góc, $\hat{\boldsymbol{I}}$ là Tensor quán tính tại khối tâm.

5.4 Tìm Tensor quán tính quanh khối tâm của một khối trụ có chiều dài $h$, bán kính $r$, mật độ $\rho$. Giả sử trục dọc là trục $\boldsymbol{z}$.

5.5 Tìm phương trình chuyển động của cánh tay 3 khớp như trong Hình 5.8 bằng phương pháp Newton-Euler dưới dạng hệ thức truy hồi. Giả sử vector trọng trường trùng với trục $\boldsymbol{z}_0$.

(Hình 5.8 Cánh tay 3 khớp)

KIẾN THỨC CƠ BẢN VỀ KỸ THUẬT ROBOT - Previous 4. Cơ cấu và Động học (Kinematics / 運動学) của Cánh tay Robot 2 Next - KIẾN THỨC CƠ BẢN VỀ KỸ THUẬT ROBOT 6. Phân tích sai số và nhận dạng tham số (Error Analysis and Parameter Identification / 誤差解析とパラメータ同定)

Leave a Comment

Chia sẻ:

5. Động lực học (Dynamics / 動力学) của cánh tay Robot

Or copy link

CONTENTS