Điều khiển quỹ đạo robot bốn bậc tự do

Bài viết Điều khiển quỹ đạo robot bốn bậc tự do giới thiệu một bộ điều khiển theo phương pháp điều khiển trượt (slide mode control) để điều khiển các khớp theo quỹ đạo biến khớp vừa xác định. Các kết quả được kiểm tra mô phỏng bằng phần mềm Matlab.

62 Phạm Trường Tùng ĐIỀU KHIỂN QUỸ ĐẠO ROBOT BỐN BẬC TỰ DO CONTROLLING THE PATH OF DOF ROBOT Phạm Trường Tùng Trường Đại học Phạm Văn Đồng; pttung@pdu.edu.vn Tóm tắt - Robot bậc tự dạng RRR.R dạng robot có nhiều ưu điểm so với robot bậc tự khác nhờ khả định hướng vị trí cách linh hoạt Bài báo giới thiệu cách thức điều khiển điểm tác động cuối robot bám theo quỹ đạo không gian Descarte cho trước cách tính tốn ma trận biến đổi phương pháp biến đổi tọa độ Trên sở ma trận nhất, tính tốn động học ngược robot thiết kế quỹ đạo biến khớp robot để cấu chấp hành cuối bám theo quỹ đạo cho Với mơ hình động lực học robot, báo giới thiệu điều khiển theo phương pháp điều khiển trượt (slide mode control) để điều khiển khớp theo quỹ đạo biến khớp vừa xác định Các kết kiểm tra mô phần mềm Matlab Abstract - The 4DOF (degree of freedom) robot RRR.R has more advantages than other 4DOF robots through flexible orientation and position This paper presents a method to control the end effector of 4DOF robot to follow the path that is desired on Descarter-coordinates by calculating the homogeneous matrix On the basis of homogeneous matrix, the robot’s inverse kinematic dynamics is calculated and the robot‘s joint space trajectory is designed On the basis of dynamics, this paper proposes a controller that is designed by slide mode control theory to control joints according to the determination of joint trajectories The results are simulated by using Matlab Từ khóa - robot; 4DOF; quỹ đạo biến khớp; quỹ đạo cung tròn; điều khiển trượt Key words - robot; 4DOF; joint trajector; arc path; slide mode control Đặt vấn đề Robot 4DOF có kết cấu khớp quay RRR.R bị hạn chế khả định hướng không gian (so với robot có số bậc tự cao hơn), khó thực nguyên cơng địi hỏi khắt khe vị trí hướng cấu chấp hành cuối (như nguyên công sơn, hàn…) Trong đó, quỹ đạo cung trịn khơng gian dạng quỹ đạo thường sử dụng thực tế (như nguyên công hàn ống) Bài viết giải tốn định vị trí hướng robot bám theo cung trịn khơng gian nằm trường công tác robot thiết kế điều khiển để điều khiển robot theo quỹ đạo thiết kế phẳng O0X0Y0 β0, góc hợp tia ORB với giao tuyến mặt phẳng α mặt phẳng O0X0Y0 βf, góc hợp hướng cấu chấp hành cuối với giao tuyến mặt phẳng α mặt phẳng O0X0Y0 β(t) Điểm E thể vị trí cấu chấp hành cuối robot Cơ cấu chấp hành cuối di chuyển theo hướng cung trịn từ A đến B (Hình 2) Kết nghiên cứu 2.1 Bài toán đặt Cho robot 4DOF có bốn khớp quay, cho cung trịn định hướng nằm mặt phẳng vng góc với mặt phẳng O0X0Z0 gắn đế robot nằm trường công tác robot Điều khiển cấu chấp hành cuối robot bám theo cung tròn có phương vector hướng qua tâm cung trịn (t)  B Z0 x OR y E   z A Y0  O0   X0 Hình Xác định giá trị toán (t)  R B OR B OR f  Z0 E   A Y0   O0  X0 Hình Mơ hình tốn 2.2 Giải tốn Gọi góc hai mặt phẳng α mặt phẳng O0X0Z0 φ, góc hợp tia ORA với giao tuyến mặt phẳng α mặt A Hình Mơ hình điểm tác động cuối robot quỹ đạo Gắn vào điểm OR hệ trục tọa độ ORxyz, cho mặt phẳng ORxy trùng với mặt phẳng α, trục ORx trùng với đường ORE, chiều ORx chiều vector ORE Như ma trận biến đổi mô tả vị trí hướng hệ tọa độ ORxyz so với hệ tọa độ O0X0Y0Z0 là: H = Trans(P).Rot(z, φ).Rot(x,900).Rot(z,β(t)) ISSN 1859-1531 - TẠP CHÍ KHOA HỌC VÀ CƠNG NGHỆ ĐẠI HỌC ĐÀ NẴNG, SỐ 11(96).2015, QUYỂN     Trong P P  X OR i  YOR j  Z OR k Vậy ma trận biến đổi là:  0 X OR    YOR  Trans ( P )   0 Z OR     0 0  cos( )  sin( )  sin( ) cos( ) Rot ( z ,  )   0  0   0 0    0 1 0   Rot ( x, 90 )     0  0 1    cos(  (t ))  sin(  (t ))  sin(  (t )) cos(  (t )) Rot ( z ,  (t ))   0  0  Vậy ma trận biến đổi là:  C  ( t ) C  C S   (t )  H (t )    S (t )    C S  ( t ) S  S  ( t ) S C C (t ) 0 (1) 0 0 0  1 (2) (3) 0 0 0 0  1 XO   YO   ZO    (4) R R B  y O z   x n E OR a Để tìm vị trí góc quay khớp, ta giải phương trình (6) với biến khớp, ta có: T4= H(t)basePobj4TTool-1 (7) Như theo (7): C (t ) C S ( t ) C S X OR   R  a  C (t ) C   C S S  ( t ) S C YOR   R  a  C (t ) S  Ro T4    (t )  C (t ) ZOR   R  a  S  ( t )    S (t )   0 bot bậc tự kết cấu kiểu RRR.R có mơ hình động học sau [1]: (5) a2 a3 a4 Y1 Y2 Y3 Y4 O3 O1 X1 Z1 d1 O2 Z2 X2 X3 Z3 O4 X4 Z4 Z0 Y0 O0 X0 Hình Mơ hình động học robot Với mơ hình động học vậy, phương trình động học robot sau: C1C234 C1 S234 S1 a C1C234  a3C1C23  a2 C1C2     S1C234 S1 S234 C1 a S1C234  a3 S1C23  a2 S1C2   T4   C234 a S 234  a3 S23  a2 S2  d1   S234   0   Giả sử vị trí khâu chấp hành cuối nằm vị trí thỏa mãn điều kiện tồn nghiệm, theo [1] ta có nghiệm tốn học robot là: 1   arctan 2(ax , a y )  =arctan2(A,B)  2 2 (8)  =arctan2(   C , Px  Py  a3  a2 ) 3 2  a3  a2  = 234    3  Với: a y z o Trong đó: - Hệ tọa độ O4 mơ tả hướng vị trí điểm cuối khâu thứ    - Hệ tọa độ E với vector hướng n, o, a mơ tả hướng vị trí điểm tác động cuối gắn robot (ví dụ đầu mũi hàn…) Như ta có:  0 1 R  0 a    0 0  0 1 0  base   ; Pobj   TTool    0 0 0     0     0 0  R Trong đó: Cβ(t) = cos(β(t)); Sβ(t) = sin(β(t)); Cφ = cos(φ); Sφ = sin(φ); Vị trí đến tay máy mơ tả phương trình sau [2,5,6]: T44TTool = H(t)basePobj (6) Trong đó: - T4 ma trận biến đổi mô tả hướng vị trí hệ tọa độ gắn khâu chấp hành cuối so với hệ tọa độ gốc - 4TTool ma trận biến đổi mô tả hướng vị trí điểm tác động cuối công cụ hệ tọa độ gắn khâu chấp hành cuối - H(t) ma trận biến đổi nhất, hàm theo thời gian, mô tả hệ tọa độ làm việc OR đối tượng hệ tọa độ gốc robot - basePobj ma trận biến đổi mô tả hướng vị trí cấu chấp hành cuối so với hệ tọa độ làm việc OR Điểm tác động cuối robot mơ tả Hình x 63 A Hình Mơ tả điểm tác động cuối robot C1 = cos1; S1 = sin1; C2 = cos2; S2 = sin2; C12 = cos(1 + 2); S12 = sin(1 + 2) … 64 Phạm Trường Tùng a3C3  a2  (a3 S23  a2 S2 )  a3 S3 (a3C23  a2C2 ) ; 2 a3C3  a2   a3 S3  a C  a2  (a3C23  a2C2 )  a3 S3 (a3 S23  a2 S2 ) B 3 2 a3C3  a2   a3 S3  A nx  C (t ) C ; ny  C (t ) S  C (t ) S S ; nz  S (t ) ; ox  S (t ) C ; oy  S (t ) S ; oz  C (t ) ; ax  S ; a y  C (t ) ; az  0; px  X OR   R  a C (t ) C ; p y  YOR   R  a C (t ) S ; pz  Z OR   R  a S (t ) Px  C1 px  S1 p y  a C234 ; Py  pz  d1  a S234 Để chuyển động khớp liên tục mượt, ta chọn quy luật vận tốc góc điểm tác động cuối so với tâm quay OR hàm bậc (tức  (t ), hàm bậc hay  (t ) hàm bậc 3) [2,5] Do đó, ta có hàm  (t ) theo thời gian sau: 3 f  0   20   f t f  (t )  0  0 t  t tf (9) 0   f t f   f  0   t tf     Với 0  f vận tốc góc thời gian t=0 t= tf, tf khoảng thời gian để robot di chuyển từ vị trí định vị góc  đến góc  f 2.3 Thiết kế điều khiển robot Xét mơ hình động lực điều khiển robot sử dụng động điện chiều kích từ độc lập sau: La(t) Uf (t) Ra(t) Rf (t) Lf (t) eb(t) Ua(t) m(t) Mm(t) Jm Hình Mơ hình động chiều kích từ độc lập JL(t) L(t) ML(t) ua (t )    (t )  m  m  K b m (t ) (t ) Từ ta có phương trình trạng thái hệ thống sau:  dx1  dt  x2  dx2  (t )ua (t )   (t ) x2  (t ) Kb x1 (11)   dt   y  x1   m  Với: K a  Ra J (t ) La dJ (t )  Ka (t )   ; (t )  La J (t )  K a K a dt  La J (t ) Ta thấy mơ hình đối tượng điều khiển (11) có tham số biến đổi tùy theo vị trí khớp robot theo thời gian, ta sử dụng phương pháp điều khiển bền vững để điều khiển Cụ thể trường hợp này, ta lựa chọn phương pháp điều khiển trượt (Sliding Mode Control) để thiết kế điều khiển [3,4] Gọi:  yd(t) tín hiệu đặt (là hàm biến thiên theo thời gian)  e(t) = yd(t) – y(t) sai lệch tín hiệu đặt tín hiệu điều khiển Do mơ hình bậc 2, nên ta sử dụng hàm trượt có dạng: de (12) s  k1e  dt Với k1> để phương trình s(e) có đa thức đặc tính A(p) = k1 + p đa thức Hurwitz Từ hàm trượt s ta có: d ( yd  y ) de (13) s  k1e   k1 ( yd  y )  dt dt dyd   dy   (14)  s   k1 yd     k1 y   dt   dt   dyd   Vậy mặt trượt s   k1 yd  (15)    k1 x1  x2  dt   Với hàm trượt s, nhiệm vụ điều khiển phải tạo s0 để có e0 điều tương đương với: ds (16) sgn( s )  dt Chọn K > 0, từ điều kiện trượt ta được: ds sgn( s )   K dt Từ (15), (17) ta được: K   dyd   d  k1 yd     k1 x1  x2   dt    sgn( s )   K dt Mm(t) Jm m(t) Hình Truyền động cho khớp robot Ta có phương trình vi phân mơ tả mối quan hệ đầu vào hệ thống (điện áp) đầu hệ thống (vận tốc góc trục động cơ): (10)  dy d d y d dx1 dx2    k1   ( k1  )  sgn( s )   K dt dt dt   dt  dyd d yd  dy   k1    k1  (t )ua (t )  (t ) K b x1 dt  dt  dt   (t ) x2   sgn( s )   K (17) ISSN 1859-1531 - TẠP CHÍ KHOA HỌC VÀ CƠNG NGHỆ ĐẠI HỌC ĐÀ NẴNG, SỐ 11(96).2015, QUYỂN 65   k1e  yd (t)ua (t ) (t )Kb x1 (t )x2  sgn(s)  K(18)  k1e  yd (t)ua (t) (t)Kb x1 (t)x2  K sgn(s) (19) Từ (19), ta   (t )ua (t )  K sgn(s)  k1 e yd  (t ) x2  (t )Kb x1 (20) Ta được: ua (t )   K sgn(s)  k1e  yd (t)x2 (t)Kb x1  (21) (t ) 2.4 Kết Các kết mô phần mềm Matlab cho thấy điều khiển thiết kế hoạt động tốt, thời gian đáp ứng nhanh, sai lệch tĩnh nhỏ phạm vi cho phép Tuy nhiên, có tượng Chatterring sử dụng hàm sign(s) Hiện tượng Chatterring giảm hẳn ta sử dụng hàm bão hịa sat(s) thay cho sign(s) [4] Hình 11 Vị trí góc vận tốc góc trục khớp Mơ với cung trịn có tọa độ tâm OR (0.6m; 0m; 0.2m); bán kính R = 0.25m; cung tròn giới hạn   1350  f  1800 Các thông số robot d1 = 0.1m; a2 = 0.3m; a3 = 0.3m; a4 = 0.1m; a = 0.05m Thông số động cơ: Ka = 0.01; Kb = 0.01; Ra = ; La = 0.05H; Jm = 1N.m Vận tốc ban đầu 0 = 0.02 rad/s; vận tốc cuối  f = Các hệ số K = k1 = Kết sau: Kết mô ta thấy sai số cấu chấp hành cuối e = 6,6.10-5m Hình Vị trí góc vận tốc góc trục khớp Hình 12 Quỹ đạo điểm tác động cuối robot Hình Vị trí góc vận tốc góc trục khớp Hình 13 Sai số điểm tác động cuối robot Hình 10 Vị trí góc vận tốc góc trục khớp 3 Kết luận Kết nghiên cứu hoàn thành việc thiết kế quỹ đạo robot 4DOF định hướng vị trí theo cung trịn cho trước nằm trường cơng tác robot Trên 66 Phạm Trường Tùng sở quỹ đạo biến khớp tìm được, tác giả thiết kế điều khiển kiểu slide mode để điều khiển động khớp robot Kết tính tốn mơ phần mềm Matlab với cung tròn khác cho chất lượng điều khiển khả quan Tuy nhiên, điều khiển kiểu slide mode có nhược điểm dao động tín hiệu điều khiển lớn, dễ gây hư hỏng cho thiết bị điện khí Chính vậy, tác giả có đề xuất giải pháp điều khiển khác để khắc phục nhược điểm Nhìn chung với kết đạt được, việc thiết kế điều khiển quỹ đạo theo toán robot 4DOF kết cấu RRR.R mô tả hợp lý Giải thuật tốn áp dụng vào thực tế robot thực nguyên công yêu cầu điều khiển quỹ đạo theo cung tròn TÀI LIỆU THAM KHẢO [1] Phạm Trường Tùng, Điều khiển đồng robot bậc tự do, Luận văn thạc sĩ kĩ thuật, Đại học Đà Nẵng, 2011 [2] Phạm Đăng Phước, Robot công nghiệp, NXB Xây dựng, Hà Nội 2007 [3] Nguyễn Doãn Phước – Phan Xuân Minh – Hán Thành Trung, Lý thuyết điều khiển phi tuyến, NXB Khoa học kĩ thuật, Hà Nội 2008 [4] Jean –Jacques E.Slotine – Weiping Li, Applied Nonlinear Control, Prentice Hall, New Jersey 1991 [5] Reza-N.Jazar, Theory of applied Robotics, Springer New York Dordrecht Heidelberg London, New York 2006 [6] Henry W.Stone, Kinematic Modeling Identification, and control of robotic Manipulators, Kluwer Academic Publishers, United State of America 1987 (BBT nhận bài: 25/08/2015, phản biện xong: 10/09/2015) ... yêu cầu điều khiển quỹ đạo theo cung tròn TÀI LIỆU THAM KHẢO [1] Phạm Trường Tùng, Điều khiển đồng robot bậc tự do, Luận văn thạc sĩ kĩ thuật, Đại học Đà Nẵng, 2011 [2] Phạm Đăng Phước, Robot công... việc thiết kế quỹ đạo robot 4DOF định hướng vị trí theo cung trịn cho trước nằm trường cơng tác robot Trên 66 Phạm Trường Tùng sở quỹ đạo biến khớp tìm được, tác giả thiết kế điều khiển kiểu slide... để điều khiển động khớp robot Kết tính tốn mô phần mềm Matlab với cung tròn khác cho chất lượng điều khiển khả quan Tuy nhiên, điều khiển kiểu slide mode có nhược điểm dao động tín hiệu điều khiển