Đề tài Nghiên cứu hệ thống điều khiển trượt cho chuyển động tay máy 3 bậc tự do

- 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) Hình 2.7: Giản đồ pha theo luật điều khiển HOSM 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