Đề tài Nâng cao chất lượng hệ thống điều khiển quỹ đạo cho Robot Scara 3 bậc tự do ứng dụng phương pháp điều khiển trượt phi tuyến

a ngoài thì hệ sẽ ổn định tại 0. - Nếu x(t) cắt mọi đường cong họ v nào theo chiều từ ngoài vào trong thì hệ sẽ ổn định tiệm cận tại 0. 2.5. Phương pháp điều khiển trượt cho Robot n bậc tự do 2.5.1. Cơ sở toán học Ta xem xét hệ động học sau: x(n) = a(X) + B(X).u (2.11) trong đó đại lượng vô hướng x là đầu ra mong muốn, đại lượng vô hướng u là tín hiệu điều khiển đầu vào, là vectơ trạng thái, a(X) là hàm phi tuyến không biết chính xác và B(X) là ma trận biểu diễn độ khuếch đại điều khiển không biết chính xác. Trạng thái ban đầu Xd(0) phải là: Xd(0) º X(0) (2.12) và Ngoài ra, ta định nghĩa bề mặt biến thiên theo thời gian s(t) trong không gian trạng thái R(n) bằng phương trình vô hướng S(X;t) = 0 trong đó: S(X;t) = (2.13) với l là một hằng số dương. Ví dụ nếu n = 2 thì tức s là tổng mức ảnh hưởng của sai lệch vị trí và sai lệch vận tốc. Việc giữ giá trị vô hướng S bằng 0 có thể giải quyết được bằng cách chọn luật điều khiển u trong (2.11) sao cho ở bên ngoài s(t) ta có: (2.14) trong đó h là hằng số dương. (2.14) cho thấy rằng khoảng cách đến bề mặt s, được tính bằng S2, giảm xuống theo quỹ đạo hệ thống. Vì thế nó buộc các quỹ đạo hệ thống hướng tới bề mặt s(t) như minh họa trong hình 2.3 dưới đây. s(t) Hình 2.3 Hỡnh 2.4 x Thời gian tớn hiệu điều khiển chạm vào mặt trượt S = 0 xd(t) 0 Mặt phẳng trượt x(t) Hỡnh 2.5 S = 0 xd(t) x Chattering x(t) Bắt đầu từ điểm xuất phát ban đầu nào đó, quỹ đạo trạng thái chạm đến mặt phẳng trượt, sau đó sẽ trượt dọc theo mặt trượt và hướng đến xd với tốc độ hàm mũ, với hằng số thời gian 1/l (hình 2.4). Tóm lại, từ phương trình (2.13) chọn một hàm S, sau đó chọn luật điều khiển u trong (2.11) sao cho S2 duy trì một hàm Lyapunov của hệ thống kín, bất chấp sự thiếu chính xác của mô hình và sự có mặt của nhiễu loạn. Trình tự thiết kế do đó sẽ bao gồm 2 bước: + Bước một, chọn luật điều khiển u thỏa mãn điều kiện trượt (2.14). + Bước hai, luật điều khiển không liên tục u đã được chọn trong bước một được làm nhẵn một cách thích hợp để có sự dung hòa tối ưu giữa dải thông điều khiển và tính chính xác của quỹ đạo, đồng thời khắc phục hiện tượng chattering (hình 2.5). Xét một hệ phi tuyến bậc hai có phương trình trạng thái như sau: (2.15) với là ma trận biểu thị trạng thái của hệ thống. 2.5.1.1. Các giả thiết của (2.15) Giả thiết có phương trình động lực học của Robot như sau: Với : là tín hiệu điều khiển. là vectơ lực Coriolis là nhiễu ngoài chưa biết. Hàm a(X) không được biết chính xác nhưng có ngưỡng giới hạn là một hàm xác định (2.16) Gọi bx là giá trị riêng của B(X), bx min và bx max lần lượt là giá trị riêng nhỏ nhất và lớn nhất của B(X). Đặt bx = (bx max/bx min)1/2, ta được: (2.17) 2.5.1.2. Các bước xây dựng bộ điều khiển trượt Sai lệch quỹ đạo: (2.18) + Bước 1: Định nghĩa mặt s(t) như sau: (2.19) trong đó l là hằng số dương. Nếu n = 2 thì mặt s(X,t) là: S(X,t) = = 0 (2.20) + Bước 2: Tính u để cho trạng thái hệ thống tiến về mặt s(t) và nằm trên đó như trên hình 2.2. Để có được điều đó, xét một hàm năng lượng của hệ thống kín. Giả sử có điểm cân bằng tại điểm x = 0 tại đó V(x) cực tiểu. Nếu chứng minh được: (2.21) thì điểm x = 0 được gọi là điểm ổn định. Theo nguyên lý ổn định Lyapunov, chọn một hàm: với S ¹ 0 Phải làm cho , nghĩa là: . Đây là điều kiện để hệ thống luôn luôn ổn định tiệm cận toàn thể tại S = 0. Khi điều kiện (2.17) được thỏa mãn thì trạng thái của hệ thống luôn luôn được đưa về trên mặt trượt S = 0 và giữ trên đó. Đó là yêu cầu của bước 2. Như vậy phải thiết kế tín hiệu điều khiển u sao cho điều kiện (2.17) được thỏa mãn. Ta có: = (2.22) Chọn tín hiệu đầu vào theo công thức sau [12]: (2.23) Trong đó: và (2.24) với sgn(S) = [sgn(S1), ..., sgn(Sn)]T K = diag(K1, ..., Kn); Ki > 0 với i = 1, 2, ..., n (2.25) Đối với hệ phương trình trạng thái (2.15), nếu các giả thiết (2.16) và (2.17) đều được thỏa mãn và luật điều khiển được chọn như trong (2.23) với (2.26) thì sai số quỹ đạo e = xd – x sẽ hội tụ về 0, nghĩa là xd º x. Từ đây ta sẽ xem xét lý thuyết tổng hợp bộ điều khiển kiểu trượt cho cơ cấu Robot n bậc tự do. Bỏ qua thành phần trọng lực g(q), ta có được: (2.27) Giả định rằng các thông số được đánh giá và các giá trị thực của chúng có mối liên hệ theo các bất đẳng thức sau: (2.28) (2.29) Trong đó B(q), là các hàm đã biết. Điều này cho thấy cả ma trận quán tính và lực liên kết trên khớp được đánh giá với sai số xác định. Phương trình ĐLH (2.27) có thể được viết lại như sau: (2.30) Với: (2.30’) B(q) = H-1(q) Dễ thấy B(q) là ma trận nghịch đảo của H(q). Nhiệm vụ điều khiển là tính mô men t thích hợp sao cho vectơ vị trí thực qt (góc quay) luôn bám theo quỹ đạo đặt qd . Chọn sai số trạng thái và mặt trượt có dạng mô tả sau: e = qd – qt (2.31) , với C = CT > 0 (2.32) Dễ thấy rằng, việc duy trì trên mặt trượt (s = 0) sẽ dẫn đến q(t) ® qd. Thực tế khi chọn s = 0 thì phương trình (2.32) trở thành: (2.33) Phương trình (2.33) chỉ có nghiệm duy nhất e = 0. Nói cách khác, nó đặc trưng cho hệ ĐLH ổn định tiệm cận có e = 0 là giải pháp duy nhất, từ đó điều kiện bám qt ® qd được thoả mãn. Do vậy, vấn đề cần giải quyết của luật điều khiển là tìm mô men động tại các khớp ti sao cho duy trì quỹ đạo Robot trên mặt trượt. Vận dụng lý thuyết ổn định Lyapunov, vấn đề chọn t có thể chuyển thành xét tính ổn định hàm năng lượng V. Chọn hàm V có dạng: V = (1/2).sT.s > 0 (2.34) Vi phân hàm V ta có: (2.35) Do vậy điều kiện để hệ ổn định là: (2.36) Điều kiện (2.36) được gọi là điều kiện trượt. Khi điều kiện trượt được thoả mãn, hệ thống kín sẽ ổn định tiệm cận, toàn bộ và xảy ra hiện tượng bám của tín hiệu ra qt so với tín hiệu đặt qd mặc dù tồn tại các phần không mô hình được, nhiễu hai sai lệch ban đầu (q(0) ¹ qd(0)). Nếu điều kiện trượt được thoả mãn theo biểu thức sau: ; a > 0; . (2.37) thì mặt trượt s = 0 sẽ được bám (lần thứ nhất) trong khoảng thời gian nhỏ hơn T0. (2.38) Vi phân hai vế (2.32) và thay (2.31) vào phương trình ta có: (2.39) Từ (2.30) và (2.39) ta có: (2.40) Chọn mô men vào có dạng: (2.41) Trong đó: * * sgn(s) = [sgn(s1), sgn(s2), ..., sgn(sm)]T * K > 0, là ma trận (n´n) Ma trận hệ số K phải chọn đủ lớn sao cho duy trì được điều kiện trượt, mặc dù tồn tại các thành phần không mô hình được hay nhiễu. Trường hợp việc đánh giá các thông số là chính xác () thì điều kiện trượt (2.36) có thể được biểu diễn dưới dạng sau: (2.42) Từ phương trình (2.40) liên hệ tín hiệu điều khiển t và tín hiệu ra ta thấy tín hiệu vào bị gián đoạn khi cắt ngang mặt trượt s(t) do vậy sẽ dẫn tới hiện tượng lập bập ở đầu ra. Hiện tượng này có thể khắc phục bằng việc lọc các thành phần gián đoạn trong miền s lân cận mặt cắt. Trường hợp vectơ có dạng: (2.43) thay và đặt R = -B(q) ta có: (2.44) Khi đó điều kiện trượt sẽ trở thành: (2.45) Do đó, nếu chọn K thoả mãn: (2.46) thì điều kiện trượt (2.36) được thoả mãn. Tương đương với các điều kiện (2.28) và (2.29) là các điều kiện sau: (2.47) (2.48) Khi đó ma trận K thoả mãn (2.36) có thể được chọn như sau: (2.49) 2.5.2. Phương pháp nâng cao chất lượng hệ điều khiển trượt Điều khiển theo chế độ trượt (SMC) được biết đến như là một trong những kỹ thuật phổ biến và đơn giản để điều khiển bền vững cho hệ thống Robot khi có nhiễu ngoài và thay đổi theo môi trường hoạt động với độ đáp ứng nhanh và đặc tính điều khiển tốt. Tuy nhiên, trong các ứng dụng thực tế, mô hình SMC cổ điển có hai nhược điểm quan trọng sau: Thứ nhất, phạm vi giới hạn của nhiễu phải nhận biết được để giải quyết các vấn đề điều khiển. Nhưng trong các ứng dụng thực tế, do sự thay đổi giá trị các thông số của hệ là khó tiên đoán và nhiễu hệ thống lại thay đổi phụ thuộc vào môi trường và cũng khó để nhận biết được, do đó hệ số khuếch đại chuyển mạch pha trượt của luật SMC truyền thống không thể tính toán chính xác được. Thứ hai là bộ SMC truyền thống luôn phải chịu sự lập bập (hiện tượng chattering) do tín hiệu điều khiển ở đầu vào là không liên tục và sự đáp ứng trễ ở trong pha trượt. Nó là kết quả của đáp ứng điều khiển chậm và sự rung của các phần tử chuyển mạch cơ khí ở tần số cao. Từ đó người ta đưa ra những phương pháp tiếp cận mới vẫn dựa trên mô hình trượt SMC nhưng có cải tiến để khắc phục những nhược điểm đã nêu ở trên. Một trong những ý tưởng chính được phát triển đó là sử dụng khâu tích phân ở đầu ra của tín hiệu phía sau khâu rơle (hình 2.6): s(x) Robot u x z xđ Hình 2.6: Lược đồ điều khiển trượt cải tiến sử dụng mô hình trượt SMC Một đầu vào ảo z được đặt trước đầu vào điều khiển thực u của hệ thống được mô tả như là một phần của trạng thái của hệ thống. Hệ thống với các biến trạng thái [xT u]T được mô tả bởi hệ phương trình dưới đây: (2.50) Với đầu vào ảo z được tính bởi luật: z = K.sign(S(.)) (2.51) với K là một hằng số nào đó Như vậy bộ điều khiển robot sẽ bao gồm một bộ chuyển mạch rơle (2.51) và một khâu tích phân. Phương pháp điều khiển này đặc biệt được ưa dùng để điều khiển các hệ thống robot có các mặt trượt được tích hợp với các bộ điều khiển tích hợp cấp thấp (ví dụ như ở các bộ điều chế PWM để điều khiển động cơ điện). Khi tần số chuyển mạch lớn thì phương pháp này tỏ ra kém hiệu quả hơn, chính vì thế người ta đã phát triển các phương pháp khác có hiệu quả hơn để nâng cao chất lượng điều khiển. 2.5.2.1. Phương pháp dùng bộ điều khiển với chế độ trượt bậc cao HOSMC – High Order Sliding Mode Controller. Mặt trượt được sử dụng để minh họa hành vi động học của khớp tay robot là một phương trình vi phân bậc nhất: (2.52) Chúng ta đi mô tả mặt trượt (2.52) để xác định hành vi quan hệ của hệ thống điều khiển, ta thu được kết quả: (2.53) Do đó bậc quan hệ của chế độ điều khiển trượt là r=2. Giả sử đáp ứng bước nhảy ban đầu của giá trị đặt () thì phương trình vi phân bậc 2 của mặt trượt có thể viết lại như sau: (2.54) với là các ký hiệu được cho ở [28]. Giả sử rằng F > 0; |j| < F; 0 < Gm £ g £ GM [21], với s0, u0 <1 và F, Gm, GM là các hằng số dương. Đặt y1 = s thì ta có phương trình: (2.55) Phát biểu thuật toán điều khiển HOSM: Luật điều khiển được xây dựng dựa trên tính bền vững và tính bất tiến theo thời gian cho hệ phi tuyến. (2.56) Với VM : là một hằng số dương và gc là một hàm liên tục. gc(y1) = -l1|y1|r.sign(y1) ; l1 > 0; 0.5 £ r < 1. (2.57) (2.58) Giá trị l1 lớn thì quá trình gia tốc để tiến đến mặt trượt càng nhanh và cho phép sự ổn định và bền vững tốt hơn. (hình 2.7) y1 y2 y2=gc(y1) Bộ điều khiển HOSMC đảm bảo được quá trình điều khiển bền vững với nhiễu chưa biết của hệ thống và cho phép dịch chuyển an toàn hơn ở tốc độ thấp. Đường trượt chuyển đổi y2=gc(y1) là đường cong, không còn tuyến tính. Tuy nhiên với khối lượng tính toán toán học quá lớn, cồng kềnh và việc lựa chọn các thông số gặp nhiều khó khăn cho việc thử nghiệm hệ thống nên phương pháp này chỉ thích hợp với các bộ xử lý tốc độ cao và ứng dụng trong điều khiển cơ học, đặc biệt là điều khiển với các mẫu platform đa chuyển động để trợ giúp cho quá trình dịch chuyển của bệnh nhân liệt nửa người trong y tế (các ứng dụng chuyên biệt). Do vậy với robot công nghiệp thì phương pháp này tỏ ra chưa thích hợp. 2.5.2.2. Phương pháp lớp biên để làm giảm hiện tượng chattering [23]. Phương pháp điều khiển trượt ngày nay đã được sử dụng ngày càng rộng rãi bởi nó có độ chính xác và bền vững cao với các thông số nhiễu chưa biết. Người ta phát triển mô hình trượt mới SMC thay thế phương pháp truyền thống cũ để khắc phục các nhược điểm đã phân tích ở trên. Sử dụng hệ số chuyển mạch thông minh và lớp biên phụ thuộc vào phương pháp SMC với cấu trúc đơn giản cho hệ thống robot với nhiễu chưa biết được hoàn thiện. Để khắc phục hiện tượng chattering, thay hàm dấu sgn(S) bằng hàm bão hòa sat(S): (2.59) và làm nhẵn tín hiệu điều khiển không liên tục trong một lớp biên mỏng B(t): (2.60) Hình 2.8: Định nghĩa lớp biên B(t) f e e xd 0 x S = 0 Lớp này bao quanh mặt trượt S = 0 với bề dày F và độ rộng e = F/l. Nếu luật điều khiển bảo đảm cho điều kiện trượt (2.26) được thỏa mãn ở bên trong lớp biên B(t) thì sau một thời gian hữu hạn, sai số điều khiển sẽ bé hơn e, nghĩa là: (2.61) Khi thì là hàm liên tục nên tín hiệu sẽ giảm chattering, nhưng xảy ra sai lệch quỹ đạo. 2.4.2.3. Phương pháp dùng hàm Sat_PI - chuyển mạch tích phân bão hòa [3]. Bằng cách thay hàm chuyển mạch signum bằng hàm chuyển mạch tích phân-bão hòa (sat-PI), hiện tượng chattering sẽ giảm xuống và chất lượng điều khiển được nâng cao. Đây là một bộ điều khiển trượt mới, sử dụng hàm chuyển mạch là hàm tích phân-bão hòa (sat-PI). 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, sau đó đã được kiểm nghiệm bằng thực nghiệm trên mô hình thực tế Robot Gryphon (xem [3]). Kết quả trên mô phỏng và thực nghiệm chứng minh được tính đúng đắn và khẳng định việc chọn luật điều khiển trong phương pháp điều khiển trượt (sat-PI) đã nâng cao chất lượng và khắc phục được nhược điểm của hệ điều khiển trượt truyền thống. Để giảm hiện tượng chattering và sai lệch quỹ đạo, định nghĩa một hàm tích phân-bão hòa (sat-PI) như sau: (2.62) trong đó KI là hệ số tích phân dương, to là thời điểm đầu khi trạng thái hệ thống đi vào trong lớp biên B(t). Bây giờ thay hàm signum trong luật điều khiển u ở (2.23) bằng hàm tích phân-bão hòa: (2.63) Trong đó ueq và K được chọn như trong (2.24) và (2.25) và . Giả sử rằng hệ số tích phân KI được chọn đủ lớn sao cho: (2.64) Bất đẳng thức (2.64) có nghĩa s(s) là hàm tăng khi S > 0 và hàm giảm khi S < 0. Đối với hệ thống (2.11), nếu các giả thiết (2.16) và (2.17) thỏa mãn và luật điều khiển được chọn như trong (2.63), với Ki được chọn theo (2.26) và s(S) được chọn như (2.62), thì các quỹ đạo trạng thái luôn hướng về các mặt trượt. Điều đó đã được chứng minh và kiểm nghiệm ở [3]. Với luật điều khiển u được xây dựng theo (2.63) trong đó hàm chuyển mạch s(s) được định nghĩa theo (2.62), nếu ta chọn các hệ số Ki theo (2.64) và theo (2.26) thì (với i = 1, 2, ..., n), điều kiện trượt được thỏa mãn và sai số quỹ đạo ei sẽ hội tụ tiệm cận về 0. Việc sử dụng các hàm sat và sat-PI bảo đảm loại bỏ được hiện tượng chattering. Hàm sat-PI bảo đảm sai số quỹ đạo nhỏ hơn so với hàm sat và đặc biệt hàm sat-PI đạt được tính ổn định của tín hiệu điều khiển tại điểm không, trong khi hàm sat không có được điều này. Tuy nhiên việc chọn luật điều khiển u và tính toán các tham số cho tín hiệu điều khiển u gặp phải những phức tạp trong việc tính toán, thời gian tính toán tương đối dài, trong khi đó hàm sat cho ta sự lựa chọn đơn giản hơn và dễ dàng hơn trong điều khiển và tính chọn. 2.5. Kết luận Ở chế độ trượt, hệ thống làm việc như hệ bậc 1 vì quỹ đạo tương ứng với đường thẳng chuyển đổi. Ở quá trình quá dộ bậc sẽ giảm đi ở chế độ trượt, đó là đặc điểm của nguyên lý làm việc này. Việc tổng hợp hệ làm việc ở chế độ trượt có thể thực hiện theo phương pháp áp đặt nghiệm cực hay các phương pháp tối ưu khác. Điều chỉnh theo chế độ trượt cho phép sử dụng cơ cấu điều khiển tác động nhanh; với những điều kiện nhất định hệ thống sẽ có tính bền vững nghĩa là sự thay đổi thông số của hệ không làm ảnh hưởng đến hành vi của nó. Điều khiển trượt cho phép điều khiển bền vững, tuy nhiên khuyết điểm chính của phương pháp này là hoạt động điều khiển không liên tục và gây ra hiện tượng chattering ngoài ý muốn. Nhìn chung nó tạo ra đáp ứng thời gian chậm do tần số chuyển đổi bị giới hạn. Việc sử dụng các hàm sat và sat-PI bảo đảm loại bỏ được hiện tượng chattering. Hàm sat bảo đảm sai số quỹ đạo nhỏ và đặc biệt là hàm sat đạt được tính ổn định của tín hiệu điều khiển. Phương án dùng hàm chuyển mạch Sat đã được đề xuất ở đầu chương. Thuật toán đưa ra có khả năng điều khiển hệ phi tuyến bậc cao giảm đáng kể hiện tượng chattering, sai lệch quỹ đạo điều khiển thấp hơn so với hàm sat, hệ điều khiển làm việc ổn định, ít chịu ảnh hưởng của tải, do đó chất lượng điều khiển được cải thiện đáng kể. Ngoài ra, phương án còn có ưu điểm là khối lượng tính toán nhỏ, thuật điều khiển ít phức tạp, đáp ứng nhanh, hệ thống kín ổn định. Việc so sánh các kết quả mô phỏng giữa các trường hợp hàm signum, hàm sat và hàm sat-PI đã minh chứng tính đúng đắn của thuật toán đã nêu ra trong việc nâng cao chất lượng của hệ điều khiển. Cấu trúc mô hình thực nghiệm đã được xây dựng trên một đối tượng có tính phi tuyến mạnh là Robot Sraca Serpent với 3 bậc tự do và qua kết quả thực nghiệm đã khẳng định tính đúng đắn của kết quả nghiên cứu (các kết quả được thể hiện ở chương 5: Mô phỏng), cho thấy thuật toán đề xuất hoàn toàn có thể ứng dụng vào thực tế điều khiển các hệ thống động lực học có tính phi tuyến mạnh, có yêu cầu đáp ứng nhanh và độ chính xác cao, quỹ đạo thực tế bám sát so với quỹ đạo đặt yêu cầu. Chương 3 MÔ TẢ TOÁN HỌC ĐỐI TƯỢNG - ROBOT SCARA SERPENT 3.1. Các thông số và vùng làm việc của Robot Scara Serpent. Robot SCARA (Selectively Compliant Articulated Robot Arm) có nghĩa là có thể lựa chọn dễ dàng khớp nối cánh tay Robot. 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 Serpent (hình 3.1) - một loại cơ bản trong nhóm Robot công nghiệp này. H×nh 3.1: Robot Scara Serpent vµ c¸c h×nh chiÕu 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 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 pendant. 3.1.1. Cấu tạo tay máy Robot Serpent. 3.1.1.1. Cấu hình của robot Serpent. H.2-1 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: a a Hình 3.2: Cấu hình và hệ trục tọa độ gắn trên khúc tay của Robot Robot 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. 3.1.1.2. Các thông số kỹ thuật của Robot Scara Serpent Bảng 3.3: Các thông số động học của robot Serpent Stt Thông số Ghi chú (*) 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 5 a2 = 0.15 m ChiÒu dµi thanh nèi gi÷a 2 khíp fore vµ cæ tay 6 d3 ChiÒu dµi thanh nèi d3 phô thuéc vµo chÕ ®é lµm viÖc cña tay m¸y. *: KÝch th­íc ®éng häc t­¬ng øng trªn h×nh 3.2 B¶ng 3.4. Th«ng sè c¸c ®éng c¬ cña robot tt Loại U (V) I (A) M (Nm) N (vg/ph) P (W) J (Kg.m2) R (W) L (mH) m (Kg) 1 J9ZF 12 4,8 4.10-2 2100 15 0,32.10-4 1,38 100 0,6 2 J9ZF 12 4,8 4.10-2 2100 15 0,32.10-4 1,38 100 0,6 3 J12ZF 12 4,8 1,210-2 2100 26 1,5.10-4 0,95 100 1 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 Serpent. Trong đó: Độ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) 3.1.2. Giới hạn không gian làm việc của Robot Serpent Các biến khớp có các giới hạn góc quay như sau : q1 = 0 ¸ 6600dp tương ứng q1 = -96 0 ¸ 960 ( so với trục 0x ). q2 = 0 ¸ 7900dp tương ứng với q2 = -115 0 ¸ 1150 (so với trục thanh 1). Hình 3.5: Giới hạn góc quay của 2 khớp 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 trên hình 3.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 3.6): Hình 3.6: Giới hạn không gian làm việc của robot 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 là đường có tô mầu ở trên, có đường giới hạn bên trong là một cung tròn có bán kính r = 0.2309 (m) , 2 điểm mút tương ứng với vị trí hai góc q1 , q2 đều bằng 0 dp và vị trí q1=6600dp, q2 = 7900 (dp); đườ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 4 trong bản luận văn này). 3.2. Độ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 3.2. 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. 3.2.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 [16] để mô tả đầy đủ vị trí của của toàn thân RBCN. Hình 3.2 mô tả các hệ trục toạ độ gắn với các khúc tay của robot Serpent. Bước 2: Xây dựng bảng thông số DH. Bảng 3.7: 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: T3 = A1.A2.A3.A4 An= (3.1) Thay sè liÖu trong b¶ng tham sè cã: ; Ký hiệu: S1 Û Sin ; C1 Û Cos S2 Û Sin ; C2 Û Cos S4 Û Sinq4 ; C4 Û Cosq4 S12Û Sin(+) ; C12 Û Cos(+) Các bước tính toán: 1. 2. 3. 4. Ma trận 0T4 biểu diễn tay máy Rôbốt trong hệ toạ độ gốc. (3.2) Mặt khác theo ký hiệu tổng quát: (3.3) Với: lần lượt là các vector đị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 Rôbốt : nx = C1(C2 C4 + S2S4) – S1(S2C4 – C2S4) (3.4) ny = S1(C2 C4 + S2S4) + C1(S2C4 – C2S4) (3.5) nz = 0 (3.6) ox = C1(S2C4- C2 S4) + S1(C2C4 + S2S4) (3.7) oy = S1(S2C4 - C2 S4) - C1(C2C4 + S2S4) (3.8) oz = 0 (3.9) ax = 0 (3.10) ay = 0 (3.11) az = -1 (3.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 (3.13) y = py = a1.S1 + a2.S12 (3.14) z = pz = - d3 (3.15) 3.2.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ó: (3.16) Do đó: (3.17) Từ đó tính được góc q2: q2= atan2(sinq2,cosq2) (3.18) Thế C1, S1 vào phương trình (3.13) và (3.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 (3.16) thu được : (3.19) q1= atan2(S1,C1) (3.20) Từ phương trình (3.21) ta có: d3 = - pz (3.21) Mặt khác từ phương trình (3.4) có: nx = C1(C2 C4 + S2S4) – S1(S2C4 – C2S4) (3.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) (3.23) sin(q1+q2-q4) = (3.24) (3.25) Vậy hệ phương trình động học ngược của Robot Serpent là: q1= atan2(S1,C1) (3.26) q2= atan2(S2,C2) d3 = - pz 3.3. Động lực học Rôbốt 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 ĐLH của Robot được biểu diễn như sau: (3.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 (3.28) q(t) - Vectơ [n x 1] biến khớp: q(t) = [q1(t), q2(t), ... qn(t)]T. (3.29) H(q) - Ma trận [n x n], có các phần tử Hik sau đây: . (j, k =1, 2, ...n) (3.29) (3.30) h(q, ) - Vectơ [n x 1] lực ly tâm và Coriolit: h(q, ) = [h1, h2, ... , hn]T. (3.31) . (j =1, 2, ...n) (3.32) (3.33) (3.34) g(q) - Vectơ [n x 1] lực trọng trường: g(q) = [g1, g2, ... , gn]T. (3.35) (3.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