Đề tài Nâng cao chất lượng điều khiển robot Scara

khớp. , là tốc độ đặt và tốc độ thực của khớp. Tính Robot t U e - - Hình 1.1: Sơ đồ cấu trúc hệ điều khiển động lực học ngược. Và phương trình sai số tương ứng sẽ là: (1.4) Các hệ số KD, Kp, KI được chọn theo điều kiện ổn định của Lyapunov để sai số giữa quỹ đạo chuyển động chuẩn và quỹ đạo chuyển động thực hội tụ tại điểm 0 không phụ thuộc vào điều kiện ban đầu. Ưu điểm của phương pháp này là khử được tính phi tuyến và sự ràng buộc trong phương trình động lực học. Nhược điểm của nó là phải biết được đầy đủ chính xác các thông số cũng như đặc tính động lực học robot, đồng thời cũng phát sinh tính toán phụ. Thuật toán tính toán điều khiển U sẽ liên quan các phép tính lượng giác nên phải thực hiện một số phép nhân ma trận vectơ và ma trận phụ. Thời gian tính toán lớn là một yếu tố ảnh hưởng đến sự hạn chế của phương pháp này. 1.2.2. Phương pháp điều khiển phản hồi phân ly phi tuyến Phương pháp này được xây dựng trên cơ sở lý thuyết của điều khiển phân ly cho hệ thống phi tuyến bằng phản hồi tuyến tính hoá tín hiệu ra. Từ phương trình động lực học: (1.5) Ma trận H không đơn nhất nên ta có thể viết lại như sau: (1.6) Phương trình này gồm các phương trình vi phân cấp hai cho mỗi biến, vì lẽ đó qua hai lần vi phân phương trình đầu ra thì hệ số của tín hiệu U sẽ khác 0. Lúc này tín hiệu U sẽ xuất hiện trong phương trình đầu ra: (1.7) Với: Tín hiệu của bộ điều khiển được chọn sao cho đảm bảo hệ thống phân ly là: (1.8) Trong đó: ,. Từ phương trình (1.8) ta nhận thấy tín hiệu điều khiển cho khớp i chỉ phụ thuộc vào các biến động lực học và tín hiệu vào E(t). Thay từ phương trình (1.8) vào phương trình (1.5) ta được: (1.9) Hay: (1.10) Phương trình (1.10) biểu thị vào ra phân ly của hệ thống. Các hệ số được chọn theo tiêu chuẩn ổn định. 1.2.3. Phương pháp điều khiển thích nghi theo sai lệch Hình 1.2: Hệ thống điều khiển thích nghi theo sai lệch. Đối tượng điều khiển Quỹ đạo chuyển động chuẩn Phương trình Neuton-Euler xm(t) + - Bộ điều khiển tối ưu một nấc + + x(t) Hệ thống nhận dạng bình phương tối thiểu đệ quy Dựa trên cơ sở lý thuyết sai lệch, Lee và Chung đã đề xuất thuật toán điều khiển đảm bảo robot luôn bám quỹ đạo chuyển động đặt trước với phạm vi chuyển động rộng và tải thay đổi rộng. Phương pháp điều khiển thích nghi theo sai lệch được xây dựng trên cơ sở phương trình sai lệch tuyến tính hoá lân cận quỹ đạo chuyển động chuẩn. Hệ thống điều khiển gồm hai khối: Khối tiền định (truyền thẳng - feedforward) và khối phản hồi (feedback) như Hình 1.2. Khối tiền định tính toán mô men của robot ứng với quỹ đạo chuyển động chuẩn theo phương trình Newton-Euler. Khối phản hồi thực hiện tính toán thành phần mômen sai lệch theo luật tối ưu một nấc nhằm bù sai lệch vị trí và tốc độ của khớp dọc theo quỹ đạo chuyển động chuẩn. Khối đánh giá tham số thực hiện theo sơ đồ nhận dạng bình phương tối thiểu thời gian thực đệ quy các tham số và hệ số phản hồi của hệ tuyến tính hoá được cập nhật và chỉnh định ở mỗi chu kỳ mẫu. Mômen tổng đặt lên cơ cấu chấp hành sẽ gồm hai thành phần: mômen danh định được tính theo phương trình Newton-Euler từ khối tiền định và mômen bù sai lệch sẽ được tính bởi khối phản hồi thực hiện theo luật tối ưu một cấp. Phương pháp điều khiển thích nghi theo sai lệch có hai ưu điểm cơ bản: Nó cho phép chuyển từ vấn đề điều khiển phi tuyến về điều khiển tuyến tính quanh quỹ đạo chuẩn. Việc tính toán mômen danh định cũng như mômen sai lệch được thực hiện độc lập và đồng thời. Tuy nhiên phương pháp này gặp khó khăn do khối lượng tính toán quá lớn và do đó thời gian tác động sẽ chậm, khó tối ưu trong việc điều khiển robot. 1.2.4. Phương pháp điều khiển thích nghi theo mô hình mẫu Cơ cấu điều chỉnh Đối tượng điều khiển Mô hình chuẩn Tín hiệu đặt Bộ điều chỉnh e + - Đầu ra y ym Hình 1.3: Hệ thống điều khiển thích nghi theo mô hình chuẩn. Trong số các phương pháp điều khiển thích nghi (điều khiển thích nghi thông qua điều chỉnh hệ số khuếch đại, điều khiển thích nghi tự chỉnh, điều khiển thích nghi theo mô hình chuẩn) thì phương pháp điều khiển thích nghi theo mô hình chuẩn (Model Reference Adaptive Control - MRAC) được sử dụng rộng rãi nhất và tương đối dễ thực hiện. Nguyên lý cơ bản của điều khiển thích nghi theo mô hình chuẩn dựa trên sự lựa chọn thích hợp mô hình chuẩn và thuật toán thích nghi. Thuật toán thích nghi được tính toán dựa trên tín hiệu vào là sai lệch giữa đầu ra của hệ thống thực và mô hình chuẩn từ đó đưa ra điều chỉnh hệ số khuếch đại phản hồi sao cho sai lệch đó là nhỏ nhất. Sơ đồ khối chung của hệ thống điều khiển thích nghi theo mô hình chuẩn được trình bày trên Hình 1.3. Phương pháp điều khiển thích nghi theo mô hình chuẩn có một số ưu điểm quan trọng là nó không bao gồm mô hình toán học phức tạp và không phụ thuộc vào tham số môi trường... Tuy nhiên, phương pháp này chỉ thực hiện được cho mô hình đơn giản tuyến tính với giả thiết bỏ qua sự liên hệ động lực học giữa các khớp của robot. Hơn nữa sự ổn định của hệ thống kín cũng là một vấn đề khó giải quyết với tính phi tuyến cao của mô hình động lực học robot. 1.2.5. Phương pháp điều khiển động lực học ngược thích nghi Đối tượng điều khiển Cơ cấu điều khiển Cơ cấu thích nghi Nhận dạng x a b u y Hình 1.4: Sơ đồ khối tổng quát hệ thích nghi. Là phương pháp tổng hợp các kỹ thuật nhằm tự động chỉnh định các bộ điều chỉnh trong mạch điều khiển nhằm thực hiện hay duy trì ở một mức độ nhất định chất lượng của hệ khi thông số của quá trình được điều khiển không biết trước hoặc thay đổi theo thời gian. Việc phân tích các hệ thống điều khiển có chất lượng cao luôn là vấn đề trọng tâm trong quá trình phát triển của lý thuyết điều khiển tự động nói chung và vấn đề nâng cao chất lượng hệ thống điều khiển bám chính xác quỹ đạo chuyển động của robot nói riêng. Tùy thuộc vào các tiêu chuẩn phân loại mà có các hệ điều khiển thích nghi khác nhau: Hệ có tín hiệu tìm hay không có tín hiệu tìm; hệ điều khiển trực tiếp hay gián tiếp; hệ cực trị hay hệ giải tích; hệ có mô hình mẫu hay hệ không có mô hình mẫu; hệ tự chỉnh hay hệ tự tổ chức vv... đang được phát triển và và áp dụng để tổng hợp các hệ thống điều khiển quỹ đạo với chỉ tiêu chất lượng cao. Phương pháp tổng quát hóa các hệ thích nghi có ý nghĩa rất lớn trong việc bao quát một số lượng lớn các bài toán thích nghi, đơn giản được việc tìm hiểu nguyên lý cơ bản của ngay cả các hệ phức tạp, trên cơ sở đó xây dựng các bài toán mới, các thiết bị cụ thể mới. Vấn đề điều khiển bám chính xác quỹ đạo robot là một vấn đề luôn nhận được sự quan tâm chú ý. Hiện nay sự phát triển mạnh mẽ của kỹ thuật về phần cứng và phần mềm đã cho phép giảm thời gian tính toán, điều đó dẫn tới những động lực cho việc thúc đẩy sự phát triển của các hệ thống điều khiển quỹ đạo thích nghi cho robot. 1.2.6. Phương pháp điều khiển trượt Điều khiển chuyển động bất biến với nhiễu loạn và sự thay đổi thông số có thể sử dụng điều khiển ở chế độ trượt. Điều khiển kiểu trượt thuộc về lớp các hệ thống có cấu trúc thay đổi (Variable Structure System - VSS) với mạch vòng hồi tiếp không liên tục. Phương pháp điều khiển kiểu trượt có đặc điểm là tính bền vững rất cao do vậy việc thiết kế bộ điều khiển có thể được thực hiện mà không cần biết chính xác tất cả các thông số. Chỉ một số các thông số cơ bản hoặc miền giới hạn của chúng là đủ cho việc thiết kế một bộ điều khiển trượt (Variable Structure Controller - VSC). CHƯƠNG II MÔ TẢ ĐỐI TƯỢNG - ROBOT SCARA SERPENT 2.1. Một số loại robot Scara của các hãng sản xuất Robot Scara là một trong những robot phổ biến nhất trong công nghiệp. Chuyển động của robot này rất đơn giản nhưng lại phù hợp với các dây chuyền và ứng dụng hữu hiệu trong nhiệm vụ nhặt và đặt sản phẩm. Robot Scara (Selectively Compliant Articulated Robot Arm) có nghĩa là tay máy lắp ráp chọn lọc. Cấu trúc động học loại tay máy này thuộc hệ phỏng sinh, có các trục quay, các khớp đều là thẳng đứng. Nó có cấu tạo hai khớp ở cánh tay, một khớp ở cổ tay và một khớp tịnh tiến. Các khớp quay hoạt động nhờ động cơ điện có phản hồi vị trí. Khớp tịnh tiến hoạt động nhờ xi-lanh khí nén, trục vít hoặc thanh răng. Một số loại robot Scara của các hãng sản xuất: Hình 2.1a: Turbo Scara SR60 của hãng Bosch. Hình 2.1b: Assembly Scara Robot của Hirata. Hình 2.1c: Scara Robot của EPSON. Hình 2.1d: Scara Robot của DENSO. Hình 2.1e: Scara Robot of ADEPT. Hình 2.1f: Scara Robot of RANOME. Hình 2.1g: Scara Robot of KUKA. Hình 2.1h: Scara Robot of STAUBLI. Hình 2.2: Robot Scara Serpent của FEEDBACK. Trong phần này luận văn sẽ xây dựng mô tả toán học đối tượng robot Scara Serpent của FEEDBACK. 2.2. Các thông số và vùng làm việc của robot Scara Serpent Do chuyển động của robot Scara đơn giản, dễ dàng nên nó được sử dụng khá phổ biến trong công nghiệp. Ở đây nghiên cứu robot Scara Serpent (Hình 2.3) một loại cơ bản trong nhóm robot công nghiệp này. Hình 2.3: Robot Scara Serpent. Chiều cao của Robot có thể thay đổi dễ dàng bằng cách thay đổi vị trí gá thân robot trên trục cơ bản, giúp tay máy thuận lợi trong việc thay đổi công việc. Với thiết kế động cơ truyền động cho cổ tay được đặt trên trục cơ bản và liên hệ với cổ tay bằng đai truyền, nên nó đảm bảo được góc quay của cổ tay không thay đổi trong quá trình tay máy chuyển động. Truyền động cho 2 khớp của tay máy và cổ tay bằng động cơ servo một chiều có phản hồi vị trí tạo thành một vòng điều khiển kín. Chuyển động thẳng đứng được thực hiện bằng piton khí nén. Robot Scara Serpent có thể được lập trình từ máy tính bằng cách đặt dữ liệu cho mỗi trục. Hoặc điều khiển bằng tay sử dụng thiết bị lái điện (steering) cho tay máy dùng các cuộn dây điện từ trong giá treo (pendant). 2.2.1. Cấu tạo tay máy robot Scara Serpent 2.2.1.1. Cấu hình của robot Scara Serpent Bao gồm một chuỗi các thanh cứng được liên kết với nhau bởi các khớp: Hình 2.4: Cấu hình và các hệ trục tọa độ gắn trên Robot Scara Serpent. Robot Scara Serpent gồm 3 khớp chuyển động quay và một khớp chuyển động tịnh tiến. Gắn cho mỗi thanh nối một hệ trục toạ độ, ta có: Khớp 1 quay quanh trục z0 góc θ1. Khớp 2 quay quanh trục z1 góc θ2. Khớp 3 chuyển động tịnh tiến theo trục z2 đoạn d3. Khớp 4 quay quanh trục z3 góc θ4. 2.2.1.2. Các thông số kỹ thuật của robot Scara Serpent Thông số của động cơ 1, 2, 3 tương ứng với các khớp 1, 2, 4 của tay máy robot Scara Serpent (xem bảng 2.1). Động cơ 1 truyền động cho khớp 1 (main). Động cơ 2 truyền động cho khớp 2 (fore). Động cơ 3 truyền động cho khớp 4 - khớp cổ tay (wrist). Bảng 2.1: Thông số các động cơ của robot. TT Loại U(V) I(A) M(Nm) N(v/p) P(W) J(Kg.m2) R(W) L(mH) m(Kg) 1 J9ZF 12 4,8 4.10-2 2100 15 0,32.104 1,38 100 0,6 2 J9ZF 12 4,8 4.10-2 2100 15 0,32.104 1,38 100 0,6 3 J12ZF 12 4,8 1,2102 2100 26 1,5.10-4 0,95 100 1 Bảng 2.2: Các thông số động học của robot Scara Serpent. TT Thông số Kích thước động học 1 m1 = 4 Kg Khối lượng thanh nối 1. 2 m2 = 1.5 Kg Khối lượng thanh nối 2. 3 m3 = 2 Kg Khối lượng thanh nối 3. 4 m4 = 0.6 Kg Khối lượng thanh nối 4. 5 a1 = 0.25 m Chiều dài thanh nối giữa 2 khớp main và fore . 6 a2 = 0.15 m Chiều dài thanh nối giữa 2 khớp fore và cổ tay . 7 d3 Chiều dài thanh nối d3 phụ thuộc vào chế độ làm việc của tay máy. 2.2.2. Giới hạn không gian làm việc của robot Scara Serpent Các biến khớp có các giới hạn góc quay như sau : q1 = -96 0 ¸ 960 ( so với trục Ox ). q2 = -115 0 ¸ 1150 (so với trục thanh 1). Chuyển động quay của khớp thứ nhất có hình chiếu bằng trong hệ trục toạ độ OX0Y0 và OX1Z1 (Hình 2.5) tương ứng với góc quay tổng trong thực tế là 1920. Chuyển động quay của khớp thứ hai có hình chiếu bằng trong hệ trục toạ độ OX1Y1 và OX2Y2 tương ứng với góc quay tổng là 2300. Từ đó ta có thể thấy được hình chiếu giới hạn không gian làm việc của nó (Hình 2.6). Hình 2.5: Giới hạn góc quay của 2 khớp. Hình 2.6: Giới hạn không gian làm việc của robot Scara Serpent. Như vậy khoảng không gian mà tay máy có thể với tới là toàn bộ hình trụ với đáy có đường giới hạn bên trong là một cung tròn có bán kính r = 0.231 (m) và đường giới hạn bên ngoài là đường tròn bán kính R = 0.4 (m). Khi biết được vị trí nào mà tay máy có thể đến được chúng ta có thể lập trình trong Matlab để tìm vị trí, quỹ đạo nào mà tay máy có thể vươn tới (được xét đến ở chương III trong bản luận văn này). 2.3. Động học robot Scara Serpent Robot Scara Serpent có cấu trúc động học được biểu diễn như trên Hình 2.4. Robot có 3 trục quay và 1 bàn kẹp, tuy nhiên ba khớp động đầu tiên được gọi là bộ phận cơ bản vì trước hết, nhờ chúng tay máy có thể thực hiện bước chủ yếu trong thao tác định vị, tức là đưa bàn kẹp đến lân cận điểm làm việc, sau đó nhờ khớp động còn lại bàn kẹp được định hướng và vi chỉnh đến vị trí gia công chính xác. 2.3.1. Động học thuận Việc xây dựng các phương trình động học thuận của robot được tiến hành tuần tự theo các bước sau: Bước 1: Xác định các hệ toạ độ Ta sử dụng quy ước Denavit-Hartenberg để mô tả đầy đủ vị trí của của toàn thân robot công nghiệp. Hình 2.4 mô tả các hệ trục toạ độ gắn với các khúc tay của robot Scara Serpent. Bước 2: Xây dựng bảng thông số DH Bảng 2.3: Tham số Denavit – Hartenberg của robot Scara Serpent. Thanh nối ai (0) ai qi(rad) di (m) Biến Chuyển động 1 0 a1 q1 0 q1 Quay 2 -1800 a2 q2 0 q2 Quay 3 0 0 0 d3 d3 Tịnh tiến 4 0 0 q4 0 q4 Quay Khảo sát với 3 trục khớp quay đầu tiên tương ứng với quỹ đạo của khớp quay 4 trong mặt phẳng OX0Y0. Ma trận T4 là ma trận biểu diễn tay máy robot trong hệ trục tọa độ gốc: T4= A1.A2.A3.A4 An= (2.1) Thay số liệu trong bảng tham số có: Ký hiệu: S1 Û Sinq1 ; C1 Û Cosq1 S2 Û Sinq2 ; C2 Û Cosq2 S4 Û Sinq4 ; C4 Û Cosq4 S12 Û Sin(q1+q2); C12 Û Cos(q1+q2) Các bước tính toán: Bước 1: Bước 2: Bước 3: Bước 4: (2.2) Ma trận 0T4 biểu diễn tay máy robot trong hệ toạ độ gốc. Mặt khác theo ký hiệu tổng quát: (2.3) Với: lần lượt là các vectơ định vị, vectơ định hướng, vectơ tới và vectơ vị trí để biểu diễn hướng và vị trí của tay máy trong không gian làm việc. Từ ma trận trên ta có hệ phương trình động học thuận tay máy robot: nx = C1(C2 C4 + S2S4) – S1(S2C4 – C2S4) (2.4) ny = S1(C2 C4 + S2S4) + C1(S2C4 – C2S4) (2.5) nz = 0 (2.6) ox = C1(S2C4- C2 S4) + S1(C2C4 + S2S4) (2.7) oy = S1(S2C4 - C2 S4) - C1(C2C4 + S2S4) (2.8) oz = 0 (2.9) ax = 0 (2.10) ay = 0 (2.11) az = -1 (2.12) Và hệ phương trình xác định vị trí của điểm tác động cuối như sau: x = px = a1.C1 + a2.C12 (2.13) y = py = a1.S1 + a2.S12 (2.14) z = pz = - d3 (2.15) 2.3.2. Động học ngược Động học ngược: xác định các biến khớp khi biết vị trí tay. Từ phương trình động học thuận có: (2.16) Do đó: (2.17) Từ đó tính được góc q2: q2= atan2(sinq2,cosq2) (2.18) Thế C1, S1 vào phương trình (2.13) và (2.14) thu được: (a1+a2C2).C1 – a2S2.S1 = px a2S2.C1 +( a1+a2C2).S1 = py Giải phương trình bậc nhất với ẩn C1, S1 và sử dụng (2.16) thu được : (2.19) q1= atan2(S1,C1) (2.20) Từ phương trình (2.15) ta có: d3 = - pz (2.21) Mặt khác từ phương trình (2.4) có: nx = C1(C2 C4 + S2S4) – S1(S2C4 – C2S4) (2.22) Rút gọn theo các công thức lượng giác thu được: nX = cosq1.cos(q2-q4) – sinq1.sin(q2-q4) = cos(q1+q2-q4) (2.23) sin(q1+q2-q4) = (2.24) (2.25) Vậy hệ phương trình động học ngược của robot Scara Serpent là: (2.26) 2.4. Động lực học robot Scara Serpent Để mô tả mối quan hệ giữa lực, mômen với vị trí, vận tốc và gia tốc của đối tượng robot, cần phải xây dựng được phương trình động lực học, từ đó phục vụ cho công việc thiết kế và điều khiển robot. Vì vậy cần phải tính toán đầy đủ các thông số của đối tượng trước khi đưa vào mô phỏng. Phương trình động lực học của robot được biểu diễn như sau: (2.27) Trong đó: t(t) - Vectơ [n x 1] lực động tạo nên ở n khớp động: t(t) = [t1(t), t2(t), ..., tn(t)]T (2.28) q(t) - Vectơ [n x 1] biến khớp: q(t) = [q1(t), q2(t), ... qn(t)]T. (2.29) H(q) - Ma trận [n x n], có các phần tử Hik sau đây: . (j, k =1, 2, ...n). (2.30a) (2.30b) h(, ) - Vectơ [n x 1] lực ly tâm và Coriolit: h(, ) = [h1, h2, ... , hn]T. (2.31) . (j =1, 2, ...n). (2.32) . (2.33) (2.34) g(q) - Vectơ [n x 1] lực trọng trường: g(q) = [g1, g2, ... , gn]T. (2.35) . (2.36) Với robot Scara Serpent gồm 4 chuyển động (3 chuyển động quay và 1 chuyển động tịnh tiến) và mô phỏng với 3 chuyển động quay có các phương trình cụ thể sau: Vectơ [4´ 1] lực động: t(t) = [t1(t), t2(t), t3(t), t4(t)]T. Vectơ [4´1] biến khớp: q(t) = [q1(t), q2(t), d3(t), q4(t)]T. 2.4.1. Hàm Euler - Lagrange và các vấn đề động lực học Lagrange định nghĩa sự khác biệt giữa động năng và thế năng của hệ thống: L = K – P (2.37) Trong đó: K : là động năng của hệ thống. P : là tổng thế năng của hệ thống. Do đó phương trình động lực học được xác định bằng biểu thức: (2.38) Trong đó: qi = qi đối với khớp quay; qi = ri đối với khớp tịnh tiến. ti : mômen lực động tại khớp thứ i. Động năng của khớp thứ i: (2.39) Với Ji là mômen quán tính của khớp thứ i. 2.4.2. Động lực học robot Scara Serpent Theo cấu hình, robot Scara Serpent có các thông số ở Bảng 2.4: Bảng 2.4: Thông số của robot Scara Serpent. Khớp 1 Khớp 2 Khớp 3 Khớp 4 Biến q1 q2 d3 q4 Chiều dài a1 a2 d3 0 Khối lượng m1 m2 m3 m4 Vận tốc v1 v2 v3 v4 Chiều dài tâm khối lg1 lg2 lg3 lg4 Giả sử khối lượng nằm ở đầu mút các thanh nối, ta có chiều dài tâm khối chính là chiều dài của thanh nối: lg1 = a1 lg2 = a2 lg3 = d3 Ký hiệu chiều dài các khớp: a1 = l1 a2 = l2 d3 = l3 Hệ qui chiếu gắn với trục toạ độ (O0x0y0z0) trên khớp thứ nhất. Khi đó mặt phẳng (O0x0y0) là mặt phẳng đẳng thế. 2.4.2.1. Tính toán động năng và thế năng cho từng khớp Khớp 1: Trong đó: x1, y1, z1 là hình chiếu của thanh nối số 1 lên các trục x, y, z. Trong đó: J1 momen quán tính khớp 1. (2.40) Khớp 2: (2.41) Khớp 3: (2.42) Khớp 4: Tổng động năng của hệ thống: K = K1 + K2 + K3 + K4 (2.44) Đặt tâm khối tại đầu mút của thanh nối, ta có: lg1= l1; lg2= l2; lg3= l3 (2.45) Tổng thế năng của hệ thống: P = P1+ P3+ P3+ P4 = -(m3+ m4).g.l3 (2.46) 2.4.2.2. Phương trình động lực học Lagrange của hệ thống: (2.47) Các phần tử của phương trình động lực học: (2.48) Khớp 1: (2.49) a. b. (2.50) c. (2.51) Khớp 2: (2.52) a. b. (2.53) c. (2.54) Khớp 3: (2.55) a. b. (2.56) c. (2.57) Khớp 4: (2.58) a. b. (2.59) c. (2.60) Như vậy phương trình động lực học của hệ thống được biểu diễn bằng các phương trình sau: (2.61) (2.62) (2.63) (2.64) Trong đó: m1234 = m1 + m2 + m3 + m4. m234 = m2 + m3 + m4 . (2.65) m34 = m3 + m4 ; Với m4 = m40 + mt m40 : khối lượng của khớp 4 mt : khối lượng của tải được nối với khớp 4. J124 = J1 + J2 + J4 . J24 = J2 + J4 ; Với J4 = J40 + Jt J40 : mô men quán tính của khớp 4. Jt : mô men quán tính của tải được nối với khớp 4. Với t1 , t2 , t4 lần lượt là mômen động tại các khớp quay 1, 2 và 4. t3 = F3 là lực động đặt lên khớp tịnh tiến 3. Nếu chỉ xét mômen động với 3 khớp quay thì có thể viết gọn lại ba phương trình động lực học (2.61), (2.62), (2.64) để tiện cho quá trình tính toán như sau: hay: (2.66) Các thành phần trong phương trình động lực học được xác định: (2.67) 2.5. Mô tả đối tượng bằng hệ phương trình trạng thái Đặt biến trạng thái cho từng khớp như sau: X(t) = (2.68) ,, (2.69) Tín hiệu vào: (2.70) Hệ phương trình vi phân trạng thái của các khớp được viết như sau: Khớp 1: (2.71) Khớp 2: (2.72) Khớp 4: (2.73) Trong đó: bij(X) là các thành phần tương ứng của ma trận H-1. . . . . . . (2.74) . . . ai(X) là thành phần thứ i của vectơ: (2.75) CHƯƠNG III XÂY DỰNG MÔ HÌNH HỆ THỐNG ĐIỀU KHIỂN 3.1. Cấu trúc hệ thống điều khiển robot Quỹ đạo đặt Bộ điều chỉnh Cảm biến Môi trường Tay máy n Robot thường tự động thực hiện các nhiệm vụ dịch chuyển hoặc các thao tác. Các hoạt động của robot thường được lập trình, việc thực hiện các nhiệm vụ thường làm thay đổi một phần hoặc toàn bộ vị trí của robot trong không gian. Có thể coi robot hiện đại như những người máy, có thể thực hiện các thao tác phức tạp, có độ chính xác cao. Hình 3.1: Sơ đồ cấu trúc chung của hệ thống điều khiển tay máy robot. Điều khiển chuyển động của tay máy trong không gian là xác định n thành phần mômen lực tổng quát tác động lên các khớp, mômen lực tổng quát được cung cấp bởi cơ cấu chấp hành. Quá trình điều khiển phải đảm bảo bộ điều khiển sẽ thực hiện điều khiển chuyển động của các khớp theo quỹ đạo q(t) sao cho q(t) luôn bám qđ(t), với qđ(t) là vectơ quỹ đạo chuyển động mong muốn. Sơ đồ cấu trúc chung của hệ thống điều khiển tay máy cho ở Hình 3.1, tay máy sẽ bám theo quỹ đạo đặt trước (được lập trình sẵn hoặc có thể đưa vào từ chương trình phầm mềm kết nối bên ngoài) sử dụng bộ điều chỉnh để điều khiển chuyển động và lực, sử dụng các cảm biến để thu nhận các thông số về môi trường làm việc đảm bảo cho hoạt động bền vững, khử được nhiễu ngoài n, nhận biết và đáp ứng thích nghi được với những sự thay đổi của môi trường cũng như sự thay đổi các tham số động học của của hệ thống. 3.2. Xây dựng quỹ đạo chuyển động chuẩn Trong điều khiển quỹ đạo robot, phải điều khiển tay robot bám theo một quỹ đạo xác định trước (quỹ đạo chuẩn). Quỹ đạo chuẩn đó là một hàm phụ thuộc thời gian. Việc xây dựng quỹ đạo chuyển động chính là tìm phương trình mô tả quỹ đạo robot theo thời gian. Có hai bài toán thiết kế quỹ đạo cho robot: Thiết kế quỹ đạo cho tay robot: xác định xd(t), yd(t). Thiết kế quỹ đạo cho khớp robot: xác định qid(t). Việc thiết kế quỹ đạo khớp robot có ưu điểm: q(t) là lượng đặt trực tiếp, hệ điều khiển là hệ điều khiển vị trí khớp, có thể xác định được điều kiện giàng buộc của động cơ và hệ truyền động, giải bài toán động học ngược đơn giản. Nhược điểm là khó đảm bảo chính xác quỹ đạo tay. Thiết kế quỹ đạo tay robot có ưu điểm: lượng đặt là vị trí tay robot nên đảm bảo chính xác quỹ đạo. Nhược điểm: phải giải bài toán động học ngược phức tạp với khối lượng tính toán lớn và khó tính được điều kiện giàng buộc. Yêu cầu điều khiển sao cho khớp robot chuyển động từ vị trí q0 đến vị trí qc trong thời gian td. Với dữ kiện ban đầu như vậy có các dạng quỹ đạo như sau: Dạng quỹ đạo bậc 3 q(t) = at3 + bt2 + ct +d (3.1) Việc thiết kế quỹ đạo là xác định các hệ số a, b, c, d. Dạng quỹ đạo 2 – 1 – 2 Trong quá trình chuyển động từ q0 đến qc trải qua ba giai đoạn: Giai đoạn tăng tốc: q(t) là hàm bậc hai theo t. Giai đoạn chuyển động đều: q(t) là hàm bậc nhất theo t. Giai đoạn giảm tốc: q(t) là hàm bậc hai theo t. Dạng quỹ đạo 4 – 1 – 4 Khớp Robot chuyển động từ q0 đến qc trong thời gian td biết: ; ; ; ; ; ; (3.2) Quỹ đạo chuyển động có ba giai đoạn: Giai đoạn tăng tốc: q(t) là hàm bậc 4 theo t. Giai đoạn chuyển động đều: q(t) là hàm nhất theo t. Giai đoạn giảm tốc: q(t) là hàm bậc 4 theo t. Phần này sẽ thiết kế quỹ đạo chuyển động của các khớp cho robot với dạng quỹ đạo 4 – 1 – 4, đảm bảo độ chính xác khi xây dựng quỹ đạo chuẩn. Bài toán xây dựng quỹ đạo chuẩn là lựa chọn quỹ đạo chuyển động giữa vị trí đầu và cuối của tay robot với thời gian chuyển động cho trước. Nội dung mục này sẽ giải quyết bài toán quỹ đạo chuyển động chuẩn cho các biến khớp. Ta chọn một biến khớp bất kỳ qi thay cho góc quay qi (i =1, 2, 3). Thời gian chuyển động từ vị trí đầu (x0, y0) tới vị trí cuối (xc, yc) là td. Giá trị ban đầu của q tại thời điểm ban đầu (t = 0) là q0 và giá trị tại t = td là qc. ; ; (3.3) Đồ thị quỹ đạo chuyển động chuẩn ở trên Hình 3.2: Hình 3.2: Dạng quỹ đạo chuyển động chuẩn. Quỹ đạo chuyển động xuất phát từ q0 đến qc sẽ qua ba giai đoạn: gia tốc, chuyển động với tốc độ không đổi và giảm tốc. Để đơn giản cho việc tính toán, có thể chọn tham số ta bằng 1/2 khoảng thời gian gia tốc hoặc giảm tốc. Ta cũng xác định các điểm phụ của quỹ đạo chuyển động q01 và qc2 tại t = ta và t = td – ta là: q01 = q0 và qc2 = qc. Nối q01 và qc2 bằng một đường thẳng và xác định điểm q02, qc1 tại t = 2ta và t = td – 2ta. Bằng cách chọn điểm phụ như trên, quỹ đạo đoạn cd là một đường thẳng với tốc độ không đổi, quỹ đạo đoạn ac và df có thể chọn là đa thức bậc bốn có dạng: q = a0 + a1t + a2t2 + a3t3 + a4t4. (3.4) 3.2.1. Xác định giá trị q02 và qc1 Như cách vẽ đã trình bày ở trên, đường nối be là đường thẳng. Giá trị q02 và qc1 tại t = 2ta và t = td – 2ta, có thể được xác định từ giá trị đầu q0 và cuối qc dựa trên phương trình bậc nhất của đường thẳng be: (3.5) (3.6) 3.2.2. Phương trình đoạn cd Quỹ đạo đoạn cd là đường thẳng biểu diễn bởi phương trình: (3.7) với (2ta £ t £ td – 2ta) Thay (3.5) vào (3.7) và viết gọn lại ta có được: (3.8) Trong đó: (3.9) 3.2.3. Phương trình đoạn ac Quỹ đạo đoạn ac biểu diễn bởi phương trình dạng đa thức bậc bốn (3.10): qac = a0ac + a1act + a2act2 + a3act3 + a4act4. (3.10) Các hệ số của phương trình đoạn ac được xác định từ điều kiện đầu và cuối: tại t = 0: (3.11) Tại t = 2ta: (3.12) Lấy đạo hàm cấp 1 và cấp 2 của (3.10) và sử dụng (3.11) và (3.12), các hệ số được xác định như sau: (3.13) 3.2.4. Phương trình đoạn df Quỹ đạo đoạn df biểu diễn bởi phương trình dạng đa thức bậc bốn (3.14): qdf = a0df + a1dft + a2dft2 + a3dft3 + a4dft4 (3.14) Các hệ số của phương trình đoạn ac được xác định từ điều kiện đầu và cuối: (3.15) Cuối cùng các hệ số được xác định như sau: (3.16) Như vậy quỹ đạo chuyển động của 3 khớp 1, 2, 4 của Robot Serpent có thể được xác định bằng các phương trình (3.8), (3.10) và (3.14) với các hệ số của các phương trình được xác định từ các giá trị của vị trí đầu và vị trí cuối của các khớp tương ứng bởi các biểu thức: (3.9), (3.13) và (3.16). 3.3. Thiết kế bộ điều khiển cho tay máy robot Scara Serpent ba bậc tự do 3.3.1. Hệ phương trình động lực học Lagrange Hệ phương trình động lực học Lagrange của tay máy robot Scara Serpent được viết dưới dạng ma trận sau : (3.17) hay: Trong đó t1, t2 và t4 lần lượt là các mômen điều khiển tác động lên khâu 1, khâu 2 và khâu 4 (từ 2.61 đến 2.65). Với các tham số Hij , T được cho theo (2.66), (2.67) (đã xét ở chương 2): và: m1234 = m1 + m2 + m3 + m4 ; m234 = m2 + m3 + m4 . m34 = m3 + m4 ; m4 = m40 + mt ; m40 : khối lượng của khớp 4 mt : khối lượng của tải được nối với khớp 4. J124 = J1 + J2 + J4 ; J24 = J2 + J4 ; J4 = J40 + Jt J40 : mô men quán tính của khớp 4. Jt : mô men quán tính của tải được nối với khớp 4. 3.3.2. Hệ phương trình trạng thái Biến trạng thái cho khớp 1, 2 và 4 như cho ở 2.68 ¸ 2.70: và tín hiệu vào Hệ phương trình vi phân trạng thái của các khớp 1, 2 và 4 được viết như sau: Khớp 1: (3.18) Khớp 2: (3.19) Khớp 4: (3.20) Từ các phương trình (3.18) đến (3.20), ta có hệ phương trình trạng thái của khớp 1 và 2, 4 dưới đây: Khớp 1: (3.23) Với : (3.24) = Khớp 2: (3.25) Với : (3.26) Khớp 4: (3.27) Với : (3.28) 1 s 1 s Phương trình (3.23) x11=q1 1 s 1 s x21=q2 Phương trình (3.25) 1 s 1 s x41=q4 Phương trình (3.27) Hình 3.3: Mô hình hóa đáp ứng đầu ra thực của robot. Như vậy mô hình tay máy robot ba bậc tự do là một hệ nhiều đầu vào nhiều đầu ra, được mô tả bằng ba hệ nhỏ, mỗi hệ tương ứng với từng khớp 1, 2 và 4, được đặc trưng bởi ba hệ phương trình vi phân trạng thái (3.23), (3.25) và (3.27). Các hệ phương trình này có thể dựng để mô hình hóa trên máy tính cũng như để tổng hợp luật điều khiển cho tay máy. 3.3.3. Lựa chọn phương pháp điều khiển và bộ điều khiển PID Phương pháp điều khiển được lựa chọn là phương pháp điều khiển động lực học ngược với đầu vào bộ điều khiển là sai lệch giữa tín hiệu đặt và tín hiệu đầu ra. Đầu ra là tín hiệu điều khiển uđk, ở bộ điều khiển PID là uPID. PID uđk e Hình 3.4: Sơ đồ bộ điều khiển PID Hàm truyền của bộ điều khiển: Bộ điều khiển PID được sử dụng khá rộng rãi vì tính đơn giản của nó cả về cấu trúc lẫn nguyên lý làm việc. Muốn hệ thống có được chất lượng như mong muốn thì phải phân tích đối tượng rồi trên cơ sở đó chọn các tham số KP, KI, KD cho phù hợp. Phương pháp Ziegler – Nichols. Ziegler – Nichols là phương pháp xác định hệ số KP, hằng số thời gian tích phân TI và hằng số thời gian vi phân TD dựa trên đặc tính quá độ của hệ thống điều khiển. Có hai phương pháp hiệu chỉnh Ziegler – Nichols đều hướng tới mục tiêu đạt độ quá điều chỉnh khoảng 25%. Phương pháp Ziegler – Nichols. Trường hợp 1. Xác định thông số bộ điều khiển PID dựa vào đáp ứng bậc thang đơn vị của hệ hở (nếu đối tượng không chứa các khâu tích phân hay nghiệm phức liên hợp thì đường quá độ của đối tượng có dạng chữ S) : Hình 3.5: Đáp ứng bậc thang đơn vị của hệ hở. T1: thời gian trễ T2: hằng số thời gian Phương pháp Ziegler – Nichols. Trường hợp 2. T1 và T2 được xác định bằng cách vẽ đường tiếp tuyến với đường cong S tại điểm uốn, đường tiếp tuyến này cắt trục hoành tại T1 và đường y(t)=K là điểm có hoành độ T2. Khi đó mô hình đối tượng có dạng: Bảng 3.1: Thông số bộ PID. Thông số Bộ ĐK KP TI TD P T2/T1K 0 PI 0.9T2/T1K T1/0.3 0 PID 1.2T2/T1K 2T1 0.5T1 Xác định thông số bộ điều khiển PID dựa vào đáp ứng của hệ kín ở biên giới ổn định. Hình 3.6: Đáp ứng của hệ kín ở biên giới ổn định. Bước 1: Đặt TI = ∞, TD= 0, thay đổi KP từ 0 tới giá trị giới hạn Kgh ứng với đầu ra hệ thống kín có dao động ở biên giới ổn định. Dao động này tương ứng với chu kỳ giới hạn Tgh. Bước 2: Thông số bộ PID được xác định theo bảng : Bảng 3.2: Thông số bộ PID . Thông số Bộ ĐK KP TI TD P 0.5Kgh 0 PI 0.45Kgh 0.83Tgh 0 PID 0.6Kgh 0.5Tgh 0.125Tgh Với: ; CHƯƠNG IV MÔ PHỎNG VỚI MÔ HÌNH ROBOT SCARA SERPENT 4.1. Đặt vấn đề Trong chương III đã tiến hành xây dựng mô hình của các khâu tính toán, mô hình robot và bộ điều khiển PID. Sau đó đã xây dựng các chương trình phục vụ mô phỏng và khảo sát các chế độ làm việc của hệ thống. Trong chương này sẽ trình bày các kết quả khảo sát nhận được thông qua các chương trình mô phỏng nêu trên. Thông qua việc đánh giá sai số quỹ đạo đặt và quỹ đạo thực để đánh giá chất lượng điều chỉnh bám chính xác của phương pháp điều khiển được nghiên cứu. Việc mô hình hóa và nghiên cứu mô phỏng hệ thống điều khiển chuyển động được thực hiện trên nền Matlab-Simulink, giao diện GUI. 4.2. Sơ đồ mô hình hóa các khâu của hệ thống 4.2.1. Mô hình chung của robot Tạo quỹ đạo chuyển động chuẩn Bộ điều khiển mô men động Cơ cấu chấp hành (Robot) ui Phản hồi Hình 4.1: Sơ đồ khối mô hình hóa robot Scara Serpent sử dụng bộ điều khiển mômen động. Động lực học của robot Scara Serpent được mô tả bằng hệ phương trình trạng thái (2.71), (2.72) và (2.73) tương ứng với 3 khớp quay của robot. Mỗi hệ phương trình vi phân cho từng khớp gồm hai phương trình vi phân với biến trạng thái là góc quay và tốc độ góc của khớp . Các biến trạng thái đó sẽ được xác định bằng tích phân các trạng thái tương ứng (Hình 3.3). 4.2.2. Mô hình khối tạo quỹ đạo chuyển động chuẩn Hình 4.2 mô tả sơ đồ khối tạo quỹ đạo chuyển động chuẩn. Các điều kiện đầu và cuối của quỹ đạo chuyển động chuẩn được tính thông qua các toạ độ đặt (x0, y0), (xc, yc) và thời gian chuyển động (td). Quỹ đạo chuyển động chuẩn của cả 3 khớp được xác định theo 3 giai đoạn: gia tốc, tốc độ không đổi và giảm tốc, biểu diễn bằng các phương trình (3.8), (3.10) và (3.14). Hình 4.2: Sơ đồ khối tạo quỹ đạo chuyển động chuẩn. 4.2.3. Mô hình bộ điều khiển Hình 4.3: Mô hình bộ điều khiển cho khớp 1 của robot Scara Serpent. Các khớp 2, 4 có mô hình bộ điều khiển cũng tương tự như khớp 1. Bộ tham số [KP, KI, KD] của bộ điều khiển PID được chọn theo phương pháp Ziegler – Nichols. 4.3. Giao diện chương trình mô phỏng robot Scara Serpent Khi chạy chương trình mô phỏng ta đánh lệnh >> RBSCARA_SP trên cửa sổ chính của chương trình Matlab, trên màn hình hiện ra bảng làm việc chính như sau: Hình 4.4: Menu chính của chương trình. Chương trình RBSCARA_SP gồm các chức năng chính sau: Nhập các thông số cho mô hình. Động học thuận. Động học ngược. Mô phỏng. Hiển thị. Thoát. Chương trình được thiết kế trên giao diện GUI trong Matlab, với các nút thực hiện các chức năng như trên. Các chức năng của chương trình được giới thiệu như ở dưới: Các thông số của robot Scara Serpent được nhập vào khối thông số của robot như Hình 4.5. Hình 4.5: Thông số của mô hình robot Scara Serpent. Khi ta nhấn vào nút Động học thuận: Chương trình tính vị trí điểm tác động cuối của robot khi biết giá trị góc quay các khớp như Hình 4.6. Hình 4.6: Động học thuận robot Scara Serpent. Nhập giá trị góc quay các biến khớp 1 và 2 với giới hạn góc quay như sau: q1 = -96 0 ¸ 960 (so với trục Ox); q2 = -115 0 ¸ 1150 (so với trục thanh 1). Chương trình sẽ báo lỗi nếu giá trị nhập vào không nằm trong giới hạn góc quay: Hình 4.7: Động học thuận robot Scara Serpent (báo lỗi). Khi ta nhấn vào nút Động học ngược: Chương trình tính góc quay của các khớp khi biết vị trí tọa độ của tay máy robot như Hình 4.8. Hình 4.8: Động học ngược robot Scara Serpent. Tay robot được thiết kế chuyển động theo một quỹ đạo xuất phát từ vị trí ban đầu có toạ độ điểm đầu (x0, y0) đến vị trí cuối có toạ độ điểm cuối (xc, yc) với thời gian chuyển động là td (Hình 4.2). Các toạ độ ban đầu và kết thúc phải thoả mãn điều kiện (4.1) và giá trị định vị Nx phải thoả mãn điều kiện (4.2). Và . (4.1) Giá trị định vị : < 1. ( 4.2) Chương trình sẽ báo lỗi nếu giá trị (X, Y, ) nhập vào không nằm trong giới hạn: Hình 4.9: Động học ngược robot Scara Serpent (báo lỗi). Khi ta nhấn vào nút Mô phỏng: Mô hình sẽ hiện ra như Hình 4.10. Hình 4.10: Sơ đồ khối mô hình chuẩn robot Scara Serpent. Khi ta nhấn vào nút Hiển thị: Đưa ra bảng thông số của robot như Hình 4.11. Hình 4.11: Bảng thông số của robot Scara Serpent. 4.4. Kết quả mô phỏng hệ thống trên Matlab-Simulink 4.4.1. Thông số của robot và quỹ đạo chuyển động Các thông số danh định của robot được sử dụng để thiết kế, tính toán và mô phỏng như sau: Bảng 4.1: Các thông số của tay máy. Tham số Khâu l (m) lg (m) m (kg) J (kg.m2) Khâu 1 0.25 0.25 4 0.021 Khâu 2 0.15 0.15 1.5 0.0034 Khâu 4 0 0 0.6 0.001 Robot được thiết kế chuyển động theo một quỹ đạo ban đầu có tọa độ (x0, y0) đến vị trí cuối (xc, yc) trong thời gian chuyển động td với ta là thời gian gia tốc (hoặc giảm tốc). Tải định mức: m t = 2(kg ) và mômen quán tính của tải Jt = 0.0004 (kg.m2). Đáp ứng nhanh: td = 1.2(s), ta = 0.15(s). Đáp ứng chậm: td = 4(s), ta = 0.5(s). Bảng 4.2: Các tham số đặt. x0(m) 0 xc(m) 0.25 y0(m) 0.3 yc(m) 0 4.4.2. Đặc tính của hệ thống khi robot làm việc với tải khác nhau Trong phần này dùng bộ tham số [KP, KI, KD] của bộ điều khiển PID trong Matlab- Simulink là: Khớp 1: PID1 = [100, 0, 15] Khớp 2: PID2 = [95, 0, 14] Khớp 4: PID4 = [90, 0, 12] a. Khảo sát hệ thống trong trường hợp thời gian đặt td nhỏ, đáp ứng nhanh (td = 1.2(s), ta = 0.15(s)) Đặc tính của hệ thống khi làm việc không tải: Hình 4.12: Quỹ đạo đặt các khớp khi làm việc không tải. Hình 4.13: Quỹ đạo thực các khớp khi làm việc không tải. Hình 4.14: Sai số về góc quay các khớp khi làm việc không tải. Hình 4.15: Vận tốc góc đặt các khớp khi làm việc không tải. Hình 4.16: Vận tốc góc thực các khớp khi làm việc không tải. Hình 4.17: Sai số về vận tốc góc các khớp khi làm việc không tải. Đặc tính của hệ thống khi làm việc với 1/2 tải định mức: Hình 4.18: Quỹ đạo đặt các khớp khi làm việc với 1/2 tải định mức. Hình 4.19: Quỹ đạo thực các khớp khi làm việc với 1/2 tải định mức. Hình 4.20: Sai số về góc quay các khớp khi làm việc với 1/2 tải định mức. Hình 4.21: Vận tốc góc đặt các khớp khi làm việc với 1/2 tải định mức. Hình 4.22: Vận tốc góc thực các khớp khi làm việc với 1/2 tải định mức. Hình 4.23: Sai số về vận tốc góc các khớp khi làm việc với 1/2 tải định mức. Đặc tính của hệ thống khi làm việc với tải định mức: Hình 4.24: Quỹ đạo đặt các khớp khi làm việc với tải định mức. Hình 4.25: Quỹ đạo thực các khớp khi làm việc với tải định mức. Hình 4.26: Sai số về góc quay các khớp khi làm việc với tải định mức. Hình 4.27: Vận tốc góc đặt các khớp khi làm việc với tải định mức. Hình 4.28: Vận tốc góc thực các khớp khi làm việc với tải định mức. Hình 4.29: Sai số về vận tốc góc các khớp khi làm việc với tải định mức. b. Khảo sát hệ thống trong trường hợp thời gian đặt td tương đối dài, đáp ứng chậm (td = 4(s), ta = 0.5(s)) Đặc tính của hệ thống khi làm việc không tải: Hình 4.30: Quỹ đạo đặt các khớp khi làm việc không tải. Hình 4.31: Quỹ đạo thực các khớp khi làm việc không tải. Hình 4.32: Sai số về góc quay các khớp khi làm việc không tải. Hình 4.33: Vận tốc góc đặt các khớp khi làm việc không tải. Hình 4.34: Vận tốc góc thực các khớp khi làm việc không tải. Hình 4.35: Sai số về vận tốc góc các khớp khi làm việc không tải. Đặc tính của hệ thống khi làm việc với 1/2 tải định mức: Hình 4.36: Quỹ đạo đặt các khớp khi làm việc với 1/2 tải định mức. Hình 4.37: Quỹ đạo thực các khớp khi làm việc với 1/2 tải định mức. Hình 4.38: Sai số về góc quay các khớp khi làm việc với 1/2 tải định mức. Hình 4.39: Vận tốc góc đặt các khớp khi làm việc với 1/2 tải định mức. Hình 4.40: Vận tốc góc thực các khớp khi làm việc với 1/2 tải định mức. Hình 4.41: Sai số về vận tốc góc các khớp khi làm việc với 1/2 tải định mức. Đặc tính của hệ thống khi làm việc với tải định mức: Hình 4.42: Quỹ đạo đặt các khớp khi làm việc với tải định mức. Hình 4.43: Quỹ đạo thực các khớp khi làm việc với tải định mức. Hình 4.44: Sai số về góc quay các khớp khi làm việc với tải định mức. Hình 4.45: Vận tốc góc đặt các khớp khi làm việc với tải định mức. Hình 4.46: Vận tốc góc thực các khớp khi làm việc với tải định mức. Hình 4.47: Sai số về vận tốc góc các khớp khi làm việc với tải định mức. c. Đánh giá sai số của hệ thống Trong trường hợp thời gian đặt td nhỏ, đáp ứng nhanh (td = 1.2(s), ta = 0.15(s)): Bảng 4.3. Đánh giá sai số góc lớn nhất các khớp của robot. Sai số Tải Sai số góc quay lớn nhất (rad) Khớp 1 Khớp 2 Khớp 4 Không tải 1/2 tải định mức Tải định mức Bảng 4.4. Đánh giá sai số vận tốc góc lớn nhất các khớp của robot. Sai số Tải Sai số vận tốc góc lớn nhất (rad/s) Khớp 1 Khớp 2 Khớp 4 Không tải 1/2 tải định mức Tải định mức Trường hợp thời gian đặt td tương đối dài, đáp ứng chậm (td = 4(s), ta = 0.5(s)): Bảng 4.5. Đánh giá sai số góc lớn nhất các khớp của robot. Sai số Tải Sai số góc quay lớn nhất (rad) Khớp 1 Khớp 2 Khớp 4 Không tải 1/2 tải định mức Tải định mức Bảng 4.6. Đánh giá sai số vận tốc góc lớn nhất các khớp của robot. Sai số Tải Sai số vận tốc góc lớn nhất (rad/s) Khớp 1 Khớp 2 Khớp 4 Không tải 1/2 tải định mức Tải định mức d. Kết luận Kết quả mô phỏng cho thấy các thanh nối của robot bám chính xác các quỹ đạo chuyển động chuẩn với sai số góc quay tĩnh cũng như sai số góc quay trong giai đoạn di chuyển tốc độ không đổi rất nhỏ. Sai số góc quay trong giai đoạn gia tốc và giảm tốc cũng đủ nhỏ ở giới hạn cho phép. Với các mức tải: Không tải, nửa tải, tải định mức kết quả mô phỏng cho thấy sai số góc và vận tốc góc tăng lên khi tải thay đổi. Khi thay đổi thời gian đặt ta đều thấy quỹ đạo thực bám khá sát quỹ đạo đặt với sai số rất nhỏ. Có thể thấy rất rõ điều này trên kết quả mô phỏng. Sai số góc và vận tốc góc khi đáp ứng chậm (td tương đối dài) nhỏ hơn khi đáp ứng nhanh (td nhỏ). Điều đó khẳng định hệ thống ổn định hơn khi đáp ứng chậm (thời gian đặt td tương đối dài). Qua kết quả mô phỏng việc xây dựng thuật toán điều khiển robot theo phương pháp động lực học ngược sử dụng bộ điều khiển PID cho robot Scara Serpent với ba khớp động đã đáp ứng được yêu cầu đặt ra, sai số của hệ thống nằm ở giới hạn cho phép. KẾT LUẬN Luận văn đã nghiên cứu về lý thuyết điều khiển, từ đó xây dựng thuật toán điều khiển robot theo phương pháp động lực học ngược sử dụng bộ điều khiển PID cho robot Scara Serpent với ba khớp động. Luận văn đã kiểm nghiệm tính đúng đắn của thuật toán điều khiển thông qua việc xây dựng các phương trình động lực học cho robot Scara Serpent dựa vào thông số đã cho của nhà sản xuất. Đưa ra công thức tính động học thuận và động học ngược cho robot Scara Serpent, xây dựng mô hình toán học cho cơ cấu truyền động robot, tổng hợp hệ truyền động. Thiết kế bộ điều khiển PID theo phương pháp Ziegler – Nichols. Đánh giá chất lượng tĩnh và động của hệ thống khảo sát hệ thống ở các chế độ làm việc khác nhau bằng phương pháp mô hình hoá hệ thống và mô phỏng trên phần mềm Matlab-Simulink. Kết quả mô phỏng cho thấy các thanh nối của robot bám chính xác các quỹ đạo chuyển động chuẩn với sai số góc quay tĩnh cũng như sai số góc quay trong giai đoạn di chuyển tốc độ không đổi rất nhỏ. Sai số góc quay trong giai đoạn gia tốc và giảm tốc cũng đủ nhỏ ở giới hạn cho phép. Kết quả nghiên cứu ảnh hưởng của bộ số (KP, KI, KD) đến độ chính xác điều khiển bám theo quỹ đạo chuẩn .Nếu bộ (KP, KI, KD) được chọn phù hợp thì sai số quỹ đạo chuyển động và độ quá điều chỉnh sẽ nhỏ, chọn bộ số (KP, KI, KD) sao cho đem lại sự tối ưu cho hệ thống điều khiển về cả giải pháp kỹ thuật lẫn kinh tế. Chất lượng động và độ tác động nhanh của hệ thống cũng được kiểm nghiệm với tín hiệu đặt biến thiên nhanh. Kết quả thu được cho thấy hệ thống đảm bảo phản ứng nhanh với các tín hiệu đặt và sai số quỹ đạo đủ nhỏ. Mặc dù luận văn chưa có kết quả thực nghiệm, song những kết quả thu được đã tạo cơ sở tốt cho việc thiết kế một hệ thống điều khiển bám chính xác quỹ đạo chuyển động của robot trong thực tế. TÀI LIỆU THAM KHẢO 1. Phạm Công Ngô: Lý thuyết điều khiển tự động. Nhà xuất bản Khoa học Kỹ thuật. Hà nội 1998. 2. Lê Hùng Lân: Lý thuyết điều khiển tự động. Trường Đại học Giao thông Vận tải. Hà nội 2000. 3. Richard C.Dorf, Robert H.Bishop: Modern Control System, 10Editor. Person Prentice Hall, Inc., 2005. 4. Ogata, K. (2002). Modern Control Engineering, Fourth Edition. Prentice Hall, Inc. Dr. Young, G. Textbook & Lecture notes: MAE4053 Automatic Control Systems. 5. Nguyễn Phùng Quang: Matlab& Simulink dành cho kỹ sư điều khiển tự động. Nhà xuất bản Khoa học Kỹ thuật. Hà nội 2004. 6. The MathWorks, Inc: Matlab2007 Users Guide. 7. Duane Hanselman, Bruce Littlefield: Mastering Matlab 5 A Comprehensive Tutorial and Reference. Prentice Hall, Inc.,1998. PHỤ LỤC 1: CÁC SƠ ĐỒ KHỐI 1.Sơ đồ khối mạch điều khiển robot Scara Serpent 2. Sơ đồ khối khâu tạo quỹ đạo chuyển động chuẩn 3. Sơ đồ khối khâu điều chỉnh 4. Sơ đồ khối bộ điều khiển PID cho các khớp 5. Sơ đồ khâu robot Scara Serpent 6. Sơ đồ khối các tín hiệu ra 7. Nhập các giá trị tham số cho mô hình PHỤ LỤC 2: CÁC CHƯƠNG TRÌNH M FILES (chạy trên nền Matlab/Simulink) 1. Tính toán Quỹ đạo chuyển động chuẩn % Tinh toan Quy dao chuyen dong chuan % Xay dung quy dao chuyen dong chuan cho 3 khop quay cua Robot Serpent % qi0, qic: goc quay dau va cuoi cua khop quay thu i function qdc=qdcdc(u) % Toa do diem dau: DDau=[X0;Y0] X0=u(1); Y0=u(2); % Toa do diem cuoi: DCuoi=[Xc;Yc] Xc=u(3); Yc=u(4); % td: Dat thoi gian chuyen dong td=u(5); % Thoi gian gia toc (hoac giam toc) ta=u(6); % Thong so cua Robot: a1,a2 a1=u(7); %(m) a2=u(8); %(m) t=u(9); % thoi gian thuc nx=u(10); % CHUONG TRINH CHINH % Dong hoc nguoc va tinh cac goc quay cua tung khop tai diem dau va diem cuoi q20=Gocquay2(X0,Y0,a1,a2); q2c=Gocquay2(Xc,Yc,a1,a2); q10=Gocquay1(X0,Y0,q20,a1,a2); q1c=Gocquay1(Xc,Yc,q2c,a1,a2); q40=Gocquay4(q10,q20,nx); q4c=Gocquay4(q1c,q2c,nx); %Khoi tao cac he so cua quy dao chuan cua 3 khop h1=Init(q10,q1c,ta,td); h2=Init(q20,q2c,ta,td); h4=Init(q40,q4c,ta,td); %Quy dao chuyen dong chuan 3 khop quay cua Robot Serpent qdc1= Quydaochuan(t,h1,ta,td); qdc2= Quydaochuan(t,h2,ta,td); qdc4= Quydaochuan(t,h4,ta,td); qdc=[qdc1;qdc2;qdc4]; %KET THUC CHUONG TRINH CHINH %CHUONG TRINH TINH GIA TRI %Tinh cac gia tri tuong ung goc quay 2: q20,q2c function gq2=Gocquay2(x,y,a1,a2) C2=(x^2+y^2-a1^2-a2^2)/(2*a1*a2); S2=sqrt(1-C2^2); gq2=atan2(S2,C2); %Tinh cac gia tri tuong ung goc quay 1: q10,q1c function gq1=Gocquay1(x,y,q2,a1,a2) C1=((a1+a2*cos(q2))*x+a2*sin(q2)*y)/(x^2+y^2); S1=((a1+a2*cos(q2))*y-a2*sin(q2)*x)/(x^2+y^2); gq1=atan2(S1,C1); %Tinh cac gia tri tuong ung goc quay 4: q40,q4c function gq4=Gocquay4(q1,q2,nx) KK=sqrt(1-nx^2); tetaphu=atan2(KK,nx); gq4=q1+q2-tetaphu; %Xay dung quy dao chuyen dong chuan %qac=a0ac+a3ac*t^3+a4ac*t^4 %qcd=a0cd+a1cd*(t-2ta) %qdf=a0df+a1df*t+a3df*t^3+a4df*t^4 function quydao=Quydaochuan(t,h,ta,td) a0ac=h(1);a3ac=h(2);a4ac=h(3); a0cd=h(4);a1cd=h(5); a0df=h(6);a1df=h(7);a3df=h(8);a4df=h(9);qcuoi=h(10); if td<=0 td=0; quydao=-1; %Error elseif (td>0)&&(td<=4*ta) %Gom 2 giai doan: gia toc, giam toc if (t>=0)&&(t<=td/2) %Gia toc quydao=a0ac+a3ac*t^3+a4ac*t^4; %Gia toc elseif (t>td/2)&&(t<=td) %Giam toc quydao=a0df+a1df*(t-td/2)+a3df*(t-td/2).^3+a4df*(t-td/2).^4; %Giam toc elseif t>td quydao=qcuoi; else quydao=-1; %Error end else %if td>(4*ta) %Gom 3 giai doan: gia toc, chay deu, giam toc if (t>=0)&&(t<2*ta) quydao=a0ac+a3ac*t^3+a4ac*t^4;%Gia toc elseif (t>=2*ta)&&(t<=(td-2*ta)) quydao=a0cd+a1cd*(t-2*ta);%Chay deu elseif (t>(td-2*ta))&&(t<=td) quydao=a0df+a1df*(t-td+2*ta)+a3df*(t-td+2*ta).^3+a4df*(t-td+2*ta).^4;%Giam toc elseif t>td quydao=qcuoi; else quydao=-1;%Error end end %Tinh cac he so cua quy dao chuan ung voi tung khop function heso=Init(qdau,qcuoi,ta,td) a0ac=0;a3ac=0;a4ac=0;a0cd=0;a1cd=0;a0df=0;a1df=0;a3df=0;a4df=0; if (td>0)&&(td<=4*ta) %Quy dao doan ac:(thay ta=td/4) a0ac=qdau; a3ac=(qcuoi-qdau)/(td^3/8); a4ac=-(qcuoi-qdau)/(td^4/8); %Quy dao doan cd: a0cd=0; a1cd=0; %Quy dao doan df: a0df=qcuoi-(qcuoi-qdau)/2; a1df=(qcuoi-qdau)/(td/2); a4df=-(3*qcuoi-3*a0df-a1df*td)/(td^4/16); a3df=-(a1df+a4df*(td^3/2))/(3*td^2/4); end if td>(4*ta) %Quy dao doan ac: a0ac=qdau; a3ac=(qcuoi-qdau)/(4*ta^2*(td-2*ta)); a4ac=-(qcuoi-qdau)/(16*ta^3*(td-2*ta)); %Quy dao doan cd: a0cd=qdau+(qcuoi-qdau)*ta/(td-2*ta); a1cd=(qcuoi-qdau)/(td-2*ta); %Quy dao doan df: a0df=qcuoi-(qcuoi-qdau)*ta/(td-2*ta); a1df=(qcuoi-qdau)/(td-2*ta); a4df=-(3*qcuoi-3*a0df-4*a1df*ta)/(16*ta^4); a3df=-(a1df+32*a4df*ta^3)/(12*ta^2); end heso=[a0ac,a3ac,a4ac,a0cd,a1cd,a0df,a1df,a3df,a4df,qcuoi]; 2. Tính momem chuẩn % Tinh toan momem chuan function momem=momemchuan(u) q2d=u(1); dq1d=u(2);ddq1d=u(3); dq2d=u(4);ddq2d=u(5); dq4d=u(6);ddq4d=u(7); mt=u(8);Jt=u(9); % Thong so cua robot J1=0.021; J2=0.0034; J3=0.006; J40=0.001; J4=J40+Jt; J124=J1+J2+J4; J24=J2+J4; m1=4; m2=1.5; m3=2; m40=0.6; m4=m40+mt; m1234=m1+m2+m3+m4; m234=m2+m3+m4; m34=m3+m4; l1=0.25; l2=0.15; % Tinh T T=-(m234*l1*l2*sin(q2d)); % Tinh toan cac gia tri Hij H11=m1234*l1^2+m234*l2^2+J124+2*m234*l1*l2*cos(q2d); H12=m234*l2^2+J24+m234*l1*l2*cos(q2d); H13=J4; H21=H12; H22=m234*l2^2+J24; H23=J4; H31=J4; H32=J4; H33=J4; DH=J4^2*(2*H12-H11-H22)+J4*(H11*H22-H12^2); % Dau ra % Tinh momem chuan quy dao dat u1m=H11*ddq1d+H12*ddq2d+H13*ddq4d+T*dq2d^2+2*T*dq1d*dq2d; u2m=H21*ddq1d+H22*ddq2d+H23*ddq4d-T*dq1d^2; u4m=H31*ddq1d+H32*ddq2d+H33*ddq4d; momem =[u1m;u2m;u4m]; 3. Tính góc quay ban đầu %Tinh toan goc quay ban dau function init=gocquaybd(u) x=u(1);%m y=u(2);%m a1=u(3);%(m) a2=u(4);%(m) nx=u(5); %q20=Gocquay2(X0,Y0,a1,a2) %q10=Gocquay1(X0,Y0,q20,a1,a2); %q40=Gocquay4(q10,q20,nx); %Tinh cac gia tri tuong ung goc quay 2: q20 C2=(x^2+y^2-a1^2-a2^2)/(2*a1*a2); S2=sqrt(1-C2^2); q20=atan2(S2,C2); %Tinh cac gia tri tuong ung goc quay 1: q10 C1=((a1+a2*cos(q20))*x+a2*sin(q20)*y)/(x^2+y^2); S1=((a1+a2*cos(q20))*y-a2*sin(q20)*x)/(x^2+y^2); q10=atan2(S1,C1); %Tinh cac gia tri tuong ung goc quay 4: q40 KK=sqrt(1-nx^2); thetaphu=atan2(KK,nx); q40=q10+q20-thetaphu; %Dau ra init=[q10;q20;q40]; 4. Tính góc điều khiển thực % Tinh toan goc dieu khien thuc function ddtheta=tinh_ddtheta(u) % Momem dieu khien u1=u(1); u2=u(2); u4=u(3); % Thong so cua tai mt=u(4); Jt=u(5); % Goc quay theta2 thuc te q1tt=u(6);q2tt=u(7);q4tt=u(8); % Dao ham goc quay thuc te dq1tt=u(9);dq2tt=u(10); % Goc quay ban dau q10=u(11);q20=u(12);q40=u(13); % CHUONG TRINH CHINH % Tinh cac he so heso=tinh_heso(q2tt,dq1tt,dq2tt,mt,Jt); % Tinh dao ham cap hai cua goc quay thuc ddtheta=tinh_ddthetathuc(heso,u1,u2,u4,q10,q20,q40); % Chuong trinh tinh cac he so function HS=tinh_heso(q2tt,dq1tt,dq2tt,mt,Jt) % Thong so cua robot J1=0.021; J2=0.0034; J3=0.006; J40=0.001; J4=J40+Jt; J124=J1+J2+J4; J24=J2+J4; m1=4; m2=1.5; m3=2; m40=0.6; m4=m40+mt; m1234=m1+m2+m3+m4; m234=m2+m3+m4; m34=m3+m4; l1=0.25; l2=0.15; % Tinh T T=-(m234*l1*l2*sin(q2tt)); % Tinh toan cac gia tri Hij H11=m1234*l1^2+m234*l2^2+J124+2*m234*l1*l2*cos(q2tt); H12=m234*l2^2+J24+m234*l1*l2*cos(q2tt); H13=J4; H21=H12; H22=m234*l2^2+J24; H23=J4; H31=J4; H32=J4; H33=J4; DH=J4^2*(2*H12-H11-H22)+J4*(H11*H22-H12^2); % Cac gia tri dau ra bij b11=DH^-1*(H22*H33-H23*H32); b12=DH^-1*(H13*H32-H12*H33); b13=DH^-1*(H12*H23-H13*H22); b21=DH^-1*(H23*H31-H21*H33); b22=DH^-1*(H11*H33-H13*H31); b23=DH^-1*(H13*H21-H11*H23); b31=DH^-1*(H21*H32-H31*H22); b32=DH^-1*(H12*H31-H11*H32); b33=DH^-1*(H11*H22-H12^2); % Cac gia tri h1,h2 h1=T*dq2tt^2+2*T*dq1tt*dq2tt; h2=-(T*dq1tt^2); % Cac gia tri ai a1=-(b11*h1+b12*h2); a2=-(b21*h1+b22*h2); a4=-(b31*h1+b32*h2); HS =[b11,b12,b13,b21,b22,b23,b31,b32,b33,a1,a2,a4]; % Chuong trinh tinh dao ham cap hai cua goc quay thuc function d2theta=tinh_ddthetathuc(heso,u1,u2,u4,q10,q20,q40) b11=heso(1);b12=heso(2);b13=heso(3); b21=heso(4);b22=heso(5);b23=heso(6); b31=heso(7);b32=heso(8);b33=heso(9); a1=heso(10);a2=heso(11);a4=heso(12); % Tinh dao ham cap hai cua goc quay thuc d2theta1=a1+b11*u1+b12*u2+b13*u4; d2theta2=a2+b21*u1+b22*u2+b23*u4; d2theta4=a4+b31*u1+b32*u2+b33*u4; d2theta=[d2theta1;q10;d2theta2;q20;d2theta4;q40];