ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ RRR

Đồ án môn học thiết kế tính toán và mô phỏng robot 3 bậc toàn khớp quay.

ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ PHẦN 1: THIẾT KẾ MÔ HÌNH 3D ROBOT RRR Sử dụng phần mền solidwork để thiết kế mô hình 3D robot RRR 1.thiết kế khâu: Khâu đế: Khâu 1: Khâu 2: Khâu 3: 2.Các chi tiết khác Bánh trục bánh răn Nắp truyền bánh Xylanh khí nén 3.các truyền động: Khớp 1: Bộ truyền bánh Khớp 2: Bộ truyền bánh Khớp 3: Bộ truyền bánh Tay kẹp: Sử dụng xylanh khí nén PHẦN 2: TÍNH TOÁN ĐỘNH HỌC THUẬN VÀ NGƯỢC ROBOT Cấu hình robot RRR 1.bảng tham số DH a.Xây dựng hệ tọa độ DH: • Hệ tọa độ : Gốc tọa độ đặt tâm khớp động thứ Trục dọc theo hướng trục khớp động thứ nhất,hướng từ lên Trục có phương vuông góc với hướng từ vao hình vẽ Trục xác định theo quy tắc tam diện thuận • Hệ tọa độ : Gốc tọa độ đặt tâm khớp động thứ Trục phương dọc theo trục khớp động thứ hướng từ hình vẽ Trục có phương nằm đường vuông góc chung trục hướng từ tới Trục xác định theo quy tắc tam diện thuận • Hệ tọa độ : Gốc tọa độ đặt tâm khớp động thứ Trục phương dọc theo trục khớp động thứ hướng từ hình vẽ Trục có phương nằm đường vuông góc chung trục hướng từ tới Trục xác định theo quy tắc tam diện thuận • Hệ tọa độ : Gốc tọa độ đặt tâm tay kẹp Trục phương dọc theo trục khớp động thứ hướng từ hình vẽ Trục có phương nằm đường vuông góc chung trục hướng từ tới Trục xác định theo quy tắc tam diện thuận b.cách xác định tham số động học DH vị trí hệ tọa độ khớp hệ tọa độ khớp xác định tham số , , , sau: : góc quay quanh trục để trục trùng với trục () : dịch chuyển tịnh tiế dọc trục để gốc tọa độ chuyể đến giao điểm trục trục : dịch chuyển dọc trục để điểm chuyển đến điểm : góc quay quanh trục cho trục () trùng với trục c.bảng tham số động học DH • 0 90ᶱ 0 Các ma trận truyền: Ma trận truyền khâu i so với khâu i-1:  cos q1  sin q1 A1 =    0 sin q1 − cos q1 0  cos q3  sin q3 A3 =    − sin q3 cos q3 0 0 ÷ 0÷ l1 ÷ ÷ ÷  cos q2  sin q2 A2 =    − sin q2 cos q2 0 l2 cos q2  ÷ l2 sin q2 ÷ ÷ ÷ ÷ l3 cos q3  ÷ l3 sin q3 ÷ ÷ ÷ ÷ Ma trận truyền khâu so với khâu 0:  cos q1 cos q2  sin q1 cos q2 0 A2 = A1 A2 =  sin q2   − cos q1 sin q2 sin q1 l2 cos q1 cos q2  ÷ − sin q1 sin q2 − cos q1 l2 sin q1 cos q2 ÷ cos q2 l2 sin q2 + l1 ÷ ÷ ÷ 0   cos q1 cos(q2 + q3 ) − cos q1 sin(q2 + q3 ) sin q1 l3 cos q1 cos(q2 + q3 ) + l2 cos q1 cos q2   ÷ sin q1 cos( q2 + q3 ) − sin q1 sin(q2 + q3 ) − cos q1 l3 sin q1 cos(q2 + q3 ) + l2 sin q1 cos q2 ÷ 0  A3 = A2 A3 =  sin(q2 + q3 ) ÷ cos(q2 + q3 ) l3 sin(q2 + q3 ) + l2 sin q2 + l1  ÷ 0   2.bài toán động học thuận Các thong số đầu vào :q1, q2, q3 Thông số cần xác định: điểm tác động cuối P=( Px, Py,Pz),hướng khâu cuối • Tọa độ điểm tác động cuối: Px = A3 1,4 = l3 cos q1 cos(q2 + q3 ) + l2 cos q1 cos q2 Py = A3  2,4 = l3 sin q1 cos(q2 + q3 ) + l2 sin q1 cos q2 Pz = A3 3,4 = l3 sin(q2 + q3 ) + l2 sin q2 + l1 • Hướng khâu thao tác có ma trận cosin hướng:  cos q cos(q + q )  R3 =  sin q1 cos(q2 + q3 )  sin(q2 + q3 )  − cos q1 sin(q2 + q3 ) sin q1  ÷ − sin q1 sin(q2 + q3 ) − cos q1 ÷ cos(q2 + q3 ) ÷ 3.bài toán động học ngược Thông số đầu vào tọa độ điểm tác động cuối: P=( Px; Py; Pz) hướng khâu cuối Thông số cần xác định:các biến khớp (q1; q2; q3) Ta có ma trận cấu hình khâu thao tác:  a11 ( x) a12 ( x) a13 ( x) a14 ( x)   ÷ a21 ( x) a22 ( x) a23 ( x) a24 ( x) ÷  T3 ( x) =  a ( x) a32 ( x) a33 ( x) a34 ( x) ÷  31 ÷ ÷ 0   Trong , , tọa độ điểm thao tác E: = xE(0) ,= y E(0) ,= z E(0) Còn phần tử ma trậ cosin hướng khâu thao tác Từ toán động học thuận ta có ma trận:  cos q1  sin q1 A1 (q) =    0 sin q1 − cos q1 0  cos q3  sin q3 A3 =    0 ÷ 0÷  cos q2  sin q2 A2 (q) =    l1 ÷ ÷ ÷ − sin q3 cos q3 0 l3 cos q3  ÷ l3 sin q3 ÷ ÷ ÷ ÷ − sin q2 cos q2 0 l2 cos q2  ÷ l2 sin q2 ÷ ÷ ÷ ÷ (2)  cos q1 cos(q2 + q3 ) − cos q1 sin(q2 + q3 ) sin q1 l3 cos q1 cos(q2 + q3 ) + l2 cos q1 cos q2   ÷ sin q1 cos(q2 + q3 ) − sin q1 sin(q2 + q3 ) − cos q1 l3 sin q1 cos(q2 + q3 ) + l2 sin q1 cos q2 ÷ 0  T3 ( x) = A3 = A1 A2 A3 = (3)  sin(q + q ) ÷ cos( q + q ) l sin( q + q ) + l sin q + l 3 3 2  ÷÷ 0   cho phần tử (1,4) (2,4) hai ma trận (1) (3) ta đươc: ( ( ) ) cos q l cos(q + q ) + l cos q = a = P P P 14 x 3 2  ⇒ tan q1 = y ⇒ q1 = arctan  y  Px sin q1 l3 cos(q2 + q3 ) + l2 cos q2 = a4 = Py  Px  ÷ ÷  Từ phương trình ma trận (3) ta suy ; H1 ( q1 ) H (q2 ) = D3 ( x ) ( H (q3 ) )  C3 S3  −S C   0  −1 H (q3 )  = −1 : −l3  0 ÷ ÷ ÷ ÷   C3a11 − S3a12 C a − S a −1 22 D3 ( x) ( H (q3 ) ) =  21  C3a31 − S3a32   (8) S3a11 + C3a12 S3a21 + C3a22 S3a31 + C3a32 a13 a23 a33 −l3a11 + a14  −l3a21 + a24 ÷ ÷ −l3a31 + a34 ÷ ÷  (9) J R1 0 0 ∂ω1  = = 0 0÷ ÷ ∂θ&  ÷ 1 0 J R2  sin θ  ∂ω2  ÷ = =  − cos θ ÷ & ∂θ  0÷ 1   sin θ sin θ  ∂ω3  ÷ J R3 = =  − cos θ − cos θ ÷ & ∂θ  0 ÷ 1  3.ma trận khối lượng suy rộng robot:  m11 m12   m21 m22 T T T M = ∑ (mi J Ti J Ti + J Ri Ai I i Ai J Ri )   m31 m32 i =1 = m13  ÷ m23 ÷ m33 ÷  Trong đó: 2 m11 = I1z + I y C22 + I x S 22 + I x S 23 + I y C23 + m2lc22C22 + m3 (2l2lc 3C2C23 + l22C22 + lc23C23 ) m12 = m13 = m21 = m31 = m22 = I z + I z + m2lc22 + m3 (lc23 + l22 + 2l2lc 3C3 ) m23 = m32 = I z + m3 (lc23 + l2lc 3C3 ) m33 = I z + m3lc23 4.Ma trận ly tâm coriolis : T C (q, q&) =  ∂M (q)  ∂M ( q) ( I n * q&) −  (q&* I n ) ÷ ∂q  ∂q  =  c11 c12 c  21 c22 c  31 c32 Sử dụng phần mềm Maple nhân ta : c13  c23 ÷ ÷ c33 ÷  c11 = −m2lc22 S2C2 q&2 − 2m3 (lc23 S 23C23 + l22 S 2C2 + l2lc3 (2S 2C23 − S3 ))q&2 −2m3 (lc23 S23C23 + l2lc3C2 S 23 )q&3 + 2( I x − I y )S 2C2 q&2 + 2( I x − I y ) S 23C23 (q&2 + q&3 ); c12 = c13 = & c21 = (m2lc22 S 2C2 + m3 (lc23 S 23C23 + l22 S 2C2 + l2lc (2C2 S 23 − S3 )))q& − (2( I x − I y ) S 2C2 + ( I x − I y )C 23 S 23 )q1 c22 = −2m3lc 3l2 S3q&3 c23 = −m3lc 3l2 S3q&3 c31 = (m3lc S 23 (l2C2 + lc 3C23 ) − ( I x − I y ) S 23C23 )q&1 m3l2lc S3 (2q&2 − q&3 ) c33 = m3l2lc S3q&2 c32 = 5.Thế robot π = m1 gd1 + m2 g (d1 + lc S ) + m3 g (d1 + l2 S + lc S 23 ) Từ suy :   ∂π  g= =  m2 glc 2C2 + m3 gl2C2 + m3 glc 3C23  ∂q   m3 glc 3C23 6.phuơng trình vi phân chuyển động khâu Thế biểu thức vào phương trình Lagrange loại hai : &+ C (q , q&)q&+ g (q ) = τ M (q )q& Ta nhận hệ phương trình vi phân chuyển động robot ba khâu không gian : Khâu :  I + I C + I S + I S + I C + m l C + m (2l l C C + l 2C + l C )  q & 2y 2x x 23 y 23 c2 c3 23 2 c3 23  &  1z  −m l S C q& − 2m (l S C + l S C + l l (2S C − S ))q&  c2 2 c3 23 23 2 2 c3 23  q& + = τ1  −2m3 (lc23S23C23 + l2lc3C2 S23 )q&  & & & + 2( I − I ) S C q + 2( I − I ) S C ( q + q ) 2x 2y 2 3x 3y 23 23   Khâu :  I z + I z + m2lc22 + m3 (lc23 + l22 + 2l2lc3C3 )  q&  & & && +  m32 = I z + m3 (lc + l2lc3C3 )  q − 2m3lc3l2 S3 q q3   & + { (m2lc22 S2C2 + m3 (lc23 S23C23 + l22 S2C2 + l2lc3 (2C2 S23 − S3 )))q&1 − (2( I x − I y )S2C2 + ( I3 x − I y )C23 S23 )q&1} q&1 −m3lc3l2 S3 q&32 + m2 glc 2C2 + m3 gl2C2 + m3 glc 3C23 = τ Khâu : (I 3z ) ( ) & & & + m3 (lc23 + l2lc3C3 ) q& + I z + m3lc q  &2 + m l l S q&2 + m gl C = τ + (m3lc3S23 (l2C2 + lc3C23 ) − ( I3 x − I3 y )S23C23 )q& 1 q c3 3 c3 23 PHẦN 4: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CỦA ROBOT 1.thiết kế quỹ đạo không gian khớp : Là xác định đường biểu diễn vị trí góc khớp(góc quay khớp với khớp quay độ di chuyển khớp khớp tịnh tiến) theo thời gian di chuyển từ vị trí ban đầu (A) tới vị trí cuối (B) thời gian với q biến khớp tổng quát Quỹ đạo di chuyển khớp hai vị trí thỏa mãn điều kiện : vị trí ban đầu vị trí cuối cùng, tốc độ vị trí ban đầu tốc độ vị trí cuối Do đa thức bậc phù hợp với quỹ đạo khớp robot: (t)= q&i (t ) = a1i + 2a2it + 3a3it giả thiết vận tốc điểm đầu điểm cuối 0: ; Ta có hệ phương trình: ⇔ a = q i ( A)  0i a =  1i  3(qi( B) − qi( A) ) a =  2i t2  −2(qi( B) − qi( A) )  a =  3i t3  q (t ) = q i ( A)  i  qi (tc ) = qi ( B )  &i( A) q& i (t0 ) = q  q& (tc ) = q&i( B)  i = a0i = = a0i + a1itc + a2itc + a3itc3 = a1i = = a1itc + 2a2itc + 3a3itc = Giải hệ ta hệ số phương trình quỹ đạo Bài toán cụ thể: cho điểm A(90; 0; 55) điểm B(0; 43.30; 120) Thời gian chuyển động Từ toán ngược ta xác định thong số biến khớp: q o  1( A) =  o q2( A) =  q3( A) = 0o  • =0 q o  1( B ) = 90  o q2( B) = 30  q3( B ) = 60o  hệ số phương trình quỹ đạo khớp 1: =0 =10.8 =-1.44 Phương trình động học khâu 1:  q (t ) = 10.8t −1.44t  2  q& (t ) = 21.6t − 4.32t & (t ) = 21.6 − 8.64t  q& Đường biểu diễn vị trí, vận tốc gia tốc khớp thao tác theo thời gian • Hệ số phương trình quỹ đạo khớp 2: Phương trình động học khâu 2: q (t ) = 3.6t − 0.48t   q& (t ) = 7.2t −1.44t  & &(t ) = 7.2 − 2.88t q  Đường biểu diễn vị trí, vận tốc gia tốc khớp thao tác theo thời gian • Hệ số phương trình quỹ đạo khớp cuối: 7.2 Phương trình động học khâu : q (t ) = 7.2t − 0.96t   &3 (t ) = 14.4t − 2.88t q  & &(t ) = 14.4 − 5.76t q  Đường biểu diễn vị trí, vận tốc gia tốc khớp thao tác theo thời gian 2.thiết kế quỹ đạo không gian làm việc Tay robot di chuyển từ vi trí A() không gian làm việc tới vị trí B() theo đường thẳng ta có phương trình đường thẳng AB có dạng sau: x − xA y − yA z − zA = = xB − x A y B − y A z B − z A Cho robot chuyển động thời gian 5(s) từ vị trí A(90; 0; 55) tớ vị trí B(0; 43.3; 120) Tại vị trí A khớp =0, =0, =0 Phương trình đường thẳng AB: x − 90 y−0 z − 55 t = = = − 90 43.3 − 120 − 55 ⇔  x = 90 − 18t   y = 8.66t  z = 120 + 13t  PHẦN 5: ĐIỀU KHIỂN CHUYỂN ĐỘNG ROBOT 1.Hệ thống điều khiển phản hồi không gian khớp Chức hệ thống điều khiển tạo momen cần thiết để truyền động khớ p robot đảm bảo khớp robot bám theo vị trí đặt Sử dụng cấu trúc điều khiển có dạng tỉ lệ -tích phân- đạo hàm (PID): Trong đó: ε = qd − q :là sai số vị trí khớp robot ε&= q&d − q& : sai số tốc độ khớp robot KP =diag(,,…,) :ma trận đường chéo hệ số khuếch đại khớp riêng biệt =diag(, ,….,):ma trận đường chéo hệ số đạo hàm khớp riêng biệt =diag(,….): ma trận đường chéo hệ số tích phân khớp riêng biệt Hệ thống điều khiển với cấu trúc điều khiển ổn định tuyệt đối toàn cục : Hệ tay máy với phương trình động lưc học có dạng: &+ V (q, q&) + G (q ) = τ M (q) q& &+ V (q, q&) + K P q&− K Dε&− K I ∫ ε dτ = K P qd M (q) q& Luật điều khiển có dạng: Với giả thiết bỏ qua thành phần momen trọng lực ta nhận hệ điều khiển vòng kín: &+ V (q, q&) + K P q&− K Dε&− K I ∫ ε dτ = K P qd M (q )q& Xét hàm lyapunov có dạng sau: 1 & (q )q&+ ε T K Pε V = qM 2 Ta có M ma trận dương Vậy V không âm Dạo hàm V với ta có : &− ε T K P q& V&= −q&T K D q&+ q&T M (q )q& T & q& M ( q) q&= qTV ( q, q&) = T & &− qTV (q , q&) q& M (q )q&− q&T K D q& Thành phần vecto viết dạng ma trận : V (q, q&) = Vm (q , q&& )q Trong Vm có quan hệ với M(q) sau : M (q ) = 2Vm (q, q&) − S q&T Sq& Với S ma trận đối xứng lệch nên =0 : T & q& M (q )q&= qTV (q, q&) V&= −q&T K D q& Nên : Bất đẳng thức cho thấy hệ thống ổn định tuyệt đối 2.Các thao tác thực mô Khối lượng khâu : Khâu đế =10 kg, khâu 1=8 kg, khâu =3 kg, khâu =1.5 kg Phương trình động lực học robot có dạng : &+ V (q, q&) + G (q ) = τ M (q )q& Luận điều khiển có dạng sau : M dk = K Pε + K Dε&+ K I ∫ ε dτ Với quỹ đạo khớp thiết lập phần thiết lập quỹ đạo đảm bảo điểm tác động cuối từ điểm A(90; 0; 55) tới điểm B(0; 43.30; 120) thời gian 5(s) Các hệ số khuếch đại tỉ lệ, tích phân đạo hàm lựa chọn sau : 0   5000  KP =  5000 ÷ ÷  ÷ 5000   , 0   3000  KI =  3000 ÷ ÷  ÷ 3000    70 0  K D =  70 ÷ ÷  0 70 ÷   Ta có mô hình simulink robot : , Mô hình điều khiển PID : Trong : , tín hiệu quỹ đạo đặt vận tốc đặt vào khớp robot đươc thiết lập phần thiết lập quỹ đạo q, v tín quỹ đọa vận tốc phản hồi lại từ robot(quỹ đạo thực) ta sử dụng khối: +khối Gain dùng để khếch đại tín hiệu đầu vào +khối mux chập tín hiệu đơn thành tín hiệu tổng hợp tách thành nhiều tín hiệu +khối input, ouput đầu vào tín hiệu +khối hàm biểu diễn hàm toán học có tín hiệu vào biến tín hiệu đầu giá trị hàm =(10.8*u(1)^2-1.44*u(1)^3)*pi/180 ; =(3.6*u(1)^2-0.48*u(1)^3)*pi/180 =(7.2*u(1)^2-0.96*u(1)^3)*pi/180 =(21.6*u(1)-4.32*u(1)^2)*pi/180; =(7.2*u(1)-1.44*u(1)^2)*pi/180 =(14.4*u(1)-2.88*u(1)^2)*pi/180 + khối scope hiển thị tín hiệu đầu cảu trình mô theo thời gian + khối thời gian đặt giá trị 5(s) 3.kết mô Sau mô ta kết đồ thị đường đặc tính biến: Mô men khớp theo thời gian Vị trí khớp theo thời gian Vận tốc khớp theo thời gian Sai số vị trí khớp theo thời gian