Luận văn Điều khiển robot 5 bậc tự do bằng nhận dạng giọng nói

f (p) 0 0 0 1             (2.7b) Trong đó: 11 1 1f (v)=C x+S y 12f (v)=z 13 1 1f (v)=S x-C y Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 23 Với x, y, z tương ứng là các thành phần của vectơ v Ma trận1 5T sử dụng các ma trận iA (i=2÷5) có dạng: 234 5 234 234 5 234 234 5 234 4 23 3 2 2 234 5 234 234 5 234 234 5 234 4 23 3 2 2 5 5 5 C C -S -C C -S C S C a +C a +C a S C +C -S C +C S S S a +S a +S a -S S C 0 0 0 0 1             2.2. ĐỘNG LỰC HỌC ROBOT Nghiên cứu động lực học robot là công việc cần thiết khi phân tích cũng như tổng hợp quá trình điều khiển chuyển động. Việc nghiên cứu động lực học robot thường giải quyết hai nhiệm vụ sau đây: -Xác định momen và lực động xuất hiện trong quá trình chuyển động. Khi đó qui luật biến đổi của biến khớp ( )iq t coi như đã biết. Việc tính toán lực trong cơ cấu tay máy là rất cần thiết để chọn công suất động cơ, kiểm tra độ bền, độ cứng vững, đảm bảo độ tin cậy của robot. -Xác định các sai số động tức là sai lệch so với qui luật chuyển động theo chương trình. Lúc này cần khảo sát Phương trình chuyển động của robot có tính đến đặc tính động lực của động cơ và các khâu. Có nhiều phương pháp nghiên cứu động lực học robot, nhưng thường gặp hơn cả là phương pháp cơ học Lagrange, cụ thể là dùng phương trình Lagrange - Euler. Đối với các khâu khớp của robot, với các nguồn động lực và kênh điều khiển riêng biệt, không thể bỏ qua các hiệu ứng trọng trường (gravity effect), quán tính (initial), tương hổ (Coriolis), ly tâm (centripetal)... mà những khía cạnh này chưa được xét đầy đủ trong cơ học cổ điển; Cơ học Lagrange nghiên cứu các vấn đề nêu trên như một hệ thống khép kín nên đây là nguyên lý cơ học thích hợp đối với các bài toán động lực học robot. Xét khâu thứ i của robot có n khâu 2.2.1. Hàm Lagrange Một điểm trên khâu thứ i được mô tả trong hệ tọa độ cơ bản là: i ir T r ; 1i n  (2.8) Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 24 Tốc độ của vi khối lượng dm được tính bởi công thức: 1 i i ii i j j j Tdr d r T r q r dt dt q           (2.9) Khi tính bình phương của vận tốc này ta có:    2 0 0 0, , Trr r x y z Tr rr  (2.10) Hình 2.3. Khảo sát tốc độ của vi khối lượng dm Với 11 1 1 1 n n ii i n nn a a Tr a a a             Do vậy  2 1 1 Ti i T i T i T i i Ti i i i j k j k j k T Td d r Tr rr Tr T r T r Tr r r q q dt dt q q                   (2.11) Động năng của vi khối lượng dm đặt tại vị trí ir trên khâu thứ i   2 1 1 1 1 1 1 2 2 1 2 Ti i i i Ti i i j k j k j k Ti i i i Ti i j k j k j k T T dK r dm Tr r r q q dm q q T T Tr rdm r q q q q                            (2.12) Động năng của khâu thứ i sẽ là: Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 25 1 1 1 1 1 2 1 2 Ti i i i Ti i i j k j k j kKhau i Khau i Ti i i i i j k j k j k T T K dK Tr rdm r q q q q T T Tr J q q q q                                 (2.13) Với iJ là ma trận quán tính được tính: i i T i Khau i J rdm r  Động năng của robot có n khâu được tính: 1 n i i K K   (2.14) Thế năng của khâu i có khối lượng dm, trọng tâm được xác định bởi vecto i r i i i i i i iP m g r m gT r    (2.15) Trong đó vecto gia tốc trọng trường được biểu diễn dưới dạng:  0 0 9.8 0 0 T T x y zg g g g     Thế năng của robot có n khâu được tính: 1 n i i P P   (2.16) Hàm Lagrange của một hệ thống năng lượng: L K P  (2.17) Sau khi xác định động năng và thế năng của toàn cơ cấu, ta có hàm Lagrange của robot có 5 bậc tự do: 1 1 1 1 2 2 Tn i i n ii i i j k i i i i j k ij k T T L Trace J q q m gT r q q             (2.18) 2.2.2 Phương trình động lực học robot Ta có lực tác dụng lên khâu thứ i (lực tổng quát) được xác định bởi phương trình Lagrange: Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 26 i i i d L L F dt q q       (2.19) Với hàm Lagrange đã xác định ở trên ta tính được: 2 1 1 1 T Tj jn i n n j j j j j j i j k j k m j j j i k j i k m j ik i k m i i T T T T T F Tr J q Tr J q q m g r q q q q q q                                 (2.20) Đạo hàm của ma trận 1i i iA T  đối với biến khớp iq có thể dễ dàng xác định theo công thức sau: 1i i i i i d T D A dq   (2.21) Trong đó đối với khớp quay: 0 1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 iD             Trong trường hợp 1,2, ,ni  ta có 1 2 1 1 ji j i i j j dAT A A A A A q dq      1 2 1 1j j j i i A A A D A A A  Đặt 1 2 1 1ij j j j i iU A A A D A A A  và đơn giản hóa cách viết như sau: 1 1 0 j j j i ij T D T khi j i U khi j i        (2.22) Tương tự ta có 2 1 2 1 1 1 j k k k m m m j j k m T A A A D A A D A A A q q        1 1 1 1 1 1 0 , k m k k k m m j m k jkm m m m k k j T D T T D T khi k m j U T D T T D T khi m k j khi k m j                (2.23) Cuối cùng ta có Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 27   1 1 1 j j jn n n T T j i jk j ji k jkm j ji k m j ji j j i k j i k m j i F Tr U J U q Tr U J U q q m gU r              (2.24) Hay 1 1 1 n n n i ik k ikm k m i k k m F D q h q q c        (2.25) Hoặc viết dưới dạng ma trận ( ) ( , ) ( )F D q q V q q C q   (2.26) Trong đó: D thể hiện tác dụng quán tính, là một ma trận đối xứng (nxn) V thể hiện tác dụng của lực ly tâm và Cariolis, là một vecto (nx1) C thể hiện tác dụng của lực trọng trường, cũng là một vecto (nx1) Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 28 2.2.3 Phương trình động lực học robot 5 bậc tự do Ta xem xét một cơ cấu tay máy 5 DOF như sơ đồ hình 2.4 Hình 2.4 Hệ tọa độ của robot 5 DOF Thông số DH của robot 5 DOF như trong bảng 2.2 Bảng 2.2 Khâu Khớp nối i i id ia 1 0-1 1 090 1d 0 2 1-2 2 0 0 2a 3 2-3 3 0 0 3a 4 3-4 4 090 0 4a 5 4-5 5 0 5d 0 Ma trận chuyển đổi vị trí giữa hệ tọa độ (i) và (i-1) 1 1 1 1 1 1 0 0 0 0 0 1 0 0 0 0 1 c s s c A d                  ; 2 2 2 2 2 2 2 0 0 0 0 0 1 0 0 0 0 1 c s a c s c A                   ; 3 3 3 3 3 3 3 0 0 0 0 0 1 0 0 0 0 1 c s a c s c A                   Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 29 4 4 4 4 4 4 4 4 0 0 0 1 0 0 0 0 0 1 c s a c s c a A                   ; 5 5 5 5 5 5 0 0 0 0 0 0 1 0 0 0 1 c s s c A d                  Ma trận chuyển đổi vị trí giữa hệ tọa độ i và hệ tọa độ gốc 0 1 2i i iT T A A A  1 1 1 1 1 1 1 0 0 0 0 0 1 0 0 0 0 1 c s s c T A d                   ; 1 2 1 2 1 1 2 2 1 1 2 1 1 2 2 2 2 2 2 1 2 10 0 0 0 1 c c c s s a c c c s s s c a s c T A A s c d                                 1 2 3 1 2 3 1 2 3 1 3 2 1 1 2 1 2 3 2 3 1 1 2 3 2 3 1 3 1 2 1 1 2 2 3 1 2 3 3 2 2 3 2 3 3 2 2 3 2 3 3 1 2 3 1 30 0 0 0 1 c c c c s s c c s c c s s a c c a c c c c c s s s s c s s c s s c a s c a c c s T A A A c s c s c c s s d a c s                                                                      4 1 2 3 4T A A A A 5 1 2 3 4 5T A A A A A Phương trình động lực học 5 5 5 1 1 1 i ik k ikm k m i k k m F D h c          ; 1 5i   (2.27) ( ) ( , ) ( )F D V C       (2.28) Với robot có 5 bậc tự do thì:  1 2 3 4 5, , , , T       1 2 3 4 5, , , , T           1 2 3 4 5,F ,F ,F ,F T F F F Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 30 Ma trận  D    11 12 13 14 15 21 22 23 24 25 31 32 33 34 35 41 42 43 44 45 51 52 53 54 55 D D D D D D D D D D D D D D D D D D D D D D D D D D                 (2.29)      11 11 1 11 21 2 21 31 3 31T T TD Tr U J U Tr U J U Tr U J U      41 4 41 51 5 51T TTr U J U Tr U J U       12 22 2 21 32 3 31 42 4 41T T TD Tr U J U Tr U J U Tr U J U    52 5 51TTr U J U ; 21 12D D      13 33 3 31 43 4 41 53 5 51T T TD Tr U J U Tr U J U Tr U J U   ; 31 13D D    14 44 4 41 54 5 51T TD Tr U J U Tr U J U  ; 41 14D D  15 55 5 51TD Tr U J U ; 51 15D D      22 22 2 22 32 3 32 42 4 42T T TD Tr U J U Tr U J U Tr U J U    52 5 52TTr U J U      23 33 3 32 43 4 42 53 5 52T T TD Tr U J U Tr U J U Tr U J U   ; 32 23D D    24 44 4 42 54 5 52T TD Tr U J U Tr U J U  ; 42 24D D  25 55 5 52TD Tr U J U ; 52 25D D      33 33 3 33 43 4 43 53 5 53T T TD Tr U J U Tr U J U Tr U J U      34 44 4 43 54 5 53T TD Tr U J U Tr U J U  ; 43 34D D  35 55 5 53TD Tr U J U ; 53 35D D    44 44 4 44 54 5 54T TD Tr U J U Tr U J U   45 55 5 54TD Tr U J U ; 54 45D D Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 31  55 55 5 55TD Tr U J U Vecto ( , )V   - vecto (5x1) lực ly tâm và Coriolit  1 2 3 4 5( , ) , , , , T V h h h h h   1 2 3 4 5, , , , T          11 12 13 14 15 21 22 23 24 25 , 31 32 33 34 35 41 42 43 44 45 51 52 53 54 55 H i i i i i i i i i i i v i i i i i i i i i i i i i i i h h h h h h h h h h h h h h h h h h h h h h h h h                ; 1,2,3,4,5i = , T i i vh H  Như vậy vec tơ ( , )V   được biểu diễn dưới dạng sau: 1 1, 2 2, 3 3, 4 4, 5 5, ( , ) T v T v T v T v T v h H h H V h H h H h H                                      (2.30) Với 11i h           111 111 1 11 211 2 21 311 3 31 411 4 41 511 5 51T T T T Th Tr U J U Tr U J U Tr U J U Tr U J U Tr U J U          112 212 2 21 312 3 31 412 4 41T T Th Tr U J U Tr U J U Tr U J U    512 5 51TTr U J U ; 121 112h h      113 313 3 31 413 4 41 513 5 51T T Th Tr U J U Tr U J U Tr U J U   ; 131 113h h    114 414 4 41 514 5 51T Th Tr U J U Tr U J U  ; 141 114h h  115 515 5 51Th Tr U J U ; 151 115h h      122 222 2 21 322 3 31 422 4 41T T Th Tr U J U Tr U J U Tr U J U    522 5 51TTr U J U      123 323 3 31 423 4 41 523 5 51T T Th Tr U J U Tr U J U Tr U J U   ; 132 123h h Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 32    124 424 4 41 524 5 51T Th Tr U J U Tr U J U  ; 142 124h h  125 525 5 51Th Tr U J U ; 152 125h h      133 333 3 31 433 4 41 533 5 51T T Th Tr U J U Tr U J U Tr U J U   ;    134 434 4 41 534 5 51T Th Tr U J U Tr U J U  ; 143 134h h  135 535 5 51Th Tr U J U ; 153 135h h    144 444 4 41 544 5 51T Th Tr U J U Tr U J U  ;  145 545 5 51Th Tr U J U ; 154 145h h  155 555 5 51Th Tr U J U Với 22i h         211 211 2 22 311 3 32 411 4 42 511 5 52T T T Th Tr U J U Tr U J U Tr U J U Tr U J U           212 212 2 22 312 3 32 412 4 42 512 5 52T T T Th Tr U J U Tr U J U Tr U J U Tr U J U    ; 221 212h h      213 313 3 32 413 4 42 513 5 52T T Th Tr U J U Tr U J U Tr U J U   ; 231 213h h    214 414 4 42 514 5 52T Th Tr U J U Tr U J U  ; 241 214h h  215 515 5 52Th Tr U J U ; 251 215h h        222 222 2 22 322 3 32 422 4 42 522 5 52T T T Th Tr U J U Tr U J U Tr U J U Tr U J U    ;      223 323 3 32 423 4 42 523 5 52T T Th Tr U J U Tr U J U Tr U J U   ; 232 223h h    224 424 4 42 524 5 52T Th Tr U J U Tr U J U  ; 242 224h h  225 525 5 52Th Tr U J U ; 252 225h h      233 333 3 32 433 4 42 533 5 52T T Th Tr U J U Tr U J U Tr U J U   ; Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 33    234 434 4 42 534 5 52T Th Tr U J U Tr U J U  ; 243 234h h  235 535 5 52Th Tr U J U ; 253 235h h    244 444 4 42 544 5 52T Th Tr U J U Tr U J U  ;  245 545 5 52Th Tr U J U ; 254 245h h  255 555 5 52Th Tr U J U Với 33i h       311 311 3 33 411 4 43 511 5 53T T Th Tr U J U Tr U J U Tr U J U        312 312 3 33 412 4 43 512 5 53T T Th Tr U J U Tr U J U Tr U J U   ; 321 312h h      313 313 3 33 413 4 43 513 5 53T T Th Tr U J U Tr U J U Tr U J U   ; 331 313h h    314 414 4 43 514 5 53T Th Tr U J U Tr U J U  ; 341 314h h  315 515 5 53Th Tr U J U ; 351 315h h      322 322 3 33 422 4 43 522 5 53T T Th Tr U J U Tr U J U Tr U J U   ;      323 323 3 33 423 4 43 523 5 53T T Th Tr U J U Tr U J U Tr U J U   ; 332 323h h    324 424 4 43 524 5 53T Th Tr U J U Tr U J U  ; 342 324h h  325 525 5 53Th Tr U J U ; 352 325h h      333 333 3 33 433 4 43 533 5 53T T Th Tr U J U Tr U J U Tr U J U   ;    334 434 4 43 534 5 53T Th Tr U J U Tr U J U  ; 342 324h h  335 535 5 53Th Tr U J U ; 353 335h h    344 444 4 43 544 5 53T Th Tr U J U Tr U J U  ; Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 34  345 545 5 53Th Tr U J U ; 354 345h h  355 555 5 53Th Tr U J U Với 44i h     411 411 4 44 511 5 54T Th Tr U J U Tr U J U  ;    412 412 4 44 512 5 54T Th Tr U J U Tr U J U  ; 421 412h h    413 413 4 44 513 5 54T Th Tr U J U Tr U J U  ; 431 413h h    414 414 4 44 514 5 54T Th Tr U J U Tr U J U  ; 441 414h h  415 515 5 54Th Tr U J U ; 451 415h h    422 422 4 44 522 5 54T Th Tr U J U Tr U J U  ;    423 423 4 44 523 5 54T Th Tr U J U Tr U J U  ; 432 423h h    424 424 4 44 524 5 54T Th Tr U J U Tr U J U  ; 442 424h h  425 525 5 54Th Tr U J U ; 452 425h h    433 433 4 44 533 5 54T Th Tr U J U Tr U J U  ;    434 434 4 44 534 5 54T Th Tr U J U Tr U J U  ; 443 434h h  435 535 5 54Th Tr U J U ; 453 435h h    444 444 4 44 544 5 54T Th Tr U J U Tr U J U  ;  445 545 5 54Th Tr U J U ; 454 445h h  455 555 5 54Th Tr U J U Với 55i h   511 511 5 55Th Tr U J U ;  512 512 5 55Th Tr U J U ; 521 512h h  513 513 5 55Th Tr U J U ; 531 513h h  514 514 5 55Th Tr U J U ; 541 514h h  515 515 5 55Th Tr U J U ; 551 515h h Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 35  522 522 5 55Th Tr U J U ;  523 523 5 55Th Tr U J U ; 532 523h h  524 524 5 55Th Tr U J U ; 542 524h h  525 525 5 55Th Tr U J U ; 552 525h h  533 533 5 55Th Tr U J U ;  534 534 5 55Th Tr U J U ; 543 534h h  535 535 5 55Th Tr U J U ; 553 535h h  544 544 5 55Th Tr U J U ;  545 545 5 55Th Tr U J U ; 554 545h h  555 555 5 55Th Tr U J U Vecto ( )C  :  1 2 3 4 5( ) , , , , T C c c c c c  (2.31) ( ) 1 1 11 1 2 21 2 3 31 3 4 41 4 5 51 5 g gU gU gUc m U r m r m gU r m r m r= - + + + + ( ) 2 2 22 2 3 32 3 4 42 4 5 52 5 gU gU Uc m r m gU r m r m g r= - + + + ( ) 3 3 33 3 4 43 4 5 53 5 U Uc m gU r m g r m g r= - + + ( ) 4 4 44 4 5 54 5 U Uc m g r m g r= - + ( ) 5 5 55 5 Uc m g r= - Tính toán trên máy tính ta thu được ( ) ( , ) ( )F D V C       1 1 11 2 2 22 3 3 33 4 4 44 5 5 55 ( ) F h c F h c F D h c F h c F h c                                                                (2.32) Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 36 2.3. THIẾT KẾ BỘ ĐIỀU KHIỂN CÁC KHỚP 2.3.1. Cấu trúc hệ thống điều khiển Chức năng của hệ thống điều khiển chuyển động là đảm bảo tay robot chuyển động bám theo quỹ đạo đặt trước trong môi trường làm việc. Chuyển động tay của robot được thực hiện nhờ các hệ thống truyền động khớp robot. Trên cơ sở đó, có hai dạng hệ thống điều khiển chuyển động: hệ thống điều khiển ở không gian khớp và hệ thống điều khiển ở không gian làm việc. Ở hệ thống điều khiển khớp, đại lượng điều khiển là vị trí của khớp robot: góc quay đối với khớp quay; độ dịch chuyển thẳng đối với khớp tịnh tiến. Bộ điều khiển được thiết kế đảm bảo vị trí khớp luôn bám theo vị trí đặt, tức là sai lệch vị trí khớp hội tụ về không với thời gian nhỏ nhất. Vị trí đặt của khớp được tính toán từ lượng đặt của vị trí tay robot trong không gian làm việc thông qua khâu tính toán động học ngược. Ưu điểm của phương pháp điều khiển ở không gian khớp là bộ điều khiển tác động trực tiếp đến hệ thống truyền động của khớp. Tuy nhiên, hệ thống điều khiển này khó đảm bảo độ chính xác vị trí của tay khi tồn tại các sai lệch trong cơ cấu cơ khí hoặc thiếu thông tin về quan hệ vị trí giữa tay robot và đối tượng Hình 2.5. Sơ đồ khối hệ thống điều khiển ở không gian khớp Hệ thống điều khiển không gian làm việc có chức năng duy trì trực tiếp sai lệch vị trí của tay robot trong không gian làm việc bằng không. Lượng đặt của hệ thống điều khiển là vị trí đặt tay trong không gian làm việc và lượng phản hồi là vị trí thực của tay. Khâu tính toán động học ngược sẽ thuộc mạch vòng Cảm biến Động học ngược Bộđiều khiển BBĐ Động cơ Cơ cấu robot Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 37 điều khiển phản hồi. Ưu điểm của phương pháp điều khiển này là tác động trực tiếp các biến không gian làm việc. Nhược điểm là khối lượng tính toán lớn do tồn tại khâu tính toán động học ngược trong mạch vòng điều khiển. Hình 2.6. Sơ đồ khối hệ thống điều khiển ở không gian làm việc 2.3.2 Thiết kế bộ điều khiển pid điều khiển đối tượng Phương trình Lagrange của robot 5 DOF là phương trình vi phân phi tuyến bậc hai, biến điều khiển đầu vào F lực tác động lên từng khớp của robot là chưa biết, (ta giả thiết rằng cơ cấu chấp hành robot được xem là nguồn tạo ra lực và momen tác động). Bài toán đặt ra là điều khiển lực tác động lên từng khớp ra sao để đạt được vị trí cuối mong muốn. Có nhiều kỹ thuật và phương pháp điều khiển, loại đơn giản nhất ta xem xét ở đây là điều khiển khớp nối độc lập như sơ đồ hình 2.7 Mỗi khớp nối của tay máy robot được xem như là hệ thống một vào một ra. Bộ điều khiển Cơ cấu chấp hành (BBĐ+ĐC) Đối tượng robot 5 DOF Thiết bị đo u Gc(s) Gp(s) Cảm biến Bộđiều khiển BBĐ Động cơ Cơ cấu robot Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 38 Hình 2.7. Sơ đồ hệ thống điều khiển vòng kín a. Tổng hợp vòng điều chỉnh động cơ Hệ thống điều chỉnh tốc độ quay cho động cơ một chiều có nhiều vòng phân cấp với một khâu điều chỉnh tốc độ quay w ở vòng ngoài và khâu điều chỉnh dòng phần ứng ai ở vòng trong. Vòng điều chỉnh ai có tác dụng áp nhanh momen quay mt (do từ thông là hằng, momen chỉ phụ thuộc ai , ). Mô hình cơ cấu chấp hành có 3 vòng điều khiển (dòng điện, tốc độ, vị trí) như Hình 2.8 Hình 2.8 Sơ đồ hệ chấp hành có điều khiển Điều chỉnh dòng phần ứng Để thiết kế khâu điều chỉnh dòng, ta sử dụng hàm truyền đạt của mạch điện phần ứng và bỏ qua sức điện động cảm ứng bv . Vòng điều chỉnh dòng có cấu trúc như hình 2.9 Hình 2.9. Vòng điều chỉnh dòng của động cơ một chiều Nếu coi gần đúng thiết bị chỉnh lưu có điều khiển là khâu tỷ lệ với quán tính bậc nhất hàm truyền đạt của mạch phần ứng là Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 39 ( ) ( ) ( )* 1 1 1 1 1 a I a t a a i s G s u s sT R sT = = + + (2.33) Hằng số thời gian của mạch phần ứng và hệ số khuếch đại ta đã biết 1 1 ;aa a a L T T K R R = = = Khâu điều chỉnh dòng chọn là khâu PI, thiết kế theo chuẩn tối ưu module ta có 11 ; 2 2 a a RI a RI t t R TT T T T K T K T = = = = (2.34) Từ đó ta thu được hàm truyền đạt của khâu điều chỉnh dòng kiểu PI như sau ( ) ( ) ( ) * * 1 1 1 1 2 a a RI RI a a RI t a u s L G K i s i s sT T sT æ öæ ö ÷÷ çç ÷÷= = + = +çç ÷÷ çç ÷ ÷- è ø è ø (2.35) Hình 2.10. Sơ đồ khâu điều chỉnh dòng phần ứng Điều chỉnh tốc độ quay Đối với vòng điều chỉnh tốc độ quay, vòng điều chỉnh dòng vừa thiết kế là vòng cấp dưới, có thể được gom lại thành một phần của đối tượng điều khiển với hàm truyền đạt như sau: ( ) ( ) ( )* 2 2 1 1 1 2 2 1 2 a Nt a t t t i s G s i s T s T s T s = = » + + + (2.36) Cùng với hàm truyền đạt của phần cơ, đối tượng điều khiển tổng quát lúc này sẽ có mô hình hàm truyền đạt như sau: Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 40 ( ) ( ) ( )* 1 1 1 2 1 2 1 m N a t M n s K G s i s Js T s T s T ss f p = = = + + (2.37) Với hai tham số của đối tượng là hằng số thời gian cơ MT và hằng số thời gian thay thế của vòng trong Ts ta áp dụng phương pháp thiết kế theo tiêu chuẩn tối ưu đối xứng và thu được các tham số của khâu điều chỉnh 1 2 4 8 ; 2 4 M RN t RN t m T J T T T K T T K s s p f = = = = (2.38) Với các tham số trên ta thu được hàm truyền đạt tổng quát thay thế của vòng điều chỉnh tốc độ quay thiết kế theo tiêu chuẩn tối ưu đối xứng: ( ) ( ) ( )* 2 2 3 3 1 8 1 8 16 16 t NI t t t n s T s G s n s T s T s T s + = = + + + (2.40) Hình 2.11. Cấu trúc hệ thống điều chỉnh tốc độ quay của động cơ một chiều b. Thiết kế bộ điều khiển PID điều khiển đối tượng Phương trình Lagrange của robot 5 DOF là phương trình vi phân phi tuyến bậc hai, biến điều khiển đầu vào F lực tác động lên từng khớp của robot là chưa biết, (ta giả thiết rằng cơ cấu chấp hành robot được xem là nguồn tạo ra momen chủ động). Bài toán đặt ra là điều khiển lực tác động lên từng khớp ra sao để đạt được vị trí cuối mong muốn. Có nhiều kỹ thuật và phương pháp điều khiển, loại đơn giản nhất ta xem xét ở đây là điều khiển khớp nối độc lập. Mỗi khớp nối của tay máy robot được xem như là hệ thống một vào một ra, sự ràng buộc giữa các khớp được coi là thành phần nhiễu. Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 41 ĐK bám vị trí Cơ cấu chấp hành (BBĐ+ĐC) Đối tượng robot 5 DOF u*Động học ngược S Hình 2.12 Sơ đồ điều khiển độc lập một khớp nối Bộ điều khiển PID thử nghiệm biểu diễn như sau:   0 t p i dF K e K e d K e    . Với * là vị trí đặt của khớp được tính toán từ lượng đặt vị trí của tay robot trong không gian làm việc (S) thông qua khâu tính toán động học ngược; e là sai số giữa giá trị đặt và giá trị đầu ra; , ,p i dK K K là hệ số tỷ lệ, tích phân, vi phân của bộ điều khiển PID. Hàm truyền của bộ điều khiển PID viết dưới dạng:   1 1IPID p D p D I K G s K K s K T s s T s            Với ;P DI D I P K K T T K K   là hằng số thời gian tích phân và vi phân. Gắn các động cơ vào robot ta xây dựng được sơ đồ mô phỏng hệ thống điều khiển vị trí các khớp của tay máy bằng PID kinh điển. Hàm truyền đạt của cơ cấu chấp hành bao gồm khâu điều chỉnh dòng, khâu điều chỉnh tốc độ quay. Bảng 2.3 Tham số và giá trị của động cơ DC lựa chọn để mô phỏng Tham số Giá trị Điện trở phần ứng 250aR m= W Điện cảm phần ứng 4aL mH= Momen quán tính 20,012J kgm= Từ thông danh định 0,04 Vsf = Hằng số động cơ 236,8 38,2 e m K K = = Luận văn thạc sĩ kỹ thuật ---------- ĐH Kỹ thuật Công nghiệp HDKH: PGS.TS. Lại Khắc Lãi TH: KS. Ngô Thanh Hải 42 Bảng 2.4 Bảng thông số DH của robot 5 DOF lựa chọn để mô phỏng Khâu Khớp nối i i id ia 1 0-1 1 090 1 0 2 1-2 2 0 0 1 3 2-3 3 0 0 1 4 3-4 4 090 0