CHNG 4 QHQD trajectory planning

CHƯƠNG QUI HOẠCH QUỸ ĐẠO Qui hoạch quỹ đạo tạo tín hiệu vào tham chiếu cho điều khiển robot để robot di chuyển theo quỹ đạo mong muốn Quỹ đạo điểm điểm quỹ đạo qua hai điểm định trước thời gian xác định, quỹ đạo đường quỹ đạo qua nhiều điểm theo đường liên tục xác định trước Ta qui hoạch không gian biến khớp nghĩa tín hiệu tham chiếu giá trị đặt cho biến khớp qd(t) qui hoạch không gian làm việc với tín hiệu tham chiếu Xd(t) vị trí hướng không gian 4.1 QUI HO CH QU Đ O ĐIỂM ĐIỂM 4.1.2 Qu đ o đa thức Ta muốn đầu công tác qua hai điểm xác định khơng gian, từ vị trí đầu cuối đầu cơng tác ta giải tốn động học ngược dùng teach pendant di chuyển đầu công tác đến vị trí đả định đọc biến khớp tương ứng, xác định giá trị đầu cuối cho biến khớp qo =q(to), qf= q(tf) Xét cho biến khớp, ta tìm biểu thức cho q(t) thỏa mãn điều kiện vị trí vận tốc thời điểm đầu cuối, xét thêm điều kiện gia tốc q(to )  qo , q(t f )  q f q(to )  q0 , q(t f )  q f (4.1) Ta có bốn ràng buộc chọn q(t) đa thức bậc ba, thông thường t0=0 q(t )  a3t  a2t  a1t  a0 (4.2) a0  q0 a1  q0 a3t 3f  a2t 2f  a1t f  a0  q f 3a3t 2f  2a2t f  a1  q f Giải hai phương trình cuối ta a2  3(q f  q0 )  2(q0  q f )t f t 2f , a3  2(q f  q0 )  (q0  q f )t f t 3f (4.3) Trường hợp thời điểm ban đầu t0 ta thay tf (4.3) tf- t0 t (4.2) t - t0 Nếu chọn vận tốc đầu cuối ta q(t )   2(q f  q0 ) t f t3  3(q f  q0 ) t f t  q0 K THU T ROBOT- TS NGUY N Đ C THÀNH (4.4) q(t )   2(q f  q0 ) (t f  t0 ) (t  t0 )3  3(q f  q0 ) (t f  t0 )2 CH (t  t0 )  q0 NG (4.5) Hình 4.1 Qu đ o b c ba Quỹ đạo đa thức bậc ba (Hình 4.1) có gia tốc đầu cuối khác Để cho chuyển động mượt ta thêm hai điều kiện gia tốc đầu cuối 0, q(t) có bậc (PT 4.6) (4.6) q(t )  a5t  a4t  a3t  a2t  a1t  a0 a0  q0 a1  a2  a5t 5f  a4t 4f  a3t 3f  q f  q0 , 5a5t 4f  4a4t 3f  3a3t 2f  0, Giải phương trình ta 20a5t 3f  12a4t 2f  6a3t f  a5  6(q f  q0 ) / t 5f , a4  15(q f  q0 ) / t 4f , a3  10(q f  q0 ) / t 3f , a2  a1  0, a0  q0 (4.7) Đa thức bậc cao có nhiều cực trị khơng kiểm sốt địi hỏi khối lượng tinh tốn lớn, khó áp dụng thực tế Lập trình Matlab tính quỹ đạo bậc ba bậc 5, cho giá trị đầu, cuối thời gian clc; close all; theta0=10;thetaf=80;tf=3; %bac ba a3=-2*(thetaf-theta0)/tf^3 a2=3*(thetaf-theta0)/tf^2 t=0:0.01:tf; theta=a3*t.^3+a2*t.^2+theta0; thetad=3*a3*t.^2+2*a2*t; thetadd=6*a3*t+2*a2; plot(t,theta) 86 QUI HO CH QU Đ O grid on hold on plot(t,thetad,'.r') title('Quy dao bac ba') hold on plot(t,thetadd,' ') figure % bac5 b=[thetaf-theta0;0;0]; A=[tf^5 tf^4 tf^3;5*tf^4 4*tf^3 3*tf^2;20*tf^3 12*tf^2 6*tf] a=inv(A)*b t=0:0.01:tf; theta=a(1)*t.^5+a(2)*t.^4+a(3)*t.^3+theta0; plot(t,theta) title('Quy dao bac 5'); grid on hold on thetad=5*a(1)*t.^4+4*a(2)*t.^3+3*a(3)*t.^2; 4.1.2 Qu đ o v n t c hình thang Quỹ đạo đa thức cần nhiều phép tính nhân tốn thời gian, ta dùng quỹ đạo có vận tốc hình thang biểu thức q(t) gồm hai đoạn parabole hai đầu đoạn tuyến tính (LSPB Linear Segment with Parabolic Blend), việc tính tốn đơn giản Gia tốc số qc Vận tốc gồm hai đoạn dốc đầu cuối, vận tốc đầu cuối 0, vận tốc đoạn số q c nhỏ giá trị cho trước., Thời gian tăng tốc tcvà với thời gian giảm tốc Thời gian di chuyển tf, tc