7. Điều khiển Vị trí / Quỹ đạo của Robot (Robot Position/Trajectory Control / ロボットの位置／軌道制御)

Estimated reading: 77 minutes 45 views

Chuyển động của Robot được thực hiện bằng cách cấp cho nó một quỹ đạo mục tiêu tương ứng với chuyển động, sau đó điều khiển sao cho Robot di chuyển bám theo quỹ đạo mục tiêu đó. Trong chương này, chúng ta sẽ trình bày cách tạo ra quỹ đạo mục tiêu nối liền điểm khởi đầu và điểm kết thúc khi đã biết vị trí của Khâu tác động cuối (End-effector / 手先) hoặc vị trí của các khớp. Tiếp theo, chúng ta sẽ xem xét các phương pháp điều khiển vị trí và quỹ đạo khác nhau để Robot chuyển động dọc theo quỹ đạo mục tiêu đó. Các phương pháp điều khiển được đề cập ở đây bao gồm những phương pháp đang được ứng dụng trên các Robot công nghiệp hiện tại và những phương pháp được dự đoán sẽ được áp dụng trong tương lai.

7.1 Tạo quỹ đạo (Trajectory generation / 軌道生成)

Khi Robot thực hiện công việc vận chuyển đồ vật, ta lập kế hoạch chuyển động theo các bước như Hình 7.1: kẹp vật (vị trí $\text{P}_1$), nâng lên ($\text{P}_2$), di chuyển ($\text{P}_3$), và đặt xuống bàn thao tác ($\text{P}_4$). Lúc này, việc chỉ định thời gian và tốc độ di chuyển tại mỗi điểm là cần thiết. Quá trình lập kế hoạch cho các điểm đi qua và tốc độ đi qua như vậy được gọi là Lập kế hoạch quỹ đạo (Trajectory planning / 軌道計画). Việc tạo ra một quỹ đạo nối liền các điểm đã được lập kế hoạch một cách trơn tru được gọi là Tạo quỹ đạo (Trajectory generation / 軌道生成) hoặc Nội suy quỹ đạo (Trajectory interpolation / 軌道補間). Quỹ đạo được lập kế hoạch có thể được cho bởi Biến khớp (Joint variable / 関節変位) $\boldsymbol{q}$ hoặc vị trí – tư thế Khâu tác động cuối $\boldsymbol{r}$ trong Hệ tọa độ làm việc (Workspace coordinate system / 作業座標). Nói chung, khi thực hiện công việc bằng phương pháp dạy-phát lại (Teach and playback / 教示再生方式) mà không có sự bù trừ bằng Cảm biến ngoại suy (External sensor / 外界センサ), quỹ đạo được cho bởi Biến khớp $\boldsymbol{q}$ (trường hợp trước); còn khi thực hiện công việc có sử dụng Cảm biến ngoại suy, quỹ đạo sẽ được cho bởi vị trí – tư thế Khâu tác động cuối $\boldsymbol{r}$ (trường hợp sau).

Hình 7.1: Công việc vận chuyển bằng Robot

≫ 7.1.1 Tạo quỹ đạo bằng Biến khớp

(1) Quỹ đạo khi cho trước điểm đầu và điểm cuối

Giả sử ta có điểm đầu $\boldsymbol{q}_0$ và điểm cuối $\boldsymbol{q}_f$, ta sẽ tạo quỹ đạo bằng cách nội suy khoảng giữa hai điểm này bằng một đa thức theo thời gian. Khi đó, nếu gọi một phần tử bất kỳ của Biến khớp là $y$, ta có thể biểu diễn nó như sau:

$$y(t) = a_0 + a_1 t + \cdots + a_n t^n \tag{7.1}$$

Ở đây, tại điểm đầu lúc thời gian $t = 0$ ta có $y(0) = y_0$. Tại điểm cuối lúc thời gian $t_f$ thỏa mãn $y(t_f) = y_f$. Để xác định duy nhất các hệ số $a_i$ ($i = 0, \cdots, n$), ta cần $(n+1)$ điều kiện biên. Ngược lại, bậc của đa thức được quyết định bởi số lượng điều kiện biên. Từ đó, ta rút ra các phương pháp dưới đây.

1) Nội suy đa thức bậc 1

Nếu ta quyết định các hệ số của đa thức sao cho thỏa mãn vị trí của điểm đầu và điểm cuối, số lượng điều kiện biên là 2, nên bậc của đa thức sẽ là 1. Nghĩa là, các điều kiện biên được cho bởi:

$$y(0) = a_0 = y_0 \tag{7.2a}$$
$$y(t_f) = a_0 + a_1 t_f = y_f \tag{7.2b}$$

Từ đó ta có:

$$y(t) = y_0 + \frac{(y_f – y_0)t}{t_f} \tag{7.3}$$

Phép nội suy bằng công thức tính toán này được gọi là Nội suy đa thức bậc 1 hoặc Nội suy tuyến tính (Linear interpolation / 線形補間). Phương pháp này thiếu tính trơn tru của quỹ đạo do không có tính liên tục về Vận tốc (Velocity / 速度) và Gia tốc (Acceleration / 加速度) tại các ranh giới, nhưng lại là phương pháp nội suy đòi hỏi khối lượng tính toán ít nhất.

2) Nội suy đa thức bậc 3

Khi xác định đa thức từ các điều kiện về vị trí và vận tốc tại điểm đầu và điểm cuối, số lượng điều kiện biên là 4, nên bậc của đa thức sẽ là 3. Tức là:

$$y(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 \tag{7.4}$$

Lúc này, các điều kiện biên là:

$$y(0) = a_0 = y_0 \tag{7.5a}$$
$$\dot{y}(0) = a_1 = \dot{y}_0 \tag{7.5b}$$
$$y(t_f) = a_0 + a_1 t_f + a_2 t_f^2 + a_3 t_f^3 = y_f \tag{7.5c}$$
$$\dot{y}(t_f) = a_1 + 2a_2 t_f + 3a_3 t_f^2 = \dot{y}_f \tag{7.5d}$$

Giải hệ này ta được:

$$a_0 = y_0 \tag{7.6a}$$
$$a_1 = \dot{y}_0 \tag{7.6b}$$
$$a_2 = \frac{1}{t_f^2} [3(y_f – y_0) – (\dot{y}_f + 2\dot{y}_0)t_f] \tag{7.6c}$$
$$a_3 = \frac{1}{t_f^3} [-2(y_f – y_0) + (\dot{y}_f + \dot{y}_0)t_f] \tag{7.6d}$$

Phép nội suy bằng công thức (7.4) được gọi là Nội suy đa thức bậc 3 (Cubic polynomial interpolation / 3次多項式補間). Phương pháp này duy trì được tính liên tục của vận tốc, nhưng đổi lại khối lượng tính toán sẽ tăng lên so với nội suy tuyến tính.

3) Nội suy đa thức bậc 5

Khi xác định đa thức từ các điều kiện về vị trí, vận tốc và gia tốc tại điểm đầu và điểm cuối, số lượng điều kiện biên là 6, do đó bậc của đa thức sẽ là 5. Nghĩa là:

$$y(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5 \tag{7.7}$$

Lúc này, các điều kiện biên là:

$$y(0) = a_0 = y_0 \tag{7.8a}$$
$$\dot{y}(0) = a_1 = \dot{y}_0 \tag{7.8b}$$
$$\ddot{y}(0) = 2a_2 = \ddot{y}_0 \tag{7.8c}$$
$$y(t_f) = a_0 + a_1 t_f + a_2 t_f^2 + a_3 t_f^3 + a_4 t_f^4 + a_5 t_f^5 = y_f \tag{7.8d}$$
$$\dot{y}(t_f) = a_1 + 2a_2 t_f + 3a_3 t_f^2 + 4a_4 t_f^3 + 5a_5 t_f^4 = \dot{y}_f \tag{7.8e}$$
$$\ddot{y}(t_f) = 2a_2 + 6a_3 t_f + 12a_4 t_f^2 + 20a_5 t_f^3 = \ddot{y}_f \tag{7.8f}$$

Từ đó ta có:

$$a_0 = y_0 \tag{7.9a}$$
$$a_1 = \dot{y}_0 \tag{7.9b}$$
$$a_2 = \frac{1}{2}\ddot{y}_0 \tag{7.9c}$$
$$a_3 = \frac{1}{2t_f^3} [20(y_f – y_0) – (8\dot{y}_f + 12\dot{y}_0)t_f + (\ddot{y}_f – 3\ddot{y}_0)t_f^2] \tag{7.9d}$$
$$a_4 = \frac{1}{2t_f^4} [-30(y_f – y_0) + (14\dot{y}_f + 16\dot{y}_0)t_f – (2\ddot{y}_f – 3\ddot{y}_0)t_f^2] \tag{7.9e}$$
$$a_5 = \frac{1}{2t_f^5} [12(y_f – y_0) – 6(\dot{y}_f + \dot{y}_0)t_f + (\ddot{y}_f – \ddot{y}_0)t_f^2] \tag{7.9f}$$

Phép nội suy bằng công thức (7.7) được gọi là Nội suy đa thức bậc 5 (5th order polynomial interpolation / 5次多項式補間). Như vậy, bậc của đa thức càng cao thì càng thỏa mãn nhiều điều kiện biên về vị trí, vận tốc và gia tốc, nhưng khối lượng tính toán cũng sẽ tăng lên nhiều so với nội suy bậc 1.

4) Quỹ đạo từ điểm đầu đi qua quá trình tăng tốc, đẳng tốc, giảm tốc rồi đến điểm cuối

Thiết lập các điểm trung gian phụ $y_1, y_2$ nằm giữa điểm đầu tĩnh $y_0$ và điểm cuối tĩnh $y_f$. Xét quỹ đạo đi qua từng điểm lần lượt tại các thời điểm $0, t_1, t_f – t_1, t_f$, trong đó đoạn $[y_1, y_2]$ di chuyển với vận tốc không đổi (đẳng tốc). Một phương pháp để tạo ra quỹ đạo như vậy là sử dụng cách biểu diễn bằng Đa thức bậc 4-1-4 (4-1-4 polynomial / 4-1-4次多項式). Quỹ đạo trên các đoạn $[y_0, y_1]$, $[y_1, y_2]$, $[y_2, y_f]$ lần lượt được biểu diễn bằng các công thức sau:

$$y(t) = a_{10} + a_{11}t + a_{12}t^2 + a_{13}t^3 + a_{14}t^4 \tag{7.10a}$$
$$y(t) = a_{20} + a_{21}(t – t_1) \tag{7.10b}$$
$$y(t) = a_{30} + a_{31}(t – t_f) + a_{32}(t – t_f)^2 + a_{33}(t – t_f)^3 + a_{34}(t – t_f)^4 \tag{7.10c}$$

Đường cong vận tốc lúc này sẽ có dạng như Hình 7.2. Các hệ số của đa thức được xác định từ điều kiện về vị trí, vận tốc, gia tốc tại điểm đầu và điểm cuối, cùng với điều kiện liên tục về vị trí, vận tốc, gia tốc tại các điểm trung gian phụ. Cụ thể, từ điều kiện về vị trí, vận tốc, gia tốc tại điểm đầu tĩnh:

$$a_{10} = y_0 \tag{7.11a}$$
$$a_{11} = 0 \tag{7.11b}$$
$$a_{12} = 0 \tag{7.11c}$$

Tương tự, từ điều kiện về vị trí, vận tốc, gia tốc tại điểm cuối tĩnh:

$$a_{30} = y_f \tag{7.11d}$$
$$a_{31} = 0 \tag{7.11e}$$
$$a_{32} = 0 \tag{7.11f}$$

Hình 7.2: Đường cong vận tốc

Từ điều kiện liên tục về vị trí, vận tốc, gia tốc tại điểm trung gian phụ $y_1$ ở thời điểm $t_1$:

$$a_{10} + a_{13}t_1^3 + a_{14}t_1^4 = a_{20} \tag{7.11g}$$
$$3a_{13}t_1^2 + 4a_{14}t_1^3 = a_{21} \tag{7.11h}$$
$$6a_{13}t_1 + 12a_{14}t_1^2 = 0 \tag{7.11i}$$

Từ điều kiện liên tục về vị trí, vận tốc, gia tốc tại điểm trung gian phụ $y_2$ ở thời điểm $t_f – t_1$:

$$a_{20} + a_{21}(t_f – 2t_1) = a_{30} – a_{33}t_1^3 + a_{34}t_1^4 \tag{7.11j}$$
$$a_{21} = 3a_{33}t_1^2 – 4a_{34}t_1^3 \tag{7.11k}$$
$$0 = -6a_{33}t_1 + 12a_{34}t_1^2 \tag{7.11l}$$

Giải hệ phương trình này, ta nhận được:

$$\left. \begin{aligned} a_{10} &= y_0, \quad a_{11} = a_{12} = 0, \quad a_{13} = 2t_1 k, \quad a_{14} = -k \\ a_{20} &= y_f – t_1^3(2t_f – 3t_1)k, \quad a_{21} = 2t_1^3 k \\ a_{30} &= y_f, \quad a_{31} = a_{32} = 0, \quad a_{33} = 2t_1 k, \quad a_{34} = k \end{aligned} \right\} \tag{7.12}$$

Trong đó:

$$k = \frac{y_f – y_0}{2t_1^3(t_f – t_1)} \tag{7.13}$$

Lưu ý rằng vận tốc trong đoạn $[y_1, y_2]$ là:

$$v = a_{21} = \frac{y_f – y_0}{t_f – t_1} \tag{7.14}$$

Phương pháp này không chỉ định rõ ràng các điểm trung gian phụ $y_1, y_2$, nhưng yêu cầu phải chỉ định thời gian đi qua các điểm đó là $t_1, t_f – t_1$. Thông qua $t_1$, ta có thể chỉ định được Vận tốc tối đa (Maximum velocity / 最高速度).

(2) Khi cho trước điểm đầu, điểm trung gian và điểm cuối

Xem xét một quỹ đạo trơn tru đi qua các điểm $y_0, y_1, y_2, \cdots, y_n$ lần lượt tại các thời điểm $t_0, t_1, t_2, \cdots, t_n$ như Hình 7.3, sao cho vận tốc và gia tốc liên tục tại mỗi điểm trung gian. Một phương pháp để tạo quỹ đạo như vậy là dùng Đa thức bậc 4-3-4 (4-3-4 polynomial method / 4-3-4 次多項式法), trong đó đoạn $[t_0, t_1]$ và đoạn $[t_{n-1}, t_n]$ được biểu diễn bằng đa thức bậc 4 theo thời gian, còn các đoạn khác được biểu diễn bằng đa thức bậc 3, và các hệ số của các đa thức đó được xác định dựa trên điều kiện liên tục về vị trí, vận tốc, gia tốc tại mỗi điểm trung gian. Với $n$ đoạn, hai đoạn ở hai đầu là đa thức bậc 4, và $(n-2)$ đoạn còn lại là đa thức bậc 3, tổng số hệ số sẽ là:

$$5 \times 2 + 4 \times (n – 2) = 4n + 2$$

Mặt khác, nếu ta chỉ định vị trí, vận tốc, gia tốc tại điểm đầu và điểm cuối, đồng thời đưa ra điều kiện liên tục về chỉ định vị trí, vận tốc, gia tốc tại các điểm trung gian:

Hình 7.3: Tạo quỹ đạo bằng đa thức bậc 4-3-4

Ta sẽ có số lượng điều kiện biên là:

$$3 \times 2 + 2(n – 1) + 2(n – 1) = 4n + 2$$

Số lượng này hoàn toàn khớp với số lượng hệ số ở trên. Do đó, các hệ số của đa thức có thể được xác định một cách duy nhất.

Phương pháp này có thể tạo ra một quỹ đạo trơn tru đi qua các điểm khi ta chỉ định điểm trung gian và thời gian đi qua, nhưng nếu có nhiều điểm trung gian, số lượng phương trình trong hệ phương trình đồng thời (Simultaneous equations / 連立方程式) sẽ tăng lên, dẫn đến khối lượng tính toán có xu hướng gia tăng đáng kể.

【Bài toán mẫu 7.1】 Hãy tìm quỹ đạo xuất phát từ điểm tĩnh $y_0$, đi qua $y_1, y_2$ và dừng lại ở $y_3$, lần lượt đi qua các điểm đó tại thời điểm $0, 1, 2, 3$ bằng phương pháp đa thức bậc 4-3-4.

Trong đoạn $[t_0, t_1]$ quỹ đạo được biểu diễn là:

$$y(t) = a_{10} + a_{11}t + a_{12}t^2 + a_{13}t^3 + a_{14}t^4$$

Trong đoạn $[1, 2]$:

$$y(t) = a_{20} + a_{21}(t – 1) + a_{22}(t – 1)^2 + a_{23}(t – 1)^3$$

Trong đoạn $[2, 3]$:

$$y(t) = a_{30} + a_{31}(t – 2) + a_{32}(t – 2)^2 + a_{33}(t – 2)^3 + a_{34}(t – 2)^4$$

Từ điều kiện về vị trí:

$$a_{10} = y_0$$
$$a_{10} + a_{11} + a_{12} + a_{13} + a_{14} = a_{20} = y_1$$
$$a_{20} + a_{21} + a_{22} + a_{23} = a_{30} = y_2$$
$$a_{30} + a_{31} + a_{32} + a_{33} + a_{34} = y_3$$

Từ điều kiện về vận tốc:

$$a_{11} = 0$$
$$2a_{12} + 3a_{13} + 4a_{14} = a_{21}$$
$$a_{21} + 2a_{22} + 3a_{23} = a_{31}$$
$$a_{31} + 2a_{32} + 3a_{33} + 4a_{34} = 0$$

Từ điều kiện về gia tốc:

$$2a_{12} = 0$$
$$6a_{13} + 12a_{14} = 2a_{22}$$
$$2a_{22} + 6a_{23} = 2a_{32}$$
$$2a_{32} + 6a_{33} + 12a_{34} = 0$$

Bằng cách giải hệ phương trình đồng thời (Simultaneous equations / 連立方程式) này, ta có thể tạo ra được quỹ đạo.

≫ 7.1.2 Tạo quỹ đạo bằng biến vị trí – tư thế Khâu tác động cuối

Nếu ta tìm quỹ đạo giữa điểm đầu $\boldsymbol{r}_0$ và điểm cuối $\boldsymbol{r}_f$ được biểu diễn bằng vị trí – tư thế Khâu tác động cuối (End-effector position and orientation / 手先位置姿勢) thông qua phép nội suy (Interpolation / 補間) giữa $\boldsymbol{q}_0$ và $\boldsymbol{q}_f$, có thể xảy ra trường hợp quỹ đạo của Khâu tác động cuối khó dự đoán. Hơn nữa, khi muốn Khâu tác động cuối di chuyển theo đường thẳng trong Hệ tọa độ cơ sở (Base coordinate system / 基準座標系), bắt buộc phải nội suy trực tiếp quỹ đạo giữa $\boldsymbol{r}_0$ và $\boldsymbol{r}_f$. Trong những trường hợp này, bằng cách gọi một biến tùy ý của $\boldsymbol{r}$ là $y$, ta có thể sử dụng các phương pháp khác nhau đã được trình bày ở Mục 7.1.1.

【Bài toán mẫu 7.2】 Xét cánh tay 2 khớp như Hình 5.2, di chuyển từ điểm đầu $\boldsymbol{r}_0 = [0, 1]^T$ đến điểm cuối $\boldsymbol{r}_f = [1, 0]^T$ với thời gian $t_f = 1$. Hãy thử so sánh giữa quỹ đạo được nội suy tuyến tính bằng Biến khớp (Joint variable / 関節変数) và quỹ đạo được nội suy đường thẳng bằng vị trí Khâu tác động cuối.

Hình 7.4: Quỹ đạo nội suy của cánh tay 2 khớp

Biến khớp tại điểm đầu và điểm cuối tĩnh lần lượt là $\boldsymbol{q}_0 = [\pi/2, 0]^T$, $\boldsymbol{q}_f = [0, 0]^T$. Phép nội suy tuyến tính bằng Biến khớp theo công thức (7.3) sẽ là:

$$\boldsymbol{q} = \begin{bmatrix} \frac{\pi}{2}(1-t) \\ 0 \end{bmatrix} \tag{7.15}$$

Mặt khác, nội suy đường thẳng bằng biến vị trí Khâu tác động cuối là:

$$\boldsymbol{r} = \begin{bmatrix} t \\ 1-t \end{bmatrix} \tag{7.16}$$

Các quỹ đạo này được thể hiện trong Hình 7.4. Rõ ràng, quỹ đạo được nội suy tuyến tính bằng Biến khớp và quỹ đạo được nội suy đường thẳng bằng vị trí Khâu tác động cuối là khác nhau.

Khi nội suy bằng biến của Khâu tác động cuối, Tư thế (Orientation / 姿勢) của Khâu tác động cuối được cho bởi Góc Euler (Euler angles / オイラー角) sẽ được nội suy đối với các biến này. Tuy nhiên, chuyển động tư thế của Khâu tác động cuối trong Hệ tọa độ cơ sở là sự tổng hợp dưới dạng Góc Euler, nên rất khó để nắm bắt một cách trực quan. Do đó, chúng ta sẽ trình bày Phương pháp quay 1 trục (One-axis rotation method / 1軸回転法), đây là một chuyển động không gian dễ hiểu về mặt trực quan.

Hình 7.5: Phép quay quanh trục $\boldsymbol{k}$

Đầu tiên, để chuẩn bị, ta xem xét phép quay quanh một trục tùy ý. Như Hình 7.5 thể hiện, ta quay vector $\boldsymbol{p}$ quanh vector đơn vị $\boldsymbol{k}$ và tìm vector $\boldsymbol{p}^*$ sau khi quay. Để làm điều này, ta đưa vào hai vector đơn vị $\boldsymbol{i}$, $\boldsymbol{j}$ trực giao với $\boldsymbol{k}$ và vuông góc với nhau. Các thành phần theo 3 hướng trục $\boldsymbol{i}$, $\boldsymbol{j}$, $\boldsymbol{k}$ của $\boldsymbol{p}$ lần lượt là $\boldsymbol{p}^T\boldsymbol{i}$, $\boldsymbol{p}^T\boldsymbol{j}$, $\boldsymbol{p}^T\boldsymbol{k}$, do đó $\boldsymbol{p}$ có thể được biểu diễn như sau:

$$\boldsymbol{p} = (\boldsymbol{p}^T\boldsymbol{i})\boldsymbol{i} + (\boldsymbol{p}^T\boldsymbol{j})\boldsymbol{j} + (\boldsymbol{p}^T\boldsymbol{k})\boldsymbol{k} \tag{7.17}$$

Khi quay góc $\alpha$ quanh trục $\boldsymbol{k}$, $\boldsymbol{i}$ biến đổi thành $\boldsymbol{i}^* = (\boldsymbol{i}\cos\alpha + \boldsymbol{j}\sin\alpha)$, $\boldsymbol{j}$ biến đổi thành $\boldsymbol{j}^* = (-\boldsymbol{i}\sin\alpha + \boldsymbol{j}\cos\alpha)$. Thành phần theo trục $\boldsymbol{i}^*$ và $\boldsymbol{j}^*$ của $\boldsymbol{p}^*$ sau khi quay vẫn là $\boldsymbol{p}^T\boldsymbol{i}$ và $\boldsymbol{p}^T\boldsymbol{j}$, do đó:

$$\boldsymbol{p}^* = (\boldsymbol{p}^T\boldsymbol{i})(\boldsymbol{i}\cos\alpha + \boldsymbol{j}\sin\alpha) + (\boldsymbol{p}^T\boldsymbol{j})(-\boldsymbol{i}\sin\alpha + \boldsymbol{j}\cos\alpha) + (\boldsymbol{p}^T\boldsymbol{k})\boldsymbol{k} \tag{7.18}$$

Sử dụng hệ thức $(\boldsymbol{p}^T\boldsymbol{i})\boldsymbol{j} – (\boldsymbol{p}^T\boldsymbol{j})\boldsymbol{i} = (\boldsymbol{i} \times \boldsymbol{j}) \times \boldsymbol{p} = \boldsymbol{k} \times \boldsymbol{p}$ (xem Phụ lục B), ta có:

$$\boldsymbol{p}^* = \boldsymbol{p}\cos\alpha + (\boldsymbol{p}^T\boldsymbol{k})\boldsymbol{k}(1-\cos\alpha) + (\boldsymbol{k} \times \boldsymbol{p})\sin\alpha \tag{7.19}$$

Nếu chọn $\boldsymbol{p}$ là vector đơn vị $\boldsymbol{x} = [1, 0, 0]^T$ của hệ tọa độ $\Sigma = \{\boldsymbol{x}, \boldsymbol{y}, \boldsymbol{z}\}$, thì $\boldsymbol{p}^*$ sẽ trở thành vector đơn vị tọa độ $\boldsymbol{x}^*$ của hệ tọa độ sau khi quay $\Sigma^* = \{\boldsymbol{x}^*, \boldsymbol{y}^*, \boldsymbol{z}^*\}$. Từ đó ta được:

$$\boldsymbol{x}^* = \boldsymbol{x}\cos\alpha + (\boldsymbol{x}^T\boldsymbol{k})\boldsymbol{k}(1-\cos\alpha) + (\boldsymbol{k} \times \boldsymbol{x})\sin\alpha \tag{7.20}$$

Ở đây, $\boldsymbol{k} = [k_x, k_y, k_z]^T$ được biểu diễn là:

$$\boldsymbol{k} = k_x \boldsymbol{x} + k_y \boldsymbol{y} + k_z \boldsymbol{z} \tag{7.21}$$

Nên ta có:

$$\boldsymbol{x}^* = (k_x^2(1-\text{C}\alpha) + \text{C}\alpha)\boldsymbol{x} + (k_xk_y(1-\text{C}\alpha) + k_z\text{S}\alpha)\boldsymbol{y} + (k_xk_z(1-\text{C}\alpha) – k_y\text{S}\alpha)\boldsymbol{z} \tag{7.22}$$

Các vector đơn vị trục tọa độ khác cũng thu được hệ thức tương tự. Do đó, nếu gọi Ma trận quay (Rotation matrix / 回転行列) thực hiện phép quay góc $\alpha$ quanh trục $\boldsymbol{k}$ là $\boldsymbol{R}(\boldsymbol{k}, \alpha)$, từ mối quan hệ:

$$[\boldsymbol{x}, \boldsymbol{y}, \boldsymbol{z}] = \boldsymbol{R}(\boldsymbol{k}, \alpha)[\boldsymbol{x}^*, \boldsymbol{y}^*, \boldsymbol{z}^*] \tag{7.23}$$

Ta được:

$$\boldsymbol{R}(\boldsymbol{k}, \alpha) = \begin{bmatrix} k_x^2(1-\text{C}\alpha) + \text{C}\alpha & k_xk_y(1-\text{C}\alpha) – k_z\text{S}\alpha & k_xk_z(1-\text{C}\alpha) + k_y\text{S}\alpha \\ k_xk_y(1-\text{C}\alpha) + k_z\text{S}\alpha & k_y^2(1-\text{C}\alpha) + \text{C}\alpha & k_yk_z(1-\text{C}\alpha) – k_x\text{S}\alpha \\ k_xk_z(1-\text{C}\alpha) – k_y\text{S}\alpha & k_yk_z(1-\text{C}\alpha) + k_x\text{S}\alpha & k_z^2(1-\text{C}\alpha) + \text{C}\alpha \end{bmatrix} \tag{7.24}$$

Phương pháp quay 1 trục tìm $\boldsymbol{k}$ và $\alpha$ thỏa mãn $\boldsymbol{p} = \boldsymbol{R}(\boldsymbol{k}, \alpha)\boldsymbol{p}^*$, rồi tạo quỹ đạo bằng cách cho quay quanh trục $\boldsymbol{k}$ từ $0$ đến $\alpha$. Đặt ma trận quay ở tư thế ban đầu là $\boldsymbol{R}_1$, ma trận quay ở tư thế cuối cùng là $\boldsymbol{R}_2$, thì ma trận quay của tư thế cuối cùng nhìn từ tư thế ban đầu được biểu diễn là:

$${}^1\boldsymbol{R}_2 = \boldsymbol{R}_1^T \boldsymbol{R}_2 = \{r_{ij}\} \tag{7.25}$$

Áp dụng Phương pháp quay 1 trục nói trên cho ma trận quay này, ta tìm được góc quay và trục quay bằng các công thức sau:

$$\alpha = \cos^{-1}\left(\frac{r_{11} + r_{22} + r_{33} – 1}{2}\right) \tag{7.26}$$
$${}^1\boldsymbol{k} = \frac{1}{2\text{S}\alpha} \begin{pmatrix} r_{32} – r_{23} \\ r_{13} – r_{31} \\ r_{21} – r_{12} \end{pmatrix} \tag{7.27}$$

Khi $\text{S}\alpha=0$, đây là Điểm kỳ dị (Singular point / 特異点) trong toán học nên công thức trên không có ý nghĩa. Trong trường hợp này, ta gán $\alpha_f=0$ hoặc $\alpha_f=\pi$ và tính toán từ ma trận quay đã cho. Lưu ý rằng khi $\alpha_f=0$ nghĩa là không quay, và $\boldsymbol{R}({}^1\boldsymbol{k}, 0)$ trở thành Ma trận đơn vị (Identity matrix / 単位行列). Ở đây, vì trục quay được lấy tư thế ban đầu làm tiêu chuẩn, nên biểu diễn trong Hệ tọa độ cơ sở sẽ là:

$$\boldsymbol{k} = \boldsymbol{R}_1 {}^1\boldsymbol{k} \tag{7.28}$$

Kết quả là, bằng cách quay một góc $\alpha$ quanh trục $\boldsymbol{k}$, tư thế sẽ chuyển từ tư thế ban đầu sang tư thế cuối. Hơn nữa, nếu góc quay $\alpha$ được cho bởi một đa thức bậc 5 theo thời gian, thời gian kết thúc là $t_f$, và nội suy với điều kiện vận tốc, gia tốc tại điểm đầu và điểm cuối bằng 0 thì:

$$\alpha(t) = \alpha(t_f)(10\lambda^3 – 15\lambda^4 + 6\lambda^5) \tag{7.29}$$

Trong đó, $\lambda = t/t_f$. Đặc điểm của việc tạo quỹ đạo tư thế này là nó quay quanh trục $\boldsymbol{k}$ được cố định khi nhìn từ Hệ tọa độ cơ sở.

【Bài toán mẫu 7.3】 Hãy tìm quỹ đạo chuyển tư thế Khâu tác động cuối từ tư thế ban đầu ở Hình 7.6(a) sang tư thế cuối ở Hình 7.6(b) tại thời điểm $t=1$ bằng Phương pháp quay 1 trục. Giả sử ma trận quay ở tư thế ban đầu là ma trận đơn vị.

Hình 7.6: Tư thế Khâu tác động cuối

Từ hình vẽ, ma trận quay $\boldsymbol{R}(\boldsymbol{k}, \alpha)$ biểu diễn tư thế cuối nhìn từ tư thế ban đầu được cho bởi:

$$\boldsymbol{R}(\boldsymbol{k}, \alpha) = \begin{bmatrix} -1 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & 1 & 0 \end{bmatrix}$$

Khi đó $\theta = \pm\pi$ nên không thể sử dụng công thức (7.27). Trong trường hợp này, từ công thức (7.24) ta có:

$$\boldsymbol{R}(\boldsymbol{k}, \pi) = \begin{bmatrix} 2k_x^2 – 1 & 2k_xk_y & 2k_xk_z \\ 2k_xk_y & 2k_y^2 – 1 & 2k_yk_z \\ 2k_xk_z & 2k_yk_z & 2k_z^2 – 1 \end{bmatrix} = \{r_{ij}\}$$

Từ điều kiện hai ma trận quay này bằng nhau, ta có: $r_{33}=0 \Rightarrow k_z = \pm\sqrt{2}/2$, $r_{32}=1 \Rightarrow k_y = \pm\sqrt{2}/2$, $r_{31}=0 \Rightarrow k_x = 0$. Lưu ý, dấu $\pm$ tương ứng theo thứ tự (複号同順). Do đó, ta thu được 2 nghiệm:

$$\alpha = \pm\pi$$
$$\boldsymbol{k} = \pm[0, \sqrt{2}/2, \sqrt{2}/2]^T$$

Nếu chọn dấu $+$, tư thế Khâu tác động cuối mục tiêu sẽ là:

$$\boldsymbol{R}(\boldsymbol{k}, \alpha(t)) = \begin{bmatrix} \text{C}\alpha(t) & -\frac{\text{S}\alpha(t)}{\sqrt{2}} & \frac{\text{S}\alpha(t)}{\sqrt{2}} \\ \frac{\text{S}\alpha(t)}{\sqrt{2}} & \frac{1+\text{C}\alpha(t)}{2} & \frac{1-\text{C}\alpha(t)}{2} \\ -\frac{\text{S}\alpha(t)}{\sqrt{2}} & \frac{1-\text{C}\alpha(t)}{2} & \frac{1+\text{C}\alpha(t)}{2} \end{bmatrix}$$

Trong đó, nếu sử dụng nội suy đa thức bậc 5 thì $\alpha(t)$ sẽ là:

$$\alpha(t) = \pi(10t^3 – 15t^4 + 6t^5)$$

7.2 Động lực học bao gồm cả hệ thống Động cơ Servo (Dynamics including servomotor system / サーボモータ系を含めた動力学)

Các khớp được truyền động bởi Cơ cấu chấp hành (Actuator / アクチュエータ). Cơ cấu chấp hành có nhiều loại, và đặc tính động lực học của chúng cũng khác nhau. Ở đây, chúng ta sẽ khảo sát động lực học bao gồm cả cánh tay và Động cơ Servo (Servomotor / サーボモータ), đối tượng là Động cơ Servo thường được sử dụng phổ biến.

Giả sử cánh tay có $n$ Bậc tự do (Degree of freedom / 自由度) được truyền động bởi $n$ Động cơ Servo. Gọi vector biến vị trí động cơ là $\boldsymbol{q}_M$, vector điện áp đầu vào động cơ là $\boldsymbol{E}_M$, vector dòng điện động cơ là $\boldsymbol{I}_M$, và nếu bỏ qua điện cảm (Inductance / インダクタンス) của động cơ, phương trình mạch điện từ công thức (3.5) sẽ là:

$$\boldsymbol{E}_M = \boldsymbol{R}_M\boldsymbol{I}_M + \boldsymbol{K}_e\dot{\boldsymbol{q}}_M \tag{7.30}$$

Trong đó, $\boldsymbol{R}_M$, $\boldsymbol{K}_e$ là các ma trận đường chéo $n \times n$ lần lượt biểu diễn Điện trở phần ứng (Armature resistance / 電機子抵抗) và Hằng số sức điện động ngược (Back EMF constant / 逆起電圧定数). Lực sinh ra (Generated torque / 発生トルク) của động cơ $\boldsymbol{\tau}_M$ là:

$$\boldsymbol{\tau}_M = \boldsymbol{K}_T\boldsymbol{I}_M \tag{7.31}$$

Do đó, mối quan hệ giữa $\boldsymbol{\tau}_M$ và $\boldsymbol{E}_M$ là:

$$\boldsymbol{E}_M = \boldsymbol{R}_M\boldsymbol{K}_T^{-1}\boldsymbol{\tau}_M + \boldsymbol{K}_e\dot{\boldsymbol{q}}_M \tag{7.32}$$

Mặt khác, mối quan hệ giữa $\boldsymbol{\tau}_M$ và mô-men đầu ra $\boldsymbol{\tau}_L$ được biểu diễn là:

$$\boldsymbol{\tau}_M = \boldsymbol{M}_M\ddot{\boldsymbol{q}}_M + \boldsymbol{\tau}_L \tag{7.33}$$

Trong đó, $\boldsymbol{K}_T$, $\boldsymbol{M}_M$ là các ma trận đường chéo $n \times n$, lần lượt biểu diễn Hằng số mô-men (Torque constant / トルク定数) và Mô-men quán tính của động cơ. Ngoài ra, nếu gọi ma trận Tỷ số giảm tốc (Tỷ số truyền / 減速比（ギヤ比）) kích thước $n \times n$ giữa $\boldsymbol{q}_M$ và $\boldsymbol{q}$ là $\boldsymbol{\Gamma}$ thì:

$$\boldsymbol{q}_M = \boldsymbol{\Gamma}\boldsymbol{q} \tag{7.34}$$

Mối quan hệ giữa $\boldsymbol{\tau}_L$ và Lực dẫn động khớp $\boldsymbol{\tau}$ dựa trên Nguyên lý công ảo (Principle of virtual work / 仮想仕事の原理) khi xét đến Biến vị trí ảo (Virtual displacement / 仮想変位) $\delta\boldsymbol{q}_M$ và $\delta\boldsymbol{q}$ là:

$$\boldsymbol{\tau}_L^T\delta\boldsymbol{q}_M = \boldsymbol{\tau}^T\delta\boldsymbol{q} \tag{7.35}$$

Thay hệ thức (7.34) vào phương trình này ta được:

$$\boldsymbol{\tau} = \boldsymbol{\Gamma}^T\boldsymbol{\tau}_L \tag{7.36}$$

Từ các hệ thức trên, ta có:

$$\boldsymbol{\tau} = \boldsymbol{\Gamma}^T(\boldsymbol{\tau}_M – \boldsymbol{M}_M\boldsymbol{\Gamma}\ddot{\boldsymbol{q}}) \tag{7.37}$$

Vì Phương trình chuyển động (Equation of motion / 運動方程式) của Robot là:

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

Thay công thức (7.37) vào phương trình này ta được:

$$(\boldsymbol{M}(\boldsymbol{q}) + \boldsymbol{\Gamma}^T\boldsymbol{M}_M\boldsymbol{\Gamma})\ddot{\boldsymbol{q}} + \boldsymbol{h}(\boldsymbol{q}, \dot{\boldsymbol{q}}) + \boldsymbol{g}(\boldsymbol{q}) = \boldsymbol{\Gamma}^T\boldsymbol{\tau}_M \tag{7.39}$$

Công thức trên thể hiện mối quan hệ giữa lực sinh ra của động cơ và chuyển động của cánh tay. Dưới đây, để đơn giản hóa, ta sẽ thảo luận với giả định rằng Cơ cấu chấp hành là một cơ cấu tạo mô-men lý tưởng, xuất ra mô-men đúng theo lệnh điều khiển. Khi Lệnh mô-men (Command torque / 指令トルク) đã được xác định, điện áp đầu vào động cơ chỉ cần tính theo công thức (7.32), do đó giả định này không làm mất đi tính tổng quát.

7.3 Servo khớp và Servo tọa độ làm việc (Joint servo and Task coordinate servo / 関節サーボと作業座標サーボ)

Hệ thống Servo (Servo system / サーボ系) là một hệ thống điều khiển làm cho đầu ra của hệ thống chuyển động cơ khí, như vị trí và vận tốc, bám sát theo các giá trị mục tiêu. Trong điều khiển Robot, quỹ đạo mục tiêu có thể được cho bởi Biến khớp (trường hợp này gọi là Servo khớp – Joint servo / 関節サーボ), hoặc có thể được cho bởi biến vị trí – tư thế của Khâu tác động cuối trong Hệ tọa độ làm việc (trường hợp này gọi là Servo tọa độ làm việc – Task coordinate servo / 作業座標サーボ).

≫ 7.3.1 Servo khớp

Hình 7.7: Hệ điều khiển Servo khớp

Giả sử quỹ đạo mục tiêu của cánh tay được cho bằng biến góc khớp. Khi Giá trị mục tiêu (Target value / 目標値) của khớp $\boldsymbol{q}_d = [q_{d1}, q_{d2}, \cdots, q_{dn}]^T$ được đưa ra, hệ thống Điều khiển phản hồi (Feedback control / フィードバック制御系) sẽ được cấu trúc sao cho góc biến thiên của khớp $i$ trở thành giá trị mục tiêu $q_{di}$ như Hình 7.7. Cấu trúc đơn giản nhất của hệ thống phản hồi là đặt Độ lệch (Sai số / 偏差) so với giá trị mục tiêu là:

$$\boldsymbol{e}(t) = \boldsymbol{q}_d – \boldsymbol{q}(t) \tag{7.40}$$

và áp dụng Luật điều khiển phản hồi vị trí và vận tốc:

$$\boldsymbol{\tau}(t) = \boldsymbol{K}_v\dot{\boldsymbol{e}}(t) + \boldsymbol{K}_p\boldsymbol{e}(t) \tag{7.41}$$

Trong đó:

$\boldsymbol{K}_v = \text{diag}(K_{v1}, K_{v2}, \cdots, K_{vn})$: Ma trận Độ lợi phản hồi vận tốc (Velocity feedback gain matrix / 速度フィードバックゲイン行列)

$\boldsymbol{K}_p = \text{diag}(K_{p1}, K_{p2}, \cdots, K_{pn})$: Ma trận Độ lợi phản hồi vị trí (Position feedback gain matrix / 位置フィードバックゲイン行列)

Khi đó, hành vi của sai số, thay (7.41) vào (7.38), được biểu diễn là:

$$\boldsymbol{M}(\boldsymbol{q})\ddot{\boldsymbol{e}} + \boldsymbol{K}_v\dot{\boldsymbol{e}} + \boldsymbol{K}_p\boldsymbol{e} – \boldsymbol{h}(\boldsymbol{q}, \dot{\boldsymbol{q}}) – \boldsymbol{g}(\boldsymbol{q}) = \boldsymbol{0} \tag{7.42}$$

Ở đây, giả sử giới hạn trong dẫn động tốc độ thấp, tỷ số giảm tốc lớn và bỏ qua ảnh hưởng của trọng lực. Nghĩa là, khi $\boldsymbol{h}(\boldsymbol{q}, \dot{\boldsymbol{q}}) = \boldsymbol{g}(\boldsymbol{q}) = \boldsymbol{0}$, ta có thể xấp xỉ bằng:

$$\boldsymbol{M}(\boldsymbol{q})\ddot{\boldsymbol{e}} + \boldsymbol{K}_v\dot{\boldsymbol{e}} + \boldsymbol{K}_p\boldsymbol{e} = \boldsymbol{0} \tag{7.43}$$

Khi $\boldsymbol{M}(\boldsymbol{q})$ có thể xấp xỉ thành Ma trận đường chéo (Diagonal matrix / 対角行列), nó trở thành Hệ bậc 2 (2nd order system / 2次系) độc lập cho từng khớp. Do đó, nếu chọn một dạng sóng đáp ứng (Response waveform / 応答波形) thích hợp và khả thi, $\boldsymbol{K}_v$, $\boldsymbol{K}_p$ sẽ được xác định. Vì Servo khớp này xử lý từng khớp như một Hệ thống 1 đầu vào 1 đầu ra (SISO system / 1入力1出力系) đơn giản nên cấu trúc của nó rất đơn giản. Hầu hết các Robot công nghiệp hiện tại đều được thiết kế dựa trên loại Servo khớp này.

Khi Tải trọng trọng trường (Gravity load / 重力負荷) không thể bị bỏ qua, một phương pháp điều khiển là thêm Hạng mục Bù trọng lực (Gravity compensation term / 重力補償項) vào luật điều khiển:

$$\boldsymbol{\tau} = \boldsymbol{K}_v\dot{\boldsymbol{e}} + \boldsymbol{K}_p\boldsymbol{e} + \boldsymbol{g}(\boldsymbol{q}) \tag{7.44}$$

Trong trường hợp này, Điểm cân bằng (Equilibrium point / 平衡点) $\boldsymbol{q}_d$ của Hệ kín (Closed-loop system / 閉ループ系) là Ổn định tiệm cận (Asymptotically stable / 漸近安定) mà không phụ thuộc vào độ lớn của $\boldsymbol{M}(\boldsymbol{q})$, $\boldsymbol{h}(\boldsymbol{q}, \dot{\boldsymbol{q}})$, $\boldsymbol{g}(\boldsymbol{q})$, điều này có thể được chứng minh bằng Định lý ổn định Lyapunov (Lyapunov’s stability theorem / リアプノフの安定定理). Vấn đề này sẽ được trình bày ở Mục 7.3.3.

Khi không thể tính toán chính xác hạng mục trọng lực $\boldsymbol{g}(\boldsymbol{q})$ hoặc khi Lực ma sát Coulomb tác dụng lên khớp, phương pháp điều khiển nói trên nhìn chung sẽ sinh ra Sai số xác lập (Steady-state error / 定常偏差). Gọi lực ma sát Coulomb tác dụng lên khớp là $\boldsymbol{f}_r$, từ công thức (7.38) phương trình chuyển động có thể được viết lại thành:

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

Khi đó, phương trình sai số sẽ là vế trái của công thức (7.42) cộng thêm $-\boldsymbol{f}_r$. Sai số vị trí xác lập là sai số vị trí khi vận tốc và gia tốc bằng 0, do đó:

$$\boldsymbol{e}(\infty) = \boldsymbol{q}_d – \boldsymbol{q}(\infty) = \boldsymbol{K}_p(\boldsymbol{g}(\boldsymbol{q}) + \boldsymbol{f}_r) \tag{7.46}$$

Vì vậy, dù $\boldsymbol{K}_p$ có lớn thế nào thì sai số xác lập này vẫn phát sinh.

Luật điều khiển nhằm triệt tiêu sai số xác lập là phương pháp thêm Thành phần tích phân (Integral component / 積分要素) vào công thức (7.41):

$$\boldsymbol{\tau} = \boldsymbol{K}_v\dot{\boldsymbol{e}} + \boldsymbol{K}_p\boldsymbol{e} + \boldsymbol{K}_I \int_0^t \boldsymbol{e} dt \tag{7.47}$$

Trong đó:

$\boldsymbol{K}_I = \text{diag}(K_{I1}, K_{I2}, \cdots, K_{In})$: Ma trận Độ lợi phản hồi tích phân (Integral feedback gain matrix / 積分フィードバックゲイン行列).

Khi đó, phương trình sai số tương ứng với công thức (7.42) sẽ là:

$$\boldsymbol{M}\ddot{\boldsymbol{e}} + \boldsymbol{K}_v\dot{\boldsymbol{e}} + \boldsymbol{K}_p\boldsymbol{e} + \boldsymbol{K}_I \int_0^t \boldsymbol{e} dt = \boldsymbol{h}(\boldsymbol{q}, \dot{\boldsymbol{q}}) + \boldsymbol{g}(\boldsymbol{q}) + \boldsymbol{f}_r \tag{7.48}$$

Ở Trạng thái xác lập (Steady state / 定常状態), $\boldsymbol{M}$ gần như có giá trị không đổi, vì vậy khi biến đổi Laplace công thức (7.48) và sử dụng Định lý giá trị cuối (Final value theorem / 最終値の定理), ta có thể dễ dàng chứng minh được rằng khi $t \to \infty$ thì $\boldsymbol{e}(t) \to \boldsymbol{0}$.

≫ 7.3.2 Servo tọa độ làm việc

Khi muốn chỉ định một cách rõ ràng vị trí – tư thế Khâu tác động cuối của Robot trong Hệ tọa độ làm việc, giá trị mục tiêu $\boldsymbol{r}_d$ trong Hệ tọa độ làm việc sẽ được cung cấp. Lúc này, ta có thể dùng biến đổi ngược giá trị mục tiêu này thành Biến khớp, rồi áp dụng Servo khớp ở Mục 7.3.1. Tuy nhiên, ngay cả khi $\boldsymbol{r}_d$ được đưa ra chính xác, do sự không chính xác của các hằng số như Tham số Khâu của Tay máy (Manipulator link parameters), ngay cả khi dùng $\boldsymbol{q}_d$ đã tìm được, vị trí đạt được vẫn không chính xác và nhìn chung sẽ kèm theo một số sai số. Trong trường hợp này, nếu ta đo $\boldsymbol{r}$ hiện tại bằng Cảm biến ngoại suy và xây dựng một hệ thống Servo trực tiếp đặt $\boldsymbol{r}_d$ làm mục tiêu sao cho $\boldsymbol{r}$ khớp với $\boldsymbol{r}_d$, sai số có thể được giảm bớt. Phương pháp này được gọi là Servo tọa độ làm việc.

Hình 7.8: Một ví dụ về Servo tọa độ làm việc

Khi biểu diễn tư thế Khâu tác động cuối bằng Góc Euler, một trong những luật điều khiển của Servo tọa độ làm việc là:

$$\boldsymbol{\tau} = \boldsymbol{J}^T(\boldsymbol{q})\boldsymbol{K}_p(\boldsymbol{r}_d – \boldsymbol{r}) – \boldsymbol{K}_v\dot{\boldsymbol{q}} + \boldsymbol{g}(\boldsymbol{q}) \tag{7.49}$$

Trong đó, $\boldsymbol{r}$ là vị trí – tư thế Khâu tác động cuối được định nghĩa bởi công thức (4.84):

$$\boldsymbol{r} = \boldsymbol{f}(\boldsymbol{q}), \quad \boldsymbol{r}_d = \boldsymbol{f}(\boldsymbol{q}_d) \tag{7.50}$$

Và $\boldsymbol{J}(\boldsymbol{q}) = \partial\boldsymbol{f}(\boldsymbol{q}) / \partial\boldsymbol{q}^T$ là Ma trận Jacobian (Jacobian matrix / ヤコビ行列), vận tốc có mối quan hệ:

$$\dot{\boldsymbol{r}} = \boldsymbol{J}(\boldsymbol{q})\dot{\boldsymbol{q}} \tag{7.51}$$

Sơ đồ khối điều khiển (Control block diagram / 制御ブロック図) theo luật điều khiển này được thể hiện trong Hình 7.8.

Ngoài ra, nếu thêm thành phần tích phân, ta có thể nghĩ đến phương pháp:

$$\boldsymbol{\tau} = \boldsymbol{J}^T(\boldsymbol{q})\left[\boldsymbol{K}_p(\boldsymbol{r}_d – \boldsymbol{r}) + \boldsymbol{K}_I \int_0^t (\boldsymbol{r}_d – \boldsymbol{r})dt\right] – \boldsymbol{K}_v\dot{\boldsymbol{q}} + \boldsymbol{g}(\boldsymbol{q}) \tag{7.52}$$

Hệ thống điều khiển vòng kín theo luật điều khiển (7.49), (7.52) có thể được coi là hoạt động sao cho lực tỷ lệ thuận với sai số vị trí – tư thế Khâu tác động cuối $\boldsymbol{K}_p(\boldsymbol{r}_d – \boldsymbol{r})$ hoặc $\boldsymbol{K}_p(\boldsymbol{r}_d – \boldsymbol{r}) + \boldsymbol{K}_I \int_0^t (\boldsymbol{r}_d – \boldsymbol{r})dt$ giúp làm giảm sai số. Tính ổn định tiệm cận tại Điểm cân bằng $\boldsymbol{r}_d$ đã được chứng minh bằng cách sử dụng Định lý ổn định Lyapunov. Cũng cần lưu ý rằng các luật điều khiển này không bao gồm tính toán Động học ngược (Inverse kinematics / 逆運動学).

≫ 7.3.3 Tính ổn định của Hệ thống Servo

Trình bày tính ổn định của hệ thống áp dụng luật phản hồi vị trí và vận tốc có bù trọng lực trong Servo khớp:

$$\boldsymbol{\tau} = \boldsymbol{K}_v\dot{\boldsymbol{e}} + \boldsymbol{K}_p\boldsymbol{e} + \boldsymbol{g}(\boldsymbol{q}) \tag{7.53}$$

Trong đó, $\boldsymbol{K}_v$, $\boldsymbol{K}_p$ là các Ma trận xác định dương (Positive definite matrix / 正定行列) $n \times n$. Như một sự chuẩn bị cho điều đó, lý thuyết ổn định Lyapunov sẽ được trình bày.

〈Lý thuyết ổn định Lyapunov〉

Hệ phương trình tự trị phi tuyến (Nonlinear autonomous equation system / 非線形自律方程式系)

Xét hệ phương trình:

$$\dot{\boldsymbol{x}} = \boldsymbol{F}(\boldsymbol{x}) \tag{7.54}$$

Trong đó, $\boldsymbol{x}$ là vector trạng thái $n$ chiều, $\boldsymbol{F}(\boldsymbol{x})$ là hàm phi tuyến $n \times 1$ của $\boldsymbol{x}$. Giả sử $\boldsymbol{F}(\boldsymbol{x})$ và đạo hàm bậc 1 của nó liên tục trong một vùng nào đó của không gian trạng thái chứa gốc tọa độ. Điểm làm cho vế phải của phương trình (7.54) bằng 0, tức là điểm trạng thái $\boldsymbol{x}_o$ thỏa mãn:

$$\boldsymbol{F}(\boldsymbol{x}_o) = \boldsymbol{0} \tag{7.55}$$

được gọi là Điểm cân bằng (Equilibrium point / 平衡点), và ta sẽ khảo sát tính ổn định tại Điểm cân bằng này. Ở đây, việc giả định $\boldsymbol{x}_o = \boldsymbol{0}$, tức là Điểm cân bằng là gốc tọa độ $O$, cũng không làm mất đi tính tổng quát. Lý do là vì nếu $\boldsymbol{x}_o \neq \boldsymbol{0}$, ta có thể đặt $\boldsymbol{x} – \boldsymbol{x}_o$ thành một $\boldsymbol{x}$ mới và viết lại phương trình (7.54), khi đó $\boldsymbol{x} = \boldsymbol{0}$ sẽ trở thành Điểm cân bằng. Về tính ổn định của gốc tọa độ $O$, ta định nghĩa ba loại: Ổn định (Stable / 安定), Ổn định tiệm cận (Asymptotically stable / 漸近安定), và Ổn định tiệm cận toàn cục (Globally asymptotically stable / 大域的に漸近安定) (tham khảo Hình 7.9).

Hình 7.9: Định nghĩa tính ổn định

  1. Ổn định (Stable / 安定)

    Đối với một $\varepsilon > 0$ tùy ý, tồn tại một $\delta > 0$ thích hợp sao cho đối với mọi Giá trị khởi tạo (Initial value / 初期値) $\boldsymbol{x}(0)$ thỏa mãn $\|\boldsymbol{x}(0)\| < \delta$, nghiệm $\boldsymbol{x}(t)$ của công thức (7.54) từ $\boldsymbol{x}(0)$ thỏa mãn $\|\boldsymbol{x}(t)\| < \varepsilon$ tại mọi thời điểm $t \ge 0$, thì gốc tọa độ $O$ được gọi là ổn định.

  2. Ổn định tiệm cận (Asymptotically stable / 漸近安定)

    Nếu gốc tọa độ ổn định, và tồn tại một số dương $\rho < \delta$ thích hợp sao cho đối với mọi Giá trị khởi tạo $\boldsymbol{x}(0)$ thỏa mãn $\|\boldsymbol{x}(0)\| < \rho$, nghiệm $\boldsymbol{x}(t)$ từ $\boldsymbol{x}(0)$ tiến tới $\boldsymbol{x}(t) \to \boldsymbol{0}$ khi $t \to \infty$, thì gốc tọa độ $O$ được gọi là ổn định tiệm cận.

  3. Ổn định tiệm cận toàn cục (Globally asymptotically stable / 大域的に漸近安定)

    Nếu gốc tọa độ ổn định, và đối với mọi Giá trị khởi tạo $\boldsymbol{x}(0)$ tùy ý, nghiệm $\boldsymbol{x}(t)$ từ $\boldsymbol{x}(0)$ tiến tới $\boldsymbol{x}(t) \to \boldsymbol{0}$ khi $t \to \infty$, thì gốc tọa độ $O$ được gọi là ổn định tiệm cận toàn cục.

Tiếp theo, ta giải thích định nghĩa về Hàm xác định dương, Hàm xác định âm của $\boldsymbol{x}$ và Hàm Lyapunov.

  1. Hàm xác định dương (Positive definite function / 正定関数) (Hàm xác định âm (Negative definite function / 負定関数))

    Một hàm vô hướng $V(\boldsymbol{x})$ được định nghĩa trong một miền $\Omega$ chứa gốc tọa độ, nếu $V(\boldsymbol{0}) = 0$ và với mọi $\boldsymbol{x} \in \Omega$ sao cho $\boldsymbol{x} \neq \boldsymbol{0}$ mà thỏa mãn $V(\boldsymbol{x}) > 0$ ($V(\boldsymbol{x}) < 0$), thì $V(\boldsymbol{x})$ được gọi là xác định dương (âm).

  2. Hàm Lyapunov (Lyapunov function / リアプノフ関数)

    Nếu hàm $V(\boldsymbol{x})$ xác định dương trong miền $\Omega$, tồn tại đạo hàm riêng liên tục $\partial V(\boldsymbol{x}) / \partial \boldsymbol{x}$, và đạo hàm theo thời gian dọc theo quỹ đạo của hệ (7.54) là:

    $$\dot{V}(\boldsymbol{x}) = \frac{dV(\boldsymbol{x})}{dt} = \left[ \frac{\partial V(\boldsymbol{x})}{\partial \boldsymbol{x}} \right]^T \boldsymbol{F}(\boldsymbol{x}) \le 0 \tag{7.56}$$

    thì hàm $V(\boldsymbol{x})$ này được gọi là Hàm Lyapunov.

Từ những chuẩn bị trên, Định lý ổn định Lyapunov (Lyapunov stability theory / リアプノフの安定定理) được phát biểu như sau.

【Định lý ổn định】

Nếu tồn tại một Hàm Lyapunov $V(\boldsymbol{x})$ trong lân cận $\Omega$ của gốc tọa độ, thì gốc tọa độ là ổn định.

【Định lý ổn định tiệm cận】

Nếu tồn tại một Hàm Lyapunov $V(\boldsymbol{x})$ trong lân cận $\Omega$ của gốc tọa độ, và $\dot{V}(\boldsymbol{x}) < 0$ ($\boldsymbol{x} \neq \boldsymbol{0}$), thì gốc tọa độ là ổn định tiệm cận.

Khái niệm về Hàm Lyapunov có thể được minh họa qua Hình 7.10 trong trường hợp $n = 2$. $V(\boldsymbol{x})$ tạo thành một mặt parabol lồi hướng xuống dưới, và trong trường hợp quỹ đạo trên mặt parabol của quỹ đạo $\boldsymbol{x}(t)$ trên mặt phẳng $\boldsymbol{x}_1\boldsymbol{x}_2$ của hệ (7.54) là ổn định tiệm cận, thì khi $\dot{V}(\boldsymbol{x}) < 0$, nó luôn giảm dần theo thời gian và có nghĩa là nó sẽ chạm đến gốc tọa độ khi thời gian tiến đến vô cùng. Mặt khác, khi $\dot{V}(\boldsymbol{x}) = 0$, nó có nghĩa là $V(\boldsymbol{x})$ sẽ dừng lại ở một độ cao nhất định và quỹ đạo sẽ không bị phân kỳ. Chú ý rằng Định lý ổn định Lyapunov chỉ ra điều kiện đủ để hệ thống biểu diễn bởi công thức (7.54) là ổn định (hoặc ổn định tiệm cận).

Hình 7.10: Hàm Lyapunov

【Bài toán mẫu 7.4】 Hãy tìm Điểm cân bằng của con lắc đơn trong Hình 7.11 và khảo sát về tính ổn định. Giả sử khối lượng của con lắc là $m$, chiều dài của con lắc là $l$, hệ số ma sát nhớt của ổ trục là $b$, và gia tốc trọng trường là $g$.

Hình 7.11: Con lắc đơn

Phương trình chuyển động của con lắc đơn có thể được biểu diễn là:

$$ml^2\ddot{\theta} + b\dot{\theta} + mgl\sin\theta = 0$$

Để biến đổi phương trình này thành hệ phương trình vi phân bậc 1, ta đặt $x_1 = \theta, x_2 = \dot{\theta}$, ta thu được:

$$\begin{bmatrix} \dot{x}_1 \\ \dot{x}_2 \end{bmatrix} = \begin{bmatrix} x_2 \\ -\frac{b}{ml^2}x_2 – \frac{g}{l}\sin x_1 \end{bmatrix}$$

Giả sử phạm vi chuyển động của con lắc là $-\pi < \theta < \pi$, Điểm cân bằng sẽ là:

$$x_2 = 0, \quad x_1 = 0$$

Do đó, gốc tọa độ chính là Điểm cân bằng. Nếu lấy tổng của Động năng và Thế năng của con lắc này làm ứng cử viên cho Hàm Lyapunov, ta có:

$$V(\boldsymbol{x}) = \frac{1}{2}m(lx_2)^2 + mg(1 – \cos x_1) \ge 0$$

Và đạo hàm theo thời gian của hàm này dọc theo quỹ đạo là:

$$\dot{V}(\boldsymbol{x}) = ml^2x_2\dot{x}_2 + mgx_2\sin x_1 = -bx_2^2 \le 0$$

Do đó, $V(\boldsymbol{x})$ là một Hàm Lyapunov, và gốc tọa độ là ổn định. Hơn nữa, khi $t \to \infty$, ta có thể lần lượt chỉ ra rằng $\dot{V}(\boldsymbol{x}) \to 0$, $x_2 \to 0$, $x_1 \to 0$, điều này chứng tỏ tính ổn định tiệm cận của gốc tọa độ.

Dựa trên những chuẩn bị trên, chúng ta sẽ chứng minh tính ổn định của hệ thống áp dụng luật điều khiển của công thức (7.53). Phương trình chuyển động của Robot khi áp dụng luật điều khiển này từ công thức (7.38) trở thành:

$$\boldsymbol{M}(\boldsymbol{q})\ddot{\boldsymbol{q}} + \boldsymbol{h}(\boldsymbol{q}, \dot{\boldsymbol{q}}) + \boldsymbol{K}_v\dot{\boldsymbol{e}} + \boldsymbol{K}_p\boldsymbol{e} = \boldsymbol{0} \tag{7.57}$$

Ta xét ứng cử viên cho Hàm Lyapunov là:

$$V(t) = \frac{1}{2} \{ \dot{\boldsymbol{e}}^T \boldsymbol{M}(\boldsymbol{q})\dot{\boldsymbol{e}} + \boldsymbol{e}^T \boldsymbol{K}_p \boldsymbol{e} \} \tag{7.58}$$

Vì $\boldsymbol{M}(\boldsymbol{q})$ và $\boldsymbol{K}_p$ là các ma trận xác định dương nên $V(t) > 0$. Đạo hàm theo thời gian của hàm này, khi sử dụng mối quan hệ $\ddot{\boldsymbol{e}} = \ddot{\boldsymbol{q}}$, $\dot{\boldsymbol{e}} = \dot{\boldsymbol{q}}$ và công thức (7.57), ta có:

$$\begin{aligned} \dot{V}(t) &= \dot{\boldsymbol{e}}^T \left( \boldsymbol{M}(\boldsymbol{q})\ddot{\boldsymbol{e}} + \frac{1}{2}\dot{\boldsymbol{M}}(\boldsymbol{q})\dot{\boldsymbol{e}} + \boldsymbol{K}_p\boldsymbol{e} \right) \\ &= \dot{\boldsymbol{e}}^T \left( -\boldsymbol{h}(\boldsymbol{q}, \dot{\boldsymbol{q}}) + \frac{1}{2}\dot{\boldsymbol{M}}(\boldsymbol{q})\dot{\boldsymbol{e}} \right) – \dot{\boldsymbol{e}}^T \boldsymbol{K}_v \dot{\boldsymbol{e}} \end{aligned} \tag{7.59}$$

Ở đây,

$$\dot{\boldsymbol{q}}^T \frac{\partial}{\partial\boldsymbol{q}} \left( \frac{1}{2} \dot{\boldsymbol{q}}^T \boldsymbol{M}(\boldsymbol{q}) \dot{\boldsymbol{q}} \right) = \frac{1}{2} \sum_{i=1}^n \frac{\partial}{\partial q_i} (\dot{\boldsymbol{q}}^T \boldsymbol{M}(\boldsymbol{q}) \dot{\boldsymbol{q}}) \dot{q}_i = \frac{1}{2} \dot{\boldsymbol{q}}^T \dot{\boldsymbol{M}}(\boldsymbol{q}) \dot{\boldsymbol{q}} \tag{7.60}$$

Sử dụng đẳng thức này và công thức (5.15), hạng mục đầu tiên ở vế phải của công thức (7.59) sẽ bằng 0, do đó ta thu được:

$$\dot{V}(t) = -\dot{\boldsymbol{e}}^T \boldsymbol{K}_v \dot{\boldsymbol{e}} \le 0 \tag{7.61}$$

Do đó, $V(t)$ là một Hàm Lyapunov. Hơn nữa, $\dot{\boldsymbol{e}}$ làm cho $\dot{V}(t) = 0$ chỉ xảy ra khi $\dot{\boldsymbol{e}}(t) = \boldsymbol{0}$, và nghiệm thỏa mãn công thức (7.57) là $\boldsymbol{q}(t) = \boldsymbol{q}_d$. Từ đó, khi $\boldsymbol{q}(t) \neq \boldsymbol{q}_d$ thì $\dot{V}(t) < 0$. Vì vậy, hệ thống là ổn định tiệm cận.

Từ những phân tích trên, đã chứng minh được rằng hệ thống ổn định tiệm cận theo luật điều khiển của công thức (7.53), tuy nhiên, chưa có bất kỳ thảo luận nào về đặc tính của Đáp ứng quá độ (Transient response / 過渡応答). Hơn nữa, nếu phân tích chi tiết hơn, có thể thấy đây là một luật điều khiển mà sự hội tụ về Điểm cân bằng có dạng hàm mũ.

7.4 Điều khiển động (Dynamic control / 動的制御)

Ở phần trước, chúng ta đã xem xét luật điều khiển từ góc độ định vị (Positioning), ở đây ta sẽ thảo luận về điều khiển quỹ đạo liên tục. Khi biến khớp mục tiêu $\boldsymbol{q}_d(t)$ và vận tốc khớp mục tiêu $\dot{\boldsymbol{q}}_d(t)$ được đưa ra, phương pháp đơn giản nhất là sửa đổi công thức (7.53) và thiết lập luật điều khiển như sau:

$$\boldsymbol{\tau}(t) = \boldsymbol{K}_v(\dot{\boldsymbol{q}}_d(t) – \dot{\boldsymbol{q}}(t)) + \boldsymbol{K}_p(\boldsymbol{q}_d(t) – \boldsymbol{q}(t)) \tag{7.62}$$

Tuy nhiên, khi sự Can thiệp (Interference / 干渉) giữa các Khâu hoặc tác động của lực Coriolis và lực ly tâm là lớn, độ chính xác quỹ đạo đầy đủ sẽ không thể đạt được. Do đó, nhiều phương pháp điều khiển khác nhau đã được đề xuất với sự cân nhắc về đặc tính động của Robot. Gọi chung những phương pháp này là Điều khiển động (Dynamic control / 動的制御), chúng ta sẽ đề cập đến một số phương pháp tiêu biểu. Tuy nhiên, cần lưu ý rằng điều khiển động hiện tại vẫn chưa được áp dụng phổ biến trên các Robot công nghiệp, nhưng đây là hướng điều khiển sẽ được áp dụng trong tương lai cùng với yêu cầu về độ chính xác và tốc độ ngày càng cao.

≫ 7.4.1 Điều khiển mô-men tính toán

Phương trình chuyển động của cánh tay Robot $n$ bậc tự do nói chung được biểu diễn bởi:

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

Trong trường hợp Mô hình động lực học (Dynamic model / 動的モデル) của Robot, tức là công thức (7.63), đã được tính toán chính xác từ trước, có một phương pháp điều khiển gọi là Điều khiển mô-men tính toán (Computed torque method / 計算トルク制御) như thể hiện trong Hình 7.12. Luật điều khiển của phương pháp này được cho bởi:

$$\boldsymbol{\tau} = \hat{\boldsymbol{M}}(\boldsymbol{q})\ddot{\boldsymbol{q}}^* + \hat{\boldsymbol{h}}(\boldsymbol{q}, \dot{\boldsymbol{q}}) + \hat{\boldsymbol{g}}(\boldsymbol{q}) \tag{7.64}$$
$$\ddot{\boldsymbol{q}}^* = \ddot{\boldsymbol{q}}_d + \boldsymbol{K}_v(\dot{\boldsymbol{q}}_d – \dot{\boldsymbol{q}}) + \boldsymbol{K}_p(\boldsymbol{q}_d – \boldsymbol{q}) \tag{7.65}$$

Luật điều khiển này có thể được xem như là đang thực hiện Tuyến tính hóa (Linearization / 線形化) hệ thống nhờ vào hạng mục thứ 2 và thứ 3 ở vế phải của công thức (7.64), và sau đó áp dụng bù điều khiển Servo tuyến tính cho hệ thống đã được tuyến tính hóa thông qua công thức (7.65). Nói cách khác, khi mô hình là chính xác:

$$\hat{\boldsymbol{M}}(\boldsymbol{q}) = \boldsymbol{M}(\boldsymbol{q}), \quad \hat{\boldsymbol{h}}(\boldsymbol{q}, \dot{\boldsymbol{q}}) = \boldsymbol{h}(\boldsymbol{q}, \dot{\boldsymbol{q}}), \quad \hat{\boldsymbol{g}}(\boldsymbol{q}) = \boldsymbol{g}(\boldsymbol{q}) \tag{7.66}$$

Nếu thay công thức (7.64) ~ (7.66) vào công thức (7.63), các hạng mục phi tuyến sẽ bị triệt tiêu và ta thu được:

$$\ddot{\boldsymbol{q}}^* = \ddot{\boldsymbol{q}} \tag{7.67}$$

Ở đây, nếu đặt Sai số quỹ đạo (Trajectory error / 軌道誤差) là:

$$\boldsymbol{e} = \boldsymbol{q}_d – \boldsymbol{q} \tag{7.68}$$

Và thay công thức (7.67) vào công thức (7.65), ta có:

$$\ddot{\boldsymbol{e}} + \boldsymbol{K}_v\dot{\boldsymbol{e}} + \boldsymbol{K}_p\boldsymbol{e} = \boldsymbol{0} \tag{7.69}$$

Do đó, bằng cách thiết lập $\boldsymbol{K}_v$, $\boldsymbol{K}_p$ là các ma trận đường chéo xác định dương với các giá trị phù hợp, $\boldsymbol{e}$ sẽ hội tụ về 0. Ví dụ, nếu ta đặt:

Hình 7.12: Điều khiển mô-men tính toán

$$\boldsymbol{K}_v = \text{diag}(2\xi\omega_c, 2\xi\omega_c, \cdots, 2\xi\omega_c) \tag{7.70a}$$
$$\boldsymbol{K}_p = \text{diag}(\omega_c^2, \omega_c^2, \cdots, \omega_c^2) \tag{7.70b}$$

Trong đó, $\omega_c$ là Tần số tự nhiên (Natural frequency / 固有振動数) của Hệ bậc 2, và $\xi$ là Hệ số tắt dần (Damping ratio / 減衰係数). Phương pháp điều khiển này đòi hỏi phải tính toán ở tốc độ cao Mô hình động lực học và nhận dạng Tham số mô hình. Đối với yêu cầu đầu tiên, việc tính toán tốc độ cao đã trở nên khả thi nhờ vào Thuật toán Newton-Euler (Newton-Euler algorithm) đã được trình bày ở Mục 5.3. Đối với yêu cầu sau, việc ước lượng giá trị tham số có thể được thực hiện thông qua Nhận dạng tham số đã được trình bày ở Mục 6.2. Điều khiển mô-men tính toán cấu thành một hệ thống tuyến tính đối với sai số $\boldsymbol{e}$, và bằng cách thiết lập Ma trận độ lợi phản hồi (Feedback gain matrix) là một Ma trận đường chéo xác định dương, nó trở thành một Hệ thống không can thiệp (Non-interacting system / 非干渉な系) nơi mà các phần tử của sai số không ảnh hưởng lẫn nhau.

≫ 7.4.2 Điều khiển gia tốc phân giải

Là một phương pháp điều khiển tuyến tính hóa hệ thống bằng biến vị trí – tư thế của Khâu tác động cuối được đưa ra trong Hệ tọa độ làm việc và áp dụng bù Servo tuyến tính cho hệ thống đã được tuyến tính hóa, ta có phương pháp gọi là Điều khiển gia tốc phân giải (Resolved acceleration control / 分解加速度制御) như được thể hiện ở Hình 7.13. Luật điều khiển này, khi giá trị mục tiêu của biến vị trí – tư thế Khâu tác động cuối là $\boldsymbol{r}_d$, $\dot{\boldsymbol{r}}_d$, $\ddot{\boldsymbol{r}}_d$ được đưa ra, sẽ tính toán thông qua:

$$\boldsymbol{\tau} = \hat{\boldsymbol{M}}(\boldsymbol{q})\boldsymbol{J}^{-1}(\boldsymbol{q})(\ddot{\boldsymbol{r}}^* – \dot{\boldsymbol{J}}(\boldsymbol{q})\dot{\boldsymbol{q}}) + \hat{\boldsymbol{h}}(\boldsymbol{q}, \dot{\boldsymbol{q}}) + \hat{\boldsymbol{g}}(\boldsymbol{q}) \tag{7.71}$$
$$\ddot{\boldsymbol{r}}^* = \ddot{\boldsymbol{r}}_d + \boldsymbol{K}_v(\dot{\boldsymbol{r}}_d – \dot{\boldsymbol{r}}) + \boldsymbol{K}_p(\boldsymbol{r}_d – \boldsymbol{r}) \tag{7.72}$$

Trong đó:

$$\boldsymbol{r} = \boldsymbol{f}(\boldsymbol{q}), \quad \boldsymbol{r}_d = \boldsymbol{f}(\boldsymbol{q}_d) \tag{7.73}$$

$\boldsymbol{J}(\boldsymbol{q})$ là Ma trận Jacobian thỏa mãn:

$$\dot{\boldsymbol{r}} = \boldsymbol{J}(\boldsymbol{q})\dot{\boldsymbol{q}} \tag{7.74}$$

Và giả định là ma trận không suy biến trong một phạm vi xem xét thích hợp của $\boldsymbol{q}$. Khi điều kiện của công thức (7.66) thỏa mãn, thay công thức (7.71) vào công thức (7.63), và sử dụng công thức (4.98), ta có:

$$\ddot{\boldsymbol{r}} = \ddot{\boldsymbol{r}}^* \tag{7.75}$$

Hình 7.13: Điều khiển gia tốc phân giải

Hệ thống này khi được áp dụng bù Servo tuyến tính từ công thức (7.72), và nếu đặt $\boldsymbol{e} = \boldsymbol{r}_d – \boldsymbol{r}$, ta sẽ thu được một hệ thức giống với công thức (7.69). Do đó, bằng cách thiết lập $\boldsymbol{K}_v$, $\boldsymbol{K}_p$ là các ma trận đường chéo xác định dương với giá trị thích hợp, hệ thống sẽ trở nên tuyến tính và không can thiệp liên quan đến $\boldsymbol{e}$, và $\boldsymbol{e}$ sẽ hội tụ về 0. Lưu ý rằng khi Robot ở Tư thế kỳ dị (Singular configuration / 特異姿勢), do $\boldsymbol{J}$ không còn là ma trận không suy biến nên mô-men ở công thức (7.71) có thể trở nên vô hạn. Vì vậy cần có biện pháp khắc phục cho vấn đề này.

7.5 Điều khiển thích nghi (Adaptive control / 適応制御)

Các tham số của Mô hình động lực học có khả năng thay đổi theo thời gian. Hơn nữa, Tải trọng có thể biến động tùy thuộc vào nội dung công việc. Trong những trường hợp như vậy, các phương pháp điều khiển ở phần trước sẽ không thể đạt được các đặc tính như mong muốn. Ở đây, dựa trên việc Tham số động lực học $\boldsymbol{\sigma}$ nằm trong mối quan hệ được biểu diễn bởi:

$$\boldsymbol{\tau} = \boldsymbol{W}(\boldsymbol{q}, \dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}})\boldsymbol{\sigma} \tag{7.76}$$

như đã trình bày ở Mục 6.2, chúng ta sẽ mô tả về Điều khiển thích nghi (Adaptive control / 適応制御) như được minh họa ở Hình 7.14.

Hình 7.14: Điều khiển thích nghi

Đối với một Vector tham số chưa biết kích thước $m \times 1$ là $\boldsymbol{a}$ ($\boldsymbol{a}$ có thể là tham số cơ sở hoặc một phần của $\boldsymbol{\sigma}$), từ công thức (7.76), ta có thể định nghĩa một ma trận kích thước $n \times m$ là $\boldsymbol{Y}$ sao cho thỏa mãn mối quan hệ sau:

$$\boldsymbol{M}(\boldsymbol{q})\ddot{\boldsymbol{q}}_r + \boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}})\dot{\boldsymbol{q}}_r + \boldsymbol{g}(\boldsymbol{q}) = \boldsymbol{Y}(\boldsymbol{q}, \dot{\boldsymbol{q}}, \dot{\boldsymbol{q}}_r, \ddot{\boldsymbol{q}}_r)\boldsymbol{a} \tag{7.77}$$

Trong đó, $\boldsymbol{C}$ là ma trận từ công thức (5.15) thỏa mãn:

$$\boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}})\dot{\boldsymbol{q}} = \boldsymbol{h}(\boldsymbol{q}, \dot{\boldsymbol{q}}) = \dot{\boldsymbol{M}}(\boldsymbol{q})\dot{\boldsymbol{q}} – \frac{\partial}{\partial\boldsymbol{q}} \left( \frac{1}{2} \dot{\boldsymbol{q}}^T \boldsymbol{M}(\boldsymbol{q})\dot{\boldsymbol{q}} \right) \tag{7.78}$$

Từ công thức (7.60) ta có:

$$\dot{\boldsymbol{q}}^T \{ \dot{\boldsymbol{M}}(\boldsymbol{q}) – 2\boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}}) \} \dot{\boldsymbol{q}} = 0 \tag{7.79}$$

Điều này chỉ ra rằng $\{ \dot{\boldsymbol{M}}(\boldsymbol{q}) – 2\boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}}) \}$ là một Ma trận phản đối xứng (Skew-symmetric matrix / 歪対称行列). $\boldsymbol{q}_r$ là một quỹ đạo mục tiêu giả định, và nếu ta định nghĩa Sai số bám (Tracking error / 追従誤差) đối với quỹ đạo mục tiêu $\boldsymbol{q}_d$ là:

$$\boldsymbol{e} = \boldsymbol{q}_d – \boldsymbol{q} \tag{7.80}$$

thì:

$$\dot{\boldsymbol{q}}_r = \dot{\boldsymbol{q}}_d + \boldsymbol{\Lambda}\boldsymbol{e} \tag{7.81}$$

Trong đó, $\boldsymbol{\Lambda}$ là một ma trận đối xứng dương kích thước $n \times n$. Lúc này, nếu gọi giá trị ước lượng của $\boldsymbol{a}$ là $\hat{\boldsymbol{a}}$, và dùng dấu ^ để biểu thị các giá trị được tính toán từ giá trị ước lượng, từ công thức (7.77) ta có:

$$\hat{\boldsymbol{M}}(\boldsymbol{q})\ddot{\boldsymbol{q}}_r + \hat{\boldsymbol{C}}(\boldsymbol{q}, \dot{\boldsymbol{q}})\dot{\boldsymbol{q}}_r + \hat{\boldsymbol{g}}(\boldsymbol{q}) = \boldsymbol{Y}(\boldsymbol{q}, \dot{\boldsymbol{q}}, \dot{\boldsymbol{q}}_r, \ddot{\boldsymbol{q}}_r)\hat{\boldsymbol{a}} \tag{7.82}$$

Sử dụng ma trận $\boldsymbol{Y}$ này, luật điều khiển và luật ước lượng được cho như sau:

$$\boldsymbol{\tau} = \hat{\boldsymbol{M}}\ddot{\boldsymbol{q}}_r + \hat{\boldsymbol{C}}\dot{\boldsymbol{q}}_r + \hat{\boldsymbol{g}} – \boldsymbol{K}_D\boldsymbol{s} \tag{7.83}$$
$$\dot{\hat{\boldsymbol{a}}} = -\boldsymbol{\Gamma}\boldsymbol{Y}^T\boldsymbol{s} \tag{7.84}$$

Trong đó, $\boldsymbol{K}_D$ là ma trận xác định dương $n \times n$, $\boldsymbol{\Gamma}$ là ma trận xác định dương $m \times m$, và $\boldsymbol{s}$ được định nghĩa là:

$$\boldsymbol{s} = \dot{\boldsymbol{q}} – \dot{\boldsymbol{q}}_r = -\dot{\boldsymbol{e}} – \boldsymbol{\Lambda}\boldsymbol{e} \tag{7.85}$$

Việc chứng minh rằng Luật điều khiển thích nghi (Adaptive control law / 適応制御則) được đưa ra bởi công thức (7.83), (7.84) đảm bảo tính hội tụ toàn cục về quỹ đạo mục tiêu có thể được thực hiện thông qua Định lý ổn định Lyapunov như dưới đây. Đặt sai số ước lượng là $\tilde{\boldsymbol{a}} = \hat{\boldsymbol{a}} – \boldsymbol{a}$, và xét Hàm Lyapunov ứng viên là:

$$V(t) = \frac{1}{2} (\boldsymbol{s}^T\boldsymbol{M}\boldsymbol{s} + \tilde{\boldsymbol{a}}^T\boldsymbol{\Gamma}^{-1}\tilde{\boldsymbol{a}}) \tag{7.86}$$

Hàm này rõ ràng là một hàm xác định dương. Tiếp theo, ta lấy đạo hàm của hàm này và sắp xếp lại, tận dụng việc $\dot{\boldsymbol{M}} – 2\boldsymbol{C}$ là ma trận phản đối xứng:

$$\begin{aligned} \dot{V}(t) &= \frac{1}{2} \boldsymbol{s}^T (\dot{\boldsymbol{M}}\boldsymbol{s} + 2\boldsymbol{M}\dot{\boldsymbol{s}}) + \dot{\tilde{\boldsymbol{a}}}^T\boldsymbol{\Gamma}^{-1}\tilde{\boldsymbol{a}} \quad \text{(Lấy đạo hàm)} \\ &= \boldsymbol{s}^T\boldsymbol{C}(\dot{\boldsymbol{q}} – \dot{\boldsymbol{q}}_r) + \boldsymbol{s}^T\boldsymbol{M}(\ddot{\boldsymbol{q}} – \ddot{\boldsymbol{q}}_r) + \dot{\tilde{\boldsymbol{a}}}^T\boldsymbol{\Gamma}^{-1}\tilde{\boldsymbol{a}} \quad \text{(Thay công thức (7.85))} \\ &= \boldsymbol{s}^T(\boldsymbol{\tau} – \boldsymbol{M}\ddot{\boldsymbol{q}}_r – \boldsymbol{C}\dot{\boldsymbol{q}}_r – \boldsymbol{g}) + \dot{\tilde{\boldsymbol{a}}}^T\boldsymbol{\Gamma}^{-1}\tilde{\boldsymbol{a}} \quad \text{(Thay công thức (7.63))} \\ &= -\boldsymbol{s}^T\boldsymbol{K}_D\boldsymbol{s} + \tilde{\boldsymbol{a}}^T(\boldsymbol{\Gamma}^{-1}\dot{\hat{\boldsymbol{a}}} + \boldsymbol{Y}^T\boldsymbol{s}) \quad \text{(Thay công thức (7.82))} \end{aligned} \tag{7.87}$$

Vào hệ thức trên, ta thay công thức (7.84) và nhận được:

$$\dot{V}(t) = -\boldsymbol{s}^T\boldsymbol{K}_D\boldsymbol{s} \le 0 \tag{7.88}$$

Từ đây, $V(t)$ là một Hàm Lyapunov. Do đó, ta có thể kết luận rằng khi $t \to \infty$ thì $\boldsymbol{s} \to \boldsymbol{0}$. Từ công thức (7.85), điều này đồng nghĩa với việc khi $t \to \infty$ thì $\boldsymbol{e} \to \boldsymbol{0}, \dot{\boldsymbol{e}} \to \boldsymbol{0}$. Tuy sai số quỹ đạo hội tụ về 0 không bảo đảm rằng các tham số sẽ hội tụ về giá trị thực, nhưng từ công thức (7.84), giá trị ước lượng sẽ trở thành vector hằng số $\hat{\boldsymbol{a}}(\infty)$. Ở đây, nếu ma trận $\boldsymbol{Y}(\boldsymbol{q}, \dot{\boldsymbol{q}}, \dot{\boldsymbol{q}}_r, \ddot{\boldsymbol{q}}_r)$ thỏa mãn Điều kiện kích thích liên tục (Persistently exciting condition / 持続的励振条件) và là Liên tục đều (Uniformly continuous / 一様連続), thì khi $t \to \infty$, luật điều khiển sẽ thiết lập được mối quan hệ $\boldsymbol{\tau} = \boldsymbol{Y}\hat{\boldsymbol{a}}(\infty) = \boldsymbol{Y}\boldsymbol{a}$, do đó giá trị ước lượng có thể được coi là hội tụ tiệm cận về giá trị thực của tham số. Cần lưu ý rằng, phương pháp này không đòi hỏi phải đo lường gia tốc.

Việc tính toán thời gian thực của công thức (7.83), (7.84) không phải lúc nào cũng dễ dàng, nhưng nếu ta giới hạn các tham số chưa biết chỉ ở tải trọng của Khâu tác động cuối thì khối lượng tính toán sẽ giảm đi đáng kể. Khi áp dụng cho điều khiển Robot trong thực tế, việc sử dụng các phương pháp như nhận dạng tham số để giảm thiểu số lượng tham số chưa biết xuống mức thấp nhất có thể sẽ mang lại nhiều lợi thế hơn.

Chú thích:

†1 Điều kiện kích thích liên tục: Tại một thời điểm tùy ý $t > 0$, nếu tồn tại một hằng số dương $\alpha$, $T > 0$ thỏa mãn $\int_t^{t+T} \boldsymbol{Y}^T(\boldsymbol{q}, \dot{\boldsymbol{q}}, \dot{\boldsymbol{q}}_r, \ddot{\boldsymbol{q}}_r)\boldsymbol{Y}(\boldsymbol{q}, \dot{\boldsymbol{q}}, \dot{\boldsymbol{q}}_r, \ddot{\boldsymbol{q}}_r) d\tau \ge \alpha \boldsymbol{I}$, thì gọi là điều kiện kích thích liên tục. Ở đây, $\boldsymbol{I}$ là Ma trận đơn vị.

†2 Liên tục: Với một số thực tùy ý $a$ và một số dương tùy ý $\varepsilon$ nằm trong khoảng xem xét, nếu tồn tại một số dương $\delta$ sao cho khi $|x – a| < \delta$ thì $|f(x) – f(a)| < \varepsilon$, hàm $f$ được gọi là liên tục.

Liên tục đều: Với một số thực tùy ý $a$ và một số dương tùy ý $\varepsilon$ nằm trong khoảng xem xét, nếu tồn tại một số dương $\delta$ không phụ thuộc vào $a$, sao cho khi $|x – a| < \delta$ thì $|f(x) – f(a)| < \varepsilon$, hàm $f$ được gọi là liên tục đều.

(Lưu ý) Đối với “Liên tục”, chỉ cần tìm được một $\delta$ phù hợp ứng với $a$ là đủ, còn “Liên tục đều” thì phải tìm được một $\delta$ chung không phụ thuộc vào $a$.

【Bài toán mẫu 7.5】 Đối với cánh tay Robot 1 khớp như trong Hình 7.15, hãy tìm luật điều khiển và luật ước lượng bằng phương pháp điều khiển thích nghi với giả định rằng Tham số động lực học là chưa biết.

Hình 7.15: Cánh tay 1 khớp

Phương trình chuyển động là:

$$\tau = I\ddot{\theta} + mL_g g\cos\theta = [g\cos\theta, \ddot{\theta}][mL_g, I]^T$$

Từ đây, tham số chưa biết là:

$$\boldsymbol{a} = [a_1, a_2]^T = [mL_g, I]^T$$

Do đó, luật điều khiển và luật ước lượng sẽ là:

$$\tau = \hat{a}_2\ddot{\theta}_r + \hat{a}_1 g\cos\theta – K_D s$$
$$\dot{\hat{a}}_1 = -\gamma_1 g s\cos\theta$$
$$\dot{\hat{a}}_2 = -\gamma_2 \ddot{\theta}_r s$$

Trong đó, $\ddot{\theta}_r$ và $s$ là:

$$\ddot{\theta}_r = \ddot{\theta}_d + \lambda(\dot{\theta}_d – \dot{\theta})$$
$$s = -(\dot{\theta}_d – \dot{\theta}) – \lambda(\theta_d – \theta)$$

Trong đó, $[\theta_d, \dot{\theta}_d, \ddot{\theta}_d]$ là quỹ đạo mục tiêu. Bằng cách thiết lập các tham số điều khiển $\gamma_1$, $\gamma_2$, $\lambda$, $K_D$ một cách phù hợp, có thể hiện thực hóa điều khiển thích nghi.

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

7.1 Giả sử điểm đầu $q_0 = 0$, điểm cuối $q_f = 1$, thời gian kết thúc $t_f = 1$, hãy tạo quỹ đạo bằng phép nội suy bậc 1, nội suy bậc 3 và nội suy bậc 5, đồng thời vẽ đường cong vị trí, vận tốc và gia tốc cho mỗi phép nội suy.

7.2 Hãy chứng minh bằng Hàm Lyapunov rằng hệ tự trị $\dot{\boldsymbol{x}}(t) = -\boldsymbol{x}(t)$ là ổn định tiệm cận.

7.3 Đối với cánh tay 2 khớp đã trình bày trong Bài toán mẫu 6.2, khi quỹ đạo mục tiêu là $\boldsymbol{\theta}_d(t) = [\theta_{d1}(t), \theta_{d2}(t)]^T$ và được điều khiển bằng Điều khiển mô-men tính toán, hãy tìm Lực đầu vào điều khiển (Control input / 制御入力) $\boldsymbol{\tau} = [\tau_1, \tau_2]^T$. Giả sử:

$$\boldsymbol{K}_v = \text{diag}(2\xi\omega_c, 2\xi\omega_c), \quad \boldsymbol{K}_p = \text{diag}(\omega_c^2, \omega_c^2)$$

7.4 Đối với cánh tay 2 khớp đã trình bày trong Bài toán mẫu 6.2, khi quỹ đạo mục tiêu là $\boldsymbol{r}_d(t) = [r_{d1}(t), r_{d2}(t)]^T$ và được điều khiển bằng Điều khiển gia tốc phân giải, hãy tìm Lực đầu vào điều khiển $\boldsymbol{\tau} = [\tau_1, \tau_2]^T$. Giả sử:

$$\boldsymbol{K}_v = \text{diag}(2\xi\omega_c, 2\xi\omega_c), \quad \boldsymbol{K}_p = \text{diag}(\omega_c^2, \omega_c^2)$$

7.5 Đối với cánh tay 2 khớp trong Bài tập thực hành 5.1, giả sử Tham số động lực học chưa được biết, hãy tìm luật điều khiển và luật ước lượng bằng Điều khiển thích nghi.

7.7 Cảm nhận

1. Vai trò của Chương 7 trong Kỹ thuật Robot

Nếu như các chương trước (Động học, Động lực học) cung cấp “mô hình toán học” (bản vẽ kỹ thuật, phương trình vật lý), và Chương 6 cung cấp “sự tinh chỉnh” (để mô hình khớp với thực tế), thì Chương 7 chính là “bộ não chỉ huy” và “hệ thần kinh vận động” của Robot.

Nó đóng vai trò là chiếc cầu nối chuyển hóa các phương trình toán học khô khan thành những lệnh điều khiển dòng điện/điện áp cấp cho động cơ, nhằm ép cánh tay robot vật lý di chuyển đúng theo ý muốn của con người. Khônng có Chương 7, robot chỉ là một cỗ máy đứng im hoặc chuyển động giật cục, loạn xạ.

2. Các bài toán cốt lõi được giải quyết trong Chương 7

Chương này chia làm hai mảng lớn, giải quyết hai câu hỏi cơ bản: “Robot phải đi theo đường nào?” và “Làm sao để ép động cơ bám sát đường đi đó?”.

Bài toán 1: Lập kế hoạch và tạo đường đi trơn tru (Tạo quỹ đạo – Trajectory Generation)

  • Vấn đề: Con người chỉ ra lệnh “Gắp vật ở điểm A, bỏ sang điểm B”. Nhưng cánh tay robot không thể dịch chuyển tức thời. Nếu chỉ ra lệnh vị trí, động cơ sẽ giật đột ngột (gây hỏng hóc cơ khí) và chuyển động sẽ rất thô bạo.

  • Cách giải quyết: Chương này sử dụng các kỹ thuật Nội suy toán học (Interpolation) để lấp đầy khoảng trống giữa điểm A và điểm B theo thời gian.

    • Sử dụng các Đa thức thời gian (Bậc 3, Bậc 5, hoặc cấu hình 4-3-4, 4-1-4) để đảm bảo không chỉ Vị trí mà cả Vận tốc và Gia tốc đều biến thiên liên tục, trơn tru. (Gia tốc không bị giật cục nghĩa là lực cấp cho động cơ sẽ mượt mà).

    • Giải quyết bài toán nội suy không gian (nội suy góc quay từng khớp) và nội suy tọa độ làm việc (ép tay gắp đi theo đường thẳng tắp). Đồng thời đưa ra Phương pháp quay 1 trục để xoay tư thế tay gắp một cách tự nhiên nhất, tránh các lỗi kỳ dị của Góc Euler.

Bài toán 2: Điều khiển Servo cơ bản (Bám sát quỹ đạo ở tốc độ thấp)

  • Vấn đề: Khi đã có quỹ đạo lý tưởng (vị trí, vận tốc mục tiêu ở từng mili-giây), làm sao để động cơ quay đúng như vậy trong khi cánh tay rất nặng?

  • Cách giải quyết (Servo khớp / Joint Servo): Sử dụng các bộ điều khiển phản hồi truyền thống (như PD, PID).

    • Hệ thống liên tục so sánh góc quay hiện tại với góc quay mục tiêu để tạo ra Sai số (Error).

    • Dùng sai số này để tăng giảm điện áp cấp cho động cơ.

    • Đặc điểm: Phương pháp này coi mỗi khớp là một hệ thống độc lập. Nó rất đơn giản, dễ áp dụng và là nền tảng của 90% robot công nghiệp hiện nay. Nhược điểm là khi robot chạy nhanh, các khớp sẽ “chống lại nhau” (lực ly tâm, lực Coriolis), khiến phương pháp này bị giảm độ chính xác.

Bài toán 3: Điều khiển động lực học (Bám sát quỹ đạo ở tốc độ cao và độ chính xác cực cao)

  • Vấn đề: Khi robot di chuyển cực nhanh (ví dụ: robot gắp hạt linh kiện điện tử tốc độ cao), khối lượng của tay đòn sinh ra Lực quán tính, Lực ly tâm và Lực Coriolis rất lớn. Các lực này biến thiên liên tục và phá vỡ sự ổn định của bộ điều khiển PID thông thường.

  • Cách giải quyết: Sử dụng Điều khiển động (Dynamic Control), tiêu biểu là phương pháp Điều khiển mô-men tính toán (Computed Torque Control) và Điều khiển gia tốc phân giải (Resolved Acceleration Control).

    • Thay vì chỉ dùng “sai số” để phản hồi, bộ điều khiển sẽ dùng toàn bộ phương trình Động lực học (ở Chương 5) để “đoán trước” các lực cản trở (Trọng lực, Quán tính, Coriolis).

    • Nó chủ động xuất ra một lượng Mô-men chính xác để triệt tiêu hoàn toàn (tuyến tính hóa) các lực phi tuyến phức tạp này ngay trước khi chúng kịp gây ra sai số (Kỹ thuật Feedforward – Bù tiến).

    • Kết quả là robot có thể chuyển động ở tốc độ cực cao mà độ chính xác quỹ đạo vẫn giữ nguyên ở mức micromet.

Bài toán 4: Đối phó với sự thay đổi trong quá trình vận hành (Điều khiển thích nghi – Adaptive Control)

  • Vấn đề: Điều khiển động lực học ở trên yêu cầu phải biết chính xác khối lượng của robot. Nhưng nếu robot bất ngờ gắp một vật rất nặng (khối lượng thay đổi), phương trình tính toán sẽ bị sai, dẫn đến robot bị rung lắc hoặc rớt vật.

  • Cách giải quyết: Áp dụng Điều khiển thích nghi.

    • Hệ thống điều khiển không dùng các thông số tĩnh. Thay vào đó, nó dựa vào sai số quỹ đạo để tự động “cập nhật lại” các thông số khối lượng/quán tính ngay trong thời gian thực (real-time).

    • Đây là bước đệm đưa robot từ một cỗ máy tự động thuần túy trở thành một hệ thống có tính “thích nghi” và “thông minh” một phần.

KIẾN THỨC CƠ BẢN VỀ KỸ THUẬT ROBOT - Previous 6. Phân tích sai số và nhận dạng tham số (Error Analysis and Parameter Identification / 誤差解析とパラメータ同定) Next - KIẾN THỨC CƠ BẢN VỀ KỸ THUẬT ROBOT 8. Điều khiển Lực của Robot (Robot Force Control / ロボットの力制御)

Recently Viewed articles

Leave a Comment

Chia sẻ:

7. Điều khiển Vị trí / Quỹ đạo của Robot (Robot Position/Trajectory Control / ロボットの位置／軌道制御)

Or copy link

CONTENTS