Bài tập lớn môn Robotics

Bài tập lớn môn robotics đại học Bách khoa Hà nội. Cấu trúc là robot ba bậc tự do toàn khớp quay. Có tính toán đầy đủ mô hình động học, động lực học, tĩnh học, thiết kế bộ điều khiển. Mô phỏng bằng maple, matlab.

BÀI TIỂN LUẬN ROBOTICS MỤC LỤC I Xây dựng cấu trúc, thiết lập hệ phương trình động học robot……… a Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg……………………… b Thiết lập hệ phương trình động học robot……………………………… II Giải toán động học………………………………………………… …….3 2.1 Giải toán động học thuận…………… ………………………….… a, xác định vận tốc điểm tác động cuối vận tốc góc khâu thao tác …… b, xây dựng quy luật chuyển động khâu từ vẽ quỹ đạo điểm E, vận tốc điểm E vận tốc góc …………………………………………………………… 2.2 Giải toán động học ngược…………………………………………… a, giải phương pháp giải tích…………………………………………… b, 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……………………………………… …7 III Tính toán tĩnh học………………………………………………………… 11 3.1 Cho lực tác động vào khâu thao tác tìm lực dẫn động………………….11 3.2 Cho lực ( mô men ) dẫn động tính lực ( mô men ) khâu thao tác tác dụng lên đối tượng…………………………………………………………………… 14 IV Tính toán động lực học…………………………………………………… 16 4.1 Xây dựng cấu trúc động lực học thành phần cần thiết để viết phương trình động lực học…………………………………………………… 16 4.2, thiết lập phương trình Lagrang robot……………………………… 19 V Luật điều khiển………………………………………………………………22 Hệ thống điều khiển không gian khớp………………………………22 1.1 Hệ thống điều khiển phản hồi…………………………………………….22 1.2 Hệ thống điều khiển momen tính toán……………………………………23 Hệ thống điều khiển không gian làm việc………………………… 25 TÀI LIỆU THAM KHẢO …………………………………………………… 27 BÀI TIỂN LUẬN ROBOTICS I Xây dựng cấu trúc, thiết lập hệ phương trình động học robot Ta thiết lập phương trình động học theo phương pháp ma trận Denavit Hartenberg a Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg Khâu 0: đế ta chọn hệ tọa độ XoYoZo có trục Zo chọn trùng với khớp 1, trục Xo chọn tùy ý cho phù hợp hình vẽ , trục Yo chọn theo quay tắc tam diện thuận Khâu 1: ta chọn hệ tọa độ X1Y1Z1 có trục Z1 trùng với khớp 2, trục X1 ta chọn theo hướng Zo x Z1, trục Y1 chọn theo quay tắc tam diện thuận Khâu 2: ta chọn hệ tọa độ X2Y2Z2 có trục Z2 trùng với khớp 3,X2 chọn theo đường vuông góc chung Z1 Z2, Y2 chọn theo quy tắc tam diện thuận Khâu 3: Ta chọn hẹ X3Y3Z3 có trục Z3 song song Z2 X2 chọn theo đường vuông góc chung Z2 Z3, Y3 chọn theo quy tắn tam diện thuận Hình 2.1 Hệ tọa độ Robot RRR b Thiết lập hệ phương trình động học robot Từ việc chọn hệ tọa độ ta có bảng DH sau: Khâu di θi αi d1 q1 π/2 q2 a2 q3 a3 0 A =  cos(q1 ) sin( q1 ) sin( q ) −cos( q ) 1    0  0  d1    , A =  cos(q2 ) − sin( q2 )  sin(q ) cos(q2 )   0   a2 cos( q2 )  a2 sin(q2 )     BÀI TIỂN LUẬN ROBOTICS A => => 0 =  cos(q3 ) − sin(q3 )  sin(q ) cos( q3 )   0   A = A ( q) = A A −C1 S2 S1 a2 C1C2 C1C2 S C   S2   A A a3 cos(q3 )  a3 sin(q3 )     2 A − S1 S C2  −C1 a2 S1C  d1 + a2 S2    C1C23 S C  23  S 23   −C1 S 23 S1 C1 (a3C23 + a2 C2 )  − S1 S 23 −C1 S1 ( a3C23 + a2 C2 )  C23 d1 + a2 S + a3 S 23   0  = = Với C1=cos(q1), S1=sin(q1), C2=cos(q2), S2=sin(q2), S23=sin(q2+q3), C23=cos(q2+q3), q=[q1, q2, q3]T Mặt khác ta lại mô tả hướng vị trí qua ma trận sau thông qua vector p=[xE, yE, zE, α, β, η]T α, β, η góc Cardan A ( p) = cos(α )cos(η ) − cos( β )sin(η ) sin( β )   sin(α )sin( β )cos(η ) + cos(α )sin(η ) − sin(α )sin( β )sin(η ) + cos(α )cos(η ) − sin(α )cos(β )   − cos(α )sin( β )cos(η ) + sin(α )sin(η ) cos(α )sin( β )sin(η ) + sin(α )cos(η ) cos(α )cos( β )  0  0 A (q) A ( p) So sánh hai ma trận xE  yE  zE    , ta thiết lập hệ phương trình động học sau:  f1 = A ( p )[1, 4] − A ( q)[1, 4] = xE − cos(q1 ) [ a3 cos( q2 + q3 ) + a2 cos(q2 ) ] = 3  0  f = A ( p )[2, 4] − A ( q)[2, 4] = yE − sin( q1 ) [ a3 cos( q2 + q3 ) + a2 cos( q2 ) ] = 3  0  f = A ( p )[3, 4] − A ( q)[3, 4] = z E − [ d1 + a2 sin( q2 ) + a3 sin( q2 + q3 ) ] = 3  0  f = A3( p )[1,1] − A3( q)[1,1] = cos(α )cos(η ) − cos( q1 )cos(q2 + q3 ) =  0  f = A3( p )[2, 2] − A3( q)[2, 2] = − sin(α ) sin( β ) sin(η ) + cos(α )cos(η ) + sin(q1 )sin(q2 + q3 ) =  0  f = A3( p )[3,3] − A3( q)[3,3] = cos(α )cos( β ) = II Giải toán động học BÀI TIỂN LUẬN ROBOTICS 2.1 Giải toán động học thuận Để tính toán ta chọn d1=1 (m), a2=0.5 (m), a3=0.4 (m) a, xác định vận tốc điểm tác động cuối vận tốc góc khâu thao tác Từ phương trình động ta rút ( với q=[q1, q2, q3]T ) r E  xE  cos( q1 ) [ a3 cos( q2 + q3 ) + a2 cos(q2 ) ]   y  = sin( q ) a cos(q + q ) + a cos( q )  [ 3 2 ]   E     z E   d1 + a2 sin(q2 ) + a3 sin( q2 + q3 )  = d(  => r = ∂( r E ) ∂q q& = J E q& ⇒  − S1 (a3C23 + a2 C2 ) −C1 (a3 S 23 + a2 S2 ) C ( a C + a C ) − S (a S + a S ) 2 23 2  23  a3C23 + a2 C2 − a3 C1 S 23  − a3 S1 S23  a3 C23  E J = v  x& E  −S1 (a3 C23 + a2 C2 )q& − C1 (a3 S 23 + a2 S2 ) q& − a3C1 S 23 q&   y&  =  C (a C + a C )q& − S (a S + a S )q& − a S S q&  2 1 23 2 23   E   23  z& E    & & ( a3 C23 + a2 C2 )q2 + a3 C23 q3 E = Từ ma trận R ) dt E 0 E =  v = C1C23 S C  23  S23 A ta rút trân cosin hướng −C1 S23 S1  − S1 S 23 −C1  C23  => ω% =  sin(q1 )(q& + q& )   −cos( q )( q& + q& )      q&1 R& RT 2 = b, xây dựng quy luật chuyển động khâu từ vẽ quỹ đạo điểm E, vận tốc điểm E vận tốc góc BÀI TIỂN LUẬN ROBOTICS Để khảo sát kết ta xây dựng quy luật chuyển động biến khớp q1 = sin(3t )  q2 = 2t + q = cos(5t ) + 3t  q sau r E = Với t=0-> 2*Pi/5 ta vẽ đồ thị điểm tác động cuối phân mềm maple Hình 2.2 Đồ thị điểm tác động cuối x& E = BÀI TIỂN LUẬN ROBOTICS y& E z& E = = vE = x& 2E + y& 2E + z& 2E  ωy = ωx = ωy = , w3 = ω x +ω y +ω , z  Ta dùng maple để vẽ với khoảng t=0 3*Pi Hình 2.3 Đồ thị vận tốc điểm E theo t Hình 2.4 Đồ thị vận tốc góc khâu theo t Để tính toán ta sử dụng hai thủ tuc hai hàm viết maple sau: Hàm doitm có tác dụng đổi tham số ma trận có thành phần hàm theo thời gian BÀI TIỂN LUẬN ROBOTICS Hàm wmo có tác dụng tính vector vân tốc góc tuyệt đối chiếu hệ tọa độ i khâu ω% i 2.2 Giải toán động học ngược a, giải phương pháp giải tích từ hệ phương  xE = cos(q1 ) [ a3 cos( q2 + q3 ) + a2 cos( q2 ) ]   yE = sin( q1 ) [ a3 cos( q2 + q3 ) + a2 cos( q2 ) ]  z = d + a sin( q ) + a sin( q + q ) 2 3  E q1 = a tan 2( y E , xE ) Từ phương trình 1, hệ ta rút : (2.1) BÀI TIỂN LUẬN ROBOTICS xE2 + yE2 + ( zE − d1 ) = a22 + a32 + 2a2 a3 (C2 C23 + S2 S23 ) = a22 + a32 + 2a2 a3 C3  xE2 + yE2 + ( zE − d1 )2 − a22 − a32  cos(q3 ) = 2a2 a3   sin(q3 ) = ± − cos (q2 )  q3 = a tan 2(sin(q3 ), cos( q3 ))  (2.2) Ta viết lại hệ sau :  xE = (a2 + a3C3 )C1C2 − a3 S3C1 S   z E − d1 = a3 S3C2 + (a2 + a3 C3 ) S ∆= (a2 + a3C3 )C1 ∆1 = ∆2 = a3 S3 xE zE − d − a3 S3C1 (a2 + a3 C3 ) −a3 S3C1 a2 + a3C3 (a2 + a3C3 )C1 x p a3 S3 zE − d1 = (a22 + a32 + 2a2 a3 C3 )C1 = [ xE2 + yE2 + ( zE − d1 )2 ) ] C1 = a2 xE + a3 xE C3 + a3 ( z E − d ) S3C1 = a2 ( zE − d1 ) + a3 [ ( z E − d )C3C1 − xE S3 ] a2 xE + a3 xE C3 + a3 ( z E − d ) S3 C1  cos( q ) =  [ xE2 + yE2 + ( zE − d1 )2 ) ] C1   sin(q ) = a2 ( z E − d1 ) + a3 [ ( z E − d )C3C1 − xE S3 ]  [ xE2 + yE2 + ( zE − d1 )2 ) ] C1  BÀI TIỂN LUẬN ROBOTICS Tính atan2 triệt tiêu C1 mẫu hệ =>  a x + a x C + a ( z − d )S C a ( z − d1 ) + a3 [ ( z E − d )C3C1 − xE S3 ]  q1 = a tan  E E 3 E , E ÷ xE + yE + ( zE − d1 ) ) xE2 + yE2 + ( z E − d1 )2 )   (2.3) Từ (2.1), (2.2), (2.3) ta xác định dạng giải thức biến khớp liên hệ với điểm tác động cuối, xác định biến khớp ngược lại ta tìm góc cardan b, 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  f1 = xE − cos( q1 ) [ a3 cos( q2 + q3 ) + a2 cos(q2 ) ] =   f = y E − sin( q1 ) [ a3 cos( q2 + q3 ) + a2 cos(q2 ) ] =   f = z E − [ d1 + a2 sin( q2 ) + a3 sin( q2 + q3 ) ] =  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 Dưới doạn chương trình viết maple sử dụng phương pháp Newton-Raphson lưu kết tính toán file txt đoạn vẽ đồ thị từ file txt viết matlab Với quỹ đạo điểm tác động cuối đường thẳng có phương trình sau BÀI TIỂN LUẬN ROBOTICS  xE = − t + 0.6   yE = 0.4t + 0.3  z = 0.5t + 0.4  E Đoạn chương trình viết maple > > > > > > > > 10 BÀI TIỂN LUẬN ROBOTICS 19 BÀI TIỂN LUẬN ROBOTICS IV Tính toán động lực học 4.1 Xây dựng cấu trúc động lực học thành phần cần thiết để viết phương trình động lực học 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,  cos( q1 )  sin(q ) C1 = A1 1C1 =     sin( q1 )      − cos( q1 )   − (d1 − lc1 ) = d1       0  0  0  l   c1  => r =  l  c1  c1  0      1   C1C2 − C1 S2 S1 a2 C1C2   − (a2 − lc )  S C − S S −C a S C    2 2 0    C2 = A2 C2 = =  S2 C2 d1 + a2 S2       0     lc C1C2  l S C   c 2  => r c2  d1 + lc S2       C1C23 − C1 S23 S1 C1 (a3C23 + a2 C2 )   − (a3 − lc3 )   S C − S S − C S (a C + a C )    23 1 23 2  0 C3 = A3 C3 =  23 =  S23 C23 d1 + a2 S2 + a3 S23       0    0  T & ω%1 = R1 R1 =  q&     q&1 S  ω% = R2T R& =  q&1C2   q&   C1 (lc3C23 + a2 C2 )  S (l C + a C )   c 23 2  => r = c3  d1 + a2 S2 + lc3 S23       ω% = R3T R& =   20  lc C1C2  =  lc S1C2   d1 + lc S2  q&1 S 23  q&1C23  q& + q&   C1 (lc3 C23 + a2 C2 )  S (l C + a C )   c 23 2   d1 + a2 S + lc3 S23  BÀI TIỂN LUẬN ROBOTICS Từ vị trí trọng tâm vận tốc góc ta tính ma trân Jacobi tịnh tiến quay sau 0 0  JT =  0   0  JT J R1 JT  −lc S1C2 −lc C1 S =  lc C1C2 −lc S1 S2  lc C2 0    − S1 (lc3C23 + a2 C2 ) − C1 (lc3 C23 + a2 C2 ) =  C1 (lc 3C23 + a2 C2 ) − S1 (lc3 C23 + a2 C2 )  a2 C2 + lc3C23 0 0 = 1 0  0     J R2  S2 0  =  C2 0    J R1 − lc3 C1 S23  − lc S1 S23  lc 3C23   S 23 = C23  0 0 0  1  Ma trận ten xơ 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: I =  I1x 0    0 I1 y  0 I   1z  , I = I2x 0   0 I y  0 I   2z  , I =  I3x 0    0 I3 y  0 I  3z    Động năng, ma trân khối lượng M(q): T  T T = q& ∑ ( J TiT mi J Ti + J Ri I i J Ri )  q& = q& T M ( q ) q&  i =1  m11 m12 m13  T M (q ) = ∑( J TiT mi J Ti + J Ri I i J Ri ) = m21 m22 m23  i =1 m31 m32 m33  21 BÀI TIỂN LUẬN ROBOTICS Với thành phần sau : m11 = I1 y + I y cos(q2 + q3 ) + I x sin(q2 + q3 ) + I x sin( q2 ) + [m3 lc23 cos(2q3 ) + 2m3 a2 lc3 cos(q3 ) + I y + m2 lc22 + m3 a22 ]cos(q2 )2 + m3lc23 sin(q3 )2 − [m3lc a2 sin(q3 ) + m3lc23 sin(2q3 )]sin(2q2 ) m12 = m21 = m13 = m31 = m22 = I z + I z + m2 lc22 + m3 [a22 + lc23 + 2a2 lc cos(q3 )] m23 = m32 = I z + m3 lc23 + m3 a2 lc cos(q3 ) m33 = I z + m3lc23 Π = −∑ mi g T 0 r Ci i =1 Biểu thức hệ : với go = [ ,0 ,-g ]T Π = m1lc1 g + m2 g[d1 + lc sin(q2 )] + m3 g[d1 + a2 sin(q2 ) + lc sin(q2 + q3 )]  Công ảo lực suy rộng không giả điểm tác động  FE = [-Fx , −Fy , −Fz ]T cuối robot chịu lực : T δ A = τ 1δ q1 + τ 2δ q2 + τ 3δ q3 + F δ ( rE ) = U1δ q1 + U 2δ q2 + U 3δ q3 + F T ( J E δ q )  Vector lực suy rộng viết dạng cột lấy từ tổng công ảo có dang sau: Q np = [Q1 , Q2 , Q3 ]T =U + J ET F (4.1) T E Trong U =[U , U , U ] vector momen dẫn động, J tính từ phần động  − S1 (a3C23 + a2 C2 ) − C1 (a3 S 23 + a2 S )  C (a C + a C ) − S (a S + a S ) 23 2  23 2  a3C23 + a2 C2 E học J = 4.2, thiết lập phương trình Lagrang robot 22 − a3C1 S 23  − a3 S1 S23  a3C23  BÀI TIỂN LUẬN ROBOTICS Chúng ta biết phương trình lagrang loại viết tọa độ suy rộng sau: d  ∂T  ∂T ∂Π =− + Q np  ÷− dt  ∂q& i  ∂qi ∂qi Để tránh dài dòng ta không chứng dạng ma trận phương trình Lagrang áp dụng ( tham khảo cách chứng minh Robot công nghiệp GS TSKH Nguyễn Văn Khang ) && + C ( q, q& )q& + G (q ) = Q np M (q)q Từ (4.1) viết lại dạng sau : && + C (q, q& ) q& + G (q ) = U + J ET F M (q)q Ta tính ma trân Clioris từ ma trận M(q) công thức tính sau & 3x3 C (q, q& ) = [cij (q,q)]  ∂ mij (q) ∂ m jk (q)  cij = ∑  − ÷ ∂ qi  k =  ∂ qk Vector G(q) momen trọng lực tính sau : G (q) = [g1 (q), g (q), g3 (q)]T gi (q ) = ∂Π ∂ qi Như phương trình vi phân chuyển động robot cấu hình RRR không gian có dạng ma trân sau: ta xác định && + C (q, q& ) q& + G (q ) = U + J ET F M (q)q • Ma trận khối lương M(q) ma trận quán tính clitoris C 23 BÀI TIỂN LUẬN ROBOTICS m11 m12 m13  T ⊕ M ( q) = ∑( J TiT mi J Ti + J Ri I i J Ri ) = m21 m22 m23  i =1 m31 m32 m33  Với thành phần sau : m11 = I1 y + I y cos(q2 + q3 ) + I x sin( q2 + q3 ) + I x sin( q2 ) + [m 3lc23 cos(2q3 ) + 2m3 a2 lc3 cos( q3 ) + I y + m2lc22 + m3 a22 ]cos(q2 )2 + m3lc23 sin(q3 ) − [m3lc3 a2 sin(q3 ) + m3lc23 sin(2q3 )]sin(2q2 ) m12 = m21 = m13 = m31 = m22 = I z + I z + m2 lc22 + m3 [a22 + lc23 + 2a2 lc3 cos(q3 )] m23 = m32 = I z + m3 lc23 + m3 a2 lc3 cos(q3 ) m33 = I3 z + m3 lc23 c11 ⊕ C ( q, q& ) =  c21  c31 c12 c22 c32 c13  c23   c33   24 BÀI TIỂN LUẬN ROBOTICS Với thành phần sau : c11 =  ( I x − I y − m3lc23 )sin(2q2 + 2q3 ) + ( I x − I y − m3 a22 − m2 lc22 )sin(2q2 ) − 2m3lc a2 sin( q3 + 2q2 )  q& +  ( I x − I y − m3lc23 )sin(2q2 + 2q3 ) − m3lc3 a2 sin( q3 + 2q2 ) − m3lc3 a2 sin q3  q& c12 = c13 = c21 =  ( I y − I x + m2 lc22 + m3 a22 )sin(2q2 ) + ( I y − I x + m3lc23 )sin(2q2 + 2q3 ) + 2m3lc a2 sin(q3 + 2q2 )  q&1 c22 = − 2m3 lc3 a2 sin q3 q& c23 = − m3lc a2 sin q3 q& c31 =  ( I y − I x − m3lc23 )sin(2q2 + 2q3 ) + m3lc a2 sin( q3 + 2q2 ) + m3lc a2 sin q3  q&1   c32 = m3lc a2 sin q3  q& − q& ÷   c33 = m3 lc3 a2 q& 2 • E Vector G(q) ma trận J (q) ⊕ G ( q) = [g1 , g , g ]T gi = g = m2 glc cos q2 + m3 g [ lc cos(q2 + q3 ) + a2 cosq2 ] g = m3 glc cos(q2 + q3 ) ⊕  − S1 ( a3C23 + a2 C2 ) −C1 (a3 S 23 + a2 S2 ) J E = C1 (a3C23 + a2 C2 ) − S1 (a3 S23 + a2 S2 )  a3 C23 + a2 C2 −a3C1 S 23  −a3 S1 S23  a3C23  Với C1=cos(q1), S1=sin(q1), C2=cos(q2), S2=sin(q2), S23=sin(q2+q3), C23=cos(q2+q3), q=[q1, q2, q3]T FE = [-Fx , −Fy , −Fz ]T Điểm tác động cuối robot chịu lực : 25 BÀI TIỂN LUẬN ROBOTICS Để tính toán ta sử dụng thủ tục hàm viết maple để tính ma trận C ( q, q& ) Jacobi quay , thành phần ma trận tính thành phần ma trận jacobi tịnh tiến, tính ma trân M(q) sử dụng câu lệnh có thư viện maple : V Luật điều khiển 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 bám theo vị trí đặt Hệ thống điều khiển không gian khớp Tín hiệu đặt quỹ đạo bậc 1.1 Hệ thống điều khiển phản hồi Luật điều khiển 26 BÀI TIỂN LUẬN ROBOTICS Hình 5.1.Sơ đồ cấu trúc hệ thống điều khiển robot với điều khiển PD Ta có phương trình động lực học : M (q)q&& + C (q, q& )q& + G (q ) = U + J ET F V (q, q& ) = C (q, q& )q& M = U , gọi H(q)=M(q)tránh nhầm với M vector momen , 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 =0 phương trình động lực học rút gọn sau: M = H (q ).q&& + V (q, q& ) + G (q ) M dk = K p (qd − q) + K d ( q& d − q& ) = K p ε + K d ε& Luật điều khiển : (5.1) K p = diag ( K p1 , K p , , K pn ) Trong : - 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: 27 BÀI TIỂN LUẬN ROBOTICS V L = T (ε Kp.ε +q& T.H q& ) (5.2) 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 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: V& L = T &&) &&T.H q& +q& T.H& q& +q& T.H q (ε& Kp.ε +ε T Kp.ε& +q Do tính chất đối xứng thành phần V& L = ε& T Kp.ε + T ε T Kp.ε q& H q& , , ta rút gọn T & && q& H q& +q& T.H q Từ phương trình động lực học với giả thiết thành phần momen trọng lực G(q), nhận phương trình sau : V& L =ε & T Kp.ε + T & q& H q& +q& T [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 (5.1) ta có V& L =q & T Kd.ε& −q& T.C (q, q& ).q& + q& T.H& (q).q& Trong đó: V (q, q& ) = C (q, q& ).q& Do ma trận  & H −C = −q & T Kd.ε& +q& T.( H& −C)q& ma trận đối xứng ngược => T V& L =−q& Kd q& ≤0 q& T.( H& − C )q& = (5.3) Từ (5.2) ,(5.3) cho thấy rằng, mức độ dương VL phụ thuộc vào Kp; mức độ V& L âm 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 28 BÀI TIỂN LUẬN ROBOTICS 1.2 Hệ thống điều khiển momen tính toán Luật điều khiển Hình 5.6 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 M = H (q ).q&& + V (q, q& ) + G (q ) Dựa phương trình động lực học : (5.4)  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 ) (5.5) 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 q&& = U dk cấp hai sau: phương trình vi phân tuyến tính cấp độc lập 29 BÀI TIỂN LUẬN ROBOTICS khớp Do thiết kế điều khiển độc lập có cấu chúc PD U dk = q&&d + K p ε + K d ε& cho khớp sau : (5.6) Từ (5.4),(5.5) (5.6) ta rút phương trình vi phân sai số vị trí hệ ε&& + K d ε& + K p ε = thống kín có dạng sau : s2 I + Kd s + K p = Phương trình đặc tính dạng toán tử Laplace : Viết cho khớp riêng lẻ (đó khâu quán tính bậc hai): 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 toán theo yê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) σ ≤ 20% Để đạt độ điều chỉnh , 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 ξ: ω = ξ.4T n qd σ=100.e ξ.ωn 1−ξ − (5.8) 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 toá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 30 BÀI TIỂN LUẬN ROBOTICS 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 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 : 31 BÀI TIỂN LUẬN ROBOTICS Hình 5.11 Sơ đồ điều khiển TÀI LIỆU THAM KHẢO 32 BÀI TIỂN LUẬN ROBOTICS [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] Điều khiển Robot công nghiệp – TS Nguyễn Mạnh Tiến [4] Cơ sở Robot công nghiệp -GS.TSKH Nguyễn Văn Khang [5] Robot công nghiệp – GS TSKH Nguyễn Thiện Phúc [6] Phần help maple In màu 4,5,10 33 [...]... vector momen truyền động khớp robot được xác định như sau: M dk = J T  K p ( S d − S ) + K d ( S& d − S& )  Sơ đồ khối của hệ thống như sau : 31 BÀI TIỂN LUẬN ROBOTICS Hình 5.11 Sơ đồ điều khiển TÀI LIỆU THAM KHẢO 32 BÀI TIỂN LUẬN ROBOTICS [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] Điều khiển Robot công nghiệp – TS Nguyễn Mạnh Tiến [4]... [3] = a3C1 S23 Fx + a3 S1 S 23 Fy − a3 ( S12 S23 + C12 C23 ) Fz + (a3 − c3 )m3 gS 23 S12 + (a3 − c3 )m3 gC23C12 = U 2 Như vậy ta có hệ phương trình 3 ẩn Fx, Fy, Fz giải ta được : 18 BÀI TIỂN LUẬN ROBOTICS 19 BÀI TIỂN LUẬN ROBOTICS IV Tính toán động lực học 4.1 Xây dựng cấu trúc động lực học và các thành phần cần thiết để viết phương trình động lực học Gọi lc1, lc2, lc3 lần lượt là khoảng cách từ gốc... plot(d1,d4,'c','LineWidth',2);xlabel('t');ylabel('q3(rad)'); 11 BÀI TIỂN LUẬN ROBOTICS 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 % Ta thu được các đồ thị sau: Hình 2.5 Đồ thị q1, q2 theo t 12 BÀI TIỂN LUẬN ROBOTICS Hình 2.6 Đồ thị q3, và điểm tác động... Fz ]   S1 S23 a3 Fx − C1C23 a3 Fy   Bài toán đặt ngược lại đó là khi biết các lực dẫn động 2 U = [U1 ,U 2 ,U 3 ]T M 21 [3] 3 M 32 [3] 1 tương ứng bằng , ta tìm được Momen dẫn động các khớp  0 M 10 [2] , FE 3 = [ -FX , − FY , − Fz ]T Với khớp 1: Từ hình ta thấy momen dẫn động cho khâu 1 tại khớp 1 dọc theo trục Y nên ta có U1 = 1M 10 [2] 17 1 BÀI TIỂN LUẬN ROBOTICS M 10 = 0 R1T 0 M 10 1 => 1M 10... khối tâm của từng khâu 1, 2, 3  Xét khâu 3: 13 BÀI TIỂN LUẬN ROBOTICS   −c3 C1C23 0 0  1  rc1 = R 3 rc1 =  − c3 S1C23   −c3 S23  3 rc 3 = [ -c3 , 0, 0]T => 3  T  a3C1C23    r3 = [ a 3 , 0, 0] 0  0r = 3 r3 =  a3 S1C23  3 R 3   a3 S 23       , và 0 P 3 = [ 0, 0, − m3 g]T Thay vào công thức đã được đã được thiết lập trong bài giảng robot 0 0 0   F32 = FE 3 − P3 0... bằng tăng giá trị Kd Nâng cao độ chính xác tinh của hệ thống điều khiển đạt được bằng tăng hệ số Kp của khâu khuếch đại Tuy nhiên ,Kp và Kd quá lớn sẽ làm giảm độ ổn định và chất lượng quá trình quá độ như độ quá điều chỉnh , thời gian quá độ tăng 28 BÀI TIỂN LUẬN ROBOTICS 1.2 Hệ thống điều khiển momen tính toán Luật điều khiển Hình 5.6 Sơ đồ điều khiển Phươngpháp cơ bản của luật điều khiển là lựa chọn... để tím lực và momen viết bằng maple như sau : 3.2 Cho lực ( mô men ) dẫn động tính lực ( mô men ) khâu thao tác tác dụng lên đối tượng 16 BÀI TIỂN LUẬN ROBOTICS Nhận xét : robot ở đây là 3 bậc RRR không gian tương ứng 3 momen dẫn U = [U1 ,U 2 ,U 3 ]T động như vậy bài toán tính lực ( mô men ) khâu thao tác tác dụng lên đối tượng khi giải ra chỉ cho ta ba thành phần hoặc 3 lực hoặc 3 mô men hoặc 2 lực...  T 0   r1 = [ 0, −c1 , 0]  0 0 1  r = r =  0  1 R 1 1   −c1   Thay vào công thức: 0 0 0   F10 = F21 − P1 0 0 0 0 0 0   M 10 = M 21 + r%1 F10 − r%c1 P1 Ta tính được : 15 BÀI TIỂN LUẬN ROBOTICS    − Fx 0   − Fy  F10 =     (m + m + m ) g − F  z  3 2 1    − M x + S23 [(a 3 − c3 )S1m3 g + a3 Fy − a3 S1 Fz ]+a 2 S1C2 m3 g + (a2 − c2 )S1C2 m2 g + a2 S 2 Fy − a2 S1C2... 0 R& 3 =   20  lc 2 C1C2  =  lc 2 S1C2   d1 + lc 2 S2  q&1 S 23  q&1C23  q& 2 + q& 3   C1 (lc3 C23 + a2 C2 )  S (l C + a C )   1 c 3 23 2 2   d1 + a2 S 2 + lc3 S23  BÀI TIỂN LUẬN ROBOTICS Từ các vị trí trọng tâm và vận tốc góc ta tính được các ma trân Jacobi tịnh tiến và quay sau 0 0 0  JT 1 =  0 0 0   0 0 0  JT 2 1 J R1 JT 2  −lc 2 S1C2 −lc 2 C1 S 2 =  lc 2... 1  T T = q& ∑ ( J TiT mi J Ti + J Ri I i J Ri )  q& = q& T M ( q ) q& 2  i =1 2  m11 m12 m13  T M (q ) = ∑( J TiT mi J Ti + J Ri I i J Ri ) = m21 m22 m23  i =1 m31 m32 m33  3 21 BÀI TIỂN LUẬN ROBOTICS Với các thành phần như sau : m11 = I1 y + I 3 y cos(q2 + q3 ) 2 + I 3 x sin(q2 + q3 ) 2 + I 2 x sin( q2 ) 2 + [m3 lc23 cos(2q3 ) + 2m3 a2 lc3 cos(q3 ) 1 + I 2 y + m2 lc22 + m3 a22 ]cos(q2