∗ Điều khiển đầu vào không gian khớp: Thay hệ phương trình (2.20) vào (2.19) được
(2.21)
Đặt sai lệch vị trí và sai lệch vận tốc trong không gian khớp là:
Theo phương trình (2.21) ta đề xuất đưa ra luật điều khiển có dạng sau:
(2.22)
Ởđây: là gia tốc đặt trước. là các hằng số dương.
là luật điều khiển bền vững mới. Thay (2.22) vào (2.21) và rút gọn ta được:
47
Đặt:
(2.24) Coi tất cả các khớp là quay và theo các giả thiết phía trên ta có:
(2.25) Ta chuyển (2.25) sang dạng sau:
(2.26) Với là hằng số dương.
Đặt: phương trình (2.23) trở thành hệ phương trình sau: (2.27) Hệ kín (2.27) có thểđiều khiển bằng phương pháp backstepping, do đó ta chọn là luật điều khiển cho đến khi hội tụ về 0. Do đó ta đề xuất bằng :
(2.28) Với: là hằng số dương.
∗ Chứng minh tính ổn định (2.28) .Ta chọn hàm Lyapunov như sau:
(2.29) Lấy đạo hàm (2.29) theo thời gian ta được:
(2.30) Thay (2.27) và (2.28) vào (2.30) ta được:
(2.31) Theo (2.31) ta thấy rằng , do đó luôn luôn hội tụ về không. ∗ Luật điều khiển trong không gian khớp:
Luật điều khiển backstepping được hình thành trên cơ sở chứng minh tính ổn
định của hệ thống vòng kín (2.27). Ta chọn hàm Lyapunov như sau:
48
(2.32)
Đạo hàm (2.32) theo thời gian được:
(2.33) Thay (2.27), (2.28) vào (2.33) ta được:
(2.34) Rút gọn (2.34) ta được: (2.35) Theo các giả thiết ta có: (2.36) Theo (2.36), luật điều khiển mới (Robot control law) được viết như sau:
(2.37) Trong đó: γ là hằng số dương.
Thay (2.37) vào (2.36) ta được:
(2.38) Theo (2.26), (2.27), (2.28) và (2.38) ta có: (2.39) Ởđây: và là các hằng số dương. Theo (2.37) và (2.39), và là các hệ sốđiều khiển. Để đảm bảo ta phải lựa chọn những hệ số trên cho phù hợp (để
49
Do vậy hệ thống vòng kín (2.27) ổn định toàn cục phụ thuộc vào các hệ số điều khiển , .
Tóm lại , luật điều khiển trong không gian khớp là: (adsbygoogle = window.adsbygoogle || []).push({});
(2.40)
Với :
Như vậy thuật toán Backstepping trong không gian khớp không có gì phức tạp và ta hoàn toàn có thểđiều chỉnh các hệ sốđiều khiển để đạt được chất lượng mong muốn . Tuy nhiên , trong thực tế nhiều ứng dụng cần robot phải di chuyển tới những vị trí mong muốn gắn với hệ trục tọa độ cố định , mà việc chuyển đổi các thông số đặt trong không gian làm việc sang không gian khớp là gặp khó khăn do nghiệm của bài toán động học ngược khó giải và không phải duy nhất . Vì vậy mà chúng ta cần thiết lập một luật điều khiển Backstepping cho cả không gian làm việc nhưđược trình bày dưới đây .
2.4.3. Luật điều khiển backstepping trong không gian làm việc:
Như đã chứng minh ở trên thì hệ thống vòng kín (2.27) ổn định toàn cục khi sử dụng luật điều khiển trong không gian khớp (2.40), nhưng theo phần giới thiệu, thì đểđảm bảo bám theo quỹđạo một cách chính xác trong không gian làm việc mà sử dụng luật (2.40) thì không được. Ta phải chuyển luật điều khiển robot backstepping từ không gian khớp sang không gian làm việc.
Phương trình động lực trong không gian khớp được biểu diễn như sau:
(2.41)
Trong đó:
Trong phương trình có chưa các thành phần không xác định nên ta tách (2.41) thành 2 thành phần: Thành phần xác định (known), thành phần không xác định (unknown).
50
Chứng minh hệ thống (2.41) ổn định toàn cục, tương tự như chứng minh trong không gian khớp ta có sử dụng hàm Lyapunov sau:
Từđó rút ra được luật điều khiển trong không gian làm việc như sau:
(2.42) Với :
Trong đó: và γ là các hằng số dương.
là vectơ vận tốc đặt trong không gian làm việc.
là vectơ sai lệch vị trí và vận tốc tương ứng trong không gian làm việc.
∗ Thay đổi luật điều khiển trong không gian làm việc:
Một vấn đề quan trong trong luật điều khiển backstepping là yêu cầu tính nhạy phải được xem xét ở đây. Luật điều khiển (2.42) được tao ra bằng cách đo các giá trị: vị trí góc khớp , vận tốc , vị trí điểm cuối , vận tốc điểm cuối . Vị trí khớp thường được đo bằng encoder, vận tốc khớp có thểđo trực tiếp hoặc đạo hàm của , được đo bằng hệ thống quan sát (vision). Tuy nhiên thường không đo lường mà có thểđược tính toán.
Luật điều khiển (2.40) được thay đổi như sau:
(2.43) Với :
51
Trong đó: là ma trận ước lượng Jacoby.
Luật điều khiển (2.43) lúc này chỉ cần đo các giá trị và vị trí điểm cuối . Do đó luật (2.43) được dùng trong thực tế nhiều hơn so với luật (2.42). Nhưng do ma trận Jacoby là ước lượng nên trong luật (2.43) có sai lệch tính toán vận tốc. Do vậy hệ thống vòng kín ổn định trong một giới hạn bị chặn đều cho phép khi sử dụng luật điều khiển (2.43).
∗ Kết luận
Luật điều khiển trong không gian làm việc không cần đo giá trị vận tốc .
Để xác định sai lệch vận tốc chỉ cần tính toán thông qua ma trận Jacoby ước lượng và giá trị đặt Trong đó, ma trận Jacoby ước lượng ta chỉ cần dùng các thông số
robot là các đại lượng ước lượng mà không cần phải biết chính xác. (adsbygoogle = window.adsbygoogle || []).push({});
Sai lệch giữa vị trí thực và vị trí đặt trong không gian làm việc sẽđược giảm
52
CHƯƠNG 3
ÁP DỤNG PHƯƠNG PHÁP ĐIỀU KHIỂN BACKSTEPPING
CHO ROBOT SCARA 4 BẬC TỰ DO
53
Hình 3.1 Hình ảnh robot SCARA (của hãng Serpent và Adept one).
SCARA (Selectively Compliance Assembly Robot Arm) là tay máy lắp ráp chọn lọc. Robot SCARA có 4 bậc tự do với cấu tạo 2 khớp quay ở cánh tay, 1 khớp quay ở
cổ tay và 1 khớp tịnh tiến, các khớp quay hoạt động nhờ các động cơđiện một chiều có phản hồi vị trí tạo 1 vòng điều khiển kín, chuyển động thẳng đứng được thực hiện bằng pittông khí nén hoặc trục vít thanh răng.
Chiều cao của Robot có thể thay đổi dễ dàng bằng cách thay đổi vị trí gá thân robot trên trục cơ bản, giúp tay máy thuận lợi trong việc thay đổi công việc.
Với thiết kếđộng cơ truyền động cho cổ tay được đặt trên trục cơ bản và liên hệ
với cổ tay bằng đai truyền, nên nó đảm bảo được góc quay của cổ tay không thay đổi trong quá trình tay máy chuyển động.
Truyền động cho 2 khớp của tay máy và cổ tay bằng động cơ servo một chiều có phản hồi vị trí tạo thành một vòng điều khiển kín. Chuyển động thẳng đứng được thực hiện bằng piton khí nén.
Robot Scara Serpent có thểđược lập trình từ máy tính bằng cách đặt dữ liệu cho mỗi trục. Hoặc điều khiển bằng tay sử dụng thiết bị lái điện (steering) cho tay máy dùng các cuộn dây điện từ trong giá treo (pendant).
Đối với robot Scara serpent thì giới hạn góc quay là xung quanh trục x và xung quanh trục thanh nối 1. Như vậy không gian làm việc của robot là một phần hình trụ có bán kính giới hạn trong và giới hạn ngoài tùy thuộc chiều dài 2 thanh nối 1 và 2.
54
Hình 3.2. Không gian làm việc của một loại robot Scara Serpent.
* Gắn các hệ tọa độ:
+ Hệ tọa độ { } cốđịnh gắn với thân Robot.
+ Quan hệ giữa hệ tọa độ { } với { } : quay góc quanh trục ; tịnh tiến đoạn trên trục .
+ Quan hệ giữa hệ tọa độ { } với { } : quay góc quanh trục , tịnh tiến đoạn trên trục và quay góc trên quanh trục .
+ Quan hệ giữa hệ tọa độ { } với { } : tịnh tiến đoạn trên trục . + Quan hệ giữa hệ tọa độ { } với { } : quay góc quanh trục . Đây là hệ tọa độ gắn với tay kẹp, khâu tác động cuối.
55
Hình 3.3. Chọn các hệ tọa độ gắn với các khớp của robot SCARA.
Bài toán động học thuận cho phép xác định vị trí của tay kẹp robot trong không gian làm việc từ các biến khớp. Bảng thông số D – H: Thanh nối 1 2 3 4 Ma trận biểu diễn quan hệ giữa khung tọa độ với
56
(3.1)
Lần lượt thay các thông số, ta được:
;
= (3.2) (adsbygoogle = window.adsbygoogle || []).push({});
Ma trận biểu diễn tọa độ tay máy trong hệ trục tọa độ gốc: ; Ta lần lượt tính:
; ;
;
57 Rút gọn: (3.3) Trong đó: Theo ký hiệu tổng quát: , vậy ta có hệ phương trình động học thuận là : ; ; ; ; ; ; ; ; ; (3.4)
3.2. Giải bài toán động học ngược robot SCARA :
Cho phép xác định vị trí các khớp khi đã biết tọa độ tay kẹp robot trong khung tọa độ
gốc.
58 Từ các phương trình động học thuận:
(3.5) Lại có:
Và :
59 Thay ngược trở lại tìm
(3.6) Từ phương trình động học thuận có ngay:
Với và đã tính được ở trên : (3.7) 3.3.Động lực học robot SCARA : Hàm Lagrange :
60 Trong đó là tổng động năng, là tổng thế năng hệ thống . Phương trình động lực học: Với là biến khớp: với khớp quay và với khớp tịnh tiến . lực đặt lên khớp tịnh tiến. mômen khớp quay động năng thanh nối thế năng thanh nối
61
Hình 3.4. Tọa độ khối tâm các thanh nối. robot SCARA
Thanh nối 1:
Tọa độ của khối tâm được xác định theo phương trình sau :
Hai thành phần của tốc độ khối tâm của thanh nối 1 là:
Bình phương tốc độ dài của khối tâm thanh nối 1 là :
Động năng của khối tâm thanh nối 1là:
(3.8) Thế năng thanh nối 1là:
(3.9)
62
Tọa độ của khối tâm được xác định theo phương trình sau :
Hai thành phần của tốc độ khối tâm của thanh nối 2 là:
Bình phương tốc độ dài của khối tâm thanh nối 2 là :
Động năng của khối tâm thanh nối 2 là:
(3.10)
Thế năng thanh nối 2 là: (adsbygoogle = window.adsbygoogle || []).push({});
(3.11)
Thanh nối 3:
63 Ba thành phần của tốc độ khối tâm của thanh nối 3
Bình phương tốc độ dài của khối tâm thanh nối 3 là :
Động năng của khối tâm thanh nối 3 là:
(3.12) Thế năng của thanh nối 3 tính theo:
(3.13)
Thanh nối 4:
Tọa độ của khối tâm được xác định theo phương trình sau :
64 Động năng của khối tâm thanh nối 4 là : (3.14) Thế năng của thanh nối 4 là: (3.15) Nếu đặt khối tâm tại điểm giữa của các thanh nối thì: Tổng động năng là : (3.16) Tổng thế năng là:
65
(3.17)
Phương trình động lực học
Thay động năng và thế năng của các thanh nối vào hàm Lagrange ta được :
(3.18)
Mômen của khớp 1:
Lấy đạo hàm của hàm Lagrange lần lượt theo các biến và và sau một số phép biến đổi ta được:
66 Với :
(3.19)
Mômen của khớp 2:
Lấy đạo hàm của hàm Lagrange lần lượt theo các biến và và sau một số phép biến đổi ta được:
67
(3.20)
Lực của khớp 3:
Lấy đạo hàm của hàm Lagrange lần lượt theo các biến và và sau một số phép biến đổi ta được:
68
(3.21)
Mômen của khớp 4:
Lấy đạo hàm của hàm Lagrange lần lượt theo các biến và và sau một số phép biến đổi ta được:
69 (adsbygoogle = window.adsbygoogle || []).push({});
Phương trình động lực học của robot SCARA có thểđược viết ở dạng phương trình ma trận sau:
Trong đó:
;
(3.23)
Với các thành phần đã tính được phía trên.
3.4. Tính toán ma trận Jacoby :
* Ma trận Jacoby:
Trong đó : và
Theo kết quả phương trình (3.6) ta có:
70
Vậy ma trận Jacoby:
(3.24)
∗ Đạo hàm ma trận Jacoby:
Tính đạo hàm các thành phần trong ma trận Jacoby, ta nhận được phương trình sai phân sau:
72 Vậy ma trận là :
(3.25)
3.5. Áp dụng lí thuyết phương pháp Backstepping vào robot SCARA 4 bậc tự do:
Luật điều khiển trong không gian khớp
∗ Luật điều khiển theo
(2.40):
Với : Trong đó:
tính theo (3.23)
73 Luật điều khiển trong không gian làm việc
∗ Luật điều khiển momen
theo (2.43):
Với :
Trong đó :
tính theo (3.23)
bao gồm cả thành phần masat và nhiễu như sau: (adsbygoogle = window.adsbygoogle || []).push({});
tính theo (3.24) và tính theo (3.25) với các thông số robot là do ta dùng đại lượng ước lượng, tính theo phương trình động học thuận.
Kết quả cụ thểđược nêu trong chương mô phỏng sau :
CHƯƠNG 4
KẾT QUẢ MÔ PHỎNG ĐIỀU KHIỂN CHUYỂN ĐỘNG ROBOT SCARA
BẰNG PHƯƠNG PHÁP BACKSTEPPING VÀ KẾT LUẬN
Nhưđã trình bày ở trên, phương pháp Backstepping có ưu điểm là áp dụng
74
làm việc . Việc mô phỏng cũng được tiến hành trên 2 không gian này với các kết quả như sau :
4.1. Điều khiển trong không gian khớp
∗ Trong thực tế, có rất nhiều mô hình có thể dùng để minh họa cho phương pháp quan sát lực và phương pháp điều khiển mà ta vừa đưa ra ở trên. Các mô hình khác nhau thì sẽ khác nhau về phương trình toán học, các thông số của khớp, các ma trận tính toán… nhưng đều có chung một phương trình động lực học dạng tổng quát. Trong phạm vi của luận văn này, để mô phỏng kết quả của những nghiên cứu lý thuyết ở trên, tác giả sẽ sử dụng mô hình một robot SCARA 4 bậc tự do gồm 3 khớp quay và một khớp tịnh tiến. Phương pháp thiết kế bộ điều khiển Backstepping cho robot này hoàn toàn có thể áp dụng cho nhiều mô hình robot khác.
∗ Ta thiết lập các file.m bao gồm :
¾ robot.m : Thiết lập các thông số, mô hình động lực học của robot SCARA.
¾ quidaokhop.m : Lập quỹ đạo chuyển động cho các khớp. Để các khớp dịch chuyển êm tới vị trí mong muốn, ta xây dựng quỹđạo dạng 2-1-2.
¾ Bcontroller.m : Xây dựng luật điều khiển theo phương pháp Backstepping cho vectơ momen đặt lên các khớp.
¾ mofong.m : Kết hợp các hàm đã khai báo trên lại, chạy file này cho ta kết quả mô phỏng qua các đồ thị.
∗ Các thông số của 1 loại robot Scara Serpent là:
Bảng 4.1. Các thông số của robot SCARA .
75
Khối lượng thanh nối 1 Khối lượng thanh nối 2 Khối lượng thanh nối 3 Khối lượng thanh nối 4
Chiều dài thanh nối 1 Chiều dài thanh nối 2
∗ Giả sử ta điều khiển cho robot chạy từ vị trí đầu chuyển
động theo quỹđạo tới vị trí
Hình 4.1.Ví dụ vị trí robot ban đầu và vị trí cuối.
∗ Sau khi chỉnh các thông số và , ta được một số kết quả sau:
76 0 0.5 1 1.5 2 0 0.5 1 1.5 2 G o c q uay k hop 1 (r a d) thoi gian (s) Goc quay khop 1
q1 thuc q1 dat 0 0.5 1 1.5 2 0 0.2 0.4 0.6 0.8 G o c q uay k hop 2 (r a d) thoi gian (s) Goc quay khop 2
q2 thuc q2 dat 0 0.5 1 1.5 2 0.1 0.15 0.2 0.25 D ich ch u y e n kh o p 3 (r a d ) thoi gian (s) Dich chuyen khop 3
d3 thuc d3 dat 0 0.5 1 1.5 2 0 0.5 1 1.5 go c qu ay k h op 4 (r a d ) thoi gian (s) Goc quay khop 4
q4 thuc q4 dat
Hình 4.2. Kết quả khi và nhỏ. (adsbygoogle = window.adsbygoogle || []).push({});
∗ Ta thấy rằng khi và quá nhỏ thì chưa đủ để làm cho hàm CLF thỏa mãn nên các giá trị thực rất xa vời. Khi tăng dần và đến một ngưỡng nào đó thì quỹ đạo thực bắt đầu bám quỹ đạo đặt , tuy nhiên chưa đủ lớn thì sẽ gây dao động.
∗ Khi ta tăng các hệ số và đủ lớn, sai lệch giảm đi rất nhiều, quỹ đạo thực bám sát quỹđạo đặt, cụ thể với các hệ số như sau :
77 0 0.5 1 1.5 2 0 0.5 1 1.5 2 G o c q uay k h op 1( ra d ) thoi gian (s) Goc quay khop 1
0 0.5 1 1.5 2 -0.5 0 0.5 1 1.5 V a n t o c go c qu ay k h op 1 (r a d /s ) thoi gian (s) Van toc goc quay khop 1 q1 thuc q1 dat 0 0.5 1 1.5 2 -0.04 -0.02 0 0.02 S a i l e c h go c qu ay k h o