Chương 2 mô tả cấu hình và vị trí của robot với các tọa độ tổng quát như sau:
[ ] (4.1) Để dễ dàng trong việc trình bày phương pháp, ta ký hiệu các tọa độ tổng quát này như sau : (4.2) Vi phân bậc nhất: ̇ ̇ ̇ ̇ ̇ ̇ ̇ (4.3) Vi phân bậc hai: ̈ ̇ ̈ ̇ ̈ ̇ ̈ ̇ ̈ ̇ ̈ ̇ ̈ ̇ (4.4) Từ phương trình động học của robot[8] (đã trình bày trong chương 2):
( ) ̈ ( ̇ ) (4.5) Suy ra:
̈ ( ) ( ̇ ) (4.6) Trong vector ( ̇ ) (xem phụ lục), đây là một vector có 7 hàng và 1 cột. Ta để ý rằng trong vector này, 3 dòng đầu không có chứa thành phần mômen điều khiển , các mômen này chỉ xuất hiện trong 4 dòng tiếp theo của vector ( ̇ ). Trong phương pháp điều khiển momen tính, ta phải tách riêng các mômen điều khiển riêng với các thành phần khác. Từ đó, vector ( ̇ ) có thể được tách thành 2 vector như sau:
( ̇ )
[ ] [ ]
(4.7)
Các giá trị chính là các mômen điều khiển Do đó từ biểu thức (4.6) ta có thể viết lại như sau:
̈ ( )
[ ]
( ) [ ]
(4.8)
Viết gọn lại như sau:
̈ ( ) ( ) * + (4.9) Tín hiệu ngõ ra để điều khiển cho robot đi được chính là các giá trị góc của thân ( ), khớp hông ( ), khớp gối ( ) của robot. Đặt các tín hiệu điều khiển này như sau, theo (4.2):
(4.10)
Từ phương trình (4.9), ta thấy là bên trái của phương trình là vi phân cấp 2 của các tọa độ tổng quát, bên phải của phương trình chính là ma trận quán tính A được nghịch
đảo và tích ma trận với vector b cộng với tích của ma trận nghịch đảo A và vector u. Trong phương pháp điều khiển mômen tính, cần phải tách riêng các mômen điều khiển, sau đó sử dụng các tham số liên quan để tính toán mômen.
Với : Lấy vi phân bậc nhất hai vế, ta được ̇ ̇ ̇ . Ta thấy vẫn chưa xuất hiện thành phần mômen, do đó ta tiếp tục vi phân bậc hai ̈ ̇ ̈.
Theo (4.9) ta được:
̈ * + (4.11) Trong đó là một ma trận mới, được lấy ở dòng thứ 3 của ma trận nghịch đảo của ma trận ( ).
Với : Lấy vi phân bậc nhất, ̇ ̇ ̇ ̇ ̇ . Vi phân bậc hai, ta được, ̈ ̇ ̇ ̈ ̈ .
Theo (4.9) ta được:
̈ * + * + (4.12) Trong đó là các ma trận mới, lần lượt được lấy ở dòng thứ 5 và ở dòng thứ 4 của ma trận nghịch đảo của ma trận ( ).
Với : Tính toán tương tự ta có:
̈ * + (4.13) Trong đó là một ma trận mới, được lấy ở dòng thứ 6 của ma trận nghịch đảo của ma trận ( ).
̈ * + (4.14) Trong đó là một ma trận mới, được lấy ở dòng thứ 7 của ma trận nghịch đảo của ma trận ( ).
Gọi là sai số giữa tín hiệu ngõ ra và tín hiệu đặt (tín hiệu tham chiếu: ). Tín hiệu đặt ở đây chính là các góc mà ta mong muốn cho robot chuyển động theo. Ta có . Để sai số giữa tín hiệu ra và tín hiệu tham chiếu tiến về 0 khi thì phương trình sau phải thỏa mãn:
̈ ̇ (4.15) Trong đó và là các hằng số dương được chọn sao cho đủ nhanh và không có dao động. Từ (4.15) ta có phương trình đặc trưng như sau:
(4.16)
Nghiệm của phương trình đặc trưng này có dạng: √ . Chọn giá trị để có giá trị thỏa mãn (4.16). Cách chọn giá trị và sẽ được trình bày trong phần 4.3.
Với tín hiệu thứ nhất : . Theo (4.15) ta được: ̈ ̇
Thế vào, ta được: ̈ ̇ ̈ ̇
Thế lần lượt các tham số ̈ ̇ vào, ta được:
̈ ̇
Chuyển vế :
Với tín hiệu thứ hai : Tính toán tương tự ta có : ̈ ̇ ̈ ̇ Thế lần lượt các tham số ̈ ̇ vào, ta được:
( ) ( ) ( ) ( ) ̈ ̇
Chuyển vế :
( ) (4.18)
( ) ( ) ( ) ̈ ̇ Với tín hiệu thứ ba : Tương tự ta có :
̈ ̇ ̈ ̇ Thế lần lượt các tham số ̈ ̇ vào, ta được:
̈ ̇
Chuyển vế :
̈ ̇ (4.19) Với tín hiệu thứ tư : Tương tự :
̈ ̇ ̈ ̇ Thế các tham số ̈ ̇ vào , ta được:
̈ ̇
Chuyển vế :
Từ (4.17), (4.18), (4.19) và (4.20) ta có thể gom nhóm : [ ] (4.21) [ ̈ ̇ ( ) ( ) ( ) ̈ ̇ ̈ ̇ ̈ ̇ ] Ma trận [ ]
được định nghĩa như sau: là một ma trận có kích cỡ 4x4.
Các phần tử trong ma trận này được lấy từ ma trận nghịch đảo của ma trận ( ). Nếu viết rõ ràng thì ma trận này sẽ như sau:
Từ công thức (4.21) ta thấy là ta đã tính toán được vector mômen thông qua các tham số khác của cấu hình hệ robot. Phần 4.3 sẽ là phần trình bày về cách chọn các giá trị và sao cho sai số của hệ thống là nhỏ nhất. Phần 4.3 cũng sẽ trình bày về bộ điều khiển mômen tính trên nền Matlab/Simulink
4.3.1. Giá trị và trong độ sai biệt giữa tín hiệu ra – tín hiện tham chiếu
Giá trị và được chọn ngẫu nhiên sao cho sai biệt giữa tín hiệu ra – tín hiệu tham chiếu là nhỏ nhất. Để kiểm tra hai giá trị này, ta tạo một chương trình kiểm tra trong Matlab/Simulink[19] như sau:
Hình 4.2. Chƣơng trình kiểm tra hai giá trị và
Nút “khởi động” dùng để khởi động giá trị và . Ở đây ta chọn và . Chương trình dùng một hàm truyền để tính toán đáp ứng ngõ ra so với ngõ vào. Tín hiệu ngõ vào là hàm bậc đơn vị. Ta dùng một màn hình để quan sát tín hiệu ngõ ra và ngõ vào.
Hình 4.3. Đáp ứng của tín hiệu ngõ ra và ngõ vào
thấy, tín hiệu khi qua hàm truyền thì trong khoảng thời gian từ 0.07 đến 0.15 thì tín hiệu tiến dần đến tín hiệu nguyên thủy. Điều này có nghĩa là sai số giữa ngõ ra và ngõ vào của hệ thống sẽ dần đến zero.
Từ đó ta thấy, việc chọn những giá trị và là quan trọng bởi vì những giá trị này quyết định đến sự ổn định của hệ thống (hệ thống nhanh ổn định khi sai số tiến nhanh về zero). Đối với hệ thống robot, ta chọn các cặp giá trị ở (4.21) như sau :
và
và
và
và