Động lực học tay máy robot có liên kết chương trình

Nội dung text: Động lực học tay máy robot có liên kết chương trình

1. Tuyển tập Hội nghị khoa học toàn quốc lần thứ nhất về Động lực học và Điều khiển Đà Nẵng, ngày 19-20/7/2019, tr. 177-182, DOI 10.15625/vap.2019000275 Động lực học tay máy robot có liên kết chương trình Nguyễn Văn Khang1), Nguyễn Văn Quyền1), Lương Bá Trường2), Nguyễn Văn Long3) 1)Bộ môn Cơ học ứng dụng, Trường Đại học Bách khoa Hà Nội 2)Bộ môn Cơ điện tử, Trường Đại học Thủy Lợi 3)Bộ môn Cơ điện tử, Trường Đại học Kinh doanh và Công nghệ Hà Nội E-mail: 1){khang.nguyenvan2, quyen.nguyenvan}@hust.edu.vn, 2)truonglb@tlu.edu.vn, 3)chitietmay1011@gmail.com Tóm tắt Ms(),,,,ttt s++= Css (  ) s  gs () Hệ nhiều vật chịu các liên kết chương trình có thể xem thuộc (1) tFtt- T s, l vào tập hệ nhiều vật có cấu trúc mạch vòng. Các phương trình ()s ( ) chuyển động của hệ nhiều vật có cấu trúc mạch vòng thường là f (,)s0t = (2) hệ các phương trình vi phân – đại số. Trong báo cáo này trình T Trong đó s = éss, , , sù là các toạ độ suy rộng dư, bày việc áp dụng các phương trình Lagrange dạng nhân tử để ëê 12 n ûú thiết lập hệ phương trình vi phân-đại số của hệ nhiều vật chịu Ms(),t là ma trận khối lượng suy rộng của hệ, t ()t là các liên kết chương trình. Sau đó trình bày chi tiết một bài toán véctơ lực suy rộng ứng với các lực hoạt động không có động lực học thuận robot có liên kết chương trình. T thế, l = éll, , , lù là véctơ các nhân tử Lagrange, ëê 12 r ûú Từ khóa: Tay máy Robot, liên kết chương trình, phương trình T f = éf ,ff , , ù =0 là các điều kiện ràng buộc, F là vi phân-đại số, phương pháp khử nhân tử Lagrange, động lực ëê 12 r ûú s học thuận. ma trận Jacobi của f cỡ rf´ , Css(),, t là ma trận quán tính ly tâm và Coriolis. 1. Mở đầu Để thuận tiện cách viết ta đưa vào ký hiệu pss(, ,)ttt=-t (t) Css (, ,) s - gs (,) (3) Động lực học hệ nhiều vật chịu các liên kết chương 1 trình là lĩnh vực khoa học có ý nghĩa thực tế và đang Phương trình (1) bây giờ có dạng được quan tâm nghiên cứu [1-7]. Hệ nhiều vật chịu các  T Ms(),t s+ Fl=s () s ,t p1 (4) liên kết chương trình thực chất thuộc vào tập hệ nhiều vật có cấu trúc mạch vòng. Để thiết lập phương trình chuyển Đạo hàm hai lần phương trình liên kết (2) ta thu được các động của hệ nhiều vật có cấu trúc mạch vòng, người ta phương trình ¶¶ff thường sử dụng phương trình Lagrange dạng nhân tử, f =+=+=ssf0 (5) phương trình Kane dạng nhân tử, phương pháp tách cấu ¶¶s t st trúc, [8, 9]. Các phương trình mô tả chuyển động của f=++=FFssf0    hệ nhiều vật có cấu trúc mạch vòng thường là hệ các sst FFssfp=-():  + = (6) phương trình vi phân – đại số. Các phương pháp giải hệ sst2 phương trình vi phân – đại số thường được phân thành Các phương trình (4) và (6) có thể viết lại dưới dạng ma hai nhóm: trận như sau . Các phương pháp biến đổi hệ phương trình vi éM FT ù éspùéù êús ê úêú= 1 (7) phân-đại số về hệ phương trình vi phân thường. êúF 0 êlúêúp êús ê úêú2 Sau đó sử dụng thuật toán số giải hệ phương trình ë û ë ûëû vi phân thường [8,9]. Xác định các điều kiện đầu của các toạ độ suy rộng phụ thuộc và các vận tốc suy rộng phụ thuộc [10, 11] . Các phương pháp số giải trực tiếp hệ phương trình Để có thể tích phân được các phương trình vi phân vi phân đại số [8, 9]. dạng tọa độ dư, thì việc xác định các điều kiện đầu của Trong bài báo này trình bày việc áp dụng phương các tọa độ suy rộng phụ thuộc và vận tốc của chúng là pháp khử các nhân tử Lagrange tính toán động lực học cần thiết. Vì vậy trước tiện ta trình bày vấn đề này. thuận robot có liên kết chương trình. Khi giải hệ phương trình vi phân chuyển động của hệ nhiều vật có f bậc tự do, ta phải cho trước f giá trị 2. Phương pháp số giải phương trình đầu của các toạ độ suy rộng độc lập và các vận tốc suy rộng độc lập qqqq(0), , (0), (0), , (0) . Trong hệ Lagrange dạng nhân tử 11ff phương trình (2) còn có r toạ độ suy rộng dư Sử dụng phương trình Lagrange dạng nhân tử, chúng zt( ), , zt ( ) . Vì vậy ta còn phải xác định các điều kiện ta thu được hệ phương trình vi phân – đại số mô tả 1 r chuyển động của hệ nhiều vật hôlônôm chịu liên kết giữ đầu của các toạ độ suy rộng và các vận tốc suy rộng dư. và lý tưởng [8, 9]. Từ các phương trình (2) và (5) ta có
2. Nguyễn Văn Khang, Nguyễn Văn Quyền, Lương Bá Trường, Nguyễn Văn Long éRpT ù -1 êú1 f qqzz(0), , (0), (0), , (0),0== 0, ( jr 1, ) sD==êú gss(,  ,)t (16) jf()11 r p ëêú3 û (8) Hệ (16) là phương trình vi phân chuyển động của hệ f ¶¶¶fffr nhiều vật có cấu trúc mạch vòng trong dạng các toạ độ jjj0qz (0)++= 0 (0) (0) 0, åå()ik() (9) suy rộng có dư. ik==11¶¶¶qztik (jr= 1, , ) Chú ý : Phương pháp tích phân số phương trình (16) thường gặp phải các sai số tích phân. Sau mỗi bước tích Từ phương trình liên kết (8) khi biết qq(0), , (0) ta 1 f  phân, do sai số tính toán, các giá trị ssii, không còn dễ dàng xác định được các giá trị gần đúng của thỏa mãn các phương trình ràng buộc vị trí và phương zz1(0), ,r (0) (chẳng hạn bằng phương pháp đồ thị). trình ràng buộc vận tốc và dẫn đến các sai lệch:   Lấy các giá trị này làm giá trị đầu, giải hệ phương trình fs(kk ,tk )¹= 0 , 1,2, , fs ( k , s kk , tk ) ¹= 0 , 1,2, (17) đại số phi tuyến (8) bằng phương pháp lặp Theo phương pháp Baumgarte, thay vì giải phương trình Newton-Raphson tìm được các giá trị đầu của các toạ độ  suy rộng dư khá chính xác. f (,ss ,)t = 0 Để xác định các giá trị đầu của các vận tốc suy rộng ta sẽ tiến hành giải phương trình: dư, ta xét hệ phương trình (9) f(,,)ssttt++= 2ab fss  (,,)2 fs (,) 0 (18) r ¶¶¶ffff Trong đó các hệ số được chọn thỏa mãn điều kiện sau: jjj()0zq (0)=-() 0 (0) - (0), ååki (10) 0 <£ab. Các số hạng 2af và b2 f đóng vai trò số ki==11¶¶¶zqtki (jr= 1, , ) hạng điều khiển. Nhờ việc sử phương trình (18) thay cho Hệ (10) là hệ r phương trình đại số tuyến tính của r ẩn phương trình (6) ta sẽ khử dần hoặc khử hoàn toàn được sai số tích lũy trong quá trình tích phân. Như thế thay cho là zz(0), , (0). Giải hệ phương trình đại số này ta xác 1 r giải hệ phương trình: định được các giá trị đầu của các vận tốc suy rộng dư. Ms,t s+-=FlT s ,tt p ss ,  , 0 Phương pháp khử các nhân tử Lagrange [8, 9, 12] ()s ()1 ( ) (19)  Khử các nhân tử Lagrange, biến đổi hệ phương trình f (,ss ,)t = 0 vi phân đại số (1), (2) về hệ phương trình vi phân thường ta sẽ tiến hành giải hệ phương trình sau: với số phương trình bằng số tọa độ suy rộng có dư của Ms(),t s+-=FlT () s ,tt p ( ss ,  , ) 0 hệ. s 1 (20) f(,ss ,)ttt++= 2ab fss  (, ,)2 fs (,) 0 Từ (4) ta suy ra TTTT Hệ phương trình (20) có thể viết lại dưới dạng ma trận RMs+= RFls Rp1 (11) éMs,,tt FT s ù éspssùé(,  ,)t ù éù êú() ()ê úê1 ú E êúê úê= ú (21) Trong đó Rq(, z )= êú, F s0,t l pss(, ,)t êú-1 êúë () rr´ û ëê ûëúê3 ûú êú-FFzq ëû trong đó éù¶¶f f éù¶¶f f é ù 2 êú11êú11pss(, ,)tt=- p() ss , , 2abF () s ,t s  +- f fs (,) t êú 32 ëûêút êú¶¶qq êú¶¶zzêú1 f éù¶¶FF êú1 r êú=-êúEs Ä + +2,t2aaF ss - ff - F = , F = ()nttt() z êúq êúêú¶¶s t êúêúë û êú¶¶f f êú¶¶f f êúrr êúrr (22) ê¶zz ¶ ú êú¶¶qq ëû1 r ëû1 f 3. Bài toán động lực học thuận tay máy robot Sử dụng định lý trực giao [9] hai khâu có liên kết chương trình R0TTF = (12) s Áp dụng phương pháp giải hệ phương trình vi phân – Từ (11) ta có đại số trình bày ở mục trên, ta nghiên cứu bài toán động TT RMs= Rp1 (13) lực học thuận tay máy robot hai khâu có liên kết chương Hệ hai phương trình (13) và (6) có thể viết lại dưới dạng trình như hình 1. Liên kết chương trình ở đây là đòi hỏi ma trận như sau điểm thao tác E chuyển động trên một quỹ đạo cho trước. Cho tay máy 2 khâu chuyển động trong mặt phẳng thẳng éùéùRMTT Rp êúêú1 s = (14) đứng. Thanh OO1 đồng chất chiều dài l , khối lượng êúêúF p 1 ëûëûêúêús 3 m , khối tâm tại C1. Thanh O1E đồng chất chiều dài l , Nếu ta đưa vào ký hiệu 1 2 T khối lượng m , khối tâm tại C . Điểm thao tác E có khối éùRM 2 2 D = êú (15) êúF lượng m chuyển động trên đường thẳng AB. ëûêús E thì từ (14) ta suy ra hệ phương trình vi phân sau :
3. Động lực học tay máy Robot có liên kết chương trình (26) Các lực suy rộng của các lực hoạt động không thế QQ1122==tt, (27) Phương trình ràng buộc điểm thao tác E luôn chuyển động trên một đường thẳng AB xx yy xy EA= EA hay EE+-=10 xxBA yy BA ll34 Thay tọa độ điểm E từ (24) vào phương trình ràng buộc trên ta có phương trình liên kết: fllll()qq,sincos=+ q q 12 13 1 14 1 (28) ++++-=ll23sin()qq 1 2 ll 24 cos () qq 1 2 ll 34 0 Sử dụng phương trình Lagrange loại 2 dạng nhân tử æö dT綶÷ T ¶P¶* f ç ÷ -=-+-Qkl () =1, 2 dt ç¶q ÷ ¶¶qqk ¶ q Hình 1. Tay máy Robot hai khâu èøk kk k Ta thu được hai phương trình chuyển động của robot. Chọn các tọa độ suy rộng là q1 vàq2 . Mômen quán tính é ù æöm ÷ êç 1 ++mmlm÷ 2 + +2cos mll q ú khối đối với trục quay đi qua khối tâm của các khâu lần êç 212122EE÷ ()ú êèøç 3 ÷ ú  1122 ê ú q lượt là JmlJml==, æö 1 CC11 22 ê çm2 ÷ 2 ú 1212 12 ê++ç ml÷ ú ê ç 3 E ÷ 2 ú  ë èø û Vận tốc góc của các khâu: wqwqq11212==+,. éùæöm æöm Tọa độ các khối tâm và điểm thao tác êúç ÷ ç 2 ÷ 2  ++ç mllEE÷ 12cos qq 2 + + ml÷ 2 2 êúç 23÷ ç ÷ éùl ëûêúèø èø êú1 éùx cos q1 æö C êú çm2 ÷ 2 êú1 = êú2 ; -+mmll2sinqq q -ç + mll÷ q  (29) êú ()212112EEç ÷ 122 êúylC êú1 èøç 2 ÷ ëû1 êú sin q1 ëûêú2 æöm ÷ (23) +++ç 1 mmgl÷ cos q éùl ç 211E ÷ éùêúl cosqqq++2 cos() èøç 2 ÷ xC êú11 12 êú2 = êú2 æöm êúylêú ç 2 ÷ êúC 2 ++ç mgl÷ cos()qq + ëû2 êúl sinqqq++ sin ç E ÷ 212 11() 12 èøç 2 ÷ ëûêú2 æö çll13cosqq 1- ll 14 sin 1 ÷ éùx éùllcosqqq++ cos =-tlç ÷ êúE êú11212() 1 ç ÷ = (24) ç++-+llcos()qq ll sin () qq÷ êúy êúllsinqqq++ sin èø23 1 2 24 1 2 ëûêúE ëûêú11212() éæömm æöù Động năng của tay máy: êúçç22÷÷2 çç+++mll÷÷cos qq ml êúçç23EE÷÷12 2 2 1 1111122222ëûêúèø èø TmvJ=++++1122CCww mvJ CC mv EE 2222211 22 æöæömm÷÷ ++çç22ml÷÷22qqq ++ mll  sin 222222222 ççEE÷÷22 122 2 Vớivxyvxyvxy=+;; =+   =+  èøèøçç32÷÷ (30) CCCCCCEEE111222 æöm ÷ Ta dễ dàng tính được biểu thức động năng của tay máy: ++ç 2 mgl÷ cos qq + ç E ÷ 212() éùæöæömmm+ m èøç 2 ÷ êúçç12E ÷÷2 2 çç+++÷÷lmllcos q êúçç62÷÷1122 2E =-tlllcos() qq + - ll sin () qq + êúèøèø2 2123122412() T = êúq1 êúæömm÷ Các phương trình vi phân (29), (30) và phương trình đại ++ç 2 E ÷l 2 êúç ÷ 2 êúçèø62÷ số (28) tạo thành hệ 3 phương trình vi phân – đại số mô ëûtả chuyển động của tay máy robot. éùæömm æö êúçç22÷÷2 ++ççmll÷÷cos qqq ++ ml (25) êúçç23EE÷÷12 2 2 1 2 4. Mô phỏng số ëûêúèø èø æömm÷ Để giải hệ phương trình vi phân đại số trên ta cần ++ç 2 E ÷l 2q2 ç ÷ 2 2 èøç 62÷ biết các tham số của hệ và các điều kiện đầu. Các tham số của robot được cho như sau Thế năng của tay máy: lm,==2é ùéùéùéù lm, 3.5 lmlm == 5, 5, 12ëê ûëûëûëûúêúêúêú 34  =++mgy12CCEE mgy m gy 12 m===200é kg,ùéùéù m 35 kg, m 25 kg , 12ê úêúêúE æöæömm÷÷ ë ûëûëû =++çç12mml÷÷sinqqq ++ ml sin + éù22 éù çç211EE÷÷ 212()Jkgm,Jkgm=⋅=⋅450 35 . èøèøçç22÷÷ 12ëê ûëûúêú
4. Nguyễn Văn Khang, Nguyễn Văn Quyền, Lương Bá Trường, Nguyễn Văn Long Giả sử các lực tác dụng lên các khâu dẫn có dạng tp=⋅0.1 sin(2tNm )éù ⋅ , 1 ëûêú tp=⋅0.1 sin(2tNm ) + 15000éù ⋅ . 2 ëûêú Các điều kiện đầu của các tọa độ suy rộng độc lập được cho trước, còn điều kiện đầu của các tọa độ suy rộng dư được xác định từ phương trình liên kết. Ta có éùrad qq(0)== 0.5781éùrad , (0) 1.5457êú , 11ëûêú êús ëû éùrad qq(0)== -0.8957éùrad , (0) -1.7494êú . 22ëûêú êú ëûs Sử dụng phần mềm MATLAB tính toán mô phỏng số. Hình 4. Sai số của phương trình liên kết Một phần các kết quả tính toán được trình bày trên các hình từ 2 đến 9. Trong đó hình 2 là đồ thị các góc quay Để có thể kiểm tra độ chính xác của thuật toán chúng ta biểu diễn dịch chuyển góc và vận tốc góc trên cùng một q1 và q2 . Hình 3 là đồ thị vận tốc các góc này. Hình 4 là hình vẽ như hình 5 và 6. Trong đó hình 5 là đồ thị của q đồ thị biểu thị sai số tính toán thể hiện qua phương trình 1 liên kết. Kết quả trên đồ thị này cho ta độ chính xác rất   và q1 còn hình 6 là đồ thị của q2 và q2 . Hình 7 là đồ cao của thuật toán. thị nhân tử Lagrange. Trong [9] đã chỉ ra ý nghĩa của nhân tử Lagrange. Hình 2. Đồ thị các góc khớp  Hình 5. Đồ thị q1 và q1 Hình 3. Đồ thị các vận tốc góc khớp  Hình 6. Đồ thị q2 và q2
5. Động lực học tay máy Robot có liên kết chương trình Trên hình 9 biểu diễn cấu hình của tay máy (robot arm configuration) theo thời gian. Trong đó đường màu đỏ là vị trí ban đầu của tay máy, đường màu tím là vị trí cuối của tay máy. 5. Kết luận Động lực học tay máy robot chịu các liên kết chương trình là bài toán có ý nghĩa khoa học và thực tế. Bài toán này liên quan trực tiếp với bài toán lập trình quỹ đạo cho chuyển động robot. Trong bài báo này đã trình bày các nghiên cứu về bài toán động lực học thuận robot có liên kết chương trình. Các kết quả mô phỏng số cho thấy phương pháp nghiên cứu và thuật toán đề xuất có khả năng áp dụng cao. Bài toán động lực học ngược đang Hình 7. Đồ thị nhân tử Lagrange theo thời gian được nghiên cứu trong nhóm và sẽ công bố trong thời Một trong các yêu cầu của bài toán động lực học tay máy gian tới. robot có liên kết chương trình là kiểm tra xem điểm thao tác E (hoặc khâu thao tác) có thực hiện đúng chuyển Lời cảm ơn động trên quỹ đạo chương trình hay không. Vấn đề này trong các tài liệu [1-7] chưa được đề cập đến. Trong bài Bài báo này được hoàn thành với sự tài trợ bởi Quỹ Phát báo này chúng tôi đặc biệt quan tâm đến vấn đề này. Dựa triển Khoa học và Công nghệ Quốc gia (NAFOSTED). trên kết quả giải hệ phương trình vi phân đại số, trên hình 8 biểu diễn quỹ đạo chuyển động của điểm thao tác E. Tài liệu tham khảo [1] Rosen A., E. Edelstein, Investigation of a new formulation of the Lagrange method for constrained dynamic systems, ASME-Journal of Applied Mechanics, vol. 64, pp.116-122, 1987. [2] Rosen A., Applying the Lagrange method to solve problems of control constraints, ASME-Journal of Applied Mechanics, vol. 66, pp.1013-1015, 1989. [3] Jankowski K., Dynamics of controlled mechanical systems with material and program constraints - I Theory, Mechanism and Machine Theory, vol.24, No.3, pp.175-179, 1989. [4] Jankowski K., Dynamics of controlled mechanical systems with material and program constraints-II Hình 8. Quỹ đạo điểm E trong mặt phẳng Methods of solution, Mechanism and Machine Theory, vol.24, No.3, pp.181-185, 1989. [5] Jankowski K., Dynamics of controlled mechanical systems with material and program constraints-III Illustrative examples, Mechanism and Machine Theory, vol.24, No.3, pp.187-193, 1989. [6] Do Sanh, Dinh Văn Phong, Do Dang Khoa, Tran Duc, Control of program motion of dynamic system using relative motion, Technische Mechanik, Band 28, Heft 3-4, pp. 211-218, 2008. [7] Vu Duc Binh, Do Dang Khoa, Phan Dang Phong, Do Sanh, Program motion of unloading manipulatos, Vietnam Journal of Science and Technology 56(5), 662-670, Hanoi 2018. [8] De Jalon J. G., E. Bayo, Kinematic and Dynamic Simulation of Multibody Systems. Springer-Verlag, New Hình 9. Cấu hình của robot York, 1994.
6. Nguyễn Văn Khang, Nguyễn Văn Quyền, Lương Bá Trường, Nguyễn Văn Long [9] Nguyễn Văn Khang: Động lực học hệ nhiều vật (in lần thứ 2). NXB Khoa học và Kỹ thuật, Hà Nội 2017. [10] Nguyen Van Khang: Ein Beitrag zur dynamischen Analyse ebener Koppelgetriebe mit mehreren Freiheitsgraden mit Hilfe der numerischen Lösung der Bewegungsdifferentialgleichungen. Diss. A, TH Karl-Marx-Stadt, 1973. [11] Nguyen Van Khang, Über eine Methode zur Lösung der Bewegungs- gleichungen von Mechanismen mit mehreren Freiheitsgraden. WZ der TH Karl-Marx-Stadt 17, N.1, S.59-70, 1975. [12] Nguyễn Văn Khang, Nguyễn Văn Quyền, Nghiên cứu so sánh một vài phương pháp giải hệ phương trình vi phân-đại số của hệ nhiều vật có cấu trúc mạch vòng. Tuyển tập Hội nghị Cơ học kỹ thuật toàn quốc, 03-05/8/2015, Tập 2, Tr. 147-158, Đà Nẵng 2016.