MÔ HÌNH hóa và điều KHIỂN ROBOT rắn

Journal of Thu Dau Mot university, No5(7) – 2012 MÔ HÌNH HÓA VÀ ĐIỀU KHIỂN ROBOT RẮN Nguyễn Minh Tâm, Nguyễn Văn Phước Trường Đại học Sư phạm Kó thuật thành phố Hồ Chí Minh TÓM TẮT Trên sở phương trình động lực học chuyển động robot rắn hai trường hợp ma sát nhớt ma sát Coulomb, nghiên cứu phương pháp điều khiển chuyển động robot rắn thiết kế điều khiển PID với thông số tối ưu giải thuật bày đàn PSO Nghiên cứu thiết kế kiểm nghiệm việc mô robot rắn gồm đoạn thực phần mềm Matlab Từ khóa: robot rắn, điều khiển, lực ma sát * Đặt vấn đề nghiên cứu robot rắn “không có bánh xe” Bài báo giới hạn nghiên cứu di chuyển robot rắn không gian hai chiều Ngày nay, hầu hết chế robot di động có bánh xe điều khiển động Những cấu robot có bánh xe tương đối hiệu quả, dễ dàng điều khiển thích hợp với điều khiển tốc độ cao mặt phẳng Tuy nhiên, chúng không hiệu môi trường gồ ghề đòa hình lởm chởm lầy lội Những robot di chuyển chân tích cực nghiên cứu Chúng cho thấy tính thích nghi với đòa hình cao robot di chuyển bánh xe Tính thích nghi với đòa hình chí cao với robot có nhiều đoạn “bò” rắn Ngoài tính thích nghi với môi trường ra, robot hình rắn cho thấy nhiều ưu điểm robot di chuyển bánh xe chân Chúng bơi lội trèo lên Với khả đó, robot rắn ngày ứng dụng rộng rãi nhiều lónh vực như: kiểm tra, nạo vét đường ống, tìm kiếm nạn nhân vụ hoả hoạn, động đất, dò thám quân Trong báo Mô hình robot rắn Xét robot rắn gồm n đoạn kết nối với qua (n-1) khớp Giả thiết đoạn có khối lượng Ở khớp có động truyền động cho robot Bài báo xét robot rắn di chuyển không gian hai chiều Trong trường hợp robot có (n+2) bậc tự Hình Robot rắn gồm n đoạn, n-1 khớp Robot gồm n đoạn, tọa độ trọng tâm đoạn xi , yi , góc hợp đoạn với phương ngang  i , chiều dài  22  Tạp chí Đại học Thủ Dầu Một, số 5(7) - 2012 đoạn pháp tuyến; dmi khối lượng đoạn vi 2li , ωx, ωy tọa độ điểm trọng tâm theo phương x phương y Bài báo xây dựng phương trình động lực học chuyển động robot rắn hai trường hợp ma sát nhớt ma sát Coulomb phân ds (trong dmi  mi ds ); J i 2li moment quán tính đoạn thứ i Trong đó: 2.1 Lực ma sát nhớt Xét đoạn thứ i robot rắn mô tả hình  i mi li2 Ji  Chiều dương qui ước chiều lượng giác (ngược chiều kim đồng hồ) Xét toàn hệ thống gồm n đoạn Vectơ lực ma sát f moment xoắn  tác động lên robot cho bởi: f   D f T z (1)   D  (2) Ct M  0  Cn M  Trong đó: D f :  D : Cn J Hình 2: Đoạn vi phân khâu thứ i Trong đó: i; C  :    S 2li chiều dài đoạn thứ  xi , yi  tọa độ trọng tâm đoạn v~ti v~ni vận tốc theo Với: phương tiếp tuyến phương pháp tuyến với đoạn thứ i; s khoảng cách từ đoạn vi phân ds đến điểm trọng tâm đoạn thứ i;  i   cosi  cni    sin i  i   sdf ni   sin i   xi    (1.1) cosi   yi  li Trong đó: mili2  cn i  cni J ii i cti cni , cnn  hệ số ma sát theo phương M  diag m1 ,, mn  S  diag sin 1 ,, sin  n  C  diag cos 1 ,, cos  n  2.2 Trường hợp ma sát Coulomb Lực ma sát lên đoạn thứ i cho bởi: Tổng moment ma sát xoắn quanh trọng tâm đoạn thứ i là: li  pháp tuyến Lực ma sát tác động lên đoạn thứ i robot rắn:  sin i  cti  cosi   cti , ctn tiếp tuyến; cn hệ số ma sát theo phương i góc hợp đoạn thứ i với trục x  f xi  cosi    mi   sin i  f yi   S  C   Cn : diag  cn1 , f i lực ma sát đoạn thứ i mặt  i vectơ vận tốc đoạn vi phẳng trượt; p phân ds; (4) Ct : diag ct1 , thứ i; (3)  f xi  cos i  f   mi g   sin  i  yi   sin  i   ti  cos i   0   ni    vt     sat   i ,      li i    i    i    ni mi gli dzs vni , lii  (1.2) hệ số Trong đó: ma sát theo phương tiếp tuyến phương ti ni hệ số ma sát Coulomb theo hướng tiếp tuyến 23 Journal of Thu Dau Mot university, No5(7) – 2012 Trong đó: f i  i lực moment ma sát đoạn thứ i mặt phẳng trượt; g i g i 1 lực tương tác hướng pháp tuyến với đoạn thứ i, g gia tốc trọng trường, mi khối lượng đoạn vi phân   dzs vni , lii  2li li  sgn( i li đoạn kế cận thứ (i-1) (i+1); u i ui 1  i s ) s.ds    1   i    lii      vni    lii    moment động khớp thứ i i-1; góc hợp với phương ngang đoạn thứ i l   v  l   v  l    v  i i ni i i ni i i Mỗi khâu thứ i có khối lượng mi , chiều dài 2li Moment quán tính tác ni động lên đoạn thứ i Xét toàn hệ thống gồm n đoạn Vectơ lực ma sát f moment xoắn  tác    dzs S C z, L (5)  n Mg    n : diag n1 , F  C  DT u    £ T f (7)   E T f m (8) Trong đó, góc tuyệt đối:   1   n T   n MgL t : diag t1 , , tn mili2 Tổng hợp (1),(2), (7) (8) ta có phương trình chuyển động robot rắn:  F    C   R S     DT  (9)  T      u  mI      Q             S (6) Trong :   Mg  f :  t  Ji  Áp dụng đònh luật II Newton phân tích đoạn sau tổng hợp lại ta có: động lên robot cho bởi:  0  f    f sat  T z,     L   i Vò trí trọng tâm robot rắn:  , nn 1 n    mi xi  T  x   m i 1  e Mx    :     1 n  m eT My   y      mi yi    m i 1    x z   y R  T S 2.3 Phương trình chuyển động S   D  Q  0  £T      D f T  £ E  0  ET  1  A       R n1xn   1  1      R n1xn D       1  e 0 , e  1  1T  R n E  0 e  Hình 3: Phân tích lực tác động lên đoạn thứ i robot rắn Xét khâu thứ i robot rắn gồm n đoạn (xem hình 3) n m   mi i 1 24 Tạp chí Đại học Thủ Dầu Một, số 5(7) - 2012 S  diag  sin 1, ,sin n   0  C  diag  cos1, ,cosn     eT Re   mI     S T e   DF 1 C  R  S   Bu chiều L  diag  l1, , ln  , với li , J n  , với    D    :  T      e F  J i moment quán tính đoạn thứ i M  diag  m1,  , mn  , với mi khối   K  e F  J  S HS  C HC C N T    H=LAT DM 1DT  B  DF 1DT T N=M-1DT DM 1DT  1 1 K  F 1DT B 1 ;   ; e   e eT Fe AL Với phương trình chuyển động phân li, xây dựng phương pháp điều khiển cho: (1) moment xoắn khớp u điều khiển hình dáng  robot AL C  S HC  C HS C M D f :  t  0  Cn M  rắn; (2) hình dáng  D : Cn J C  :    S  Cn  diag  cn1 ,  điều khiển  robot rắn Phương pháp điều khiển chuyển động robot rắn  S  C  Ct  diag ct1 , vectơ góc tương đối đoạn kế cận  động lượng góc trung bình lượng đoạn thứ i £=  S N T (11) Trong đó: dài đoạn thứ i J  diag  J1, eT S    eT R  (10)    K  Q     S T  , ctn Khi hình dạng  , cnn  robot rắn thay đổi, lực f moment xoắn  tạo ma sát robot bề mặt trượt Kết vò trí trọng tâm  động lượng góc toàn thể  thay đổi Phương  2.4 Phân li động lực học (Dynamic Decoupling) trình (10) cho thấy điều khiển biến   dùng tín hiệu điều khiển  Trong phần này, tách phương trình chuyển động xây dựng hai phần: hình dạng chuyển động (moment xoắn khớp  góc khớp) di động quán tính (góc khớp  vò trí hướng quán tính) Việc phân li làm đơn giản việc phân tích tổng hợp chuyển động hình rắn Bằng cách tác động vào  ta điều khiển robot rắn chuyển động Mục tiêu phần xác đònh phương pháp điều khiển  để robot rắn chuyển động theo hướng đònh trước tốc độ đònh trước với hiệu suất chuyển động tối ưu Sự chuyển động robot rắn thực thông qua việc thay đổi hình dạng  robot rắn để hình thành Phương trình chuyển động robot rắn phân li có dạng sau: đường cong serpenoid 25 Journal of Thu Dau Mot university, No5(7) – 2012 3.1 Đường cong serpenoid Đònh nghóa: Đường cong serpenoid đường cong có tọa độ xác đònh bởi: x  s    cos   d (12) y  s    sin   d (13)  : a cos b   c (14) s s Trong đó: a, b c số xác đònh hình dạng đường cong serpenoid, s chiều dài cung từ gốc tọa độ tới điểm xét Hình (4c): Đường cong serpenoid với a b  10 (1) c  ; (2) c   ; (3)  c  Hình (a, b, c) trình bày đường cong serpenoid với thông số a, b c khác Nếu xấp xỉ đường cong serpenoid dạng n đoạn thẳng kết nối với Phương trình (12) (13) xấp xỉ bởi: Hình (4a) Đường cong serpenoid với (1) a  c0 i   kb  kc  xi   cos  a cos      n n  k 1 n i   kb  kc  yi   sin  a cos      n  n  k 1 n b  2  2  ; (2) a  ; (3) a  3 Hình (4b): Đường cong serpenoid với c0 a Khi đường cong serpenoid ban đầu xấp xỉ n đoạn thẳng kết nối với (hình 5)  Hình Đường cong serpenoid xấp xỉ đoạn thẳng Trên hình 5, (1) b  2 ; (2) b  6 ; (3) b  10  i góc đoạn thứ i hợp với trục x Chiều dương qui ước 26 i Tạp chí Đại học Thủ Dầu Một, số 5(7) - 2012 chiều lượng giác (ngược chiều kim đồng hồ) Khi ta có: Với  ib  ic  n n  thông số mô tả hướng toàn robot rắn v tốc độ trọng tâm theo hướng  i  a cos  Đònh nghóa:   i  i  i 1   sin  i  i   2 góc tương đối xác đònh hình dạng đường cong serpenoid rời rạc (hình5) Trong đó: b c    ,   ,   n n 2 Như vậy, góc tương đối  i thay đổi theo  : a sin  dạng hình sin với biên độ  độ lệch Hai góc kế có chênh lệch   Hình 6: Chuyển động hình rắn(   0) 3.2 Sự di chuyển hình rắn Chuyển động trườn rắn bắt chước việc thay đổi góc tương đối robot rắn theo qui luật sau: i  t    sin t  i  1    , i  1, , n  1 (14)  Trong   , ,  thông Hình 7: Chuyển động hình rắn(  số xác đònh hình dạng đường cong serpenoid,  xác đònh tốc độ lan truyền sóng hình rắn dọc theo thể Giả thiết điều khiển  thay đổi (14) kết hợp Từ kết mô hình hình ta thấy  = robot rắn di chuyển theo đường thẳng   rad / s,    lượt hai thông số xác đònh tốc độ hướng robot rắn Do đó, ta cố đònh  ,  sử dụng   để điều khiển tốc độ hướng robot rắn Trong phần này, ta xây dựng mô hình robot rắn với đoạn (n=6), khối lượng đoạn mi  kg , chiều rad / s,   rad / s Kết mô chuyển động robot rắn trình bày hình (với   ) hình 7(với   10 deg) dài đoạn cti = 10, Trong đó: v  x cos   y sin  ;   n  0 robot rắn di chuyển theo đường tròn Thiết kế hệ thống điều khiển Trong phần trước, ta thấy   lần với ma sát môi trường Robot di chuyển theo dạng hình rắn Chúng ta dùng mô để kiểm chứng giả thiết Trong phần ta mô robot rắn với thông số sau: n=6, mi  1kg , li  1m , cti  0.1 , cni  10  10 deg) li  1m , cni=0.1,      Hình trình bày sơ đồ khối hệ thống điều khiển robot rắn Bộ điều khiển bao gồm cấp điều khiển: i i 1 27 Journal of Thu Dau Mot university, No5(7) – 2012 Hình 10 Sơ đồ mô điều khiển vận tốc ( C ) robot rắn 4.2.2 Bộ điều khiển hướng Hàm truyền C C ( s)  K P  Hình 8: Sơ đồ khối hệ thống điều khiển robot rắn KI  KD2s s 4.1 Bộ điều khiển đòa phương: Chúng ta dùng điều khiển PID cho điều khiển đòa phương C  s  có nhiệm vụ tạo tín hiệu điều khiển động khớp, nhằm mục đích điều khiển tạo moment u cho tín hiệu hồi tiếp  bám theo tín hiệu đặt  * Tín hiệu Hình 11 Sơ đồ mô điều khiển vận tốc ( C si ) robot rắn 4.2.3 Bộ điều khiển toàn hệ thống  * lấy từ biến đổi T với : *   sin t  i  1    ; i = 1, ,5 Hàm truyền điều khiển C  s  sau: C ( s)  K P  KI  KDs s Hình 12: Sơ đồ mô toàn hệ thống Hình Sơ đồ mô điều khiển C Các thông số KP, KI, KD, KP1, KI1, KD1, KP2, KI2 KD2 điều khiển PID điều khiển C , C C tối ưu robot phần mềm Matlab 4.2 Bộ điều khiển vòng ngoài: thông số sử dụng giải thuật PSO: n=50 (kích thước quần thể), bird_step=3 (kích thước tìm kiếm); dim = (số biến KP, KI, KD PID); c1 = 0.1;c2 = 0.2 (hệ số gia tốc); w = 0.2; trọng số PSO Bộ điều khiển tốc độ hướng robot rắn sử dụng hai điều khiển C C để thực chuyển động mong muốn v *  * Các điều khiển C C sử dụng hai điều khiển PID với: 4.2.1 Bộ điều khiển vận tốc Hàm truyền C C ( s)  K P1   Hàm thích nghi fitness  e (t )dt Quá trình cập nhập particles dựa theo công thức (15), (16) K I1  K D1s s 28 Tạp chí Đại học Thủ Dầu Một, số 5(7) - 2012 vi(,km1)  w.vi(,km)  c1 * rand () *( Pbesti ,m  xi(,km) ) c2 * Rand () *(Gbestm  xi(,km) ) (15) Hình 15: Đồ thò vận tốc xi(,km1)  xi(,km)  vi(,km1) ; i=1,…,n; m=1,…,d (16)  Trong đó: Pbest giá trò tốt thời điểm cá thể thứ i quần thể; Gbest vò trí tốt cá quần thể thời điểm tại; k số lần lặp lại; vi ,m : Vận tốc cá thể thứ i (k ) hệ thứ k; rand () số ngẫu nhiên khoảng (0,1); xi , m vò trí cá thể thứ (k ) Hình 16: Đồ thò góc i hệ thứ k; khởi động quần thể đầu tiên: khởi động ngẫu nhiên Hàm truyền điều khiển C ,   Kết mô với C C sau tự chỉnh đònh dùng giải  thuật PSO: 2 rad  rad,  * = π/4 (rad), *  1(m / s) C ( s )  48.5536  14.7446 s 0.72078 +0.42089s s C ( s )  0.79193  0.3998 s C ( s)  1.9219  Hình 17: Đồ thò góc hướng Kết mô Hình 18: Đồ thò vận tốc Trong mô đây, mô với thông số sau: n = 6, mi  1kg li  m , ct  0.1, cni  10 ,    rad   2 rad Kết mô với 2 rad  i Hình 19 Đồ thò góc Hình 13: Sơ đồ mô hệ thống điều khiển robot rắn phần mềm matlab     Kết mô với rad,   * = (rad), *  1(m / s) Hình 14: Đồ thò góc hướng  2 rad   rad,  * = π/2 (rad), *  1(m / s) Hình 20: Đồ thò hướng 29   Journal of Thu Dau Mot university, No5(7) – 2012 hình dáng  Hình 21: Đồ thò vận tốc  điều khiển robot rắn; (2) hình dáng   robot rắn - Sự chuyển động robot rắn thực thông qua việc thay đổi việc thay đổi hình dạng  robot rắn để  hình thành đường cong serpenoid - Từ mô hình robt rắn với đoạn (n=6), khối lượng đoạn mi  kg , chiều dài đoạn Hình 22: Đồ thò góc    li  1m , cni=0.1, cti = 10,    thiết kế hệ thống điều khiển bao gồm điều khiển đòa phương điều khiển vòng Kết luận - Ứng dụng phương pháp giải thuật bày đàn PSO để xác đònh thông số điều khiển PID - Trên sở phương trình động lực học chuyển động robot rắn hai trường hợp ma sát nhớt ma sát Coulomb, với phương trình chuyển động phân li, xây dựng phương pháp điều khiển cho: (1) moment xoắn khớp u điều khiển - Mô hệ thống điều khiển Matlab, kiểm tra chất lượng tính bền vững hệ thống điều khiển * THE MODELING AND OPERATION OF SOLID ROBOT Nguyen Minh Tam, Nguyen Van Phuoc University of Technical Educaton Ho Chi Minh City ABSTRACT On the base of the kinetic equation of the solid robot’s movement in two cases: viscous friction and Coulomb friction, we study the method of controlling the solid robot’s movement and design the PID control with optimal parameters by PSO swarm algorithm The design study is tested by the simulation of the solid robot in the Matlab software TÀI LIỆU THAM KHẢO [1] M SAITO, M FUKAYA AND T IWASAKI (2002), "Serpentine locomotion with robotic snake", IEEE Control Systems Magazine, vol.22, No.1, pp.64-81 [2] P PRAUTSCH, T MITA, AND T IWASAKI (2000), "Analysis and control of a gait of snake robot", Transactions of IEEJ, Industry Applications Society, vol.120-D, No.3, pp.372-381 [3] Y SHAN AND Y KOREN (1993), “Design and motion planning of a mechanical snake”, IEEE Trans Sys Man Cyb., vol.23, no.4, pp.1091–1100 [4] M NILSSON (1998), “Snake robot free climbing”, IEEE Control Systems Magazine, vol.18, no.1, pp.21–26 [5] HASSAN K.KHALIL (2002), “Nonlinear Systems”, Prentice-Hall 30 Tạp chí Đại học Thủ Dầu Một, số 5(7) - 2012 [6] JEAN – JACQUES E.SLOTINE AND WEIPING LI (1991), “Applied Nonlinear Control”, Prentice-Hall [7] MORARI AND ZAFIRION (1989), “Robust Process Control”, Prentice-Hall [8] H.DATE, Y HOSHI AND M SAMPEI (2000), Locomotion control of a snake-like robot based on dynamic manipulability, In IEEE/RSJ Int Conferrence on Intelligent Robots and Systems, page 2236 – 2241 [9] H.DATE, Y HOSHI AND M SAMPEI (2001), Control of a Snake Robot in Consideration of Constraint Force, In IEEE Int Conferrence on Control Applications, page 966 – 971 [10] H.DATE, Y HOSHI AND M SAMPEI (2001), Locomotion control of a Snake Robot Constraint Force Attenuation, Proceedings of the American Control Reference, Arlington VA June 25 – 27, page 113 – 118 [11] F MATSUNO AND K MOGI (2000), Redundancy Controllable System and Control of Snake Robot Based on Kinematic Model, In IEEE Int Conference on Decision and Control, page 4791 – 4796 [12] NGUYỄN VĂN GIÁP (2000), Ứng dụng Matlab điều khiển tự động, NXB Đại học Quốc gia TP Hồ Chí Minh [13] NGUYỄN DOÃN PHƯỚC (2002), Lí thuyết điều khiển tuyến tính, NXB Khoa học Kó thuật, 2002 [14] NGUYỄN THỊ PHƯƠNG HÀ (2003), Lí thuyết điều khiển tự động, NXB Đại học Quốc gia TP Hồ Chí Minh [15] SHUGEN MA, NAOKY TADOKORO, BINLI, KOUSUKE INOUE (2003), “Analisys of Creeeping Locomotion of a Snake Robot on a Slope”, in Proc IEEE Int Conf Robotics & Auto., Taipei, Taiwan, page 2073 – 2078 [16] JIM OSTROWSKI, JOEL BURDICK (1996), “Gait Kinematics for a Serpentine Robot”, in Proc IEEE Int Conf Robotics & Auto., Minneapolis,Minnesota, page 2073 – 2078 [17] JIM OSTROWSKI, JOEL BURDICK (2001), “Development of a Creeping Snake-Robot”, in Proc IEEE Int Conf Robotics & Auto.,Banff,Albreta,Cannada, page 77 – 82 [18] S MA (1999), “Analysis of snake movement forms for realization of snake-like robot,” in Proc IEEE Int Conf.Robotics & Auto., Detroit, MI, pp.3007–3013 [19] JIM OSTROWSKI, JOEL BURDICK (2002), “Improvement of Manipulability for Locomotion of a Snake Robot by Mass Dirtribution”, in SICE 2002, Aug, OSAKA, page 2214 – 2217 [20] YANSONG SHAN AND YORAM KOREN (1993), “Design and Motion Planning of a Machenical Snake”, IEEE Transactions on Systems,Man,and Cybernetics, vol 23, No 4, page 1091 - 1100 [21] N PILLAY (2008), A Particle swarm optimization approach for tuning of SISO PID control loops 2008 31 ... thống điều khiển robot rắn KI  KD2s s 4.1 Bộ điều khiển đòa phương: Chúng ta dùng điều khiển PID cho điều khiển đòa phương C  s  có nhiệm vụ tạo tín hiệu điều khiển động khớp, nhằm mục đích điều. .. truyền điều khiển C  s  sau: C ( s)  K P  KI  KDs s Hình 12: Sơ đồ mô toàn hệ thống Hình Sơ đồ mô điều khiển C Các thông số KP, KI, KD, KP1, KI1, KD1, KP2, KI2 KD2 điều khiển PID điều khiển. .. trọng số PSO Bộ điều khiển tốc độ hướng robot rắn sử dụng hai điều khiển C C để thực chuyển động mong muốn v *  * Các điều khiển C C sử dụng hai điều khiển PID với: 4.2.1 Bộ điều khiển vận tốc