chương 1, em tìm hiểu được: • Nguyên lí hoạt động của robot • Các thông số kỹ thuật quan trọng • Các thành phần tổng quan của hệ thống điều khiển như động cơ, cảm biến, bộ nguồn, bộ điều khiển động cơ, vi điều khiển Qua đó hiểu được cấu tạo và nguyên lý chuẩn bị cho phần thiết kế hệ thống điều khiển trong chương tiếp theo. 9 CHƯƠNG 2: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN Robot Scara là một robot có tính ứng dụng cao nhất hiện nay. Trong công nghiệp robot được sử dụng một cách tự động với một quỹ đạo được người thiết kế đặt ra sao cho phù hợp với mục đích ứng dụng ví dụ như gắp thả linh kiện, lắp ráp linh kiện nhỏ, khắc lazer, in 3D, hàn,… Với các quỹ đạo đó thì cần có một hệ thống điều khiển đảm bảo tính đáp ứng liên tục cho robot. Nên trong chương này em sẽ thiết kế một quỹ đạo và xây dựng hệ thống điều khiển robot với quỹ đạo đó.
Tính toán động học, động lực học Đầu tiên, em tính toán động học cho robot để phục vụ cho việc khảo sát vùng làm việc để biết được tầm với của robot, từ đó thiết kế một quỹ đạo trong tầm với đó. Bài toán vị trí ❖ Mô hình hoá robot Scara: Hình 2.1 Mô hình robot Scara 4 bậc tự do 10 ❖ Từ Hình 2.1 đặt hệ quy chiếu theo phương pháp D-H (Denavit-Hartenberg) ta có Hình 2.2 là các hệ quy chiếu động được đặt tại các khâu và bộ thông số động học theo quy ước D-H được Bảng 2.2 Các hệ quy chiếu động được đặt tại các khâu theo phương pháp D-H Bảng 2. Bảng thông số D-H của Robot Scara Hệ tọa độ i di ai i 1 q1 d1 a1 0 2 q2 d2 a2 π 3 0 q3 0 0 4 q4 d4 0 0 • Trong đó: q1, q2, q3, q4 là biến khớp, tham số động học cho như sau: 11 Bảng 2.2 Các tham số động học 𝑑1 𝑎1 𝑎2 𝑑2 𝑑4 156mm 225mm 275mm 95mm 51mm ❖ Như vậy các ma trận biến đổi từ hệ Oi-1 về hệ Oi như sau: 𝐶𝑞1 −𝑆𝑞1 0 𝑎1 𝐶𝑞1 𝐶𝑞2 𝑆𝑞2 0 𝑎2 𝐶𝑞2 𝑆𝑞 𝐶𝑞1 0 𝑎1 𝑆𝑞1 𝑆𝑞 −𝐶𝑞2 0 𝑎2 𝑆𝑞2 𝑇10 = [ 1 ] 𝑇21 = [ 2 ] 0 0 1 𝑑1 0 0 −1 𝑑2 0 0 0 1 0 0 0 1 (2.1) 1 0 0 0 𝐶𝑞4 −𝑆𝑞4 0 0 0 1 0 0 𝑆𝑞 𝐶𝑞4 0 0 𝑇32 = [ ] 𝑇43 = [ 4 ] 0 0 1 𝑞3 0 0 1 𝑑4 0 0 0 1 0 0 0 1 ❖ Ma trận biến đổi thuần nhất mô tả hướng và vị trí của khâu thao tác so với khâu cơ sở: 𝑇 = 𝑇10. Bài toán thuận ❖ Giả thiết K , φ lần lượt là vị trí và hướng của khâu thao tác so với khâu cơ sở Hình 2.3 Tọa điểm cuối khâu thao tác ➔ Đối với bài toán thuận ta cần tìm xK,yK,zK và φ ❖ Khâu công tác của Robot có dạng (với ϴ=φ, α=π): 𝐶φ 𝑆φ 0 𝑥𝐾 𝑆φ −𝐶φ 0 𝑦𝐾 (2.7) 𝑦𝐾 = 𝑎2 𝑆(𝑞1 + 𝑞2 ) + 𝑎1 𝑆𝑞1 𝑧𝐾 = 𝑑1 + 𝑑2 − 𝑞3 − 𝑑4 ❖ Sử dụng chương trình MATLAB, ta mô phỏng không gian làm việc của robot như sau: Hình 2.4 Không gian làm việc của robot Scar 14 1.
Bài toán ngược ❖ Đối với bài toán ngược ta cần tìm q1, q2, q3, q4 với xK,yK,zK đã biết ❖ Từ (2.8) 𝑦𝐾 = 𝑎2 𝑆(𝑞1 + 𝑞2 ) + 𝑎1 𝑆𝑞1 ⇒ 𝑥𝐾2 + 𝑦𝐾2 = 𝑎12 + 𝑎22 + 2𝑎1 𝑎2 [(𝐶(𝑞1 + 𝑞2 )𝐶𝑞1 + 𝑆(𝑞1 + 𝑞2 )𝑆𝑞1 ] ⇔ 𝑥𝐾2 + 𝑦𝐾2 = 𝑎12 + 𝑎22 + 2𝑎1 𝑎2 𝐶(𝑞2 ) 𝑥 2 +𝑦𝐾 2 − 𝑎12 −𝑎22 cos 𝑞2 = 𝐾 ⇒{ 2𝑎1 𝑎2 sin 𝑞2 = ±√1 − cos 2 𝑞2 𝑥 2 +𝑦𝐾 2 − 𝑎12 −𝑎22 ➔ Vậy 𝑞2 = cos −1 𝐾 2𝑎1 𝑎2 ❖ Khi biết q2 ta viết lại (2.9) 𝑦𝐾 = 𝑎2 𝑆𝑞2 𝐶𝑞1 + (𝑎2 𝐶𝑞2 + 𝑎1 )𝑆𝑞1 ❖ Từ hệ phương trình (2.9) giải ra ta được: 𝑎 𝐶𝑞 + 𝑎1 −𝑎2 𝑆𝑞2 ∆= | 2 2 | = (𝑎2 𝐶𝑞2 + 𝑎1 )2 + (𝑎2 𝑆𝑞2 )2 𝑎2 𝑆𝑞2 (𝑎2 𝐶𝑞2 + 𝑎1 ) 𝑥 −𝑎2 𝑆𝑞2 ∆1 = | 𝐾 | = 𝑥𝐾 (𝑎2 𝐶𝑞2 + 𝑎1 ) + 𝑦𝐾 𝑎2 𝑆𝑞2 𝑦𝐾 (𝑎2 𝐶𝑞2 + 𝑎1 ) (𝑎2 𝐶𝑞2 + 𝑎1 ) 𝑥𝐾 ∆2 = | | = 𝑦𝐾 (𝑎2 𝐶𝑞2 + 𝑎1 ) − 𝑥𝐾 𝑎2 𝑆𝑞2 (2. Bài toán vận tốc – gia tốc 1. Bài toán vận tốc ❖ Vận tốc góc các khâu Từ các biểu thức lý thuyết: (0) 𝑇 𝜔 ̃𝑖 = 𝑅̇𝑖0 𝑅𝑖0 (0) (0) (0) 0 −𝜔𝑖,𝑧 𝜔𝑖,𝑦 𝜔𝑖,𝑥 (2.13) 0 0 (0) (0) 𝜔2 = [ 0 ] 𝜔4 = [ 0 ] 𝑞1̇ + 𝑞2̇ 𝑞1̇ + 𝑞2̇ − 𝑞4̇ 16 ❖ Vận tốc khối tâm các khâu Vị trí khối tâm C1, C2, C3, C4 được xác định bởi các kích thước l1, l2, l3, l4 như Hình 2.5 Vị trí khối tâm các khâu Từ biểu thức lý thuyết : 𝑛 (0) 𝜕𝑥𝐶𝑖 ∑( ) 𝑞̇ 𝑗 𝜕𝑞𝑗 𝑗=1 𝑛 (0) (0) 𝜕𝑦𝐶𝑖 𝑣𝐶𝑖 = ∑( ) 𝑞̇ 𝑗 (2. Bài toán gia tốc ❖ Gia tốc góc các khâu Từ các biểu thức lý thuyết: (0) 𝜀𝑖,𝑥 (0) 𝑑 (0) (0) 𝜀𝑖 = (𝜔𝑖 ) = 𝜀𝑖,𝑦 (2.
Thiết kế quỹ đạo làm việc Để khảo sát quỹ đạo làm việc của bài toán đặt ra, ta chọn quỹ đạo làm việc gắp thả vật để khảo sát : 19 Hình 2.6 Quỹ đạo gắp vật Tọa độ và hướng các điểm: S (0.2; 0) là điểm khởi động của robot.3 Bảng quỹ đạo và thời gian tương ứng Quỹ SA’ A’A Dừng gắp AA’ A’B’ B’B Dừng thả BB’ đạo mạch mạch Thời 2 2 1 2 3 2 1 2 gian(s) Để thiết kế được quỹ đạo cho Robot, người ta thường dùng phương pháp xấp xỉ đa thức bậc n hoặc dạng phối hợp. Trong nội dung báo cáo này nhóm sử dụng quỹ đạo theo đa thức bậc 3. Vì dạng này vừa đơn giản trong thiết kế đồng 20 thời thỏa mãn được các yêu cầu như : Đảm bảo tính liên tục của vận tốc -> quỹ đạo không giật cục, gấp khúc, gây sốc trong quá trình hoạt động của robot. ❖ Thiêt kế quỹ đạo trong không gian thao tác, quỹ đạo đường thẳng • Tổng quát : ta có phương trình đường thẳng đi qua 2 điểm M (x o , yo , z o ) và N (x e , ye , z e ) là: 𝑥 − 𝑥0 𝑦 − 𝑦0 𝑧 − 𝑧0 = = (2.22) 𝑥𝑒 − 𝑥0 𝑦𝑒 − 𝑦0 𝑧𝑒 − 𝑧0 • Độ dời dịch chuyển của điểm thao tác E theo phương x,y,z là đa thức bậc 3 theo thời gian: 𝑥 = 𝑎0 + 𝑎1 𝑡 + 𝑎2 𝑡 2 + 𝑎3 𝑡 3 { 𝑥̇ = 𝑎1 + 2𝑎2 𝑡 + 3𝑎3 𝑡 2 𝑦𝑒 − 𝑦0 𝑦0 𝑥𝑒 − 𝑦𝑒 𝑥0 𝑦= 𝑥+ (2.23) 𝑥𝑒 − 𝑥0 𝑥𝑒 − 𝑥0 𝑧𝑒 − 𝑧0 𝑧0 𝑥𝑒 − 𝑧𝑒 𝑥0 𝑧= 𝑥+ 𝑥𝑒 − 𝑥0 𝑥𝑒 − 𝑥0 • Với thời gian đặt cho quỹ đạo là te(s) để đi từ M đến N theo một đường thẳng.
Với điều kiện đầu: 𝑥𝐸 (0) = 𝑥𝑜 𝑥̇ (0) = 𝑥̇ 𝑜 = 0 { 𝐸 (2.25) 𝑡𝑒2 −2(𝑥𝑒 − 𝑥0 ) 𝑎3 = { 𝑡𝑒3 Sơ đồ Simulink thiết kế quỹ đạo: 21 Hình 2.7 Mô hình Simulink thiết kế quỹ đạo chuyển động Quỹ đạo các biến khớp: Hình 2.8 Quỹ đạo các góc khớp Vậy ta đã có các biến khớp ứng với quỹ đạo đã thiết kế. Các biến khớp này sẽ được sử dụng làm đầu vào khi thiết kế bộ điều khiển. Động lực học 1. Cơ sở lý thuyết Sử dụng phương pháp Lagrange để tính động lực học robot Scara, xét hệ nhiều vật có n bậc tự do với liên kết giữ, dừng.
Phương trình Lagrange loại 2 dạng ma trận: 𝑑 𝜕𝑇 𝑇 𝜕𝑇 𝑇 𝜕𝜋 𝑇 ( ) − ( ) + ( ) = 𝑄𝑘𝑡 (2.26) 𝑑𝑡 𝜕𝑞̇ 𝜕𝑞 𝜕𝑞 Trong đó: ❖ Vector toạ độ suy rộng: 𝑞 = [𝑞1 𝑞2 … 𝑞𝑛 ]𝑇 ❖ Phương trình động năng của hệ: 𝑛 1 (0) 𝑇 (0) (0) 𝑇 (0) (0) 𝑇 = ∑ (𝑣𝐶𝑘 𝑚𝑘 𝑣𝐶𝑘 + 𝜔𝑘 𝐼𝐶𝑘 𝜔𝑘 ) (2.27) 2 𝑘=1 Với: (0) • Vận tốc khối tâm vật k: 𝑣𝐶𝑘 = 𝐽𝑇𝑘 𝑞̇ (0) • Vận tốc góc vật k: 𝜔𝑘 = 𝐽𝑅𝑘 𝑞̇ (0) (𝑘) 𝑇 • Ma trận momen quán tính khối vật k: 𝐼𝐶𝑘 = 𝑅𝑘0 𝐼𝐶𝑘 𝑅𝑘0 Phương trình (2.27) có thể được viết lại như sau: 𝑛 1 (𝑘) 𝑇 𝑇 = 𝑞̇ 𝑇 [∑ (𝐽𝑇𝑘 𝑇 .29) 𝑘=1 • Phương trình (2.30) 2 23 ❖ Phương trình thế năng của hệ do trọng lực: 𝑛 𝑇 (0) Π(𝑞) = ∑ Π𝑖 (𝑞) = − ∑ 𝑚𝑖 𝑔(0) 𝑟𝐶𝑖 (𝑞) (2.32) 𝜕𝑞 𝑖=1 ❖ Lực suy rộng không thế: 𝑇 𝑄𝑘𝑡 = 𝐵𝑢 − 𝐷𝑞̇ + 𝐽𝐸,𝑇 𝐹 (0) (2.33) Với: • τ = Bu: momen/lực dẫn động tại khớp k • τd = Dq̇: cản nhớt • QF* = JE,TT.F(0): lực tác dụng tại điểm E bất kỳ Như vậy phương trình (2.26) có thể được viết lại thành: 𝜕𝑇 𝑇 𝑀(𝑞)𝑞̈ + 𝑀̇ (𝑞)𝑞̇ − ( ) + 𝐷𝑞̇ + 𝑔(𝑞) = 𝐵𝑢 + 𝐽𝐸𝑇 𝐹 (0) (2.36) 𝑘=1 1 𝜕𝑚𝑖𝑗 𝜕𝑚 𝜕𝑚𝑗𝑘 𝑐𝑖𝑗𝑘 = ( + 𝑖𝑘 − ): số hạng Christoffel symbols ở dạng thứ nhất 2 𝜕𝑞 𝜕𝑞 𝑘 𝜕𝑞 𝑗 𝑖 Vậy phương trình (2. Áp dụng ❖ Ma trận Jacobi tịnh tiến: • Công thức tổng quát: (0) (0) (0) 𝜕𝑥𝐶𝑖 𝜕𝑥𝐶𝑖 𝜕𝑥𝐶𝑖 ⋯ 𝜕𝑞1 𝜕𝑞2 𝜕𝑞𝑛 (0) (0) (0) (0) 𝜕𝑟𝐶𝑖 𝜕𝑦𝐶𝑖 𝜕𝑦𝐶𝑖 𝜕𝑦𝐶𝑖 𝐽𝑇𝑖 (𝑞) = = ⋯ (2.38) 𝜕𝑞 𝜕𝑞1 𝜕𝑞2 𝜕𝑞𝑛 (0) (0) (0) 𝜕𝑧𝐶𝑖 𝜕𝑧𝐶𝑖 𝜕𝑧𝐶𝑖 ⋯ [ 𝜕𝑞1 𝜕𝑞2 𝜕𝑞𝑛 ] • Toạ độ khối tâm các khâu so với hệ toạ độ cơ sở (tính được từ (2.40) 𝜕𝑞̇ 𝜕𝑞1̇ 𝜕𝑞2̇ 𝜕𝑞𝑛̇ (0) (0) (0) 𝜕𝜔𝑖,𝑧 𝜕𝜔𝑖,𝑧 𝜕𝜔𝑖,𝑧 ⋯ [ 𝜕𝑞1̇ 𝜕𝑞2̇ 𝜕𝑞𝑛̇ ] • Vận tốc góc các khâu (tính được từ (2. 𝐽𝑇𝑘 ) từ các ma trận Jacobi tịnh tiến tính được ở (2. 𝐽𝑅𝑘 ) từ các ma trận Jacobi quay tính được ở (2.43) 𝑚22 = 𝑚2 𝑙22 + (𝑚3 + 𝑚4 )𝑎22 + 𝐼2𝑧 + 𝐼3𝑧 + 𝐼4𝑧 𝑚33 = (𝑚3 + 𝑚4 ) 𝑚41 = 𝑚42 = 𝑚24 = 𝑚14 = −𝐼4𝑧 27 𝑚44 = 𝐼4𝑧 𝑚31 = 𝑚13 = 𝑚23 = 𝑚32 = 𝑚34 = 𝑚43 = 0 ❖ Ma trận Coriolis và ly tâm C(q, q̇) (được định nghĩa ở (2.43) nhận thấy chỉ có m11, m12, m21 chứa biến q2, các mij khác là hằng số, nên ta chỉ cần tính: 𝑐112 = −[𝑚2 𝑎1 𝑙2 + (𝑚3 + 𝑚4 )𝑎1 𝑎2 ]𝑠𝑞2 𝑐122 = −[𝑚2 𝑎1 𝑙2 + (𝑚3 + 𝑚4 )𝑎1 𝑎2 ]𝑠𝑞2 𝑐121 = −[𝑚2 𝑎1 𝑙2 + (𝑚3 + 𝑚4 )𝑎1 𝑎2 ]𝑠𝑞2 𝑐211 = [𝑚2 𝑎1 𝑙2 + (𝑚3 + 𝑚4 )𝑎1 𝑎2 ]𝑠𝑞2 (2.