Header Page of 113 LỜI NÓI ĐẦU CHƯƠNG I GIẢI BÀI TOÁN ĐỘNG HỌC 1.1 Giải toán động học thuận 1.1.1 Cơ sở lý thuyết 1.1.2 Thiết lập phương trình động học thuận cho robot RRR a Tìm ma trận biến đổi b Xác định vị trí hướng bàn kẹp 10 c Xác định vận tốc điểm tác động cuối so với hệ cố định 11 d Vận tốc góc khâu so với hệ cố định 11 e Các đồ thị thể vị trí, vận tốc điểm tác động cuối E 11 1.2 Giải toán động học ngƣợc 1.2.1 Giải toán động học ngược phương pháp giải tích 12 13 a Cơ sở lý thuyết 13 b Áp dụng giải toán ngược cho robot RRR 14 1.2.2 Giải toán ngược phương pháp số 17 a Cơ sở lý thuyết 17 b Áp dụng giải toán cho robot RRR 19 CHƯƠNG II GIẢI BÀI TOÁN TĨNH HỌC ROBOT 21 2.1 Cơ sở lý thuyết 21 2.2 Áp dụng cho Robot RRR 22 CHƯƠNG III THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CHO ROBOT 27 3.1 Giới thiệu sở thiết kế quỹ đạo 27 3.2 Tính toán thiết kế quỹ đạo chuyển động 27 3.2.1 Thiết kế quỹ đạo không gian khớp 27 3.2.2 Thiết kế quỹ đạo không gian làm việc 29 a Quỹ đạo điểm tác động cuối theo đường thẳng từ A đến B tc (s) 30 b Thiết kế quỹ đạo điểm tác động tác động cuối di chuyển theo đường tròn từ A đến B tc  s  lấy AB làm đường kính CHƯƠNG IV ĐỘNG LỰC HỌC ROBOT 32 34 Footer Page of 113 Header Page of 113 4.1 Cơ sở lý thuyết 34 4.2 Áp dụng tìm phƣơng trình vi phân cho Robot RRR 36 4.2.2 Bảng tham số động học 36 4.2.3 Các phương trình vi phân 36 III ĐIỀU KHIỂN ROBOT 37 5.1 Điều khiển phản hồi điều khiển vòng kín 37 5.2 Thiết kế điều khiển PID 38 5.3 Thiết kế điều khiển không gian khớp 40 KẾT LUẬN 44 TÀI LIỆU THAM KHẢO 45 Footer Page of 113 Header Page of 113 LỜI NÓI ĐẦU Khi xét vấn đề Robot đặt toán: động học Robot, động lực học Robot điều khiển Robot Đây bước sở ban đầu quan trọng trước thiết kế Robot Với phát triển vũ bão công nghệ thông tin ngày này, nhiều lĩnh vực khí tận dụng phát triển để tạo bước nhảy vọt, rong có công nghiệp Robot Trên sở đó, môn học Robotics mang lại cho sinh viên kiến thức vô quan trọng cho sinh viên chúng em Bên cạnh đó, tạo hội để sinh viên tiếp cận với phần mềm tính toán, mô phổ biến giới Maple Matlab Để thực tập lớn này, em xin chân thành cảm ơn thầy giáo, PGS.TS Phan Bùi Khôi tận tình, chu đáo dạy học lớp Em xin chân thành cảm ơn thầy Footer Page of 113 Header Page of 113 CHƢƠNG I GIẢI BÀI TOÁN ĐỘNG HỌC 1.1 Giải toán động học thuận 1.1.1 Cơ sở lý thuyết Vị trí khâu không gian xác định vị trí điểm định vị hướng khâu hệ quy chiếu chọn Điểm định vị điểm xác định khâu, thông thường động lực học ta hay lấy khối tâm khâu làm điểm định vị Hướng khâu xác định ma trận cosin hướng tọa độ suy rộng xác địnhvị trí vật rắn quay quanh điểm Động học robot nghiên cứu chuyển động khâu robot phương diện hình học, không quan tâm đến lực momen gây chuyển động.Động học robot toán qua trọng phục vụ tính toán thiết kế robot.Nhiệm vụ chủ yếu toán động học thuận xác định vị trí hương bàn kẹp dạng hàm biến khớp Các phương pháp ma trận 4x4 phương pháp ma trận 3x3 hay sử dụng phân tích động học robot Hai phương pháp ma trận 4x4 phổ biến phương pháp ma trận Denavit-Hartenberg phương pháp ma trận Craig Trong báo cáo chúng em trình bày áp dụng phương pháp ma trận Denavit-Hartenberg để tính toán động học robot Giải toán động học thuận robot công nghiệp phương pháp ma trận DenavitHartenberg Cách xác định trục cuả hệ tọa độ khớp Đối với robot công nghiệp ,Denavit-Hartenberg đưa cách chọn hệ trục tọa độ có gốc khớp thứ i sau: Trục zi 1 chọn dọc theo hướng trục khớp động thứ i Trục xi 1 chọn dọc theo đường vuông góc chung trục zi 2 zi 1 hương từ trục zi 2 sang trục zi 1 Nếu trục zi 1 cắt trục zi 2 hướng trục xi 1 chọn tùy ý miễn vuông góc với trục zi 1 Khi trục zi 2 zi 1 song song với nhau, trục có nhiều đường vuông góc chung , ta chọn trục xi 1 hướng theo pháp tuyến chung Gốc tọa độ Oi 1 chọn giao điểm cuả trục xi 1 trục zi 1 Trục yi 1 chọn cho hệ (Oxyz )i 1 hệ quy chiếu thuận Footer Page of 113 Header Page of 113 Hệ tọa độ (Oxyz )i 1 xác định số tài liệu quy ước hệ tọa độ khớp Chú ý: Với cách chọn hệ tọa độ , hệ tọa độ khớp (Oxyz )i 1 không cách vậy, ta có số bổ sung thích hợp sau Đối với hệ tọa độ (Oxyz )0 theo quy ước ta chọn trục z0 , trục x0 chưa có quy ước trên.Ta chọn trục x0 cách tùy ý, miễn x0 vuông góc với z0 Đối với hệ tọa độ (Oxyz )n , khớp (n+1) nên theo quy ước ta không xác định z n Trục z n không xác định nhất, trục xn lại chọn theo đường pháp tuyến trục zn1 Trong trường hợp này, khớp n khớp quay, ta chọn trục z n song song trục zn 1 Ngòai ta chọn tùy ý cho hợp lý Khi khớp thứ i tịnh tiến, nguyên tắc ta chọn trục zi 1 cách tùy ý Tuy nhiên nhiều trường hợp người ta thường chọn trục zi 1 dọc theo trục cuả khớp tịnh tiến Hình 1.1.diễn thông số Denavit-Hartenberg trục hệ tọa độ Các tham số động học Denavit-Hartenberg Footer Page of 113 Header Page of 113 Vị trí hệ toạ độ khớp (Oxyz)i hệ tọa độ khớp (Oxyz)i-1 tham số Denavit-haartenderg di , 𝜃𝑖 , ai, 𝛼𝑖 sau: d i dịch chuyển tịnh tiến dọc theo trục zi 1 để gốc ' tọa độ Oi 1 chuyển đến Oi giao điểm trục xi trục zi 1 θi : góc quay quanh trục zi 1 để trục xi 1 chuyển đến trục xi' ( xi' // xi ) : dịch chuyển tịnh tiến theo dọc trục xi αi : góc quay quanh trục xi ' để điểm Oi chuyển đến điểm Oi ' ' cho trục zi 1 ( zi 1 // zi 1 ) Chuyển đến trục zi Do hệ trục tọa độ (Oxyz)i 1 gắn liền vào khâu thứ i-1 , hệ trục tọa độ (Oxyz)i gắn liền vào khâu thứ i , vị trí cuả khâu thứ i khâu thứ i-1, xác định bới tham số Denavit-hartenberg Trong tham số trên, tham số αi luôn số , độ lớn chúng phụ thuộc vào hình dáng ghép nối khâu thứ i-1 thứ i Hai tham số lại θi d i số, biến số phụ thuộc vào khớp i khớp quay hay khớp tịnh tiến.Khi khớp i khớp quay θi biến, d i số Khi khớp i khớp tịnh tiến d i biến, θi số Chú ý việc xác định hệ tọa độ khớp khớp tịnh tiến.Trong trường hợp khớp i khớp tịnh tiến, nguyên tắc ta chọn trục zi 1 cách tùy ý, việc xác định tham số Denavit-Hartenberg phụ thuộc vào việc chọn hệ trục tọa độ Ma trận phép biến đổi, kí hiệu H i , tích na trận biến đổi bản,và có dạng sau: cos(θi )  sin(θi )cos(αi ) sin(θi )sin(αi ) cos(θi )   sin(θ ) cos(θ )cos(α )  cos(θ )sin(α ) a sin(θ )  i i i i i i i  (1.1) H   sin(αi ) cos(αi ) di   0   Ma trận H i xác định công thức (1.1) gọi ma trận Denavit-Hartenberg địa phương Phương trình xác định vị trí khâu thao tác (bàn kẹp) cuả robot Xét mô hình học robot n khâu động hình vẽ: Footer Page of 113 Header Page of 113 Hình 1.2 Robot n khâu Theo nguyên tắc nêu trên, ta thiết lập hệ trục tọa độ gắn liền với giá cố định hệ tọa độ gắn liền với vật.Gọi R0 hệ quy chiếu (Oxyz)0 gắn liền với giá cố định, hệ quy chiếu Ri  (Oxyz)i gắn liền với khâu thứ i.Ma trận H i i 1 cho ta biết vị trí hướng khâu i hệ quy chiếu Ri 1 gắn vào khâu thứ i-1 Từ suy ma trận Denavit-Hartenberg H i cho biết vị trí hệ quy chiếu Ri  (Oxyz)i hệ quy chiếu Ri 1  (Oxyz)i 1 Áp dụng liên tiếp phép biến đổi robot n khâu, ta có: Dn  H  H1H H A Dn   Tn 0 r 0 E    (1.2) `(1.3) Ma trận Dn cho biết vị trí điểm tác động cuối E hướng cuả khâu thao tác (bàn kẹp) robot hệ quy chiếu cố định R0 Như biết đặc tính hình học cuả khâu quy luật chuyển động khớp ta xác định vị trí hướng bàn kẹp Xác định vận tốc, gia tốc điểm tác động cuối vận tốc góc, gia tốc góc khâu cuả robot phương pháp trực tiếp (0) Vận tốc gia tốc dài bàn kẹp dễ dàng suy từ đạo hàm vector tọa độ rE Footer Page of 113 Header Page of 113 Vận tốc điểm thao tác:  d  0   dt xE  vEx R0   0   d  0  d  0 0 vE  rE , hay : vEy    yE   dt   0   dt vEz   d  0   zE   dt   0  (1.4) Gia tốc điểm thao tác: aE0  d  0   dt vEx   0  aEx    R0 d  0   0   d  0   v , hay :  aEy   vEy   dt E dt  aEz 0       0   d vEz  dt  (1.5) Ta tính trực tiếp vận tốc góc khâu thứ i robot dựa công thức tính vận tốc góc vật rắn thông qua ma trận cosin hướng sau:   0  A  AT ω i i (1.6)  i i   A T A  ω i i Từ (1.6) suy biểu thức vận tốc góc khâu thứ i robot Áp dụng định nghĩa gia tốc góc vật rắn, biết vận tốc góc khâi robot ta tính gia tốc góc khâu robot theo công thức sau: R0  B α  R0 d dt R0  B ω (1.7) Footer Page of 113 Header Page of 113 1.1.2 Thiết lập phƣơng trình động học thuận cho robot RRR Hình 1.3 Đặt trục tọa độ cho robot RRR Thiết lập bảng động học DH Từ hình vẽ ta tìm bảng động học: d i , θi , , αi Khâu θi di αi q1 a1 π 2 q2 q3 a2 a3 0 a Tìm ma trận biến đổi Dựa vào công thức (1.1) ta thiết lập ma trận Denavit-Hartenberg Hi sau: C1 S H1   0  0 S1 a1C1  C1 a1S1   0   0   C2 S H2   0  0 C3 S3  S C H3   0  0 0 S2 C2 0 a2C2  a2 S2      a3C3  a3 S3      Footer Page of 113 Header Page 10 of 113 Dựa vào (1.2) ta có ma trận Denavit-Hartenberg khâu thao tác khâu sau: C1C23 S C D3   23  S 23   C1S 23  S1S 23 C23 S1 C1 (a3C23  a2C2  a1 )  C1 S1 (a3C23  a2C2  a1 )    a3 S 23  a2 S   (1.8) b Xác định vị trí hướng bàn kẹp Vị trí khâu thao tác (bàn kẹp) xác định hệ tọa độ cố định R0 xác định điểm E (điểm tác động cuối) hướng khâu thao tác Theo (1.8) ta có: C1 (a3C23  a2C2  a1 )  x E   S1 (a3C23  a2C2  a1 )      a3 S23  a2 S2 (1.9) Ma trận cosin hướng (xác định hướng bàn kẹp) xác định từ ma trận T3 sau: C1C23 A   S1C23   S23 C1S23  S1S 23 C23 S1  C1    (1.10) Xác định hướng bàn kẹp: Sử dụng phép quay Roll-Pitch-Yaw phép quay hệ quy chiếu cố định R0 sang hệ quy chiếu Rn ta có ma trận cosin hướng RPY, sau: cos  φ  cos  θ  R   sin  φ  cos  θ     sin  θ  cos  φ  sin  θ  sin  ψ   sin  φ  cos  ψ  cos  φ  sin  θ  cos  ψ   sin  φ  sin  ψ  sin  φ  sin  θ  sin  ψ   cos  φ  cos  ψ  sin  φ  sin  θ  cos  ψ   cos  φ  sin  ψ  cos  θ  sin  ψ  cos  θ  cos  ψ     10 Footer Page 10 of 113 Header Page 31 of 113  Cần thiết kế quỹ đạo để Robot từ điểm (30, 0, 0) đến điểm (-25, -15, 10) 5(s) Dựa vào (3.4) (3.3) ta tìm hệ số sau: x(t )  30   -6.6t  0.88t ; y (t )  0.273x(t)-8.0578; z(t)=-0.182x(t)+5.3719 Sử dụng phương trình vừa tìm được, sử dụng Matlab ta vẽ đồ thị sau: 14 250 fx fy fz 200 fxd fyd fzd 12 10 150 100 50 0 -2 -50 Hình 3.3 Phương trình x, y,z -4 10 10 20 30 40 50 60 70 80 90 100 Hình 3.4 Vận tốc điểm tác động cuối f 0.5 fxdd fydd fzdd 0.4 10 0.3 -10 -20 0.2 -30 0.1 -40 -50 100 300 50 -0.1 200 100 -0.2 0 10 20 30 40 50 60 70 80 90 Hình 3.5 Phương trình x, y,z 100 -50 -100 Hình 3.6 Quỹ đạo chuyển động 31 Footer Page 31 of 113 Header Page 32 of 113 b Thiết kế quỹ đạo điểm tác động tác động cuối di chuyển theo đường tròn từ A đến B tc  s  lấy AB làm đường kính Ta có phương trình đường tròn không gian làm mặt phẳng hai điểm A  x0 , y0 ,  , B  xc , yc ,  lấy xi  AB xc  x0 y  y0 , yi  c ,z  0,R  2 làm đường kính  xc  xo    yc  y0   x  xi    y  yi  2  R với  x  xi  R sin(a(t ))   y  yi  R cos(a(t )) Viết dạng tham số sau  z  (3.5) để thỏa mãn điều kiện vận tốc a(t) ta thiết kế phải bậc a  t   ao  a1t  a2t  a3t  s    A  x0 , y0   v    v0   Và phải thỏa mãn điều kiện:  s  tc   B  xc , yc  v t  v    c c Từ ta tìm hệ số a0 , a1 , a2 , a3 sau x x a0  arcsin  i  R Đặt :  x  xi   x0  xi  w  arcsin  c   arcsin    R   R   a2     , a1   (3.6) 3w 2w , a  tc2 tc3 Giả sử cần tạo quỹ đạo để Robot chuyển động từ điểm A(0.7, 0) đến B(0.7, 0) Dựa vào (3.5) (3.6) ta tìm phương trình sau: a (t )  1.570796326794897+0t+ 2.356194490192345t  0.785398163397448t  x   0.7 sin(a(t ))    y   0.7 cos(a (t )) z   Sử dụng phương trình vừa tìm được, sử dụng Matlab ta vẽ đồ thị sau: 32 Footer Page 32 of 113 Header Page 33 of 113 Do thi quy dao chuyen dong diem tac dong cuoi 0.8 fx fy 0.6 f Dich chuyen (m) 0.4 0.5 0.2 0 -0.2 -0.5 -0.4 -1 0.5 -0.6 0.5 -0.8 -0.5 0.2 0.4 0.6 0.8 1.2 Thoi gian (s) 1.4 1.6 1.8 -0.5 -1 -1 Hình 3.7 Quỹ đạo chuyển động điểm cuối Do thi van toc diem tac dong cuoi Do thi gia toc diem tac dong cuoi 0.15 0.025 fxd fyd 0.1 fxdd fydd 0.02 0.015 0.05 Gia toc (m/s 2) Van toc (m/s) 0.01 -0.05 0.005 -0.005 -0.1 -0.01 -0.15 -0.2 -0.015 20 40 60 80 100 120 Thoi gian (s) 140 160 180 200 Hình 3.8 Vận tốc điểm tác động cuối -0.02 20 40 60 80 100 120 Thoi gian (s) 140 160 180 200 Hình 3.9 Gia tốc điểm tác động cuối 33 Footer Page 33 of 113 Header Page 34 of 113 CHƢƠNG IV ĐỘNG LỰC HỌC ROBOT 4.1 Cơ sở lý thuyết Trong tính toán động học robot, để xác định vị trí khâu ta cần sử dụng hệ tọa độ cố định hệ tọa độ khớp Trong toán động lực học robot ta cần thêm hệ tọa độ hệ tọa độ khâu Hệ tọa độ khâu hệ quy chiếu gắn với vật rắn, thường có gốc trùng với khối tâm Ci vật rắn, trục hướng theo trục quán tính vật rắn Trong hình (4.1) hệ Oi xi yi zi hệ tọa độ khớp, Ci iii hệ tọa độ khâu Hình 4.1 Hệ tọa độ khâu Giả sử robot hệ holonom có p vật rắn r liên kết Khi số bậc tự hệ n = 6p – r Kí hiệu biến khớp hệ q   q1 qn  T Vị trí khâu thứ i xác định bởi: Tọa độ khối tâm khâu: rCi  rCi  q, t  Ma trận cosin hướng khâu: Ai  Ai  q, t  Xét robot có liên kết hôlônôm giữ dừng, đó: rCi  rCi  q  , Ai  Ai  q  (4.1) Theo định nghĩa ma trận Jacobi tịnh tiến ma trận Jacobi quay xác định công thức: JTi  rCi q J Ri  ωi φCi  q q (4.2) 34 Footer Page 34 of 113 Header Page 35 of 113  Trong i vectơ đại số ứng với góc quay  i vật rắn thứ i, quay quanh trục quay tức thời Vận tốc khối tâm vận tốc góc vật rắn xác định công thức sau: vCi  drCi rCi  q  JTiq dt q (4.3) ωCi  dφCi φCi  q  J Ri  q  q dt q (4.4) Ta có phương trình vi phân chuyển động Robot dạng ma trận sau:   Cq, q  q  g q   τ  t  M q  q Trong đó: p p M  q    mi J JTi   JTRi I i J Ri T Ti i 1 (4.5) i 1 ma trận vuông cấp n, gọi ma trận khối lượng suy rộng robot Thế trọng lực robot Thế trọng lực khâu rôbot xác định biểu thức: i  mi gT0 rCi (4.6) T  π  g (q)     q  (4.7) Trong đó: g T vecto gia tốc trọng trường, xét trường hợp z0 trục thẳng đứng, hướng lên, ta có: g   T  g  ; rCi   xCi yCi zCi  tọa độ khối tâm thứ i T Ta định nghĩa: m jk  m m hijk   ij  ik   qk q j qi   , (số hạng có tên Christoffel symbols  dạng thứ nhất) Ta đưa vào kí hiệu: n cij  q, q    hijk  q  qk (4.8) k 1 35 Footer Page 35 of 113 Header Page 36 of 113 Suy ra: C  cij    nn ma trận li tâm – Coriolis   q đại diện cho lực quán tính ly tâm quán tính Coriolis tác dụng Thành phần C  q, q lên robot 4.2 Áp dụng tìm phƣơng trình vi phân cho Robot RRR 4.2.2 Bảng tham số động học Khâu Vị trí trọng tâm 𝑥𝑐 𝑦𝑐 𝑧𝑐 0 lC1 0 lC 0 lC Khối lượng 𝑚1 𝑚2 𝑚3 𝐼𝑥𝑥 0 Ma trận mômen quán tính 𝐼𝑦𝑦 𝐼𝑧𝑧 𝐼𝑥𝑦 𝐼𝑦𝑧 𝐼𝑧𝑥 0 𝐼1𝑦 𝐼1𝑧 0 𝐼2𝑦 𝐼2𝑧 0 𝐼3𝑦 𝐼3𝑧 4.2.3 Các phƣơng trình vi phân 36 Footer Page 36 of 113 Header Page 37 of 113 III ĐIỀU KHIỂN ROBOT 5.1 Điều khiển phản hồi điều khiển vòng kín Khi xét toán điều khiển cho tay máy công nghiệp đó, trước hết phải mô hình hóa tay máy cấu – đối tượng điều khiển, cảm biến (sensor) đặt khớp để giám sát trạng thái khớp cấu dẫn động (Actuator) gá khớp để sinh lực (moment) dẫn động khâu Chúng ta muốn điều khiển khớp robot bám theo quỹ đạo thiết kế Các phần tử dẫn động làm việc cách nhận lệnh sinh lực (mô men) Do phải sử dụng hệ điều khiển để tính toán lệnh phù hợp cho phần tử cho chúng thực quy luật chuyển động mong muốn Hình … mô tả quan hệ quỹ đạo thiết kế kết chuyển động theo quỹ đạo mong muốn  d (t ) , quy luật gia tốc Từ giá trị ban đầu quy luật q d (t ) , quy luật vận tốc q d (t ) mong muốn, điều khiển có nhiệm vụ tính toán vector lực τ , nguồn động lực để q hệ robot chuyển động với quy luật q q Nhờ sensor điều khiển đọc giá trị theo thời gian thực Chú ý toàn đường tín hiệu hình… gắn với vector n chiều với n số khớp robot Có thể nhận thấy rằng, để tính toán mô men τ khả khả quan sử dụng  d (t ) q d (t ) tính toán phần phương trình động lực học, với đầu vào q d (t ) , q thiết kế quỹ đạo Do vậy, từ kết phần tính toán động lực học, τ xác định theo công thức:   C(q, q ).q  G(q)  M(q).q   V(q, q )  G(q) τ  M(q).q (5.1) Trong đó: M(q) ma trận khối lượng suy rộng vuông cấp n V(q, q ) vector n chiều thành phần Coriolis li tâm G(q) vector n chiều thành phần lực suy rộng lực không τ tính theo công thức (5.1) giúp hệ chuyển động theo quỹ đạo mong muốn, mô hình động lực hoàn toàn đúng, hệ không chịu tác động nhiễu Nhìn chung cách để xây dựng hệ điều khiển có hiệu cao phải sử dụng 37 Footer Page 37 of 113 Header Page 38 of 113 tín hiệu phản hồi hình 5.1 Theo đó, tín hiệu phản hồi sử dụng để tính toán sai lệch vị trí e sai lệch vận tốc e e = q d  q, e = q d  q (5.2) Đối với toán điều khiển tay máy, xuất phát từ phương trình động lực học (5.1) Với cách tiếp cận điều khiển chia tách, trường hợp ta có: τ  ατ   β (5.3) với τ vector n thành phàn mô men dẫn động khớp Chúng ta chọn: α  M (q) β  V (q, q )  G (q) (5.4) Với luật điều khiển phản hồi vòng kín sau:  d  K v E  K p E τ  q (5.5) Trong đó: E  qd  q (5.6) 5.2 Thiết kế điều khiển PID Hệ thống điều khiển nêu thiết kế theo luật điều khiển PID Khi thiết kế hệ thống điều khiển ta bỏ qua động học cấu chấp hành, quán tính động Như chức điều khiển tạo moomen cần thiết để truyền động khớp robot đảm bảo khớp robot bám theo vị trí đặt Sơ đồ hệ thống điều khiển: 38 Footer Page 38 of 113 Header Page 39 of 113 Hình 5.1: Sơ đồ cấu trúc hệ thông điều khiển robot với điều khiển PID Hầu hết điều khiển robot công nghiệp có cấu trúc, mà chúng mô tả: αI β0 (5.7) Trong I ma trận đơn vị cấp n Luật điều khiển là:   K E  K Edt  d  K v E τ  q p i (5.8) Trong K v , K p K i ma trận đường chéo tham số số Với luật điều khiển giả thiết thành phần momen trọng lực G(p) bù hoàn toàn Hệ thống điều khiển với cấu trúc điều khiển trên, ổn định tuyệt đối toàn cục Thực chọn hàm Liapunov có dạng sau: 1 V  eT Me  eT K Pe 2 (5.9) Vì ma trận M(q) đối xứng xác định dương , ma trận KP chọn ma trận đối  #0 xứng, xác định dương nên V>0 với e, q Đạo hàm theo thời gian ta được:    q T Mq   q T Mq   eT K Pq V   Mq  τ  (Cq  g (q)) 39 Footer Page 39 of 113 Header Page 40 of 113   q T  K e  K q   q T Cq  q T Mq    eT K q V P D P   2C  q  q T K q   q T K Dq  q T  M D Căn vào biểu thức ta thấy hàm V giảm khi q=qd mong muốn Hàm V =0 (5.10) q #0 robot đạt đến vị trí q =0 e=const Ta có :   C(q, q )q  g(q)  K Pe  K D  K i  Edt  g (q) M(q)q (5.11) Tác động việc tăng thông số độc lập Thông số Thời gian khởi động (RISE TIME) Quá độ OVERSHOOT Thời gian xác lập SETTLING TIME Giảm Tăng Thay đổi nhỏ Giảm Giảm cấp Tăng Tăng Giảm đáng kể Giảm cấp Giảm Về lý thuyết không tác động Giảm Giảm Giảm Sai số ổn định Độ ổn định Cải thiện K d nhỏ 5.3 Thiết kế điều khiển không gian khớp Giả sử bàn kẹp E vị trí (xE,yE,zE) ứng với tọa độ khớp q i (i=1…n), nhiệm vụ điều khiển phải đưa điện áp đặt vào động để bàn d kẹp di chuyển đến hay gần đến vị trí mong muốn rE với sai lệch cho phép, tương ứng d d vị trí mong muốn bàn kẹp rE tọa độ khớp q i Sau xét luật điều khiển đơn giản đáp ứng yêu cầu trên, luật điều khiển PID với bù trọng lực Lực điều khiển tính từ vị trí mong muốn (ứng với 40 Footer Page 40 of 113 Header Page 41 of 113 d tọa độ biến khớp q i ) trạng thái chuyển động robot thời điểm mà thu thập từ sensor đặt khớp Sau tìm phương trình vi phân chuyển động, ta thiết kế điều khiển PID có bù trọng lượng Simulink sau: Hình 5.2 Thiết kế sơ đồ khối Simulink Trong bọ điều khiển PID thiết kế sau: Hình 5.3 Thiết kế điều khiển PID Sau hiệu chỉnh thông số, ta thu kết sau: 41 Footer Page 41 of 113 Header Page 42 of 113 Goc quay dat va thuc 3.5 q1 q2 q3 2.5 Goc quay (rad) 1.5 0.5 -0.5 -1 Thoi gian (s) 10 Hình 5.4 Đồ thị biến khớp đặt thực sau điều khiển Van toc goc dat va thuc 20 qd1 qd2 qd3 15 Van toc goc (rad/s) 10 -5 -10 -15 -20 -25 Thoi gian (s) 10 Hình 5.5 Đồ thị vận tốc biến khớp đặt thực sau điều khiển 42 Footer Page 42 of 113 Header Page 43 of 113 Do thi sai so goc khop 2.5 1.5 0.5 -0.5 -1 10 Hình 5.6 Đồ thị sai số góc khớp Do thi sai so van toc goc khop 20 15 10 -5 -10 -15 -20 -25 10 Hình 5.6 Đồ thị sai số vận tốc góc khớp Nhận xét: Nhìn vào đồ thị ta có thấy răng, nhờ điều khiển mà tín hiệu bám tốt so với tín hiệu vào Tuy nhiên, sai số điều chỉnh Đặc biệt, góc khớp sai số lớn vận tốc góc bám tốt 43 Footer Page 43 of 113 Header Page 44 of 113 KẾT LUẬN Bài tập lớn tìm kết sau: Vị trí điểm thao tác E hướng khâu thao tác Tìm tọa độ suy rộng biết trước phương trình di chuyển khâu thao tác Thiết kế quỹ đạo không gian khớp không gian làm việc Tìm phương trình vi phân chuyển động Robot Sử dụng điều khiển PID để điều khiển không gian khớp Tuy nhiên, nhiều khía cạnh mà tập lớn bỏ qua thời gian trình độ chưa cho phép Các thông số điều khiển phải điều chỉnh lại để tham số tối ưu Cuối cùng, em xin chân thành cảm ơn thầy cô trường tạo môi trường học tập tốt cho sinh viên, thầy cô hướng dẫn, truyền đạt cho sinh viên Chân thành cảm ơn! 44 Footer Page 44 of 113 Header Page 45 of 113 TÀI LIỆU THAM KHẢO Phan Bùi Khôi, Bài giảng Robotics Trường Đại học Bách khoa Hà Nội, 2009 Nguyễn Thiện Phúc, Robot công nghiệp NXB Khoa học Kỹ thuật, 2002 Nguyễn Văn Khang, Chu Anh Mỳ, Cơ sở Robot công nghiệp NXB Giáo dục Việt Nam, 2011 Lung Wen Tsai, Robot Analysis, The Machenics of Serial And Parallel Manipulator, NewYork, 1999 Nguyễn Doãn Phước, Lý thuyết điều khiển tuyến tính NXB Khao học Kỹ thuật, Hà Nội, 2009 Nguyễn Phùng Quang, Nguyễn Phùng Quang, Matlab & Simulink dành cho kỹ sư điều khiển tự động, NXB Khoa học kỹ thuật, 2003 45 Footer Page 45 of 113 ... vi phân cho Robot RRR 36 4.2.2 Bảng tham số động học 36 4.2.3 Các phương trình vi phân 36 III ĐIỀU KHIỂN ROBOT 37 5.1 Điều khiển phản hồi điều khiển vòng kín 37 5.2 Thiết kế điều khiển PID 38... kế điều khiển không gian khớp 40 KẾT LUẬN 44 TÀI LIỆU THAM KHẢO 45 Footer Page of 113 Header Page of 113 LỜI NÓI ĐẦU Khi xét vấn đề Robot đặt toán: động học Robot, động lực học Robot điều khiển. .. Denavit-Hartenberg để tính toán động học robot Giải toán động học thuận robot công nghiệp phương pháp ma trận DenavitHartenberg Cách xác định trục cuả hệ tọa độ khớp Đối với robot công nghiệp ,Denavit-Hartenberg