bài tập lớn robot mô phỏng đại học bách khoa BKA

ĐỀ BÀI: Hình 1: Robot Scalar 2DOF Cho robot Scalar 2DOF có thông số khớp sau: l1 =0.5 ; lg1 = 0.25; l2 =0.5 ; lg2 =0.25 ; m1 =25 ; m2 =25 ; J1 = 5; J2 = 5; =; =; K1 = 21; K2 = 30; T1 = T2 = 0.05; BÀI TẬP SỐ 1: Mỗi khớp điều khiển hệ driver có hàm truyền: = Khớp có K=21, T=0,05; khớp có K=21, T=0,05 1.Thiết kế điều khiển vị trí cho khớp biết |Ui|10, độ điều chỉnh không lớn 5%, thời gian độ nhỏ Ta coi thời gian độ thời gian hệ thống lần đầu đạt đến giá trị sai số 2% so với giá trị đặt Mô hình điều khiển: Hàm truyền đối tượng: = = sử dụng phương pháp tối ưu module để tìm điều khiển tối ưu cho hệ thống Đối tượng có hàm truyền nên ta sử dụng phương pháp bù số thời gian lớn đối tượng để tìm điều khiển Bộ điều khiển có dạng: = = Với: = 4T; == *Khớp có K=21, T=0.05 =>=0.2; =0.48 =>= Lắp thông số vào điều khiển ta đồ thị: 1.4 1.2 0.8 0.6 0.4 0.2 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 Hệ thống có độ điều chỉnh 5.34% thời gian độ 0,55, chưa đáp ứng yêu cầu Nhưng so với mục tiêu độ điều chỉnh 5% 5,34% số sát Do ta chỉnh lại tham số điều khiển chút để hệ thống đạt yêu cầu Giảm Kp giúp độ điều chỉnh giảm, thời gian độ thay đổi Giảm Ki giúp độ điều chỉnh giảm thời gian độ tăng Ta chọn tham số vừa đủ để độ điều chỉnh đạt 5% mà thời gian độ đủ nhỏ Chọn điều khiển = Ta đồ thị: 1.4 1.2 0.8 0.6 0.4 0.2 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 0.7 0.8 0.9 Độ điều chỉnh 4,66%, thời gian độ 0,5s *Khớp có K=30, T=0,05 => T_I=0.2; K_p=1/3 => G_PI= ; 1.4 1.2 0.8 0.6 0.4 0.2 0 0.1 0.2 0.3 0.4 0.5 0.6 Độ điều chỉnh 6,36%, thời gian độ 0,585s, chưa đạt yêu cầu Chọn lại tham số điều khiển = 1.4 1.2 0.8 0.6 0.4 0.2 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 Độ điều chỉnh 3,66%; thời gian độ 0.58s 2.Chọn quỹ đạo thẳng quỹ đạo tròn, tính quỹ đạo cho khớp Giả sử điểm cuối robot vị trí có tọa độ (,), ta xác định góc quay khớp robot Ta có: =.cos + cos(+) (1) =.sin + sin(+) (2) Bình phương vế (1) (2), sau cộng với ta được: + = + +2 cos => = arccos Từ (1) (2) ta lại có: -.cos = cos(+) (3) -.sin = sin(+) (4) Bình phương vế (3) (4), sau cộng với ta được: + + - cos - sin = =>.cos + sin = =>cos + sin = Đặt sinα = =>cosα = =>sin(α) = => = arcsin - arcsin Như vậy, ta biết vị trí tọa độ robot, ta tính góc quay khớp robot 2.1.Robot theo quỹ đạo thẳng từ (0,0.5) đến (0.5,0) khoảng thời gian 10s Robot chuyển động theo quỹ đạo có phương trình y = 0.5-x (0) Thay y = 0.5-x; =0.5; =0.5 vào công thức tìm ta = arcsin–arcsin =arcsin-arcsin = arccos Mặt khác, robot chuyển động từ vị trí x có giá trị đến 10s, coi robot chuyển động liên tục x= , thay vào ta được: = arcsin - arcsin = arccos Lệnh vẽ đồ thị , khoảng thời gian (0,10) thực matlab >>ezplot('asin((2*t^2-10*t+25)/(10*(2*t^2-10*t+25)^0.5))-asin(t/(2*t^210*t+25)^0.5)',[0,10]) (q1) >>ezplot('acos((2*t^2-10*t-25)/50)',[0,10])(q2) Ta thu kết quả: asin((2 t 2-10 t+25)/(10 (2 t 2-10 t+25)0.5))-asin(t/(2 t 2-10 t+25)0.5) 0.6 0.4 0.2 -0.2 -0.4 -0.6 -0.8 -1 t 10 10 (rad) acos((2 t 2-10 t-25)/50) 2.5 1.5 0.5 0 t (rad) 2.2.Robot chuyển động theo quỹ đạo tròn từ (0,0.5) đến (0.5,0) thời gian 10s, tâm đường tròn gốc O(0,0): Robot chuyển động theo quỹ đạo tròn có phương trình + = 0.25 với Thay + = 0.25; =0.5; =0,5 vào công thức tìm ta được: = arcsin(0.5)-arcsin(2x) = arcsin(0.5)-arcsin(0,2t) = arccos(-0.25) Ta có đồ thị quỹ đạo góc khớp theo thời gian: asin(0.5)-asin(0.2 t) 0.6 0.4 0.2 -0.2 -0.4 -0.6 -0.8 -1 t 10 10 (rad) acos(-0.25) 2.5 1.5 0.5 x (rad) 3.Mô Matlab-Simulink, so sánh quỹ đạo với quỹ đạo thực tế: 3.1.Quỹ đạo thẳng: Tại thời điểm ban đầu t=0 thì: = arcsin(0.5)-arcsin(0); = arccos (-0,5) Sơ đồ khối mô Simulink: Trong Clock dùng để tạo thời gian, 10 (s) Cụ thể khối tính góc đặt khớp: * = arcsin = arccos - arcsin Kết mô phỏng: 0.6 0.4 0.2 -0.2 -0.4 -0.6 -0.8 -1 -1.2 10 10 đặt thực 2.5 1.5 0.5 -0.5 đặt thực *Nhận xét: Sau khoảng thời gian nhanh chóng hệ thống đạt đến giá trị sát với giá trị xác lập, đồ thị quan sát đường gần trùng Nhưng khoảng 0.5s cuối hệ không ổn định cho Để đánh giá cách xác hơn, ta quan sát sai lệch tĩnh hệ thống: 0.1 0.08 0.06 0.04 0.02 -0.02 -0.04 -0.06 -0.08 -0.1 10 10 Sai lệch góc khớp Sai lệch nhỏ, khoảng từ đến 0.009 0.25 0.2 0.15 0.1 0.05 -0.05 -0.1 -0.15 -0.2 Sai lệch góc khớp Sai lệch nhỏ thời gian từ đến 9s, khoảng từ đến 0.02, từ 9s đến 10s lại lớn 3.1.Quỹ đạo tròn: Tại thời điểm ban đầut=0 thì: = arcsin(0.5); = arccos (-0,25) Sơ đồ khối mô Simulink: *Khối tính = arcsin(0.5)-arcsin(0,2t) Khối tính = arcos(-0.25) Kết mô phỏng: 0.6 0.4 0.2 -0.2 -0.4 -0.6 -0.8 -1 -1.2 đặt thực 10 2.5 1.5 0.5 10 10 đặt thực 0.15 0.1 0.05 -0.05 -0.1 Sai lệch góc quay khớp 1 0.8 0.6 0.4 0.2 -0.2 -0.4 -0.6 -0.8 -1 10 Sai lệch góc quay khớp BÀI TẬP SỐ 2: a) Xây dựng mô hình robot Giả thiết khớp sinh momen M tác dụng bệ nối 1; khớp sinh momen M2 tác dụng nối 2; trọng lực có hướng theo trục y Ký hiệu động nối i Ki Pi (i=1,2) Ta có: Động nối 1: 1 K1 = m1l g θ + J1 θ 2 Thế nối 1: P1 = m1 gl g1 sin θ1 Tọa độ tâm khối nối 2:  x2 = l1cosθ1 + lg 2cos(θ1 +θ )   y2 = l1 sin θ1 + l g 2sin(θ1 +θ ) Tốc độ tâm khối nối 2:  x = − l sin θ θ − l sin( θ + θ )( θ + θ 2) 1 g2    y = l1cosθ1 θ + lg 2cos(θ1 +θ )(θ + θ )  →Bình phương tốc độ dài tâm nối 2: v = x + y = l θ + l (θ + θ ) + 2l1l g 2cosθ (θ + θ θ ) 2 2 g2 Động nối 2: 1 K = m2v2 + J (θ + θ ) 2 1 2 K = m2 [l1 θ + lg (θ + θ ) + 2l1l g 2cosθ (θ + θ θ )] + J (θ + θ ) 2 Thế nối 2: P2 = m2 g[l1 sin θ1 + lg sin(θ1 + θ )] Hàm lagrange robot: L = ( K1 + K ) − ( L1 + L2 ) 1 = m1l g θ + J1 θ + 2 2 + m2 [l1 θ + lg (θ + θ ) + 2l1l g 2cosθ (θ + θ θ )] + + J (θ + θ ) − m1 glg1 sin θ1 − m2 g[l1 sin θ1 + lg sin(θ1 + θ )] Mômen khớp 1: M1 = d ∂L ∂L ( )−( ) dt ∂ θ ∂θ1 = [m1l g + J1 + m2 (l + l +[m2 (l 2 g2 g2 + 2l1l g 2cosθ ) + J ]θ1 + l1l g 2cosθ ) + J ]θ − m2l1l g sin θ (2θ1 θ + θ 22 ) + m1 gl g1cosθ1 + m2 g[l1 sin θ1 + l g 2cos(θ1 + θ )] Mômen khớp 2: M2 = d ∂L ∂L ( )−( ) dt ∂ θ ∂θ 2 = [m2 (l g2 + l1l g 2cosθ ) + J ]θ1 + (m l + m2 gl g 2cos(θ1 + θ ) Đặt M  θ  M = 1 q= 1 M  θ  , Ta có mô hình toán học robot: M = H ( q ) q + V ( q, q ) + G ( q ) Trong đó: 2 g2 + J ]θ + m2l1l g sin θ θ12  H11 H12  H =   H 21 H 22  : ma trận quán tính Với:  H11 = m1l g21 + J1 + m2 (l12 + lg22 + 2l1lg 2cosθ ) + J   H12 = H 21 = m2 (lg + l1l g 2cosθ ) + J   H 22 = m2lg + J   − m l l sin θ ( θ + θ θ ) g 2 2   V =   m2l1l g sin θ θ1  : thành phần momen nhớt hướng tâm g  G= 1  g2  : thành phần momen trọng lực Với:   g1 = m1 gl g 1cosθ1 + m2 g[(l1cosθ1 + l g 2cos(θ1 + θ )]    g = m2 glg 2cos(θ1 + θ ) Thực mô matlab: Mô hình matlab: Trong đó: - Hàm tính G: function y = G(q) % thong so ro bot m1 = 25; m2 = 25; l1 = 0.5; l2 = 0.5; g = 9.8; J1 = 5; J2 = 5; lg1= l1/2; lg2 = l2/2; % ma tran G g1 = m1*g*lg1*cos(q(1))+m2*g*[l1*cos(q(1))+lg2*cos(q(1)+q(2))]; g2 = m2*g*lg2*cos(q(1)+q(2)); y = [g1;g2]; - Hàm tính V: function y = V(q,dq) % thong so robot m1 = 25; m2 = 25; l1 = 0.5; l2 = 0.5; g = 9.8; J1 = 5; J2 = 5; lg1= l1/2; lg2 = l2/2; % ma tran V V1 = -m2*l1*lg2*sin(q(2))*[dq(2)^2+2*dq(1)*dq(2)]; V2 = m2*l1*lg2*sin(q(2))*dq(1)^2; y = [V1;V2]; - Khối H −1 (q ) Hàm tính H −1 (q ) function y = H(q,M) % thong so robot m1 = 25; m2 = 25; l1 = 0.5; l2 = 0.5; g = 9.8; J1 = 5; J2 = 5; lg1= l1/2; lg2 = l2/2; % bien khop theta1=q(1); theta2=q(2); % ma tran H(q) H11=m1*lg1^2+J1+m2*(l1^2+lg2^2+2*l1*lg2*cos(theta2))+J2; H12=m2*(lg2^2+l1*lg2*cos(theta2))+J2; H21=H12; H22=m2*lg2^2+J2; H = [H11 H12;H21 H22]; % ma tran nghich dao H^(-1)(q) Hinv = inv(H); % Gia toc khop y = Hinv * M; Tạo khối robot (Create Subsystem) c) Thiết kế điều khiển PD bù trọng trường - Mô hình toán học robot: M = H ( q ) q + V ( q, q ) + G ( q ) Trong ma trận H, V, G xác định theo - Thuật toán điều khiển: M dk = K P e − K D q + G ( q ) Trong đó: sai lệch góc qay: Tham số điều khiển e = qd − q K  KP =    K1  K  KD =   0 K  Mô hình điều khiển matlab: Các hệ số khuếch đại tỷ lệ đạo hàm lựa chọn: 150  KP =    15  3.50  KD =    3.5 Kết mô phỏng (không chạy được).!!!!! [...]... khớp 2 Sai lệch khá nhỏ trong thời gian từ 0 đến 9s, chỉ khoảng từ 0 đến 0.02, nhưng từ 9s đến 10s lại khá lớn 3.1.Quỹ đạo tròn: Tại thời điểm ban đầut=0 thì: = arcsin(0.5); = arccos (-0,25) Sơ đồ khối mô phỏng trên Simulink: *Khối tính = arcsin(0.5)-arcsin(0,2t) Khối tính = arcos(-0.25) Kết quả mô phỏng: 0.6 0.4 0.2 0 -0.2 -0.4 -0.6 -0.8 -1 -1.2 0 1 2 3 4 5 đặt và thực 6 7 8 9 10 3 2.5 2 1.5 1 0.5 0... điều khiển PD bù trọng trường - Mô hình toán học robot: M = H ( q ) q + V ( q, q ) + G ( q ) Trong đó các ma trận H, V, G được xác định theo trên - Thuật toán điều khiển: M dk = K P e − K D q + G ( q ) Trong đó: sai lệch góc qay: Tham số bộ điều khiển e = qd − q K 0  KP =  1   0 K1  K 0  KD =  2  0 K 2  Mô hình bộ điều khiển trên matlab: Các hệ số khuếch đại tỷ lệ và đạo hàm được lựa chọn:... 2 ]θ 2 − m2l1l g 2 sin θ 2 (2θ1 θ 2 + θ 22 ) + m1 gl g1cosθ1 + m2 g[l1 sin θ1 + l g 2cos(θ1 + θ 2 )] Mômen của khớp 2: M2 = d ∂L ∂L ( )−( ) dt ∂ θ ∂θ 2 2 = [m2 (l 2 g2 + l1l g 2cosθ 2 ) + J 2 ]θ1 + (m l + m2 gl g 2cos(θ1 + θ 2 ) Đặt M  θ  M = 1 q= 1 M 2  θ 2  , Ta có mô hình toán học robot: M = H ( q ) q + V ( q, q ) + G ( q ) Trong đó: 2 2 g2 + J 2 ]θ 2 + m2l1l g 2 sin θ 2 θ12  H11... 2cos(θ1 + θ 2 ) Thực hiện mô phỏng trên matlab: Mô hình trên matlab: Trong đó: - Hàm tính G: function y = G(q) % thong so ro bot m1 = 25; m2 = 25; l1 = 0.5; l2 = 0.5; g = 9.8; J1 = 5; J2 = 5; lg1= l1/2; lg2 = l2/2; % ma tran G g1 = m1*g*lg1*cos(q(1))+m2*g*[l1*cos(q(1))+lg2*cos(q(1)+q(2))]; g2 = m2*g*lg2*cos(q(1)+q(2)); y = [g1;g2]; - Hàm tính V: function y = V(q,dq) % thong so robot m1 = 25; m2 = 25;... 9 10 6 7 8 9 10 đặt và thực 0.15 0.1 0.05 0 -0.05 -0.1 0 1 2 3 4 5 Sai lệch góc quay khớp 1 1 0.8 0.6 0.4 0.2 0 -0.2 -0.4 -0.6 -0.8 -1 0 1 2 3 4 5 6 7 8 9 10 Sai lệch góc quay khớp 2 BÀI TẬP SỐ 2: a) Xây dựng mô hình robot Giả thiết khớp 1 sinh ra momen M 1 tác dụng giữa bệ và thanh nối 1; khớp 2 sinh ra momen M2 tác dụng giữa thanh nối 1 và 2; trọng lực có hướng theo trục y Ký hiệu động năng và thế... H(q,M) % thong so robot m1 = 25; m2 = 25; l1 = 0.5; l2 = 0.5; g = 9.8; J1 = 5; J2 = 5; lg1= l1/2; lg2 = l2/2; % bien khop theta1=q(1); theta2=q(2); % ma tran H(q) H11=m1*lg1^2+J1+m2*(l1^2+lg2^2+2*l1*lg2*cos(theta2))+J2; H12=m2*(lg2^2+l1*lg2*cos(theta2))+J2; H21=H12; H22=m2*lg2^2+J2; H = [H11 H12;H21 H22]; % ma tran nghich dao H^(-1)(q) Hinv = inv(H); % Gia toc khop y = Hinv * M; Tạo khối robot (Create... năng thanh nối 2: P2 = m2 g[l1 sin θ1 + lg 2 sin(θ1 + θ 2 )] Hàm lagrange của robot: L = ( K1 + K 2 ) − ( L1 + L2 ) 2 2 1 1 2 = m1l g 1 θ 1 + J1 θ 1 + 2 2 2 2 1 2 2 2 + m2 [l1 θ 1 + lg 2 (θ 1 + θ 2 ) + 2l1l g 2cosθ 2 (θ 1 + θ 1 θ 2 )] + 2 1 + J 2 (θ 1 + θ 2 ) 2 − m1 glg1 sin θ1 − m2 g[l1 sin θ1 + lg 2 sin(θ1 + θ 2 )] 2 Mômen của khớp 1: M1 = d ∂L ∂L ( )−( ) dt ∂ θ ∂θ1 1 = [m1l g 1 + J1 + m2...  KP =  1   0 K1  K 0  KD =  2  0 K 2  Mô hình bộ điều khiển trên matlab: Các hệ số khuếch đại tỷ lệ và đạo hàm được lựa chọn: 150  KP =    0 15  và 3.50  KD =    0 3.5 Kết quả mô phỏng (không chạy được).!!!!! ... arcsin - arcsin Như vậy, ta biết vị trí tọa độ robot, ta tính góc quay khớp robot 2.1 .Robot theo quỹ đạo thẳng từ (0,0.5) đến (0.5,0) khoảng thời gian 10s Robot chuyển động theo quỹ đạo có phương trình... 10s lại lớn 3.1.Quỹ đạo tròn: Tại thời điểm ban đầut=0 thì: = arcsin(0.5); = arccos (-0,25) Sơ đồ khối mô Simulink: *Khối tính = arcsin(0.5)-arcsin(0,2t) Khối tính = arcos(-0.25) Kết mô phỏng: ... góc quay khớp 1 0.8 0.6 0.4 0.2 -0.2 -0.4 -0.6 -0.8 -1 10 Sai lệch góc quay khớp BÀI TẬP SỐ 2: a) Xây dựng mô hình robot Giả thiết khớp sinh momen M tác dụng bệ nối 1; khớp sinh momen M2 tác dụng