Báo cáo môn học Robotics

1. Giải bài toán động học. + Cho một quy luật chuyển động của các khâu của robot. + Tính vận tốc điểm tác động cuối E, vận tốc góc khâu thao tác, vận tốc góc các khâu. 2. Tính toán tĩnh học. + Cho lực vào khâu thao tác tại điểm E gồm vectơ lực F và mômen M(F=Fx ,Fy ,FzT,M=Mx ,M y,MzT) tính lực (mô men) dẫn động tại các khớp đảm bảo robot cân bằng tĩnh(bỏ qua ma sát)

BÁO CÁO ROBOTICS 2016 NỘI DUNG CẦN ĐẠT ĐƯỢC Nội dung: Giải toán động học + Cho quy luật chuyển động khâu robot + Tính vận tốc điểm tác động cuối E, vận tốc góc khâu thao tác, vận tốc góc khâu Tính tốn tĩnh học + Cho lực vào khâu thao tác điểm E gồm vectơ lực F mômen M(F=[Fx ,Fy ,Fz]T,M=[Mx ,M y,Mz]T) tính lực (mơ men) dẫn động khớp đảm bảo robot cân tĩnh(bỏ qua ma sát) Tính tốn động lực học Coi khâu robot đồng chất, tiết diện ngang không đáng kể + Xác định tham số động lực học robot + Tính động robot Điều khiển mô Chọn luật điều khiển phù hợp, thiết kế hệ thống điều khiển CHƯƠNG 1: BÀI TOÁN Nguyễn Duy Khánh BÁO CÁO ROBOTICS 2016 ĐỘNG HỌC VỊ TRÍ – ĐỘNG HỌC VI PHÂN 1.Mơ hình robot, thơng số động học bảng DH θi θ1 - π/2 θ3 + Ma trận truyền biến đổi DH : cos(1 ) � � sin( ) A1  � � � � di αi d1 d2 0 a3 π/2 π/2 0 sin(1 ) 0�  cos(1 ) � � d1 � � 0 1� Nguyễn Duy Khánh BÁO CÁO ROBOTICS �0 � 1 A2  � �0 � �0 2016 1 � 0 0� � d2 � � 0 1� cos(3 )  sin(3 ) � � sin( ) cos( ) A3  � � 0 � � 0 a3 cos( ) � a3 sin( ) � � � � � sin(1 )  cos(1 ) sin(1 ).d � �0 �0  cos( )  sin( )  cos( ).d � 1 2� A2  A1 A2  � � � 1 0 d1 � � 0 �0 � sin(1 ) cos( )  cos(1 ) sin(1 )[ a3 sin( )  d ] � �sin(1 ) sin(3 ) � cos(1 ) sin(3 )  cos(1 ) cos(3 )  sin(1 )  cos(1 )[ a3 sin(3 )  d ]� 0 � � A3  A2 A3  �  cos(3 ) � sin( )  a3 cos( )  d1 � � 0 � � Vị trí điểm thao tác hướng khâu thao tác + Ma trận ma trận cosin hướng xác định theo phép quay Roll-Pitch-Yaw, Cardan Euler: c11 ( ,  , ) c12 ( ,  , ) c13 ( ,  , ) � � c ( ,  , ) c22 ( ,  , ) c23 ( ,  , ) A3 (t )  �21 � c31 ( ,  , ) c32 ( ,  , ) c33 ( ,  , ) � 0 � Xe � Ye � � Ze � � 1� Với XE, YE, ZE vị trí điểm tác động cuối ; α, β, η biểu diễn hướng khâu thao tác Phương trình động học nhận được: Từ ta xác định vị trí điểm tác động cuối hướng khâu thao tác nghiệm hệ phương trình: Nguyễn Duy Khánh BÁO CÁO ROBOTICS 2016 �xE  sin(1 )[ a3 sin( )  d ] �y   cos( )[a sin( )  d ] 3 �E � �z E  a3 cos(3 )  d1  � sin(  )   cos(1 ) � �  sin( ) sin(  )sin( )  cos( )cos( )  cos( q1 ) cos( q3 )  � cos(  ) sin( )  sin(1 )cos( ) � Động học vi phân 3.1 Vận tốc điểm tác động cuối xE � �sin(1 )[ a3 sin(3 )  d ] � � � � rE  � yE �  cos(1 )[ a3 sin(3 )  d ]� � � � � � � �  a cos(  )  d z 3 � �E � �  v E  d( r dt E )  �( r � q E ) q& & &� � cos(q1 )[a3 sin(3 )  d ]&  sin(1 ) d  sin(1 ).a3 cos( )3 � & &� 0 vE  � sin(1 )[a3 sin(3 )  d ]&  cos(1 ) d  cos(1 ).a3 cos( ) � � � a3 sin( )& � � 3.2 Vận tốc góc khâu * Khâu thao tác: + Từ ma trận A3 , rút ma trận cosin hướng: sin(1 ) cos(3 )  cos(1 ) � �sin(1 ) sin(3 ) � R3  �  cos(1 )sin(3 )  cos(1 ) cos(3 )  sin(1 ) � � � �  cos(  ) sin(  ) 3 � � �sin(1 ) sin(3 ) cos(1 )sin( )  cos( ) � � � R � sin(1 ) cos( )  cos(1 ) cos(3 ) sin( ) � �  cos( ) �  sin(1 ) � � & & & � � 1 cos(1 )sin(3 )  3 sin(1 ) cos( ) 1 sin(1 ) sin(3 )  & & cos(1 ) cos( ) sin( ) �& � &T & & & R3  � 1 cos(1 ) cos( )  & sin(1 ) sin( ) 1 sin(1 ) cos( )   cos(1 ) sin( )  cos( ) � � � & & sin(1 ) cos(1 ) � � T Nguyễn Duy Khánh BÁO CÁO ROBOTICS 2016 &0 T 0 %  R3 R3 � & � & %   � � & & sin  cos  � � & sin  � & 1 cos 3 � � � cos(q3 )q&1 � � � 3  � sin(q3 )q&1 � � � & � q � � *Khâu 1: + ma trận cosin hướng: R1  R&1  &0 T  %  R1 R1  0 1  *Khâu 2: + Ma trận cosin hướng: R2  Nguyễn Duy Khánh BÁO CÁO ROBOTICS 2016 R&2  &0 T 0 %  R2 R2  0 2  3.3 Gia tôc điểm tác động cuối + & &� � cos(q1 )[a3 sin(3 )  d ]&  sin(1 ) d  sin(1 ).a3 cos( ) � & &� vE  � sin(1 )[a3 sin( )  d ]&  cos(1 ) d  cos(1 ).a3 cos( ) � � � a3 sin(3 )& � � � aE  v&E  Ta tính vào trường hợp cụ thể sau: Để khảo sát chuyển động điểm E, ta cho chuyển động biến khớp: q1  sin(2t ) � � q2  3t  0.5 � � q3  sin(5t )  2t � Khi đó: �sin(sin(2t ))(0,5sin(sin(5t )  2t )  3t  0,5) � rE  � cos(sin(2t ))(0,5sin(sin(5t )  2t )  3t  0,5) � � � � � 0,5sin(sin(5 t  t ))  0.5 � � Nguyễn Duy Khánh BÁO CÁO ROBOTICS 2016 Quỹ đạo điểm thao tác cuối: Hình 3.1 Đồ điểm tác động cuối E (với t = -> Pi/2) �x&E  2cos(sin(2t))cos(2t)(0,5sin(sin(5t)+2t)+3t+0,5)+sin(sin(2t))(0,5cos(sin(5t)+2t).(5cos(5t)+2)+3) � �y&E  2sin(sin(2t))cos(2t)(0,5sin(sin(5t)+2t)+3t+0,5)  cos(sin(2t))(0,5cos(sin(5t)+2t)(5cos(5t)+2)+3) �z&  0,5sin(sin(5t)+2t).(5cos(5t)+2) �E vE  x&2  y&2E  z&2E � 3 x  2cos(2t)cos(sin(5t)+2t) � 3 y  2cos(2t)sin(sin(5t)+2t) � � 3 z  5cos(5t)+2 � E  3 x  3 y  3 z Nguyễn Duy Khánh BÁO CÁO ROBOTICS Hình 3.2 Đồ thị vận tốc điểm cuối E theo t theo t 2016 Hình 3.3 Đồ thị vận tốc góc khâu Bài toán động học ngược + Cho vị trí, vận tốc, gia tốc điểm tác động cuối, hướng, vận tốc góc, gia tốc góc khâu thao tác Tính tọa độ biến khớp + Đặt: q1 = θ1, q2 = d1, q3 = θ3 4.1 Giải phương pháp giải tích Từ hệ phương trình: �xE  sin(q1 )[a3 sin(q3 )  q2 ] � �yE   cos(q1 )[a3 sin(q3 )  q2 ] �z  a cos(q )  d 3 �E Từ phương trình (3): �d  z � q3  a cos � E � � a3 � Kết hợp phương trình (1) (2): q1  a tan  xE yE  Từ phương trình (1): q2  � �d1  z E xE xE  a3 sin(q3 )   a3 sin � a cos � � sin( q1) sin(a tan( xE yE )) � a3 � � � � � � � � 4.2 Xây dựng quy luật chuyển động khâu thao tác E giải động học ngược phương pháp số Newton-Raphson �f  xE  sin(q1 )[ a3 sin( q3 )  q2 ]  � �f  yE  cos(q1 )[a3 sin(q3 )  q2 ]  �f  z  a cos(q )  d  E 3 � �f1 � � F � �f � � �f � � Bài toán biết xE(t), yE(t), zE(t) thời điểm t ta tìm vector q=[q1, q2, q3]T thời điểm Ta lấy giá trị sát giá trị đầu để tiến hành trình lặp Newton-Raphson Quá trình lặp dừng lại sai số lần k+1 với lần k nhỏ giá trị cho phép Chương trình viết phần mềm Maple Nguyễn Duy Khánh BÁO CÁO ROBOTICS 2016 Với quỹ đạo điểm tác động cuối đường thẳng có phương trình sau �xE  t  � �yE  0, 4t  0.3 �z  0,5t  0.1 �E Đoạn chương trình viết maple > > > > > > > > > > Vẽ đồ thị Matlab dựa vào file data.txt % -Nguyễn Duy Khánh BÁO CÁO ROBOTICS 2016 fid = fopen('D:/data.txt', 'r'); a = fscanf(fid, '%g %g %g %g %g %g %g', [7 inf]) fclose(fid); d1=a(1,:);%t d2=a(2,:);%q1 d3=a(3,:);%q2 d4=a(4,:);%q3 d5=a(5,:);%x d6=a(6,:);%y d7=a(7,:);%z figure(1); plot(d1,d2,'c','LineWidth',2);xlabel('t');ylabel('q1(rad)'); legend('q1'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(2); plot(d1,d3,'c','LineWidth',2);xlabel('t');ylabel('q2(rad)') legend('q2'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(3); plot(d1,d4,'c','LineWidth',2);xlabel('t');ylabel('q3(rad)'); legend('q3'); set(gcf,'Color',[1.0,1.0,1.0]); grid on figure(4); plot3(d5,d6,d7,'c','LineWidth',2); legend('quy dao diem tac dong cuoi E'); set(gcf,'Color',[1.0,1.0,1.0]); grid on % Nguyễn Duy Khánh 10 BÁO CÁO ROBOTICS 2016 Đồ thị q1, q2 theo t Đồ thị q điểm tác động cuối vẽ từ q1, q2, q3 Nguyễn Duy Khánh 11 BÁO CÁO ROBOTICS 2016 CHƯƠNG 2: TĨNH HỌC ROBOT + Dựa vào hệ phương trình đệ quy để tính lực khớp: 0 � �Fi ,i 1  Fi 1,i  Pi �0 0 i 0 i % �M i ,i 1  M i 1,i  r% i 1 Fi ,i 1  r ci Pi + Từ ma trận truyền DH ta có ma trận cosin hướng: cos(q1 ) sin(q1 ) � � R1  � sin( q1 ) cos( q1 ) � � � � � � � 0 sin(q1 ) cos(q1 ) � �0 � R2  �0 cos(q1 )  sin(q1 ) � � � �  0 � � sin( q1 ) cos( q3 )  cos( q1 ) � �sin( q1 )sin( q3 ) � R3  � cos(q1 ) sin(q3 )  cos( q1 ) cos(q3 )  sin(q1 ) � � � �  cos( q ) sin( q ) 3 � � + Gọi c1, c2, c3 khoảng cách từ gốc tọa độ khâu 1, 2, đến khối tâm khâu 1, 2, *Tính tốn khâu 3: Nguyễn Duy Khánh 12 BÁO CÁO ROBOTICS 3 r c3  [  c3 , 0, 0]T � r r c3 sin( q1 ) cos( q3 )  cos( q1 ) � �sin(q1 ) sin(q3 ) � T  R rc  � cos( q1 ) sin( q3 )  cos( q1 ) cos( q3 )  sin( q1 ) � [  c , 0, 0] � � �  cos( q ) sin( q ) 3 � � 0 3 2016 c3 sin(q1 )sin(q )c3 � � �  �cos(q1 )sin(q1 )c3 � � � � cos(q )c 3 � � r  [  a , 0, 0]T 3 sin( q1 ) cos( q3 )  cos( q1 ) � �sin( q1 )sin( q3 ) r  R 3 r 33  � cos(q1 ) sin(q3 )  cos( q1 ) cos( q3 )  sin( q1 ) � [  a , 0, 0]T � � � sin( q3 ) � �  cos(q3 ) �    sin(q1 )a 3sin(q3 )   � � � � r �    cos(q1)a 3sin(q )  � � �     a 3cos(q )   � � + Các ma trận đối xứng lệch thiết lập: a 3cos(q )  cos(q1)a 3sin(q3 ) � � � � r%  � a 3cos(q ) sin(q1 )a 3sin(q3 ) � � � cos(q1)a 3sin(q )  sin(q1 )a 3sin(q ) � � cos(q )c3 cos(q1 )sin(q1 )c3  � � � � r%  � cos(q )c3 sin(q1 )sin(q )c3 � �cos(q )sin(q )c   sin(q )sin(q )c � 1 3 � � T P  [ 0, 0, m3g] c3 0 � �F3,2  F4,3  P3 �0 0 30 % �M 3,2  M 4,3  r% F3,2  r c P3 Mặt khác: T � �F3  [  Fx ,  Fy ,  Fz ] �0 T �M  [  M x ,  M y ,  M z ] Thay vào công thưc ta tính lực momen khâu 3: Nguyễn Duy Khánh 13 BÁO CÁO ROBOTICS 2016 � �  Fx � �0 � � � F32  �  Fy � � �  Fz  m3 g � � � � � � M x +a 3cos(q3 )Fy  cos(q1 )a 3sin(q )Fz +cos(q1 )sin(q1 )c3m 3g � � � �M  M  a cos(q )F  sin(q )a sin(q )Fz+sin(q )sin(q )c m g � 3 x 3 3 � � 32 � y � � M +cos(q )a3sin(q )F +sin(q )a sin(q )F � z x 3 y � � � * Khâu 2: sin(q1 )c � � 2 r c  [  c , 0, 0]T � r c  R 2 rc  � cos(q1 )c � � � � � � 0  � 0  � � r% � 0 �cos(q )c   sin(q )c 2 � cos(q1 )c  � � sin(q1 )c � � � 2 sin(q1 )q � � � r  [ q , 0, 0] � r2  R r  � cos(q1 )q � � � � 0  � � T 0 0  � � r% � 0 �cos(q )q   sin(q )q 2 � P 2 cos(q1 )q  � � sin(q1 )q � � �  [ 0, 0, m2 g]T 0 � �F2,1  F3,2  P2 �0 0 20 % �M 2,1  M 3,2  r% F2,1  r c P2 Thay vào cơng thưc ta tính lực momen khâu 2: �  Fx � � �0 � �  Fy �F21  � � � � �  F  m g  m g � � z � � �M x +a 3cos(q3 )Fy +(  q  a 3sin(q3 ))cos(q1 )Fz+(q m3g+sin(q )c3 m3 g+c2 m2 g)cos(q1 ) � � � �0 M  � M y  a 3cos(q )Fx +(  q  a 3sin(q ))sin(q1 )Fz +(q m 3g +sin(q )c m3g+c2 m g)sin(q1 ) � 21 � � � � M z +(a 3sin(q )+q )cos(q1 )Fx +(a 3sin(q )+q )sin(q1 )Fy � � � � *Khâu 1: Nguyễn Duy Khánh 14 BÁO CÁO ROBOTICS 2016  0   � � r c1  [ 0, c1 , 0]T � r1c1  R1.1r1c1  �  0   � � � �    c   � 1� � �0 c1   0 � � � r% c1 0 � � �0 0 � � � 0    � � 1 r  [ 0, d1 , 0]T � r 10  R1.1r10  � 0   � � � � d1 � � � �0 d1   0 � � � r% � d1 0 � �0  0 � � � 1 P  [ 0, 0, m1g]T 0 � �F10  F21  P1 �0 0 0 %0 �M 10  M 21  r% F10  rc1 P1 Từ ta có khâu 1: �  Fx � � �0 � �  Fy �F10  � � � � �  F  m g  m g  m g � � z � � �  M x +(a 3C3  d1 )Fy +(  q  a S3 )C1Fz +(q m +S3c3m +c m )gC1 � � � �M  �  M y +(  a 3C3 +d1 )Fx +(  q  a 3Sq )S1Fz +(q m +S3c 3m +c 2m )gSq1 � 10 � � � �  M z +(a 3S3 +q )C1Fx +(a 3S3 +q )sS1Fy � � � � CHƯƠNG 3: ĐỘNG LỰC HỌC 1.Tính ma trận jacobi tịnh tiến quay Gọi lc1, lc2, lc3 khoảng cách từ gốc tọa độ O0, O1, O2 đến khối tâm khâu 1, 2, gọi i Cj tọa độ trọng tâm khâu thứ j hệ Ri Nguyễn Duy Khánh 15 BÁO CÁO ROBOTICS 2016 Các tọa độ vận tốc góc khâu: � � �0 � � � �0 � 0 0� ( d  l ) � rc1  � c1 � rc1 0 A1.1 rc1  � � � 0 0� � � � lc1 � J T  � � � � �� � � 0 1 � � � �=> � �=> sin(q1 )(lc  q ) � � �0 � �cos(q )(l  q ) � � � l c2 � rc  A2 rc  � rc  � c � � � d1 �0 � � � � � � �=> �1 �=> cos(q1 )(lc  q ) sin(q1 ) � � � JT  � sin(q1 )(lc  q ) cos(q1 ) � � � 0 0� � � ( a3  lc ) � � �sin(q1 )(sin(q )l c +q ) � � � � cos(q1 )(sin(q )lc +q ) � � rc 0 A3 rc  � � rc  � � � � cos(q )lc +d1 � � � � � � �=> � �=> JT cos(q1 )(sin(q )lc +q ) sin(q1 ) sin(q1 )cos(q )lc � � � sin(q1 )(sin(q )lc3 +q ) cos(q1 ) cos(q1 )cos(q )l c3 � � � � � 0 sin(q )l c3 � � 0� 0 0� � � � � � % R&  1  � q&1 � J R1  � 0�  R � � � � � 0 0 �� � � �q&1 � �1 0� � � T0& %2  R2 R2  2  �0 � J R  � 0 0� � � � � � � 0 0 � � � � T 1 �q&1cos(q3 ) � �cos(q ) 0 � � � � %3  R R&  3  �q&1 sin(q3 ) � J R3  � sin(q3) 0 � � � & � � � q 0 � � � � T 3 2.Tính tenxo quán tính khâu robot Nguyễn Duy Khánh 16 BÁO CÁO ROBOTICS 2016 Ma trận tenxơ quán tính hai khâu 1, với trục gắn vào khối tâm song song với hệ trục khâu tương ứng hệ quán tính chính: = � � m.l c1     0       0 � � �12 � � m.l c1 � 0          0 � � 12 � � 0      0      0 � � � � � �,  = � � m.l c       0       0 � � � 12 � � m.l c � 0          0 � � 12 � � 0      0      0 � � � � � �,  = � � � � 0    0      0 � � m.l c � 0          0 � � � 12 � � m.l 2c � � 0      0       � 12 � 3.Tính động năng, robot 3.1 Động robot Ti  T v ci mi v ci  i i T I i i i 2 Ac1  A1.1 Ac1  cos(1 ) � � sin(1 ) � � � � � r � vc1  r& , v c1 c1 � sin(1 ) �� �  cos(1 ) � � � d1 �� �� 0 1� � 0 0 0 � � � lc1 � � � � T c1 � sin(1 )  cos(1 ) sin(1 ).d �� �0 �0  cos( )  sin( )  cos( ).d �� 1 2� � � � �� 1 0 d1 � �� 0 �0 Ac  A2 Ac  � � 0 0 0 � � � lc � � � � T � rc 2 � vc  r& c2 , v c2 sin(1 ) cos( )  cos(1 ) sin(1 )[ a3 sin( )  d ] � �sin(1 ) sin(3 ) � cos(1 )sin( )  cos(1 ) cos( )  sin(1 )  cos(1 )[ a3 sin( )  d ]� � � �  cos(3 ) � sin(3 )  a3 cos(3 )  d1 � � 0 Ac  A3 Ac  � 0 � 0 � � � 0 � � � � lc � 0 � � � � 0 � � T � rc33 � vc3  r& c3 , v c3 Nguyễn Duy Khánh 17 2016 BÁO CÁO ROBOTICS + Mặt khác theo ta tính J T , J T , J T , J R1 J R , J R , 1 ,   , phần Ta thay vào cơng thức tìm kết động Ti  T v ci mi v ci  i i T I i i i 2 3.2 Thế robot T   �mi g r Ci i 1 T g  [0, 0,  g] với �   m1lc1g+m 2d1g+m3 (  cos(q ).l c +d1 )g 3.3 Tính lực suy rộng + Ta có lực suy rộng: Q j  � J Tj Ti Fi  J Tj Ri M i  n 1 J T   0 T cos(q1 )(lc  q ) sin(q1 ) � � � JT  � sin(q1 )(lc  q ) cos(q1 ) � � � � 0 � � cos(q1 )(lc  q ) sin(q1 )(lc  q ) � � � T � J T  � sin(q1 ) cos(q1 ) 0� � � � 0 � � JT cos(q1 )(sin(q )lc +q ) sin(q1 ) sin(q1 )cos(q )l c � � � � sin(q1 )(sin(q )l c +q ) cos(q1 ) cos(q1 )cos(q )l c3 � � � � 0 sin(q )l c3 � � �J T T3 cos(q1 )(sin(q3 )lc3 +q ) sin(q1 )(sin(q )lc +q ) � � � � � sin(q1 ) cos(q1 ) � � � sin(q )cos(q )l  cos(q )cos(q )l sin(q )l c 3 c 3 c � � Nguyễn Duy Khánh 18 BÁO CÁO ROBOTICS 0 0� � � � � � T J R1  � 0 �� J R1  � 0 � � 0 0� 0 � � � 1 0 � 1 � � � � � T J R  �0 0 �� J R  �0 � � �0 0 � � �0 J R3 2016 0� 0� � 0� � 0� 0� � 0� � cos(q ) 0 � cos(q ) sin(q3) � � � � � �  �sin(q3) 0 �� J R  � 0 0� � � � � � 0 0 � � � � Thay vào công thức tổng quát ta tìm lực suy rộng Q j  � J Tj Ti Fi  J Tj Ri M i  n 1 CHƯƠNG 4: ĐIỀU KHIỂN ROBOT Tất hệ thống điều khiển nêu theo luật điều khiển PD Khi thiết kế hệ thống điều khiển ta bỏ qua động học cấu chấp hành, quán tính động Như chức điều khiển tạo moomen cần thiết để truyền động khớp robot đảm bảo khớp robot ln bám theo vị trí đặt 4.1 Hệ thống điều khiển khơng gian khớp Tín hiệu đặt quỹ đạo bậc biến khớp 4.1.1 Hệ thống điều khiển phản hồi Luật điều khiển Nguyễn Duy Khánh 19 BÁO CÁO ROBOTICS 2016 Hình5.1.Sơ đồ cấu trúc hệ thống điều khiển robot với điều khiển PD & C (q, q&)q& G (q)  U  J ET F M (q)q& Ta có phương trình động lực học : , gọi & & & V ( q , q )  C ( q , q ) q H(q)=M(q)tránh nhầm với M vector momen , M  U tiếp đến ta coi robot không chịu tác dụng ngoại lực luật điều khiển bám quỹ đạo F  phương trình động lực học rút gọn sau: & V ( q, q&)  G ( q) M  H ( q).q& Luật điều khiển : M dk  K p (qd  q)  K d (q&d  q&)  K p   K d & K  diag ( K , K , , K ) p1 p2 pn Trong : p - ma trận đường chéo hệ số khuếch đại khớp riêng biệt K d  diag ( K d , K d , , K dn ) -ma trận đường chéo hệ số khuếch đại đạo hàm khớp riêng biệt Với luật điều khiển giả thiết thành phần momen trọng lực G(p) bù hoàn toàn Hệ thống điều khiển với cấu trúc điều khiển trên, ổn định tuyệt đối toàn cục Thực chọn hàm Liapunov có dạng sau: T V  ( T Kp.  q&.H q&) Hàm VL biểu thị tổng lượng hệ thống robot: Thành phần chứa Kp tỷ lệ với lượng đầu L Nguyễn Duy Khánh 20 BÁO CÁO ROBOTICS 2016 vào, thành phần sau động robot mà Kp H ma trận có hệ số dương Nên hàm VL> với q khác qd Tính đạo hàm cấp VL ta nhận được: T T T & & & & H q& q&T H&.q& q&T H q ) V&  (&.Kp.  Kp.& q L T H q& Do tính chất đối xứng thành phần  Kp. , q& , ta rút gọn T T T & & V&  &.Kp.  q&.H&.q& q&.H q Từ phương trình động lực học với giả thiết khơng có thành phần momen trọng lực G(q), nhận phương trình sau : T T T )] V&  &.Kp.  q&.H&.q& q&.[M V (q,q& Sử dụng thuộc tính phương trình động lực học áp dụng luật điều khiển T T T T T V&  q&.Kd & q&.C (q, q&).q& q&.H&(q).q& q&.Kd & q&.( H&C )q& 2 ta có Trong đó: V (q, q&)  C (q, q&).q& T L L L & H  C q&T ( H& C )q& Do ma trận ma trận đối xứng ngược => T Kd q&�0 & &  q V L   Từ cho thấy rằng, mức độ dương VL phụ thuộc vào Kp; & mức độ âm V L phụ thuộc vào Kd Do tăng tốc độ hội tụ tăng giá trị Kd Nâng cao độ xác tinh hệ thống điều khiển đạt tăng hệ số Kp khâu khuếch đại Tuy nhiên ,Kp Kd lớn làm giảm độ ổn định chất lượng trình độ độ điều chỉnh , thời gian độ tăng 4.1.2 Hệ thống điều khiển momen tính tốn Luật điều khiển Nguyễn Duy Khánh 21 BÁO CÁO ROBOTICS 2016 Hình 5.2 Sơ đồ điều khiển Phươngpháp luật điều khiển lựa chọn luật điều khiển cho khử thành phần phi tuyến phương trình động lực học phân li đặc tính động lực nối Kết nhận hệ thống tuyến tính đảm bảo độ xác chuyển động yêu cầu Dựa phương trình động lực học : & V (q, q&)  G ( q) M  H (q).q&  Phương trình mơ tả Luật điều khiển có dạng sau : M dk  H ( q )U dk  V ( q, q&)  G (q ) Cân hai phương trình đựa tính chất H(q) ma trận thực dương nên lấy nghịch đảo, ta nhận phương trình vi phân tuyến tính cấp & U dk q& hai sau: phương trình vi phân tuyến tính cấp độc lập khớp Do thiết kế điều khiển độc lập có cấu chúc PD cho khớp sau : & & U dk  q& d  K p  Kd  Từ ta rút phương trình vi phân sai số vị trí hệ thống kín có dạng sau : & K d & K p   & s2 I  Kd s  K p  Phương trình đặc tính dạng tốn tử Laplace : Viết cho khớp riêng lẻ (đó khâu quán tính bậc hai): Nguyễn Duy Khánh 22 BÁO CÁO ROBOTICS 2016 s  K di s  K pi  Các hệ số Kdi, Kpi chọn dương nên đảm bảo ổn định, chúng tính tốn theo u cấu chất lượng điều khiển độ điều chỉnh σ , Thời than độ Tqd K di  2n K pi  n2 , (5.7) Để đạt độ điều chỉnh  �20% , hệ số suy giảm ξ = 0,5-0,7 Tần số dao động tính theo thời gian độ ( Tqd) yêu cầu hệ số suy giảm ξ:  n   T qd  100.e   n 1  4.2 Hệ thống điều khiển không gian làm việc Trong hệ thống điều khiển khơng gian làm việc tín hiệu đặt trực tiếp quỹ đạo chuyển động mong muốn tay máy robot không gian làm việc , lương phản hồi tính từ vị trí khớp thơng qua khâu động học thuận Khâu tính tốn động học ngược đặt mạch vòng điều khiển phản hồi tính đổi biến khơng gian khớp Hệ thống điều khiển không gian làm việc sử dụng hiệu thực tương tác tay máy môi trường Hệ thống điều khiển ma trận Jacobien chuyển vị Luật điều khiển Lực cần thiết để di chuyển tay máy theo quỹ đạo đặt không gian làm việc xác định từ sai lệch vị trí sai lệch tốc độ khơng gian làm việc tương ứng với luật điều khiển phản hồi PD đinh điển: Gdk  K p ( Sd  S )  K d ( S&d  S&) Trong đó: Gdk - vector lực cần thiết để tay robot di chuyển theo quỹ đạo tốc độ đặt trước Sd , S : tương ứng vector vị trí đặt vector vị trí thực tay robot S&d , S& : tương ứng vector tốc độ đặt vector tốc độ thực K p  diag ( K p1 , K p , , K pn ) K d  diag ( K d , K d , , K dn ) - ma trận đường chéo hệ số khuếch đại -ma trận đường chéo hệ số khuếch đại đạo hàm Nguyễn Duy Khánh 23 BÁO CÁO ROBOTICS 2016 Vector lực tay robot biến đổi lực momen khớp robot thông qua ma trận Jacobien chuyển vị Như vector momen truyền động khớp robot xác định sau: M dk  J T � K p ( S d  S )  K d ( S&d  S&) � � � Sơ đồ khối hệ thống sau : Hình 5.3 Sơ đồ điều khiển Nguyễn Duy Khánh 24 BÁO CÁO ROBOTICS 2016 TÀI LIỆU THAM KHẢO [1] Bài giảng Robotics - PGS TS Phan Bùi Khôi [2] Động lực học hệ nhiều vật - GS TSKH Nguyễn Văn Khang [3] Cơ sở Robot công nghiệp -GS.TSKH Nguyễn Văn Khang Nguyễn Duy Khánh 25 ... Nguyễn Duy Khánh 10 BÁO CÁO ROBOTICS 2016 Đồ thị q1, q2 theo t Đồ thị q điểm tác động cuối vẽ từ q1, q2, q3 Nguyễn Duy Khánh 11 BÁO CÁO ROBOTICS 2016 CHƯƠNG 2: TĨNH HỌC ROBOT + Dựa vào hệ...BÁO CÁO ROBOTICS 2016 ĐỘNG HỌC VỊ TRÍ – ĐỘNG HỌC VI PHÂN 1.Mơ hình robot, thơng số động học bảng DH θi θ1 - π/2 θ3 + Ma trận truyền biến đổi... Hình 5.3 Sơ đồ điều khiển Nguyễn Duy Khánh 24 BÁO CÁO ROBOTICS 2016 TÀI LIỆU THAM KHẢO [1] Bài giảng Robotics - PGS TS Phan Bùi Khôi [2] Động lực học hệ nhiều vật - GS TSKH Nguyễn Văn Khang [3]