Thiết kế quỹ đạo cho điểm tác động cuối đi từ điểm A tới điểm B nhận AB làm đường kính. Dùng phương trình được tròn trong không gian là mặt phẳng giữa 2 điểm A(x0 , y0 ,0),B(xe , ye ,0) lấy AB làm đường kính.
Phương trình đường tròn (x xi )2 (y yi )2 R2 Với x i y i R
Phương trình viết dưới dạng tham số như sau: x xi
y yi
z 0
Để thỏa mãn điều kiện về vận tốc thì tính toán hàm (t) là hàm bậc 3 theo thời
gian:
(t) a0 a1t a2t2 a3t3
s(0) A(x0 , y0 , 0)
Thỏa mãn các điều kiện: 0
s(te ) B(xe , ye , 0)
v(t e ) ve 0
Từ điều kiện trên tìm được các hệ số a0 ,a1 ,a2 ,a3
a
a
a
2
a
Giả sử cần tạo quỹ đạo chuyển động từ điểm A(0.7, 0) đến B(-0.7, 0) trong thời gian te=5(s).Ta tính được các giá trị sau:
a0 1.5708 a 1 a 2 a 3 => (t) 1.5708 0.t 2.3563t2 0.7854t3y 0 z 0 Sử dụng Matlab vẽ được đồ thị: 18
Hình 8: Đồ thị x(t) và vx(t)
Hình 9: Đồ thị y(t) và vy(t)
19
https://khuonghust.wordpress.com/
CHƯƠNG 4: ĐỘNG LỰC HỌC ROBOT 4.1. Các tham số động lực học
Coi các thanh đồng chất, tiết diện ngang không đáng kể, trọng tâm của các khâu đặt tại trung điểm của nó.
Các tham số động học: Khâu xC 1 a1 2 2 0 3 a3 2
4.2. Ma trận Jacobi tịnh tiến của các khâu Khâu 1: Ta có: 0A 1 0 => Ac1
https://khuonghust.wordpress.com/
0J
Khâu 2:
Tính toán tương tự khâu 1 ta có:
0 rc2 0 A2 .2 rc2 1 d1 d 2 2 L2 0 1 0 1 J 0 a s1 a s( ) a c a s 0 0 1 a c 1 4.3. Tính ma trận jacobi quay của các khâu
https://khuonghust.wordpress.com/ c(1 ) 0 A s(1) c( 1) 0 c1 c(1 ) 0Rc1Ts( 1 ) c(1 ) 0 0 0 0 JR1 0 0 0 1 Khâu 2:
Tính toán tương tự khâu 1 ta có:
JR 2
Khâu 3:
0 R
JR3
4.4. Tính tensor quán tính của các khâu
h đ T c1 c1 S Với:
4.6. Tính toán thế năng, lực thế 123 1m1gzc1 m1gd1 2m2gzc2 m2g d1 d2 1 L2 2 3m3gzc3 m3g(d1 d2 ) G 1 G(q) G2 G 3 24
CHƯƠNG 5: ĐIỀU KHIỂN 5.1. Điều khiển phản hồi và điều khiển vòng kín.
Khi xét bài toán điều khiển cho một tay máy trong công nghiệp, trước hết chúng ta phải mô hình hóa tay máy đó là một cơ cấu – đối tượng được điều khiển, trong đó các cảm biến được đặt tại các khớp để giám sát trạng thái của các khớp và các cơ cấu dẫn động được đặt tại các khớp để sinh lực (momen) dẫn động các khâu.
Chúng ta luôn muốn điều khiển các khớp của robot bám theo đúng quỹ đạo đã thiết kế. Các phần tử dao động làm việc để nhận lệnh và sinh lực (momen). Do vậy phải sử dụng các hệ điều khiển để tính toán các lệnh phù hợp nhất cho các phần tử này sao cho chúng thực hiện các quy luật chuyển động mong muốn.
Từ các giá trị ban đầu là quy luật qd(t), quy luật vận tốc q , quy luật gia tốc mong muốn, bộ điều khiển có nhiệm vụ tính toán lực F, là nguồn động lực để các hệ robot chuyển động với quy luật q và q . Nhờ các cảm biến mà bộ điều khiển có thể
đọc được giá trị này theo thời gian thực.
Việc tính toán lực F bằng các sử dụng phương trình động lực học, với đầu vào là
qd (t),q và . Từ kết quả tính toán động lực học, F được xác định theo công
thức:
F M(q).
F được tính trong công thức trên sẽ giúp cho chuyển động theo đúng quỹ đạo mong muốn, nếu mô hình động lực là hoàn toàn đúng, hệ không chịu tác động của nhiễu. Nhìn chung các duy nhật để xây dựng một hệ điều khiển có hiệu năng cao là phải sử dụng được tín hiệu phản hồi. Theo đó, tín hiệu phản hồi được sử dụng để tính toán sai lệch giữa vị trí e và sai lệch vận tốc e .
e qd qd e
Với bộ điều khiển phản hồi kín ta có: F F m F F 0 25 download by : skknchat@gmail.com
https://khuonghust.wordpress.com/
5.2. Thiết kế bộ điều khiể n trong không gian khớp
Giả sử bàn kẹp E đang ở vị trí (xE , yE , zE ) nào đó ứng với các tọa độ khớp qi, nhiệm vụ của bộ điều khiển là phải đưa được điện áp đặt vào động cơ để bàn kẹp di chuyển đến đúng hay gần đến vị trí mong muốn rE với sai lệch cho phép, tương ứng vị trí mong muốn của bàn kẹp rE là các tọa độ khớp qi
Sau đây là xét luật điều khiển đơn giản nhất đáp ứng được yêu cầu trên, đó là luật điều khiển PID cùng với bù trọng lượng. Lực điều khiển tính từ các vị trí mong muốn (ứng với tọa độ biến khớp qi) và trạng thái chuyển động của robot tại thời điểm hiện tại mà thu thập từ các cảm biến đặt tại các khớp.
Sau khi tìm được phương trình vi phân chuyển động, ta thiết kế bộ điều khiển PID có bù trọng lượng trong Simulink như sau:
Hình 10: Bộ điều khiển PID bù trọng lượng Trong đó bộ điều khiển PID được thiết kế như sau:
Hình 11: Thiết kệ bộ điều khiển PID
26
PHỤLỤC
Chương trình tính động học thuận trong Maple > > > CẤU HÌNH 27 download by : skknchat@gmail.com
https://khuonghust.wordpress.com/
>
CÁC MA TRẬN DH
>
CÁC MA TRẬN BIẾN ĐỔI THUẦN NHẤT CỦA KHÂU I SO VỚI KHÂU GỐC
>
KẾT QUẢ
VỊ TRÍ ĐIỂM THAO TÁC CUỐI
>
>
VẬN TỐC ĐIỂM TÁC ĐỘNG CUỐI E
28
>
GIA TỐC ĐIỂM TÁC ĐỘNG CUỐI
>
VỊ TRÍ CỦA CÁC KHỚP
>
VẬN TỐC GÓC VÀ GIA TỐC GÓC CỦA CÁC KHÂU Các ma trận cosin chỉ phương
>
Đạo hàm ma trận cosin chỉ phương
>
Chuyển vị ma trận cosin chỉ phương
>
=>Ma trận sóng
>
=> Vận tốc góc các khâu (trong hệ tọa độ cố định)
>
>
29
https://khuonghust.wordpress.com/
=> Gia tốc góc các khâu (trong hệ tọa độ cố định)
>
30
KẾT LUẬN
Bài tập lớn này đã tìm được các kết quả sau:
➢ Bài toán động học
➢ Bài toán động lực học
➢ Bài toán tĩnh học
➢ Thiết kế quỹ đạo chuyển động
➢ Điều khiển
Tuy nhiên vẫn còn nhiều phần trong bài tập lớn bỏ qua và vẫn còn sai sótvì khả năng chưa cho phép. Trong phần điều khiển vẫn chưa đủ thời gian và khả năng để lập trình Simulink trên phần mềm Matlab. Rất mong có sự thông cảm của thầy cô.
Chân thành cảm ơn.
31