Gắn các hệ tọa độ (Hình 3-3)
+ Hệ tọa độ { } cố định gắn với thân Robot.
+ Hệ tọa độ { } là hệ { } sau phép quay góc quanh trục ; tịnh tiến đoạn trên trục .
+ Hệ tọa độ { } là hệ { } sau phép quay góc quanh trục , tịnh tiến đoạn trên trục .
34
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 1 Bảng thông số D – H:
Thanh nối
1
2 0
Dạng tổng quát của ma trận biểu diễn quan hệ giữa khung tọa độ với nhƣ sau: [ ] (3.1) Áp dụng với robot Planar 2DOF, lần lƣợt thay các thông số, ta đƣợc: [ ] ; [ ] ; (3.2) Ma trận biểu diễn tọa độ tay máy trong hệ trục tọa độ gốc:
[ ] (3.3) Trong đó:
Theo ký hiệu tổng quát: [
35
; ; ;
; ; ;
; ; ; (3.4) 3.2 Giải bài toán động học ngƣợc robot Pelican 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. Có 3 phƣơng pháp cơ bản là: phép đảo hƣớng; phép đảo vị trí; phƣơng pháp kết hợp. Trong luận văn sử dụng phép đảo vị trí. Ta biết trƣớc và phải tìm các nghiệm , , và tƣơng ứng. Từ các phƣơng trình động học thuận: { , { √ (3.5) (3.6) 3.3 Động lực học robot Pelican Hàm Lagrange :
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 có dạng: ( )
36 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
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 độ dài khối tâm C1 của thanh nối 1 trong {O} 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 1 là:
(3.9)
Thanh nối 2:
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à :
37
( ) Động năng của khối tâm thanh nối 2 là:
* ( ) ( ) +
(3.10) Thế năng thanh nối 2 là:
(3.11)
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: [ ] ( ) ̈ ̈ ̈ ̈ ̈
38 ( ) ̈ ̈ (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: ( ) ̈ ̈ ̈ ̈ ̈ ( ) ̈ ̈ (3.20)
Phƣơng trình động lực học của robot Pelican có thể đƣợc viết ở dạng phƣơng trình ma trận sau: ̈
Trong đó:
[ ] [ ] ;
39 [ ] * + (3.23) Với: [ ] 0 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ó: { Do đó:
40 Vậy ma trận Jacoby:
[
] (3.24) 3.5 Điều khiển Backstepping cho robot Pelican trong không gian khớp: * Luật điều khiển theo (2.40): ̈
Với :
Trong đó: tính theo (3.23) bao gồm cả thành phần masat và nhiễu nhƣ sau: * +; * + ; * + Các hệ số điều khiển (ví dụ) : * + * + * + * +
3.6 Điều khiển Backstepping cho robot Pelican 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)
41
bao gồm cả thành phần masat và nhiễu nhƣ sau: * + ; * + ; * + Các hệ số điều khiển: * + * + * + * +
̂ 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.
[ ]
3.7 Kết luận Chƣơng III
Chƣơng III đã trình bày chi tiết các tính toán giải bài toán động học thuận, động học ngƣợc, động lực học, ma trận Jacoby và luật điều khiển dựa trên phƣơng pháp Backstepping cho robot Pelican trong không gian khớp và không gian làm việc. Các tính toán này là cơ sở để xây dựng chƣơng trình mô phỏng chuyển động của robot Pelican có bộ điều khiển đƣợc xây dựng với luật điều khiển theo phƣơng pháp Backstepping.
42
CHƢƠNG 4 KẾT QUẢ MÔ PHỎNG ĐIỀU KHIỂN CHUYỂN ĐỘNG ROBOT
PELICAN BẰNG PHƢƠNG PHÁP BACKSTEPPING VÀ BÀN LUẬN
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 Pelican hai bậc tự do gồm 2 khớp quay (Planar 2DOF). 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.
Chƣơng trình mô phỏng đƣợc viết trên nền Matlab/Simulink. Chi tiết mã chƣơng trình đƣợc trình bày trong Phụ lục PL1; PL2
4.1 Mô phỏng trong không gian khớp
- 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í * +.
- Yêu cầu điều khiển: Các khớp quay bám sát đƣờng quỹ đạo đặt; Sai lệch góc quay và sai lệch vận tốc góc quay tiến tới 0.
- 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 Pelican.
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.
MophongRobotPlanar.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ị.
43 Bảng 2: Thông số động học và đông lƣc hoc
Để quan sát, so sánh, đánh giá chất lƣợng điều khiển, ta thay đổi các thông số của bộ điều khiển để có chất lƣợng điều khiển tốt nhất.
Ví dụ: Khi các thông số bộ điều khiển: *
+ * + (4.1) Ta có kết quả mô phỏng trong không gian khớp nhƣ sau:
Thông số Đ. vị Giá trị thật Giá trị ƣớc lƣợng
K.hiệu Giá trị K.hiệu Giá trị
Chiều dài thanh nối 1 M l1 0,26 l1 0,26
Chiều dài thanh nối 2 M l2 0,26 l2 0,26
Khối lƣợng thanh nối 1 kg m1 6,5225 mˆ1 6
Khối lƣơng thanh nối 2 kg m2 2,0458 mˆ2 2
Mô men quán tính t. nối 1 kgm2 I1 0,1213 Iˆ1 0,12
44
Hình 4-1: Kết quả mô phỏng trên Matlab chuyển động bám quỹ đạo trong không gian khớp của robot Pelican với các thông số điều khiển tại (4.1)
Khi các thông số của bộ điều khiển: * + * + (4.2) Ta có kết quả mô phỏng nhƣ sau:
45
Hình 4-2: Kết quả mô phỏng trên Matlab chuyển động bám quỹ đạo trong không gian khớp của robot Pelican với các thông số điều khiển tại (4.2)
Nhận xét: Thông qua kết quả mô phỏng cho thấy:
- Bộ điều đáp ứng tốt, quỹ đạo thực bám sát quỹ đạo đặt. - Khi ta tăng các hệ số điều khiển lên từ *
+ đến * + thì sai lệch góc quay khớp 1 giảm từ 0,6x10-3 [rad] xuống 0,35x10-4 [rad] (tức là sai lệch giảm khoảng 17 lần); sai lệch góc quay khớp 2 giảm từ 5x10-4
[rad] xuống 2x10-4 (tức là sai lệch giảm 2,5 lần) và các đƣờng đặc tính miêu tả sai lệch góc quay, sai lệch vận tốc góc các khớp “trơn” hơn, ít dao động hơn.
46
4.2 Mô phỏng trong không gian làm việc
- Giả sử ta điều khiển robot từ vị trí đầu x0 = [0 0] chuyển tới vị trí: xd = [-0,3275 -0,4169]
- Yêu cầu điều khiển: sai lệch vị trí e = x –xd 0; sai lệch vận tốc e’
= x’ 0 Sử dụng Simulink với các khối Matlab funtion, ta thiết lập chƣơng trình mô phỏng theo sơ đồ khối nhƣ hình 4-3. Mã chƣơng trình đƣợc trình bày chi tiết trong Phụ lục PL2. Trong đó: Thông số động học và động lực học ƣớc lƣợng của robot nhƣ sau: mˆ1 = 6.5; mˆ2=2; =0.261; =0.263.
Chạy chƣơng trình mô phỏng, thay đổi các thông số của bộ điều khiển để có chất lƣợng điều khiển tốt nhất.
Các thông số điều khiển nhƣ sau:
* + * + , *
+ (4.3)
Hình 4-3: Sơ đồ khối mô phỏng điều khiển bám quỹ đạo trong không gian làm việc của robot Pelican
47 Kết quả mô phỏng thể hiện trên hình 4-4.
Hình 4-4: Kết quả mô phỏng chuyển động bám quỹ đạo trong không gian làm việc của robot Pelican với các thông số điều khiển tại 4-3
48
CHƢƠNG 5 KẾT LUẬN VÀ KIẾN NGHỊ
Luận văn đã trình bày các vấn đề sau: 1/ Tổng quan về Robot công nghiệp, 2/ Cơ sở phƣơng pháp Backstepping, 3/ Xây dựng mô hình toán của robot Pelican (Planar 2DOF), 4/ Áp dụng phƣơng pháp Backstepping cho điều khiển quỹ đạo của Robot.
Thuật toán điều khiển có sử dụng kỹ thuật cuốn chiếu (Backstepping) phù hợp trong điều khiển chuyển động bám quỹ đạo của robot khi các thông số động lực học không xác định trƣớc. Sự ổn định của hệ thống động lƣc học kín đƣơc chứng minh theo tiêu chuẩn ổn định Lyapunov. Các tín hiệu điều khiển có dạng toán học tƣơng đối đơn giản, do đó tăng tính khả thi khi áp dụng trong thực tế. Tính hiệu quả của thuật toán đề xuất đƣơc xác nhận lại bằng các kết quả mô phỏng trên MatLab/Simulink cho thấy Robot Pelican (Planar 2DOF) đã bám sát quỹ đạo đặt trong thời gian yêu cầu. Phƣơng pháp đề xuất có thể đƣợc sử dụng cho các Robot có số bậc tự do lớn hơn và các tác nghiệp linh hoạt hơn.
49
TÀI LIỆU THAM KHẢO
[1]. GS. TSKH. Nguyễn Thiện Phúc, “Robot công nghiệp”, Nhà xuất bản Khoa học kỹ thuật, 2006
[2]. R. Kelly, V. Santibanez and A. Loria,” Control of Robot Manipulators in joint space”, Spring Verlag, 2005
[3]. PGS. TS. Nguyễn Phạm Thục Anh, ThS. Thái Hữu Nguyên, “Áp dụng phƣơng pháp backstepping trong điều khiển bền vững chuyển động của Robot”, Hội nghị toàn quốc lần thứ 2 về Điều khiển và Tự động hóa – VCCA, 2013
[4]. Nguyễn Doãn Phƣớc, “Phân tích điều khiển hệ phi tuyến”, Nhà xuất bản Bách Khoa, 2012
50