Xây dựng cấu trúc thiết lập hệ phương trình động học Robot

BTL môn ROBOTICS 1 Mục lục Chƣơng 1 XÂY DỰNG CẤU TRÚC,THIẾT LẬP HỆ PHƢƠNG TRÌNH ĐỘNG HỌC ROBOT 1.1. Xây dựng cấu trúc robot 1.2. Thiết lập phƣơng trình động học robot Chƣơng 2 BÀI TOÁN ĐỘNG HỌC 2.1. Bài toán động học thuận 2.2. Bài toán động học ngƣợc Chƣơng 3 TÍNH TOÁN TĨNH HỌC 3.1. Tính lực dẫn động tại các khớp đảm bảo cân bằng tĩnh Chƣơng 4 TÍNH TOÁN ĐỘNG LỰC HỌC 4.1 Xây dựng cấu trúc động lực học 4.2 Cơ sở lý thuyết 4.3 Xây dựng bảng tham số động học 4.4 Ma trận jacobi các khâu 4.5 Ma trận khối lƣợng của robot 4.6 Ma trận ly tâm và quán tính coriolits 4.7 Thế năng của robot 4.8 Phƣơng trình vi phân chuyển động của các khâu Chƣơng 5 CHỌN BỘ ĐIỀU KHIỂN Phụ lục Code maple BTL môn ROBOTICS 2 Chƣơng 1 XÂY DỰNG CẤU TRÚC THIẾT LẬP HỆ PHƢƠNG TRÌNH ĐỘNG HỌC ROBOT 1.1 XÂY DỰNG CẤU TRÚC ROBOT 1.1.1 Đặt hệ quy chiếu Hình 1.1 Mô hình robot và hệ trục tọa độ - Hệ trục tọa độ OX0Y0Z0 đặt tại khâu đế, trục OZ0 có hƣớng dọc trục khớp động 1, trục OX0 nằm trong mặt phẳng vuông góc với OZo và có hƣớng từ trên xuống, trục OY0 xác định theo quy tắc bàn tay phải. - Hệ trục tọa độ OX1Y1Z1 tại khớp động 2, trục OZ1 đặt dọc trục khớp động 2, trục OX1 vuông góc với OZ0,OZ1 có hƣớng dọc theo khâu 1, trục OY1 xác định theo quy tắc bàn tay phải. - Hệ trục tọa độ OX2Y2Z2 đặt tại trục khớp động 3, trục OZ2 đặt dọc trục khớp động 3, trục OX2 vuông góc với OZ1 và OZ2 hƣớng từ OZ1 sang OZ2, trục OY2 xác định theo quy tắc bàn tay phải. - Hệ trục tọa độ OX3Y3Z3 đặt tại khâu thao tác, trục OX3 hƣớng theo hƣớng khâu 3. OZ3 song song với trục OZ2, trục OY3 xác định theo quy tắc bàn tay phải. BTL môn ROBOTICS 3 1.1.2 Thiết lập bộ thông số Denavit-Hartenbeg Từ mô hình và hệ trục tọa độ ở trên ta xây dựng đƣợc bảng thông số Danavit- Hartenbeg nhƣ sau : Khâu θi αi ai di 1 θ1 90 0 a1 d1 2 θ2 0 a2 0 3 θ3 0 a3 0 Trong đó: θi là góc quay quanh Zi-1 đển biến Xi-1 thành Xi αi là góc quay quanh Xi để biến Zi-1 thành Zi Các biến khớp là θ1, θ2, θ3, đặt các biến khớp tƣơng ứng là q1,q2,q3. Các ma trận biến đổi tọa độ thuần nhất Denavit-Hartenbeg dựa vào bộ thông số trên :               1000 010 )sin()cos(0)sin( )cos()sin(0)cos( 1 1111 1111 1 0 d qaqq qaqq A (1.1)               1000 0100 )sin(0)cos()sin( )cos(0)sin()cos( 2222 2222 2 1 qaqq qaqq A (1.2)               1000 0100 )sin(0)cos()sin( )cos(0)sin()cos( 3333 3333 3 2 qaqq qaqq A (1.3) Bảng 1.1: Bộ thông số Denavit-Hartenbeg BTL môn ROBOTICS 4 1.2 THIẾT LẬP PHƢƠNG TRÌNH ĐỘNG HỌC ROBOT Phƣơng trình động học robot nhận đƣợc trong dạng ma trận nhƣ sau : )()( 3 0 3 0 tAqA  (1.4) Trong đó                 1000 0 ..)( 1222332323 1121223131231231 1121223131231231 3 2 2 1 1 0 3 0 dSaSaCS SaCSaCSaCSSCS CaCCaCCaSSCCC AAAqA (1.5) Trong đó C1,C2,S1,S2,C23 và S23 lần lƣợt là viết tắt của cos(q1), cos(q2), sin(q1), sin(q2), cos(q2+q3), sin(q2+q3)              1000 ),,(),,(),,( ),,(),,(),,( ),,(),,(),,( )( 333231 232221 131211 3 0 zeccc yeccc xeccc tA    Trong đó cij(α,β,ɳ) là các phần tử trong ma trận Cardan                     )cos()cos()cos(sinsin)sin()cos())sin()cos()cos()sin()cos( )cos()sin()cos(cossin)sin()sin()sin()cos()cos()sin()sin( )sin()sin()cos()cos()cos(    cdR Ta có phƣơng trình dạng ma trận nhƣ sau:             1000 ),,(),,(),,( ),,(),,(),,( ),,(),,(),,( 333231 232221 131211 zeccc yeccc xeccc                    1000 0 1222332323 1121223131231231 1121223131231231 dSaSaCS SaCSaCSaCSSCS CaCCaCCaSSCCC (1.6) BTL môn ROBOTICS 5 Chƣơng 2 BÀI TOÁN ĐỘNG HỌC 2.1 BÀI TOÁN ĐỘNG HỌC THUẬN  Xây dựng quy luật chuyển động, vị trí khâu thao tác và ma trận chỉ hƣớng Chọn thông số chiều dài các khâu nhƣ sau: d1=100 mm, a1=200 mm ,a2=200 mm , a3 = 200mm Và chọn quy luật chuyển động các khâu nhƣ sau:                          1 4 1 3 2 3 2 2 1 2 1 1 8 1 3 3 1 22 4 1 1 2 2 2 tq tq tq ttq ttq ttq    với 0(s)≤t≤5(s) (2.1) Đồ thị sự biến đổi của các biến khớp: Đồ thị q1(t) BTL môn ROBOTICS 6 Đồ thị q2(t) Đồ thị q3(t) BTL môn ROBOTICS 7 Từ phƣơng trình 1.6 ta có : (2.2) Thay các giá trị của biến vào ta có: Hƣớng của bàn kẹp có thể đƣợc xác định từ các góc Cardan, ký hiệu tƣơng ứng là α, β, γ quay lần lƣợt quanh các trục x-y-z.                     )cos()cos()cos(sinsin)sin()cos())sin()cos()cos()sin()cos( )cos()sin()cos(cossin)sin()sin()sin()cos()cos()sin()sin( )sin()sin()cos()cos()cos(    cdR để tính đƣợc các góc α, β, η ta so sánh ma trận chỉ hƣớng của (1.5) và ma trận chỉ hƣớng của (1.6) giải các hệ phƣơng trình ta có :         122233 112122313 112122313 dSaSaz SaCSaCSay CaCCaCCax E E E BTL môn ROBOTICS 8                                                                                                                                                                                                     2 3 , 2 khi arctan ,,,, ,, arctan arctan ,, ,, arctan arctan ,, ,, arctan 2 3 , 2 khi arctan ,,,, ,, arctan arctan ,, ,, arctan arctan ,, ,, arctan 2 231 2 231 23 2 12 2 11 13 231 231 11 21 1 1 31 32 2 231 2 231 23 2 12 2 11 13 231 231 11 21 1 1 31 32                       CSSC S cc c CC SC c c S C c c CSSC S cc c CC SC c c S C c c  Tính vận tốc điểm tác động cuối E, vận tốc góc khâu thao tác Từ phần trên ta đã xây dựng đƣợc quy luật chuyển cũng nhƣ tìm đƣợc tọa độ của khâu thao tác cuối, các biến khớp và đạo hàm các cấp theo t đã biết : Tqqqq ],,[ 321 ],,[ 321 qqqq   T Vận tốc góc của khâu thao tác: A3=        10 3 2 2 1 1 EE rR AAA (2.3) Vận tốc của khâu thao tác chính là đạo hàm vị trí khâu thao tác theo thời gian: VE= r E=  TEEE zyx  ,,                  22232233 11122112123223112313 11122112123223112313 )( )( )( qCaqqCazV qCaqSSqCCaqqSSqCCayV qSaqSCqCSaqqSCqCSaxV EEz EEy EEx    (2.4) Thay (2.1) vào (2.4) ta tìm đƣợc vấn tốc của các khâu thao tác cuối. Vận tốc góc của khâu thao tác:              0 0 0 .~ xy xz yz T EEE RR      BTL môn ROBOTICS 9               023232323 11232311231232312311 11231232311232311231 qSqC qSqCSqSCqSSCCq qCCCqSSqqSCqCS                011 2323121 23231231 CS CSSSC SCSCC               0 )(0 )(0 13121312 1321 1321 SqSqCqCq Sqqq Cqqq    (2.5) Suy ra vận tốc góc khâu thao tác:   TTE qCqqSqq ])()[( 1132132211332                 2 1 2 132 2 132 2 1 1 ) 4 1 cos() 12 11 3()( ) 4 1 sin() 12 11 3()( tq tttCqq tttSqq z y x      (2.6)  Ứng dụng phần mềm Matlab, Maple vẽ quỹ đạo chuyển động của khâu thao tác cuối Quỹ đạo điểm khâu thao tác. Sử dụng phần mềm Maple ta vẽ đƣợc đồ thị quỹ đạo chuyển động của khâu thao tác cuối nhƣ sau : Chuyển động điểm cuối E theo phương X BTL môn ROBOTICS 10 Chuyển động điểm cuối E theo phương Y Chuyển động điểm cuối theo phương Z 2.2 BÀI TOÁN ĐỘNG HỌC NGƢỢC Bài toán động học ngƣợc thông thƣờng cho biết trƣớc vị trí của khâu thao tác yêu cầu tìm giá trị các biến khớp ứng với vị trí đó. Ở tiểu luận này robot 3 bậc tự do kiểu RRR ta không cần biết hƣớng của khâu thao tác mà vẫn có thể tìm đƣợc các góc quay tƣơng ứng. BTL môn ROBOTICS 11 2.2.1 Xây dựng quy luật chuyển động của khâu thao tác cuối Ta chọn quy luật chuyển động bất kì của khâu thao tác E của robot nhƣ sau:         tz ty tx E E E 30100 100 60600 (2.7) 2.2.2 Khảo sát bài toán động học ngƣợc của robot tìm quy luật chuyển động của các khâu Ta có phƣơng trình ma trận (xem 1.4): )()( 3 0 3 0 tAqA  Từ (2.2) kết hợp với (2.7) ta có hệ ba phƣơng trình vị trí :         30100 100 60600 122233 112122313 112122313 tzdSaSa tySaCSaCSa txCaCCaCCa E E E (2.8) Dựa vào cấu tạo hình học của robot ta xác định đƣợc q1 nhƣ sau:                t t X Y q X Y q E E E E 60600 100 arctanarctan )tan( 1 1 Nhân phƣơng trình 1 với C1 và phƣơng trình 2 với S1 ta đƣợc phƣơng trình: 11122233 100)60600( tSCtaCaCa  (2.9) Đặt:      1 111 30100 100)60600( dtP atSCtP y x Kết hợp với phƣơng trình 3 của hệ (2.8) và phƣơng trình (2.9) ta đƣợc hệ phƣơng trình sau:      y x PSaSa PCaCa 22233 22233 (2.10) Bình phƣơng hai vế của hai phƣơng trình (2.10) sau đó cộng hai phƣơng trình lại với nhau ta đƣợc phƣơng trình : BTL môn ROBOTICS 12 22 332 2 3 2 2 22 22323223223232 2 3 2 2 22 22322332 2 3 2 2 2 )(2 )(2 yx yx yx PPCaaaa PPSCSSCSCSSCCCaaaa PPSSCCaaaa    32 2 3 2 2 22 3 2 aa aaPP C yx   (2.11) Mặt khác ta có :  233 1 CS  Vậy ta tính đƣợc q3:           ) 322 32 (1artan 22 2 33 aa aaPP Cq yx (2.12) Từ hệ phƣơng trình (2.10) thay C3 và S3 vào ta có hệ phƣơng trình sau:      y x PaCaSSaC PSSaaCaC )( )( 2332332 3232332 (2.13) Giải hệ phƣơng trình trên ta có nghiệm nhƣ sau:                  323 2 2 2 3 33332 2 323 2 2 2 3 33332 2 2 2 Caaaa SaPCaaP S Caaaa SaPCaaP C xy yx (2.14) Vậy ta tính đƣợc q2:                323 2 2 2 3 33332 323 2 2 2 3 33332 2 22 artan Caaaa SaPCaaP Caaaa SaPCaaP q yxxy (2.15) BTL môn ROBOTICS 13 Chƣơng 3 TÍNH TOÁN TĨNH HỌC 3.1 Tính lực dẫn động tại các khớp đảm bảo cân bằng tĩnh Theo đầu bài ta có các lực tác dụng vào khâu thao tác tại điểm E gồm các vector lực FE3, và mô men M :           00 00 2 r 00 2 r 00 2 r 00r 00ar ]00a[r M T 1 C1 1 T 2 C2 2 T 3 C3 3T 11 1 T 22 2T 33 3T E33 gmP aaa a MMMFFFF ii T zyx T ZYXE                       Tính lực và momen của khâu 3 tác dụng lên khâu 2 tại khớp 3 Hệ phƣơng trình cân bằng dạng mà trận khảo sát trong hệ tọa độ cơ sở :       3 0 3 0 2,3 0 3 0 3, 0 2,3 0 3 0 3, 0 2,3 0 ~~ PrFrMM PFF cE E BTL môn ROBOTICS 14 0 0 ~~ 0 0 33 0 33 0 2,3 0 332,3 0                                                                             gmr F gmF F r Mz My Mx M F gmF F gm F F F F c z y x z y x z y x (3.1) Trong đó :                                23 3 231 3 231 3 3 3 3 0 3 0 233 2313 2313 3 3 3 0 3 0 2 2 2 và S a CS a CC a rRr Sa SSa CCa rRr cc (3.2) Chú ý : 3 2 2 1 1 0 3 0 RRRR  =               02323 1231231 1231231 CS CSSCS SSCCC Thay (3.2) vào (3.1) ta tìm đƣợc 2,3 0 M :                                                                                                                0 0 0 22 2 0 2 22 0 0 0 0 ~~ 0 0 3 231 3 231 3 231 3 23 3 231 3 23 3 3 23132313 2313233 2313233 3 0 3 0 2,3 0 3 0 3, 0 2,3 0 32,3 0 gm CC a CS a CC a S a CS a S a F gmF F CCaSSa CCaSa SSaSa M M M PrFrMM F gmF F gm F F F F z y x z y x cE z y x z y x                                        2313 3 231332313 2313233 233323132333 2,3 32,3 0 2 )( 2 1 )( CgCm a CCgmFyaCFzSaMz CFzSaFxSaMy gSmaCFzSaSgmFyaMx M F gmF F F z y x (3.3) BTL môn ROBOTICS 15  Tính lực và momen của khâu 2 tác dụng lên khâu 1 tại khớp 2 Hệ phƣơng trình cân bằng dạng mà trận khảo sát trong hệ tọa độ cơ sở :       2 0 2 0 1,2 0 2 0 2,3 0 1,2 0 2 0 2,3 0 1,2 0 ~~ PrFrMM PFF c (3.4) Trong đó:                                    0100 0 0 010 0 0 22 12121 12121 22 22 11 11 2 1 1 0 2 0 CS CSSCS SSCCC CS SC CS SC RRR                                2 2 21 2 21 2 2 2 2 0 2 0 22 212 212 2 2 2 0 2 0 2 2 2 và S a CS a CC a rRr Sa CSa CCa rRr cc (3.5) Thay (3.5) và (3.3) vào (3.4) ta đƣợc:                                                   2122213222122313332313323132313 212222313232 2222123222323323132333 1,2 0 231,2 0 2 1 2 1 )( 2 1 )( 2 1 CgCmaCCgmgmFyaCSaFCgCmagmFCgCmaCFyCaCFxSaMz CFzCaFxSaCFzCaFxSaMy gmSaCFzSagmgmFSagmSaCFzSaSgmFaMx M F gmmF F F xy yy z y x  Tính lực và momen của khâu 1 tác dụng lên khâu 0 đế tại khớp 1 Hệ phƣơng trình cân bằng dạng mà trận khảo sát trong hệ tọa độ cơ sở :       1 0 1 0 0,1 0 1 0 1,2 0 0,1 0 1 0 1,2 0 0,1 0 ~~ PrFrMM PFF c (3.7) Trong đó : BTL môn ROBOTICS 16 010 0 0 11 11 1 0            CS SC R                               0 2 2 và 0 1 1 1 1 2 2 2 0 2 0 11 11 1 1 1 00 S a C a rRrSa Ca rRr cc (3.8) Thay (3.8) và (3.6) vào (3.7) ta đƣợc hệ phƣơng trình:                                                                                0 1 2 1 321 0 0 1,2 0 0,1 0 1231230,1 0 Fxd g m mmFyd MM F gmmmF F gm F gmmF F F z y x z y x                                                                    213 2 2123132312310,1 0 212231230,1 0 21 3 2 22312330,1 0 1230,1 0 2 2 2 2 3 33 12232 2 1 3212 2 23 2 3 3 CCgm gm Fya CSFxaCgCm a CFyCaCFxSaMzzM FxdCFzCaFxSaCFzCaFxSaMyyM g m mmFydCFzSa Fygm gm SaCFzSaS a gmFyaMxxM F gmmmF F F z y x (3.9) BTL môn ROBOTICS 17 Chƣơng 4 TÍNH TOÁN ĐỘNG LỰC HỌC 4.1 XÂY DỰNG CẤU TRÚC ĐỘNG HỌC Vì các khâu coi nhƣ thanh đồng chất tiết diện ngang không đáng kể nên ta có trọng tâm mỗi khâu nằm tại trung điểm của nó. 4.2 Cơ sở lý thuyết Động năng của robot có dạng: 1 1 ( ) ( , ) 2 2 T TT q M q q q b q q  Trong đó : ( , )b q q = ( )M q q ,  1... T nb b b Phƣơng trình Lagrange loại II: BTL môn ROBOTICS 18 ( ) ( ) ( ) ( ) T T T T T T d T T dt q q q T T q M q M q q q q d T M q q M q q dt q                                            Sử dụng định lý đạo hàm riêng theo vector tích của hai ma trận ta có: 1 1 ( ) ( ) 2 2 T T T n T q b q b b I q q q q q                 (4.1) Trong đó: 1 1 1 1 . ( ) ... ... . n T T T T T n n n n n n n n b I q b I e e b e I b e I q b I                        1... ( ) ( ) TT T nb b b M q q q M q    (4.2) Mặt khác:                          q q qMIq q qM qqqM q q q b q n TTT           0 ( ) ( )T Tnq M q I q M q   (4.3) Thay (2) và (3) vào (1) ta đƣợc: ( ) ( ) T TT Tq M q M q q q q           , ( ) ( ) T d T q q M q q dt q       (4.4) Tính toán tƣơng tự ta có:   1 1 1 ( ) ( ) 0 2 2 2 1 1 ( ) ( ) ( ) ( ) 2 2 1 ( ) 1 ( ) ( ) 0 ( ) 2 2 T T T T n T T n T T n n T q b b q b b I q q q q q q q M q q q M q q q q I M q q q q M q M q q q I q q I q q                                                 (4.5) Thay (4) và (5) vào phƣơng trình lagrange II ta đƣợc: BTL môn ROBOTICS 19 1 ( ) ( ) ( ) ( ) 2 ( ) ( ) ( ) T T T n n M q M q q M q q q q I q q M q M q I q q                      Đặt : ( ) 1 ( ) ( , ) ( ) ( ) ( , ) 2 T n n M q M q v q q I q q I q C q q q q q               Ta có: 1 ( ) ( , ) ( ) ( ) 2 T n M q v q q M q q q I q q         (4.6) Theo định lý đạo hàm toàn phần và đạo hàm riêng ta có: ( ) ( ) ( )n M q M q I q q     Thay vào (6) ta đƣợc: ( ) 1 ( ) ( , ) ( ) ( ) ( , ) 2 ( ) 1 ( ) ( , ) ( ) ( ) 2 T n n T n n M q M q v q q I q q I q C q q q q q M q M q C q q I q q I q q                         Ma trận ly tâm và coriolis có dạng: ( ) 1 ( ) ( , ) ( ) ( ) 2 T n n M q M q C q q I q q I q q           Khi đó phƣơng trình vi phân chuyển đông của các khâu : ( ) ( , ) T M q q C q q q q         4.3 Xây dựng bảng tham số động lực học Khâu Vị trí trọng tâm Khối lƣợng Ma trận mômen quán tính Cx Cy Cz xxI yyI zzI xyI yzI zxI 1 2 1a 0 0 1m xI1 yI1 zI1 0 0 0 2 2 2a 0 0 2m xI2 yI2 zI2 0 0 0 3 2 3a 0 0 3m xI3 yI3 zI3 0 0 0 Bảng 4.1 Bảng mô tả vị trí trọng tâm khối lượng và mô men quán tính khối của từng khâu BTL môn ROBOTICS 20 4.3 Ma trận Jacobi của các khâu Tạo độ trọng tâm của khâu i trong hệ tọa độ 0 tính nhƣ sau : ci i iici rRrr . 000  Với cir 0 là tọa độ trọng tâm khâu i trong hệ tọa độ i ci i r là tọa độ trọng tâm khâu i trong hệ tọa độ i iR 0 là ma trận quay biến đổi hệ 0 thành hệ i ir 0 là tọa độ của gốc tọa độ i trong hệ tọa độ 0 Ta có các ma trận tọa độ trọng tâm của các khâu nhƣ sau :                                                       1 1 1 1 1 11 11 1 11 11 1 0 )1sin( 2 )cos( 2 0 0 2 010 0 0 )sin(. )cos(. d q a q a a CS SC d qa qa rC (4.7)                                                            12 2 212 1121 22 22 12121 12121 122 11212 11212 2 0 2 2 0 0 2 0 dS a CSl CaCC aa CS CSSCS SSCCC dSa SaCSa CaCCa rC (4.8)                                                               12223 3 11231 3 212 11212231 3 3 2323 1231231 1231231 123322 112313212 112313212 3 0 2 2 2 0 0 2 0 dSaS a SaCS a CSa CaCCqCC a a CS CSSCS SSCCC dSaSa SaCSaCSa CaCCaCCa rC Từ (4.7) (4.8) và (4.9) ta có ma trận Jacobi tịnh tiến của các khâu :                      000 00 2 00 2 1 1 1 1 1 1 C a S a q r J CT  (4.10)                       0 2 1 0 0 2 1 2 1 0 2 1 2 1 22 21211212 21211212 1 2 Ca SSaaCCCa SCaaSCSa q r J CT  (4.11) BTL môn ROBOTICS 21                        23323322 23132313212112313212 23132313212112313212 1 3 2 1 2 1 0 2 1 2 1 2 1 2 1 2 1 2 1 CaCaCa SSaSSaSSaaCCCaCCa SCaSCaSCaaCCSaCSa q r J CT  (4.12) Cũng từ các ma trận Denanvit- Hartenberg ta xác định đƣợc các ma trận cosin chỉ hƣởng của các khâu (xem 1.1, 1.5, 3.5) : Ma trận cosin chỉ hƣớng của khâu 1 :            010 0 0 11 11 1 0 CS SC R Ma trận cosin chỉ hƣớng của khâu 2 :              022 12121 12121 2 0 CS CSSCS SSCCC R Ma trận cosin chỉ hƣớng của khâu 3:              02323 1231231 1231231 3 0 CS CSSCS SSCCC R Toán tử sóng của vector vận tốc góc của khâu 1:                 000 00 00 ~ 1 . 1 11 ` 1 q q RRT    Suy ra vận tốc góc và ma trận Jacobian của khâu 1 :   qJq R T  11 ` 1 00 ~                001 000 000 1 1 q J R   (4.13) Toán tử sóng của vector vận tốc góc của khâu 2:              0 0 0 ~ 1212 121 121 22 ` 2 SqCq Sqq Cqq RRT     BTL môn ROBOTICS 22 Suy ra vận tốc góc và ma trận Jacobian của khâu 2 :   qJqCqSq R T  211212 ` 2 ~                001 00 00 1 1 2 2 C S q J R   (4.14) Toán tử sóng của vector vận tốc góc của khâu 3:               0 )(0 )(0 ~ 13121312 1321 1321 33 ` 3 SqSqCqCq Sqqq Cqqq RRT     Suy ra vận tốc góc và ma trận Jacobian của khâu 3 :   qJqCqqqqS R T  31132321 ` 3 )()( ~                001 0 0 11 11 3 3 CC SS q J R   (4.15) 4.5 Ma trận khối lƣợng suy rộng của robot 3 1 ( )T T Ti Ti Ti Ri i i i Ri i M m J J J A I A J    = 11 12 13 21 22 23 31 32 33 m m m m m m m m m           Trong đó: m12=m13=m31=m21=0 BTL môn ROBOTICS 23 4.6 Ma trận ly tâm và coriolis : ( ) 1 ( ) ( , ) ( * ) ( * ) 2 T n n M q M q C q q I q q I q q           = 11 12 13 21 22 23 31 32 33 c c c c c c c c c                      3 2 1 ).,( c c c qqqC  Sử dụng phần mềm Maple nhân ra ta đƣợc : Trong đó : ii qdiffq  BTL môn ROBOTICS 24 4.7 Thế năng của robot 1 1 2 1 2 2 3 1 2 2 3 23( ) ( )      m gd m g d l S m g d l S l S Từ đó suy ra :               3 2 1 g g g q g Trong đó : 4.8 Phuơng trình vi phân chuyển động của các khâu Thế các biểu thức vào phƣơng trình Lagrange loại hai : ( ) ( , ) ( )M q q C q q q g q    Ta nhận đƣợc hệ phƣơng trình vi phân chuyển động của robot ba khâu trong không gian : Khâu 1 : BTL môn ROBOTICS 25 Khâu 2 : Khâu 3 : Trong đó qdiffq qqd i ii    2 BTL môn ROBOTICS 26 Chƣơng 5 CHỌN BỘ ĐIỀU KHIỂN Nhiêm vụ của bài toán điều khiển là tìm ra quy luật của lực/ mô men do các động cơ điện tạo ra tác dụng lên các khâu để đảm bảo robot chạy đúng theo quy luật qd(t) cho trƣớc, nhằm thực hiện một số nhiệm vụ nào đó. Trên cơ sở chuyển động mong muốn qd(t) đƣợc định nghĩa trƣớc và chuyển động hiện tại của robot đƣợc đo bởi các cảm biến đặt tại khớp, bộ điều khiển có nhiệm vụ đƣa ra các lực/mômen cần thiết. các lực/mômen này tác động làm cho robot thực hiện chuyển động mong muốn một cách ổn định và chính xác. Sơ đồ khối của bộ điều khiển cho robot có dạng nhƣ hình 5.1 d  _ Để có đƣợc luật điều khiển đáp ứng các yêu cầu vừa nêu, thông thƣờng luật điều khiển dựa trên động lực học ngƣợc đƣợc sử dụng. Với luật điều khiển này lực/mô men của các bộ phận dẫn động đƣợc tính nhƣ sau: ( ). ( . ). ( )M q q C q q q g q    (5.1) Giả thiết thành phần mômen trọng lực G(q) đƣợc bù hoàn toàn, sơ đồ hệ thống điều khiển phản hồi với cấu trúc điều khiển PD có dạng đơn giản nhƣ sau: q(t) qd(t) Bộ điều khiển PD Robot Cảm biến Hình 5.1 Sơ đồ điều khiển robot BTL môn ROBOTICS 27 q  q q q Tín hiệu đặt vị trí qd đƣợc so sánh với vị trí thực của khớp q, sai lệch đƣợc đặt vào khâu khuếch đại với hệ số Kp. Tín hiệu ra của khâu tỉ lệ đƣợc cộng đại số với tín hiệu tỉ lệ với tốc độ của khớp và đặt cơ cấu chấp hành của robot: dk p DK K    (5.2) Hay viết với khớp thứ i ta có: dki pi i di iK K    (5.3) Trong đó : dq q   sai số vị trí khớp robot. dq q   sai số tốc độ khớp robot. 1 2( , ,..., )p p p pnK diag K K K là ma trận đƣờng chéo các hệ số khuếch đại của từng khớp hợp riêng. 1 2( , ,..., )d d d dnK diag K K K là ma trận đƣờng chéo các hệ số đạo hàm của từng khớp hợp riêng. Hệ thống điều kiển với cấu trúc bộ điều khiển (3…) ổn định tuyệt đối toàn cục. thật vậy, chọn hàm lyapunov có dạng : 1 ( . . ) 2 T T L pV K q Hq   (5.4) Hàm VL biểu thị tổng năng lƣợng hệ thống robot: thành phần 1 2 T pK  tỉ lệ năng lƣợng đầu vào và thành phần 1 ( . . ) 2 Tq H q là động năng của robot. KP và H là các ma trận hệ số dƣơng; nên hàm VL > 0 với dq q . Tính đạo hàm cấp 1 hàm VL ta đƣợc: Kp Kp RB BTL môn ROBOTICS 28 1 1 1 1 1 2 2 2 2 2 T T T T T L p pV K K q Hq q Hq q Hq        (5.5) Do tính đối xứng của các thành phần T pK  , Tq Hq nên (4.5) đƣợc rút gọn lại nhƣ sau: 1 2 T T T L pV K q Hq q Hq    (5.6) Thay phƣơng trình dạng tổng quát vào phƣơng trình trên với giả thiết không có thành phần mômen trọng lực G(p) ta có : 1 [ ( , ) ] 2 T T T L pV K q Hq q C q q q      (5.7) Sử dụng thuộc tính của phƣơng trình động lực học và áp dụng luật điều khiển (4.1), Phƣơng trình (4.7) đƣợc biến đổi nhhuw sau: 1 1 ( ) ( , ) [ ( ) ( , )] 2 2 T T T T T L p pV q K q H q q q C q q q q K q H q C q q q        (5.8) Do ma trận 1 ( ) ( , ) 2 H q C q q là ma trận đối xứng ngƣợc nên 1 ( ) ( , ) 2 H q C q q = 0 với mọi q nên từ (4.7) ta nhận đƣợc : 0TL pV q K q   Bất đẳng thức trên cho thấy rằng hệ thông ổn định tuyệt đối. mức độ dƣơng của VL phụ thuộc vào Kp và mức độ âm của VL phụ thuộc vào KD. Do đó tăng tốc độ hội tụ bằng cách tăng giá trị ma trận KD.