Page 1
Trajectory planning -2- Chương 10. THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG Trajectory planning điểm tựa (trung gian) A điểm đầu Vật cản nhanh không gian tự do Nguyễn Quang Hoàng Vật cản Email: hoang.nguyenquang@hust.edu.vn B chậm không gian chật hẹp, chậm có rào bảo vệ điểm cuối Các điểm tựa khi thiết kế quỹ đạo quanh các Chuyển động trong không gian tự do và trong vật cản không gian chật hẹp, có rào bảo vệ Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Trajectory planning -3- Trajectory planning – Motion planning -4- 1. Khái niệm đường di chuyển (path) và quỹ đạo chuyển động (trajectory) Động học: giải quyết các mối quan hệ hình học về vị trí / vận tốc giữa không gia khớp 2. quỹ đạo nối điểm đầu điểm cuối q(t) và không gian thao tác (làm việc) x. 3. quỹ đạo qua các điểm tựa 4. Tạo luật chuyển động nhờ đa thức bậc 3, 4, 5, 6, 7 Động lực học: giải quyết mối quan hệ giữa mômen xoắn/lực tác dụng tại các khớp và 5. Tạo luật chuyển động nhờ các hàm điều hòa (sin, cos) các chuyển động của các khâu. 6. Thiết kế luật chuyển động trong không gian khớp Điều khiển: tính toán lực/mômen khớp cần thiết để thực hiện một chuyển động mong 7. Thiết kế quỹ đạo trong không gian thao tác (đề các) muốn. 8. Đường spline bậc 3 (cubic S-pline) [remind Polynomial interpolation; Thiết kế (hay lập kế hoạch) quỹ đạo: tạo ra chuyển động mong muốn của các tay máy, Lagrange polynomials are used for polynomial interpolation, Newton đáp ứng yêu cầu nhiệm vụ đặt ra. interpolation] Thông thường, người dùng sẽ được yêu cầu để xác định một số điểm và đặc tính chung của quỹ đạo (ví dụ như điểm ban đầu / cuối cùng, thời gian, vận tốc tối đa, vv), Làm sao để robot thực hiện chuyển động theo và thực tế tính toán của quỹ đạo được yêu cầu đến hệ thống điều khiển. mong muốn của người sử dụng? Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 2
Trajectory planning – Motion planning -5- Trajectory planning – Motion planning -6- Thiết kế quỹ đạo: là vấn đề quan trọng trong lĩnh vực robot, rất Các phương thức thiết kế quỹ đạo có thể là hoàn toàn khác nhau: quan trọng trong việc đo đạc kích thước, điều khiển, và sử dụng điểm-điểm động cơ điện trong máy tự động (ví dụ: đóng gói bao bì). với con đường được xác định trước hoặc: Bài toán này ban đầu xuất hiện trong ngành cơ khí (thiết kế biên trong không gian khớp (biến q); dạng cam): với công nghệ robot thì ta thay thế cam cơ khí bằng trong không gian làm việc, hoặc xác định một số điểm quan tâm cam điện (thiết kế quỹ đạo). (đầu tiên và cuối cùng điểm, điểm trung gian) hoặc toàn bộ đường hình học x = x(t). Tài liệu tham khảo: Việc thiết kế quỹ đạo mong muốn, sự cần thiết phải xác định hai 1. Siciliano – Sciavicco: Modeling and Control of Robot Manipulators. Chapter 4. khía cạnh: 2. Reza N. Jazar: Theory of Applied Robotics: Kinematics, Dynamics, and đường di chuyển hình học (path) Control. Chapter 13. luật chuyển động (motion law) 3. B. Heimann+: Mechatronik: Komponenten-Methoden-Beispielen Chapter 7. 4. L. Biagiotti · C. Melchiorri: Trajectory Planning for Automatic Machines and cùng với các ràng buộc về tính liên tục (trơn tru) của quỹ đạo và Robots. Springer 2008. tính liên tục của đạo hàm với cấp xác định (bậc 1, 2, 3, …). Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Trajectory planning – Motion planning -7- Trajectory planning – Motion planning -8- Đường hình học có thể được xác định trong không gian làm việc hoặc trong không gian Ví dụ về đường di chuyển hình học trong không gian thao tác: đoạn thẳng, khớp. Thông thường, nó được thể hiện trong một hình thức tham số như -Trong không gian làm việc: đường hình học là quỹ tích các điểm mà điểm tác động cuối đoạn của parabol, cung tròn, … của tay máy đi chuyển qua. Trong không gian khớp, quỹ đạo hình học nhận được bằng cách gán các giá p = p( )s Î Â3, s Î Â, s Î[0,L] trị đầu/cuối (có thể có các giá trung gian) cho các biến khớp, tương ứng với - Khái niệm đường hình học trong không gian khớp được hiểu một cách trừu tượng, là luật chuyển động mong muốn. một hàm q = q( )s Î Ân, s Î Â, s Î[s ,s ] Liên quan đến luật chuyển động, là cần thiết để xác định các hàm liên tục min max B, s=L z đến cấp đạo hàm nhất định (thường ít nhất là đạo hàm cấp 1 và cấp 2, tức là 0 Nếu biết thêm quy luật theo liên tục về vận tốc và gia tốc). thời gian z(s) Thông thường, các đa thức bậc N đối với biến thời gian được sử dụng để mô = 0 = 1 s = s t( ),s = s( )t A, s=0 tả luật chuyển động: đường di chuyển sẽ được gọi p(s) s p(s) là quỹ đạo chuyển động t=0 [t0 tf ] t (Trajectory) O y(s) 0 y S t( ) = P ( )t = a +a t +a t2 +...+a tN-1 +a tN p = p( )s = p( ( ))s t Î Â3, 0 s=0 s= L s x(s) N 0 1 2 N-1 N t Î Â+, t Î[t ,t ] 0 f x0 Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 3
Trajectory planning – Motion planning -9- Trajectory planning – Motion planning -10- Bài toán thiết kế quỹ đạo có đầu vào thường là: Thông thường, người sử dụng chỉ xác định một số lượng tối thiểu dữ liệu xác định đường di chuyển (qua các điểm), các thông tin về quỹ đạo, chẳng hạn như điểm ban đầu và cuối ràng buộc hình học về đường di chuyển (ví dụ như chướng ngại vật), cùng, thời gian chuyển động, vận tốc tối đa, và v.v... ràng buộc trên các động học (giới hạn khớp, vận tốc cực đại, ) ràng buộc do hệ thống truyền động (lực, mô men cực đại ) • Quỹ đạo trong không gian làm việc cho phép xem xét trực tiếp nhất các ràng buộc có thể có đối với đường dẫn di chuyển Và đầu ra thường là • quỹ đạo trong không gian khớp, hoặc không gian làm việc, được đưa ra như là một (chướng ngại vật, tính chất hình học của đường di chuyển, ...) mà chuỗi (theo thời gian) của gia tốc, vận tốc và vị trí các giá trị: có nhiều khó khăn hơn khi xem xét chúng trong các không gian khớp (do tính phi tuyến của động học) a(kT), v(kT), q(kT) hoặc a(kT), v(kT), p(kT) , k = 0,1,2,…..N_ • Quỹ đạo trong không gian khớp đơn giản hơn về tính toán và cho T là một thời gian trích mẫu (phù hợp với thời gian tính toán và thời gian xử lý các công việc liên quan như truyền số liệu đến các thiết bị truyền động. phép xem xét các vấn đề về cấu hình kỳ dị, dư dẫn động, các ràng buộc về vận tốc và gia tốc. Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Trajectory planning – Motion planning -11- Joint space trajectory planning -12- Các quỹ đạo được xác định bằng cách định nghĩa các điểm đặc trưng: Trong các trường hợp đơn giản nhất, (một phần) một quỹ đạo được xác định gán trực tiếp các thông số kỹ thuật bằng cách cho trước điều kiện ban đầu và điều kiện cuối cùng về: thời gian gán bằng cách định nghĩa các cấu hình mong muốn x trong không gian làm việc, sau chuyển động, vị trí, vận tốc, gia tốc, .... đó chúng được chuyển đổi sang trong không gian khớp nhờ sử dụng mô hình động Sau đó, vấn đề là xác định một hàm học ngược. q = q t( ) hay q = q s( ),s = s t( ) Các thuật toán để tính toán một hàm q(t) nội suy từ các điểm được đã cho được đặc trưng bởi các tính năng sau: để những điều kiện ràng buộc được thỏa mãn. quỹ đạo phải hiệu quả về mặt tính toán (thời gian tính toán nhỏ) profile vị trí và vận tốc (ít nhất) phải là các hàm liên tục theo thời gian Đây là bài toán điều kiện biên, bài toán này có thể được giải quyết dễ dàng bằng tác dụng không mong muốn (như độ cong thay đổi đột ngột) phải được giảm thiểu cách lựa chọn hàm đa thức như: hoặc hoàn toàn tránh được. Trong phần này, lập trình quỹ đạo cho một biến khớp duy nhất được xem xét. S t( ) = P ( )t = a +a t +a t2 +...+a tN-1 +a tN N 0 1 2 N-1 N Nếu số khớp nhiều hơn, một chuyển động có phối hợp cần được thiết kế (lên kế hoạch), ví dụ như cho tất cả các khớp đồng thời chuyển động [cùng thời điểm ban đầu và thời Việc chọn đa thức bậc N = 3, 5 hay 7, … phụ thuộc vào các điều kiện biên cần điểm cuối], hoặc thiết kế cho khớp nào di chuyển nhiều nhất và sau đó điều chỉnh bằng được thỏa mãn và phụ thuộc vào sự trơn tru của quỹ đạo. một hệ số thích hợp cho các khớp còn lại [hệ số tỷ lệ ở đây <=1]. Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 4
Joint space trajectory planning -13- Joint space trajectory planning ‐ đa thức bậc 3 -14- Quỹ đạo đa thức Quỹ đạo đa thức bậc 3 Nói chung, bên cạnh những giá trị ban đầu và cuối cùng, các ràng buộc khác Nếu chỉ quan tâm đến tính chất liên tục của luật chuyển động, thì việc sử dụng có thể được xác định trên các giá trị của đạo hàm theo thời gian (vận tốc, gia đa thức bậc 3 cho phép tạo ra được một quỹ đạo trơn tru về di chuyển và vận tốc tốc, giật, ...) tại thời điểm tj. Nói cách khác, người ta có xác định một đa thức thỏa mãn các điều kiện đầu và điều kiện cuối. q(t), mà đạo hàm bậc k của nó có giá trị xác định tại thời điểm tj. Xét đa thức bậc ba Về mặt toán học, những điều kiện này có thể được thể hiện như: q t( ) = a t3 +a t2 +a t +a , 3 2 1 0 n ! k a! +(k +1)!a t +...+ a tn-k = q( )k (t ) k k+1 j (n -k)! n j j q t( ) = 3a t2 +2a t +a 3 2 1 Hay ở dạng Ở đây có bốn hệ số cần được xác định, ngoài các điều kiện đầu và cuối về vị trí Ma = b các điều kiện về vận tốc cũng cần được xem xét M Î Â(n+1) (´ n+1) M, b đã biết từ số liệu đầu bài, a là véc tơ cần tìm q t( ) = q , q t( ) = q , q t( ) = q , q t( ) = q b Î Â(n+1) 1´ 0 i f f 0 i f f a = M-1b a Î Â(n+1) 1´ Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Joint space trajectory planning ‐ đa thức bậc 3 -15- Joint space trajectory planning ‐ đa thức bậc 3 -16- Quỹ đạo đa thức bậc 3 Trường hợp thời điểm đầu t = 0, ta có 0 q t( ) = a t3 +a t2 +a t +a , a = q , a = q , 3 2 1 0 0 i 1 i 3(q -q )-(2q +q )t -2(q -q )+(q +q )t a = f i i f f , a = f i i f f . q t( ) = q , q t( ) = q , q t( ) = q , q t( ) = q 2 t2 3 t3 0 i f f 0 i f f f f Phương trình xác định các hệ số: Ma = b Ví dụ: 50 t = 0, t = 4, q t( ) = a t3 +a t2 +a t +a = q i f 40 0 3 0 2 0 1 0 0 i q(0) = 10, q t( ) = 45, o a = M-1b f 30 q [ ] q t( ) = 3a t2 + 2a t +a = q q(0) = 0, q t( ) = 0 vt [o/s] 0 3 0 2 0 1 i f 20 o 2 gt [ /s ] q t( ) = a t3 +a t2 +a t +a = q Kết quả 10 f 3 f 2 f 1 f 0 f q t( ) = 3a t2 + 2a t +a = q , é1 t t2 t3 ù éa ù éq ù 0 f 3 f 2 f 1 f ê 0 0 0 ú ê 0 ú ê i ú a = [10.00, 0, 6.5625, -1.0938]T -10 ê0 1 2t 3t2 ú êa ú êq ú ê 0 0 ú ê 1 ú = ê i ú -20 hay ê ú êa ú êq ú 0 1 2 3 4 5 ê1 t t2 t3 ú ê ú ê ú t [s] ê f f f ú ê 2 ú ê f ú ê0 1 2t 3t2 ú êa ú êq ú q t( ) = 10 +6.5625t2 -1.0938t3 ë f f û ë 3 û ë f û Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 5
Joint space trajectory planning ‐ đa thức bậc 3 -17- Joint space trajectory planning ‐ đa thức bậc 3 -18- Ví dụ: Ví dụ: t = 0, t = 4, i f 50 80 q(0) = 10, q t( ) = 45, t = 0, t = 4, f 40 i f 60 q [o] q(0) = -10, q t( ) = 45, q(0) = 20, q t( ) = 0 40 f 30 vt [o/s] f o 2 q(0) = -30, q t( ) = -30 20 gt [ /s ] f 20 0 10 -20 Ma = r 0 -40 q [o] Ma = r vt [o/s] -10 -60 o 2 Kết quả 0 1 2 3 4 5 gt [ /s ] t [s] Kết quả -80 0 1 2 3 4 5 a = [10.00, 20.00, -3.4375, 0.1563]T t [s] a = -[ 10.00, -30.00, 32.8135, -5.4688]T q t( ) = 10 + 20t - 3.4375t2 + 0.1563t3 q t( ) = -10- 30t + 32.8135t2 -5.4688t3 Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Joint space trajectory planning ‐ đa thức bậc 3 -19- Joint space trajectory planning ‐ đa thức bậc 3 -20- Xét trường hợp tổng quát, một đa thức bậc 3 cần thỏa mãn hai điều kiện về vị trí và vận Quỹ đạo đa thức bậc 3 đi qua nhiều điểm (đảm bảo liên tục về dịch chuyển và vận tốc) tốc tại hai đầu ti và tf. Tìm đa thức q(t) bậc 3 trên đoạn [ti, tf] thỏa mãn: q t( ) = q , q t( ) = q , q t( ) = q , q t( ) = q Cho chuỗi thời gian t(k) cùng các giá trị về vị trí và vận tốc tương ứng: i i f f i i f f Ta viết đa thức cần tìm ở dạng sau: t = [0 = t < t < t < ...... < t = t ] 0 1 2 N f q t( ) = a +b t( -t )+c t( -t )2 +d t( -t ) ,3 t £ t £ t q = [ q , q , q ,. .. ,q ] i i i i f 0 1 2 N q = [ q , q , q ,. .. ,q ] q t( ) = b + 2 (c t -t )+ 3 (d t -t ) ,2 0 1 2 N i i Coi quỹ đạo là sự nối ghép liên tiếp N đoạn với các điều kiện đầu và cuối sau : Từ các điều kiện biên với h = t -t f i a = q , • Thời điểm đầu và cuối [t , t ], q t( ) = a = q , i k k +1 i i b = q , • Vị trí đầu và cuối [q , q ], k = 0,1,...N - 1 i k k +1 q t( ) = b = q , -3(q -q )-(2q +q )(t -t ) • Vận tốc đầu và cuối [q , q ], i i c = i f i f f i , k k +1 q t( ) = a +hb +h c2 +h d3 = q , (t -t )2 f f f i q t( ) = b + 2hc + 3h d2 = q , 2(q -q )+(q +q )(t -t ) Áp dụng N đa thức bậc 3 cho N đoạn quỹ đạo nối hai điểm liên tiếp, với các điều kiện biên f f d = i f i f f i , cần thỏa mãn (t -t )3 f i Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 6
Joint space trajectory planning ‐ đa thức bậc 3 -21- Joint space trajectory planning ‐ đa thức bậc 3 -22- Ví dụ: N = 5; Ví dụ: N = 5; 60 t = [ 0 2 4 6 8 10]; t = [ 0 2 4 6 8 10]; 40 q = [-10 20 0 30 40 40]; q = [ 0 20 40 60 40 40]; qv = [ 0 10 -10 10 0 0]; 60 qv = [ 0 0 0 0 0 0]; 20 40 0 q [o] -20 o 20 vt [ /s] gt [o/s2] -40 0 0 1 2 3 4 5 6 7 8 9 10 t [s] q [o] -20 vt [o/s] gt [o/s2] -40 0 2 4 6 8 10 t [s] Rõ ràng là tính liên tục của vị trí và vận tốc được đảm bảo, tuy nhiên gia tốc có bước nhảy Rõ ràng là tính liên tục của vị trí và vận tốc được đảm bảo, tuy nhiên gia tốc khi chuyển qua các điểm tựa. có bước nhảy khi chuyển qua các điểm tựa. Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Joint space trajectory planning ‐ đa thức bậc 5 -23- Joint space trajectory planning ‐ đa thức bậc 5 -24- Giả sử cần tìm đa thức bậc 5, là hàm của thời gian t trên đoạn [ti t tf]. Thỏa Từ các điều kiện đầu và cuối ta nhận được q t( ) = a = q , mãn điều kiện biên tại hai đầu i 0 i q t( ) = a = q , i 1 i q t( ) = 2a = q, i 2 i q t( ) = a h5 +a h4 +a h3 +a h2 +a h +a = q , t = t : q t( ) = q , q t( ) = q , q t( ) = q, f 5 4 3 2 1 0 f i i i i i i i q t( ) = 5a h4 + 4a h3 + 3a h2 +2a h +a = q , t = t : q t( ) = q , q t( ) = q , q t( ) = q . f 5 4 3 2 1 f f f f f f f f q t( ) = 20a h3 +12a h2 + 6a h +2a = q , f 5 4 3 2 f Xét đa thức bậc 5 cùng các đạo hàm của nó é1 0 0 0 0 0 ù éa ù éq ù ê ú ê 0 ú ê i ú ê0 1 0 0 0 0 ú êa ú êq ú ê ú ê 1 ú ê i ú Ax = b q t( ) = a (t -t )5 +a (t -t )4 +a (t -t )3 +a (t -t )2 +a (t -t )+a ê0 0 2 0 0 0 ú êa ú êq ú 5 i 4 i 3 i 2 i 1 i 0 ê ú ê ú ê ú ê ú ê 2 ú = ê i ú q t( ) = 5a (t -t )4 + 4a (t -t )3 + 3a (t -t )2 +2a (t -t )+a ê1 h h2 h3 h4 h5 ú êa ú êq ú ß 5 i 4 i 3 i 2 i 1 ê ú ê 3 ú ê f ú q t( ) = 20a (t -t )3 +12a (t -t )2 + 6a (t -t )+2a ê0 1 2h 3h2 4h3 5h4 ú êa ú êq ú 5 i 4 i 3 i 2 ê ú ê 4 ú ê f ú x = A-1b ê0 0 2 6h 12h2 20h3 ú êa ú êq ú êë ú ëû 5 û ë f û Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 7
Joint space trajectory planning ‐ đa thức bậc 5 -25- Joint space trajectory planning ‐ đa thức bậc 5 -26- Ví dụ Đa thức bậc 5 tìm được là: t = 0, q(0) = 10 ,o q(0) = 0, q(0) = 0, i q t( ) = 210t5 -525t4 + 350t3 +10 t = 1, q t( ) = 45 ,o q t( ) = 0, q t( ) = 0 f f f f 50 Ma trận A, véc tơ vế phải r và véc tơ hệ số a: 40 30 é1 0 0 0 0 0 ù éa ù é10ù éa ù é 10 ù ê ú ê 0 ú ê ú ê 0 ú ê ú 20 ê0 1 0 0 0 0 ú êa ú ê 0 ú êa ú ê 0 ú ê ú ê ú ê ú ê ú ê ú 10 ê ú ê 1 ú ê ú ê 1 ú ê ú ê0 0 2 0 0 0 ú êa ú ê 0 ú êa ú ê 0 ú 0 ê ú ê 2 ú = ê ú ê 2 ú = ê ú o 1 1 1 1 1 1 a 45 a 350 -10 q [ ] ê ú ê ú ê ú ê ú ê ú v/2 [o/s] ê ú ê 3 ú ê ú ê 3 ú ê ú -20 ê0 1 2 3 4 5 ú êa ú ê 0 ú êa ú ê-525ú a/10 [o/s2] ê ú ê 4 ú ê ú ê 4 ú ê ú -300 0.2 0.4 0.6 0.8 1 ê0 0 2 6 12 20ú êa ú ê 0 ú êa ú ê 210 ú t [s] ë û ë 5 û ë û ë 5 û ë û Đa thức bậc 5 tìm được là: Rõ ràng là tính liên tục của vị trí, vận tốc và gia tốc được đảm bảo, tuy nhiên đạo hàm của gia tốc [jerk= giật] có bước nhảy. q t( ) = 210t5 -525t4 + 350t3 +10 Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Joint space trajectory planning ‐ đa thức bậc 5 -27- Joint space trajectory planning ‐ đa thức bậc 5 -28- Tương tự như trường hợp sử dụng chuỗi các đa thức bậc 3 để thiết kế một quỹ Phương trình xác định hệ số của đa thức trên đọa thứ k đạo liên tục về vị trí và vận tốc qua N+1 điểm tựa. Với đa thức bậc 5 ta hoàn toàn làm tương tự, ở đây ta cần cho thêm điều kiện về h = t -t > 0, k = 0,1,2,...,N -1 k k+1 k gia tốc tại các điểm tựa. Do đó gia tốc sẽ là hàm liên tục. é1 0 0 0 0 0 ù éa ù é q ù ê ú ê 0,k ú ê k ú t = [0 = t < t < t < ...... < t = t ] ê0 1 0 0 0 0 ú êa ú ê q ú 0 1 2 N f ê ú ê 1,k ú ê k ú q = [ q , q , q ,. .. ,q ] ê0 0 2 0 0 0 ú êa ú ê q ú ê ú ê ú ê ú 0 1 2 N ê ú ê 2,k ú = ê k ú q = [ q , q , q ,. .. ,q ] ê1 h h2 h3 h4 h5 ú êa ú êq ú 0 1 2 N ê k k k k k ú ê 3,k ú ê k+1 ú q = [ q , q, q ,. .. ,q ] ê0 1 2h 3h2 4h3 5h4 ú êa ú êq ú 0 1 2 N ê k k k k ú ê 4,k ú ê k+1 ú ê0 0 2 6h 12h2 20h3 ú êa ú êq ú êë k k k ú ëû 5,k û ë k+1 û Phương trình xác định hệ số của đa thức trên đọa thứ k Quỹ đạo thiết kế q(t) trên đoạn: t £ t £ t , k = 0,1,...,N -1 k k+1 q t( ) = q ( )t = a (t -t )5 +a (t -t )4 +a (t -t )3 +a (t -t )2 +a (t -t )+a , k 5,k k 4,k k 3,k k 2,k k 1,k k 0,k q t( ) = q ( )t = a (t -t )5 +a (t -t )4 +a (t -t )3 +a (t -t )2 +a (t -t )+a , q t( ) = q ( )t = 5a (t -t )4 + 4a (t -t )3 + 3a (t -t )2 +2a (t -t )+a , k 5,k k 4,k k 3,k k 2,k k 1,k k 0,k k 5,k k 4,k k 3,k k 2,k k 1,k q t( ) = q ( )t = 5a (t -t )4 + 4a (t -t )3 + 3a (t -t )2 + 2a (t -t )+a , q t( ) = q ( )t = 20a (t -t )3 +12a (t -t )t2 + 6 (t -t )+ 2a , k 5,k k 4,k k 3,k k 2,k k 1,k k 5,k k 4,k k 3,k k 2,k q t( ) = q ( )t = 20a (t -t )3 +12a (t -t )t2 + 6 (t -t )+2a , k 5,k k 4,k k 3,k k 2,k Yêu cầu viết thuật toán để đưa ra quỹ đạo dạng số và đồ thị. Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 8
Joint space trajectory planning ‐ đa thức bậc 5 -29- Joint space trajectory planning – profile vận tốc -30- Ví dụ Thiết lập quỹ đạo có profile vận tốc dạng hình thang t = [ 0 2 4 6 8 10]; q = [ 10 20 0 30 40 40]; qv = [ 0 -10 20 3 0 0]; Một cách tiếp cận khác là sự kết hợp các đoạn cong thể hiện bằng các đa thức (như đường qa = [ 0 5 10 3 0 0]; bậc 2,3,5,7) hay các hàm lượng giác (sin, cosin) với các đoạn thẳng để tạo ra quỹ đạo di chuyển trơn (là hàm liên tục theo thời gian). Phương án này hay được sử dụng trong công nghiệp do tính đơn giản của nó. 60 Trường hợp đơn giản nhất là sử dụng profile vận tốc có dạng hình thang cân, vận tốc tăng 50 đều từ 0 đến vận tốc cực đại, duy trì vận tốc cực đại trong một khoảng thời gian, và sau đó 40 giảm đều về 0. Theo cách này ta dễ dàng áp đặt các giá trị cực đại về vận tốc và gia tốc. 30 q [o] vt [o/s] 20 Với profile vận tốc dạng hình thang quá trình chuyển động sẽ được chia thành ba pha : gt [o/s2] 10 1. Pha tăng tốc: [chuyển động nhanh dần đều, gia tốc hằng số, vận tốc tăng tuyến tính, và vị 0 trí tăng theo hàm bậc 2] -10 2. Pha vận tốc hằng: giai đoạn di chuyển với vận tốc không đổi [gia tốc bằng 0, dịch chuyển -20 là tuyến tính với thời gian] -30 3. Pha hãm (phanh, giảm tốc): [chuyển động chậm dần với gia tốc hằng số, vận tốc là tuyến -40 tính, và dịch chuyển là hàm parabol] 0 1 2 3 4 5 6 7 8 9 10 t [s] Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Joint space trajectory planning – profile vận tốc -31- Joint space trajectory planning – profile vận tốc -32- Profile vận tốc là hình thang cân. Các hàm gia tốc, vận tốc, và di chuyển như sau: a Xét trường hợp thời gian tăng tốc = thời a Pha tăng tốc: Pha vận tốc hằng: Pha giảm tốc: max max t -t t gian giảm tốc, độ lớn gia tốc như nhau. t -t t 0 £ t £ t : t £ t £ t -t : t -t £ t £ t : f a f f a f a a f a f a f t t t t ìïq ( )t = a ìïïq ( )t = 0 ìïq ( )t = -c a a ïï a 2 ï b ïï d 2 ïïq ( )t = a t +a ïíq ( )t = b ïïq ( )t = -c t +c -a -a íï ï b 1 íï max max ï a 2 1 ï ï d 2 1 ïïq ( )t = 1 a t2 +a t +a ïïîq ( )t = b t +b ïïq ( )t = - 1 c t2 +c t +c 0 £ t £ t Tăng tốc ïî a 2 2 1 0 b 1 0 ïî d 2 2 1 0 v a v v t < t < t -t v Các điều kiện để di chuyển được liên tục, từ điểm đầu đến điểm cuối: max Vận tốc hằng max a f a q(0) = q , q(0) = 0 a = q , a = 0 t -t £ t £ t i 0 i 1 t t -t t t f a f Giảm tốc t t -t t t a f a f a f a f q q q (t ) = v a t = v a = v /t a a max 2 a max 2 max a qf qf q (t ) = v b = v b a max 1 max qm qm Đồ thị luật chuyển động gồm ba đoạn: 1 q (t ) = q (t ) 1 a t2 = b t +b q thẳng và 2 parabol q a a b a 2 2 a 1 a 0 i i b = a t2 -b t = a t2 -v t , b = - v t 1 1 1 ta tm tf -ta tf t ta tm tf -ta tf t 0 2 2 a 1 a 2 2 a max a 0 2 max a Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 9
Joint space trajectory planning – profile vận tốc -33- Joint space trajectory planning – profile vận tốc -34- Phương trình chuyển động tìm được như sau: amax c = a = v /t amax ì 2 2 max a ïï 1 v tf -ta t tf -ta t ï q + max t2, 0 £ t £ t f f ï ï i 2 t a ta t q (t -t ) = v ta t ï a c f a max ïï 1 -a -c (t -t )+c = v t -t £ t £ t : -a q t( ) = ïí q + v t +v (t -t ), t £ t £ t -t max 2 f a 1 max f a f max ìï v ïï i 2 max a max a a f a ïïq ( )t = - max ï v ïï c t v ïï 1 v c = v +c (t -t ) = v t /t ï ï q - max (t -t) ,2 t -t £ t £ t v ï a v ï 1 max 2 f a max f a ï v (t -t) ï f 2 t f f a f max ïï max íq ( )t = max f ïî a ïï c t q t( ) = q , ïï a ta tf -ta tf t f f ïï 1 v (t -t)2 ta tf -ta tf t Lưu ý rằng, các thông số t ,v - c t2 +c t +c = q ïq ( )t = q - max f a max q 1 ï q 2 f 1 f 0 f ï c f 2 t 2 ïî q a q -1 v t2 /t +v t t /t +c = q f max f a max f f a 0 f f cần được chọn sao cho, tồn tại thời gian cho pha vận tốc hằng, tức là 2 qm qm c = q - 1 v t2 /t q 0 f 2 max f a q 0 < t £ t / 2 i i t t t -t t t t t t -t t t a f a m f a f a m f a f Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Joint space trajectory planning – profile vận tốc -35- Joint space trajectory planning – profile vận tốc -36- Cho biết gia tốc q Ta có q v = q t Để biểu thức trong dấu căn không âm, ta cần thêm điều kiện 0 0 max 0 a 4 |q -q | amax Từ đồ thị dịch chuyển ta có ìï 1 amax q t2 2 -4q (q -q ) ³ 0 q ³ f i ïït = t , 0 f 0 f i 0 t2 tf -ta t ï m 2 f tf -ta t f f ï f q -q ïï 1 Từ đây cho phép tính được giá trị nhỏ nhất của gia tốc. ta t v = m a , ïíq = q t( ) = (q +q ), ta t max t -t ï m m 2 f i -a m a ïï -a max ïï 1 1 max Từ đó suy ra: ïq = q t( ) = q + q t2 = q + v t Nếu lấy gia tốc Profile vận tốc dạng tam giác cân ïïî a a i 2 0 a i 2 max a v v v v 4 |q -q | ìïïq + 1 q t 2, 0 £ t £ 1 t max max f i 1 q t( ) = íï i 2 c 2 f (q +q ) / 2-q -v t / 2 (q -q ) q = t = t ïq - q (t -t) ,2 t £ t £ t f i i max a q -q 0 t2 a 2 f ïïî 1 1 v = v = m a = f i f f 2 c f 2 f f max t / 2-t max t -t t -t ta tf -ta tf t f a m a f a ta tf -ta tf t q q Như thế sẽ không còn pha ìïïq t , 0 £ t £ 1 t Với gia tốc a thì thời gian tăng tốc là bao lâu? q t( ) = íï c 2 f q q vận tốc hằng ïïq (t -t), 1 t £ t £ t f (q -q ) f ïî c f 2 f f q t = v = f i q t (t -t ) = (q -q ) qm 0 a max t -t 0 a f a f i qm ì f a ïïïq 0 £ t £ 1 t (q -q ) q t2 2 -4q (q -q ) q t( ) = í c 2 f q f i 1 0 f 0 f i q ï-ïï q 1 t < t £ t i t2 -t t - = 0 t = t - i î c 2 f f t t t -t t t a f a q a 2 f 2q t t t -t t t a m f a f 0 0 a m f a f Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 10
Joint space trajectory planning – profile vận tốc -37- Joint space trajectory planning – profile vận tốc -38- Quỹ đạo với thời gian ngắn nhất: Gia tốc chữ nhật Tóm lại • Chuyển động chia thành 2 pha: pha tăng ta = tf/2 amax 4 |q -q | tốc và pha hãm. t -t t Cho q , q , t Chọn q ³ f i • pha đầu tăng tốc với gia tốc cực đại amax f a f i f f 0 t2 t f • pha cuối hãm với gia tốc cực đại t a t ta=tf/2 f • Profile vận tốc dạng tam giác. Nếu gia tốc -a 1 q t2 2 - 4q (q -q ) của hai pha có trị số bằng nhau thì profile t max Tính t = t - 0 f 0 f i , v = q t a 2 f 2q max 0 a vận tốc sẽ là tam giác cân. Thời gian cho 2 -a v 0 pha bằng nhau = ½ t . max f v • Thời gian chuyển động sẽ là nhỏ nhất. vmax max ta tf -ta tf t Cho q , q , q 4 |q -q | q , q , q q i f 0 Chọn t ³ f i Cho i f max f q t =t /2 t t q 0 4 |q -q | a f f f q Chọn t = f i f q 1 q t2 2 - 4q (q -q ) f q m Tính t = t - 0 f 0 f i , v = q t max a 2 f 2q max 0 a 1 1 qi 0 Tính t = t , v = q t q a 2 f max 2 max f i ta tm tf -ta tf t Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Joint space trajectory planning – profile vận tốc -39- Joint space trajectory planning – profile vận tốc -40- Một số dạng cải tiến Trường hợn đặc biệt: gia tốc hình sin Profile vận tốc hình thang cân đơn giản về mặt tính toán, tuy nhiên gia tốc không liên tục. Dưới đây là một số dạng cải Gia tốc hình sin tiến, với mục đích làm trơn vận tốc và gia tốc. Một sự trả giá cho điều đó là độ phức tạp tính toán tăng lên. Gia tốc hình sin t = t /2 t < t /2 a f a f a amax max Gia tốc hình chữ nhật Gia tốc hình thang cân t =t /2 tf t -t t a f amax amax f a f t t tf -ta t tf -ta t a t f f -a t t t t -a max a a max v v -amax -amax v vmax max v v vmax vmax t t -t t t ta=tf/2 tf t a f a f t t -t t t ta tf -ta tf t q q q a f a f q Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 11
Joint space trajectory planning – gia tốc hình sin -41- Joint space trajectory planning – gia tốc hình cos -42- Trường hợp đặc biệt có thể xảy ra khi, không có thời gian chuyển động với vận tốc hằng số Trường hợn đặc biệt: gia tốc hình cos Gia tốc hình sin p2 ìïa sin(2pt /t ), 0 £ t £ t a ta = tf/2 q t( ) = (q -q )cos(pt /t ), q t( ) = íïï 0 f f 0 2t2 f i f ïï0, t < t f ïî f p 1( ) t =t /2 tf q t( ) = (q -q )sin(pt /t ), q t( ) = (q +q )-(q -q )cos(pt /t ) a f f i f i f ìïa (t / 2 )[1p -cos(2pt /t )], 0 £ t £ t 2t f i f 2 q t( ) = íïï 0 f f f t f ïï0, t < t -a ïî f 0 ïïì t é t æ öù v ï f ê f çç2pt÷÷ú ïïq +a êt - sinç ÷÷ú, 0 £ t £ t q t( ) = í i 0 2p ê 2p ççè t ÷÷øú f ïï ë f û ïïq , t < t ïî f f Như thế ta có ta=tf/2 tf t 2 (p q -q ) q -q = a t (t / 2 )p a = f i q f i 0 f f 0 t2 f 2(q -q ) q = q t( / 2) = a t / p = f i max f 0 f t f Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Lập quỹ đạo qua một tập điểm -43- Lập quỹ đạo qua một tập điểm -44- Cho tập điểm theo trật tự thời gian: Yêu cầu: q(k) = S(t ) & đạo hàm S(t) tại t bằng nhau từ k k hai phía (trái và phải) (tk, qk), k = 0,1,…, N; t(k) < t(k+1), ==> h(k) = t(k+1) - t(k) > 0 Phương án giải quyết: Tìm hàm trơn S(t) [đường cong trơn, liên tục ít nhất đến đạo hàm cấp 1] đi qua • Sử dụng nội suy đa thức, tìm được đa thức bậc N các điểm trên: N S t( ) = P ( )t = a +a t +a t2 +...+a tN-1 +a tN = åa ti N 0 1 2 N-1 N i q(k) = S(tk) & i=0 • Sử dụng đa thức nội suy Lagrange S(t) sẽ là đa thức bậc N đạo hàm S(t) tại tk bằng nhau từ hai phía (trái và phải) åN (t -t ) S t( ) = q L ( ),t L ( )t = j k k k (t -t ) k=0 0£ £j N k j j¹k đa thức Lagrange cơ sở Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 12
Lập quỹ đạo qua một tập điểm -45- Lập quỹ đạo qua một tập điểm -46- • Sử dụng nội suy đa thức, tìm được đa thức bậc N Tập điểm cho S t( ) = P ( )t = a +a t +a t2 +...+a tN-1 +a tN N 0 1 2 N-1 N • Từ điều kiện đa thức đi qua các điểm tựa S t( ) = q a +a t +a t 2 +...+a t N-1 +a tN = q , k k 0 1 k 2 k N-1 k N k Kết quả nội suy đa thức Lagrange k = 1, 2,...,N +1 • Tìm được hệ phương trình ĐSTT é1 t t2 ... tN-1 tN ù é a ù é q ù ê 0 0 0 0 ú ê 0 ú ê 0 ú ê1 t t2 ... tN-1 tN ú ê a ú ê q ú ê 1 1 1 1 ú ê 1 ú ê 1 ú ê ... ú ê ... ú ê .. ú Ta = q ê ú ê ú = ê ú ê ú ê ú ê ú ê1 t t2 ... tN-1 tN ú êa ú êq ú Kết quả nội suy dùng đường spline ê N-1 N-1 N-1 N-1 ú ê N-1 ú ê N-1 ú (kết nối các đa thức bậc 3 trên từng đoạn) ê1 t t2 ... tN-1 tN ú ê a ú ê q ú êë N N N N ú ëû N û ë N û • giải hệ phương trình đại số tuyến tính tìm các hệ số a = T-1q Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Lập quỹ đạo qua một tập điểm -47- Lập quỹ đạo qua một tập điểm -48- Do đó, bài toán tạo ra một quỹ đạo từ N+1 điểm cho trước, gọi là đường di chuyển qua (tiếp) các nhược điểm các điểm tựa, được xác định và phải được tay may với tới tại những thời điểm xác định. • số lượng các phương trình ràng buộc tăng lên, gây khó khăn cho việc Đối với mỗi biến khớp ta có N ràng buộc, và do đó ta có thể sử dụng một đa thức bậc N tìm nghiệm. để mô tả quỹ đạo đi qua các điểm cho trước đó [như sử dụng nội suy Lagrange, Nội suy • các hệ số của đa thức phụ thuộc vào tất cả các điểm cho trước, do đó, Newton, …]. Tuy nhiên, sự lựa chọn này, có những nhược điểm sau đây: nếu nó cần thay đổi một điểm, tất cả đều phải được tính toán lại. • không thể gán trước được vận tốc tại thời điểm ban đầu và thời điểm cuối cùng. • khi bậc của đa thức tăng lên, ứng xử dao động cũng tăng lên, và điều này sẽ làm cho các quỹ đạo không còn phù hợp cho các tay máy. • khi bậc của đa thức tăng lên, thì độ chính xác khi tính toán hệ số của đa thức giảm. Tìm quỹ đạo đi qua N+1 điểm • NO: một đa thức bậc N • YES: N đa thức bậc p thấp hơn N, (p<N), mỗi đa thức nội suy một đoạn của quỹ đạo. Đơn giản nhất là chọn p = 3 (tất nhiên có thể chọn p = 5,7,..). Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 13
Cubic Spline -49- Cubic Spline -50- Để thiết lập bài toán xây dựng đường spline, ta xét bài toán cho một N+1 các q S (t) k qN điểm tựa và chuỗi thời gian tương ứng qk qk+1 q t = t < t < ..... < t < t = t , h = t -t > 0 q N-1 i 0 1 N -1 N f k k+1 k 1 q = q , q , ..., q ,q = q , (initial final, ) S (t) i 0 1 N -1 N f q0 0 Đường spline S(t) bậc 3 là một hàm xác định từng khúc thỏa mãn các điều kiện sau t t t t tN-1 tN t 0 1 k k+1 S t( ) = S ( )t là đa thức bậc 3 trên mỗi đoạn t Î[t ,t ] k = 0,1,...,N -1 Ta viết N đoạn của hàm spline S(t) bậc 3 như sau k k k+1 S t( ) = q , k = 0, 2,...,N S(t) đi qua các điểm tựa S ( )t = a +b (t -t )+c (t -t )2 +d (t -t ) ,3 t Î[t ,t ] k k k k k k k k k k k k+1 Cần tìm 4N hệ số k = 0,1,...,N -1 S t( ),S t( ) &S t( ) liên tục trên toàn bộ miền thời gian t Î [t ,t ]=[0,t ] 1 N +1 f a ,b ,c &d , k = 0,1, 2,...,N -1 k k k k S(t) là hàm liên tục đến đạo hàm bậc 2 (trơn tru). để các điều kiện liên tục của đường spline được thỏa mãn Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Cubic Spline -51- Cubic Spline -52- Thiết lập phương trình xác định các hệ số của đường spline Ta nhận được 4N-2 phương trình sau: 1. S(t) đi qua các điểm tựa: S (t ) = q , k k k a = q , k = 0,1,...,N -1 (1) S (t ) = q , k = 0,1,...,N -1 k k k k+1 k+1 a +h b +h c2 +h d3 = q , k = 0,1,...,N -1 (2) 2. S(t) có các đạo hàm trái và phải tại các điểm tựa bằng nhau (có đạo hàm k k k k k k k k+1 b +2h c + 3h d2 -b = 0, k = 0,1,...,N -2 (3) liên tục tại các điểm tựa): k k k k k k+1 q 2c + 6h d -2c = 0, k = 0,1,...,N -2 (4) Sk(t) q k k k k+1 S (t ) = S (t ), q N k k k+1 k+1 k+1 q S (t ) = S (t ), k = 0,1,...,N -2 q q Sk(t) q k+1 N-1 q N k k+1 k+1 k+1 q1 Hệ 4N-2 pt cho 4N ẩn. k qk+1 q Cụ thể với q S0(t) q N-1 0 1 S ( )t = a +b (t -t )+c (t -t )2 +d (t -t ) ,3 S (t) k k k k k k k k q0 0 S ( )t = b +2c (t -t )+ 3d (t -t ) ,2 k k k k k k t t t t tN-1 tN 0 1 k k+1 S ( )t = 2c + 6d (t -t ), t Î[t ,t ], k = 0,1, 2,...,N -1 k k k k k k+1 t t t t tN-1 tN t 0 1 k k+1 Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 14
Cubic Spline -53- Cubic Spline -54- Cần thêm 2 pt bổ sung: nhận được bằng cách áp đặt điều kiện tại hai điểm đầu và cuối Biến đổi hệ trên về dạng đơn giản hơn Từ phương trình (1) ta có ngay a = q , k = 0,1, 2,...,N -1 k k S (t ) = 0 = S ( ),t Spline tự nhiên (Natural or Free Spline) là Đặt c = 1 m từ phương trình (4) suy ra 0 0 0 i đường spline mà độ cong tại hai đầu bằng k k S (t ) = 0 = S (t ) 2 N-1 N N-1 f 0 (zero curvature). m -m m + 6h d -m = 0 d = k+1 k k k k k+1 k 6h k S (t ) = A = S ( ),t Thế (a ,c ,d ) vào phương trình (2), sau đó giải tìm 0 0 0 i Spline kẹp 2 đầu (Clamped spline) là đường k k k S (t ) = B = S (t ) spline mà hệ số góc tại hai đầu được cho q -a -h c2 -h d3 q -q h h N-1 N N-1 f trước b = k+1 k k k k k b = k+1 k - k m - k (m -m ) k h k h 2 k 6 k+1 k k k S(t ) = S(t ), Not-A-Knot spline với điều kiện tại hai đầu Thay thế (ak,bk,ck,dk) vào phương trình (3) và đơn giản hóa ta nhận được 0 1 1 1 như sau (third derivative matching) é ù S (t ) = S (t ) q -q q -q N-2 N-1 N-1 N-1 h m +2(h +h )m +h m = 6ê k+2 k+1 - k+1 k ú ê ú k=0,…,N-2; N-1 pt; N+1 ẩn. k k k k+1 k+1 k+1 k+2 ê h h ú ë k+1 k û Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Cubic Spline -55- Cubic Spline -56- Cụ thể với đường spline tự nhiên (gia tốc tại thời điểm đầu và cuối bằng 0) Hoặc việt lại thành S (t ) = 0 2c = 0 m = 0 0 0 0 0 é ù é ù é ù S (t ) = 0 = S (t ) 2c = 0 m = 0 ê 1 0 0 0 0 ú ê m ú ê 0 ú 0 N-1 N N N N N êh 2(h +h ) h 0 ú ê m ú ê r ú ê 0 0 1 1 ú ê 1 ú ê 1 ú Cùng với éq -q q -q ù ê 0 h 2(h +h ) h 0 ú ê .. ú ê r ú ê ú ê 1 1 2 2 ú ê ú ê 2 ú h m +2(h +h )m +h m = 6 k+2 k+1 - k+1 k ê ú ê ú ê ú k k k k+1 k+1 k+1 k+2 ê h h ú ê 0 0 h .. h 0 0 ú ê ú = ê .. ú êë k+1 k úû k = 0,1,...,N -2 ê 2 N-3 ú ê ú ê ú ê h 2(h +h ) h 0 ú ê .. ú ê ú Ta thu được ê N-3 N-3 N-2 N-2 ú ê ú ê ú ê 0 h 2(h +h ) h ú êm ú êr ú ê N-2 N-2 N-1 N-1 ú ê N-1 ú ê N-1 ú é ù é ù é ù êê 0 0 0 0 1 ú êú ê m úú êê 0 úúû ê2(h +h ) h 0 0 ú ê m ú ê r ú ë û ë N û ë 0 1 1 1 1 ê h 2(h +h ) h 0 0 ú ê m ú ê r ú ê 1 1 2 2 ú ê 2 ú ê 2 ú é ù ê 0 h 2(h +h ) 0 ú ê .. ú ê .. ú r = 6ê 1 (q -q )- 1 (q -q ) ,ú k = 0, 2,...,N -2 ê 2 2 3 ú ê ú = ê ú k+1 ëh k+2 k+1 h k+1 k û ê .. .. .. h 0 ú ê .. ú ê .. ú k +1 k ê ú ê ú ê ú ê N-3 ú ê ú ê ú ê 0 h 2(h +h ) h ú êm ú êr ú ê N-3 N-3 N-2 N-2 ú ê N-2 ú ê N-2 ú Am = r, A Î Â(N +1) (´ N +1), m & ,r Î Â(N +1) 1´ ê 0 0 h 2(h +h )ú êm ú êr ú ë N-2 N-2 N-1 û ë N-1 û ë N-1 û é ù Giải được r = 6ê 1 (q -q )- 1 (q -q ) ,ú k = 0, 2,...,N -2 Am = r A, Î Â(N-1) (´ N-1), m & ,r Î Â(N-1) 1´ k+1 ëhk +1 k+2 k+1 hk k+1 k û m = 0, m ,m ,...,m , m = 0 m ,m ,...,m ; & m = 0,m = 0 0 1 2 N-1 N Giải được 1 2 N-1 0 N a ,b ,c ,d ; k = 0,1,...,N -1 a ,b ,c ,d ; k = 0,1,...,N -1 k k k k k k k k Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 15
Cubic Spline -57- Cubic Spline -58- Cụ thể với đường spline clamped (cho trước vận tốc tại thời điểm đầu và cuối) Ràng buộc tại thời điểm đầu S (t ) = q : çæq -q ÷ö 2h m +h m = 6çç 1 0 -q ÷÷ =: r 0 0 0 0 0 0 1 çè h 0 ÷÷ø 0 0 S (t ) = q = S ( ),t Clamped Spline là đường spline mà hệ số góc Tại các điểm tựa 0 0 0 0 i (trung gian) k = 0,1,...,N -2 S (t ) = q = S (t ) tại hai đầu được cho trước N-1 N N N-1 f éq -q q -q ù h m +2(h +h )m +h m = 6ê k+2 k+1 - k+1 k ú k k k k+1 k+1 k+1 k+2 ê h h ú tại thời điểm đầu êë k+1 k úû Ràng buộc tại thời S ( )t = b +2c (t -t )+ 3d (t -t ) ,2 æq -q ö điểm cuối S (t ) = q : 0 0 0 0 0 0 2h m +h m = 6çç 1 0 -q ÷÷ =: r N-1 N N ç ÷÷ çæ q -q ÷ö S (t ) = b = q 0 0 0 1 çè h 0 ÷ø 0 h m +2h m = 6çq - N N-1 ÷÷ =: r 0 0 0 0 0 N-1 N-1 N-1 N çç N h ÷÷ N Kết hợp lại ta được hệ N+1 pt cho N+1 ẩn è N-1 ø tại thời điểm cuối é r ù S (t ) = b +2c (t -t )+ 3d (t -t )2 é2h h 0 0 ù é m ù ê 0 ú ê 0 0 ú ê 0 ú ê é ù ú N-1 N N-1 N-1 N N-1 N-1 N N-1 ê h 2(h +h ) h 0 0 ú ê m ú ê 6ê 1 (q -q )- 1 (q -q )ú ú = b +2c h + 3h2 d = q . ê ú ê ú ê ëh1 2 1 h0 1 0 û ú N-1 N-1 N-1 N-1 N-1 N ê 0 0 1 1 ú ê 1 ú ê é ù ú ê 0 h 2(h +h ) 0 ú ê ... ú ê 6ê 1 (q -q )- 1 (q -q )ú ú với cuối ê 1 1 2 ú ê ú ê ëh2 3 2 h1 2 1 û ú ê .. .. ú ê .. ú = ê ... ú m -m ê ú ê ú ê ú 1 ê .. .. .. h 0 ú ê .. ú ê6é 1 (q -q )- 1 (q -q )ùú d = N N-1 , c = m , ê N-2 ú ê ú ê êë N-1 N-2 N-2 N-3 úûú N-1 6h N-1 2 N-1 ê h h ú ê 0 h 2(h +h ) h ú êm ú N -2 N -3 N-1 æ ö ê N-2 N-2 N-1 N-1 ú ê N-1 ú ê 6é 1 (q -q )- 1 (q -q )ù ú q -q h h ç q -q ÷ ê 0 0 h 2h ú ê m ú ê êëh N N-1 h N-1 N-2 úû ú b = N N-1 - N-1 m - N-1 (m -m ) h m +2h m = 6ççq - N N-1 ÷÷ =: r êë N-1 N-1 ú êû ë N úû ê N -1 N -2 ú N-1 h 2 N-1 6 N N-1 N-1 N-1 N-1 N çè N h ÷÷ø N êë r úû N N-1 N-1 Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Cubic Spline: clamped spline -59- Cubic Spline: Thuật toán -60- Kết hợp lại ta được hệ N+1 pt cho N+1 ẩn Cho tập N+1 gồm điểm theo trật tự thời gian Am = r, A Î Â(N +1) (´ N +1), m & ,r Î ÂN + ´1 1 t = t < t < ..... < t < t = t (initial final, ) Giải hệ tìm được i 0 1 N-1 N f q = q ,q , ..., q ,q = q m = A-1r, m = [m ,m ,...,m ]T i 0 1 N-1 N f 0 1 N Từ đó tính được các hệ số của đa thức Sk(t) 1. Tính các bước thời gian a = q h = t -t , k = 0,1, 2,...,N -1 k k k k+1 k q -q h h b = k+1 k - k m - k (m -m ) k h 2 k 6 k+1 k 2. Xây dựng ma trận hệ số A và véc tơ vế phải r (tùy thuộc loại spline) k c = 1 m 3. Giải hệ phương trình đại số tuyến tính: Am = r = > m k 2 k m -m k = 0,1, 2,...,N -1 d = k+1 k 4. Giải các hệ số a , b , c , d = > S (t), k = 0,1,…, N-1 k 6h k k k k k k Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 16
Cubic Spline: Thuật toán -61- Cubic Spline -62- 4. Tính các hệ số từ véc tơ m Ví dụ: cho bộ số liệu sau, N = 5. q -q h h i 0 1 2 3 4 5 a = q , b = k+1 k - k m - k (m -m ), k k k h 2 k 6 k+1 k t=x(i) 4.00 4.35 4.57 4.76 5.26 5.88 k m -m q(i) 4.19 5.77 6.57 6.23 4.90 4.77 c = 1 m , d = k+1 k k 2 k k 6h h(i) 0.35 0.22 0.19 0.50 0.62 k 5. Xây dựng đa thức bậc 3 cho mỗi đoạn t Î[t ,t ] Sử dụng spline tự nhiên ta nhận được k k+1 S ( )t = a +b (t -t )+c (t -t )2 +d (t -t ) ,3 k k k k k k k k t Î [t ,t ], k = 0,1,...,N -1 k k+1 k = 0,1, 2,...,N -1 spline_natural.m; % natural spline, S"(0) = 0; S"(tf) = 0 spline_clamped.m; % clamped spline, S'(0) = A, S'(tf) = B; spline_cubic.m Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Cubic Spline -63- Cubic Spline -64- Tính được các hệ số Sử dụng spline kẹp hai đầu ta nhận được (clamped spline) Tính được các hệ số Chẳng hạn đa thức bậc 3 trên hai đoạn 2 và 3 được viết ra như sau Câu hỏi: hãy kiểm tra tính liên tục của hai hàm trên tại x = 4.76 Checking Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 17
Cubic Spline -65- Multisegment linear path with blends -66- Nội suy quỹ đạo dạng LSPB –[Linear Segments with Parabolic Blends] Sử dụng các đoạn chuyển động tuyến tính + kết nối các đoạn tuyến tính bằng các đoạn uốn parabol Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Multisegment linear path with blends -67- Multisegment linear path with blends -68- Profile vận tốc dạng hình thang là một ví dụ về dạng này. q a = const q v = const k Gia tốc hình chữ nhật amax tf -ta t f ta t q 0 q -amax k+1 v vmax t0 t t t t t 1 k k+1 N q ta tf -ta tf t Yêu cầu tính toán: thời điểm tiếp nối thẳng-parabol, Lưu ý: quỹ đạo chuyển động sẽ không đi qua các điểm tựa. Cần thiết có sự điều chỉnh nếu muốn v(t0)=v(tf)=0 Điều chỉnh t0 và tN để có được 2 đoạn uốn ở đâu và cuối, (đảm bảo v0 = vf =0) Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 18
Thiết kế quỹ đạo trong không gian thao tác -69- Thiết kế quỹ đạo trong không gian thao tác -70- Bài 1. Cần di chuyển điểm cuối E từ A(xA,yA) đến B (xB,yB), trong khoảng Bài 2a. Di chuyển từ A đến B theo đường thẳng: Cho A, B, và khoảng tg thời gian [0 = t0, tf] . Xác định chuyển động không gian khớp q1(t), q2(t). [0=t , t ] . Xác định chuyển động không gian khớp: Hai khớp chuyển động đồng thời. 0 f q1(t), q2(t) B1. Giải động học ngược B, t2 = tf 1. Xác định L = AB = … từ A(x ,y ) ==> q (t ), q (t ) 2. Thiết kế s(t) sử dụng đa thức bậc 3 (5,7,….) A A 1 0 2 0 từ B(xB,yB) ==> q1(tf), q2(tf) s(0) = 0, s t( ) = L B f s(t) s(0) = 0, s t( ) = 0 P q f q y(s) 2 2 3. Xác định điểm P trên quỹ đạo A B2. Thiết kế luật chuyển động A, t = 0 r -r é ùx Q (vận tốc ban đầu và cuối = 0) 0 r ( )t = r + B A s t( ) r = ê ú ê úy P A L ê úë û [t , q (t ), q (t )] ==> q (t) q1 q1 f 1 0 1 f 1 4. Giải động học ngược vị trí (xem chương 5). O [tf, q2(t0), q2(tf)] ==> q2(t) x ( ),t y ( )t q ( ),t q ( )t x(s) P P 1 2 Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME -71- Thiết kế quỹ đạo trong không gian thao tác -72- Bài 2b. Cần di chuyển điểm cuối E từ A(x ,y ,z ) đến B (x ,y ,z ), trong khoảng Bài 3. Di chuyển từ A qua điểm tựa C đến B: A A A B B B Cho A, C, B, và chuỗi 0 = t < t < t = t. thời gian [0 = t0, tf] . Cho phương trình đường thẳng 0 1 2 f Xác định chuyển động không gian khớp: q1(t), q2(t) r -r r ( )t = r + B A s t( ) [x ( ),t y ( ),t z ( )]t P A L P P P B, t = t 2 f Sử dụng đa thức bậc 3 B C, t s t( ) = a +a t +a t2 +a t3, a ? s(t) 1 0 1 2 3 k P y(s) s(0) = 0, s t( ) = L A f s(0) = 0, s t( ) = 0 Q A, t = 0 f 0 Cho biết: A[0, -1, 2] đến điểm B[0, 1, 2], tf = 2 s. Tìm các hệ số ak? O x(s) Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 19
Thiết kế quỹ đạo trong không gian thao tác -73- Thiết kế quỹ đạo trong không gian thao tác -74- Lập trình quỹ đạo trong không gian khớp đảm bảo, bàn kẹp di chuyển từ vị trí đầu đến vị trí cuối, nhưng không đoán được bàn Nếu mong muốn khâu cuối cần di chuyển theo một đường hình kẹp sẽ di chuyển theo đường như thế nào do tình phi tuyến của học xác định nào đó [cong hoặc thẳng] trong không gian thao tác, bài toán động học vị trí. ta cần phải lập kế hoạch chuyển động trực tiếp trong không gian Một quỹ đạo tránh vật cản sẽ rất khó khăn nếu thự hiện trong đó. Tất nhiên, sau đó sẽ giải bài toán động học ngược để xác định không gian khớp. các biến khớp tương ứng. Việc lập kế hoạch có thể được thực hiện bằng cách nội suy thông qua một chuỗi các điểm mà đường di Theo yêu cầu của bài toán: chuyển cần đi qua hoặc bằng cách tạo ra các chuyển động giải tích Khâu tác động cuối (bàn kẹp, que hàn, đầu vòi phun sơn, … cần di (thô) nguyên thủy và quỹ đạo tương đối một cách đúng giờ. chuyển theo một đường nhất định, cùng với hướng xác định Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Thiết kế quỹ đạo trong không gian thao tác -75- Thiết kế quỹ đạo trong không gian thao tác -76- Nếu quỹ đạo được xác định trong không gian làm việc, thì cần thiết phải giải bài toán Một vấn đề khác: các vị trí trong tọa độ Descartes thực tế đạt được trong chuyển động học ngược để tìm quỹ đạo trong không gian khớp (nơi mà các động cơ làm động thu được bằng cách nội suy điểm q(t ) không còn đúng với các giá trị mong việc). Vì điều này sẽ làm tăng thời gian tính toán trong việc lập trình quỹ đạo, nên k muốn như thiết kế ban đầu. việc tính toán quỹ đạo trong kg làm việc và chuyển đổi sang không gian khớp được thực hiện với một tần số thấp hơn so với tần số điều khiển. Do đó, cần thiết phải nội suy các dữ liệu trước khi chuyển nó cho bộ điều khiển cấp thấp: thông thường, sử dụng nội tuyến tính cho đơn giản. Ví dụ: Đường thẳng là thể hiện các giá trị mong muốn như thiết kế ban đầu. Tuy nhiên khi thực hiện lại thu được các giá trị thay đổi xung quanh đường mong muốn. Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Page 20
Thiết kế quỹ đạo trong không gian thao tác -77- Thiết kế quỹ đạo trong không gian thao tác -78- x ,x ,...,x Đối với việc tính toán quỹ đạo không gian làm việc, có thể áp dụng một trong các kỹ thuật sử dụng 0 1 N cho các không gian khớp (thay thế các biến khớp q(t) bằng x(t), tức là một vị trí / hướng trong không (3) t ,t ,...,t (2) gian Descartes) hoặc xác định đường hình học bằng hàm giải tích (ví dụ như một hình elip) là hàm của 0 1 N thời gian (tức là p = p(t)), hoặc tốt hơn là sử dụng biểu diễn tham số phương trình của đường di chuyển p = p(s), với s = s(t) là luật chuyển động thích hợp. q ,q ,...,q x ( ),t Việc gì ở đâu ? 0 1 N d t ,t ,...,t t Î[t ,t ] B, s=L 0 1 N 0 N Nội suy trong không gian khớp (1) z 0 (1) (4) Nội suy trong không gian thao tác (2) z(s) q( ),t t Î[t ,t ] = 0 = 1 0 N Động học ngược tại các điểm tựa (3) A, s=0 p(s) s p(s) (5) Động học ngược cả quá trình (4) t=0 [t0 tf ] t O y(s) 0 x( ),t t Î[t ,t ] Động học thuận (5) y0 (6) 0 N s=0 s= L s x(s) x = x(t ) So sánh các giá trị vào và ra (6) k k x0 Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Thiết kế quỹ đạo trong không gian thao tác -79- Thiết kế quỹ đạo trong không gian thao tác -80- Xét bài toán vị trí điểm tác động cuối (end-effector): Với quỹ đạo tham số ta có các liên hệ sau B, s=L z0 Lập trình một quỹ đạo trong không gian làm việc, thường là một đường hình học p (đoạn thẳng, cung tròn, elip,. . . ) được định nghĩa là một hàm z(s) của một tham số s(t): p = p(s). A, s=0 p(s) Tham số s = s(t) được tính bằng cách sử dụng một trong những phương s p(s) pháp đã trình bày trong phần quỹ đạo trong không gian khớp. O y(s) 0 Một cách tiếp cận cổ điển là tạo hàm s(t) là kết hợp các đoạn tuyến tính y0 Quỹ đạo không chứa điểm kỳ dị nếu: x(s) với các hàm bậc 2, sin, cos, [lưu ý việc hạn chế giật, thay đổi gia tốc đột (đường cong liên tục, trơn tru) ngột, tránh kích thích dao động kết cấu, ...). x0 dp dx s( ) dy s( ) dz s( ) ¹ 0 ¹ 0, ¹ 0, ¹ 0 ds ds ds ds Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME Nguyen Quang Hoang ‐ Department of Applied Mechanics ‐ SME
Showing the first 20 pages. Download the document to see it all.
Comments (0)
No comments yet. Be the first!