nghiên cứu thuật toán thích nghi, ứng dụng cho điều khiẻn chuyển động robot trong không gian decac luận văn thạc sĩ khoa học điều khiển và tự động hóa Hà Nội 9 - 2011... --- Đào Minh
Trang 1nghiên cứu thuật toán thích nghi, ứng dụng cho điều khiẻn chuyển động
robot trong không gian decac
luận văn thạc sĩ khoa học
điều khiển và tự động hóa
Hà Nội 9 - 2011
Trang 2-
Đào Minh Tuấn
nghiên cứu thuật toán thích nghi, ứng dụng cho điều khiẻn chuyển động
robot trong không gian decac
Chuyên ngành : Điều khiển và tự động hóa
luận văn thạc sĩ điều khiển và tự động hóa
người hướng dẫn khoa học :
TS Nguyễn Phạm Thục Anh
Hà Nội 9 – 2011
Trang 3Lời cam đoan
Tôi là Đào Minh Tuấn - học viên lớp cao học tự động hóa khóa 2009 - 2011 xin cam đoan:
- Luận văn tốt nghiệp thạc sĩ này là công trình nghiên cứu của bản thân
dưới sự hướng dẫn của TS Nguyễn Phạm Thục Anh
- Các kết quả trong luận văn tốt nghiệp thạc sĩ này là trung thực, không
phải sao chép toàn văn của bất kỳ công trình nào khác
Hà Nội, ngày 26 tháng 09 năm 2011
Học viên
Đào Minh Tuấn
Trang 4Lời cảm ơn
Xin chân thành cảm ơn Khoa Điện, Viện đào tạo sau đại học - Trường Đại Học Bách khoa Hà Nội đã tạo điều kiện thuận lợi giúp tác giả hoàn thành luận văn tốt nghiệp này
Xin chân thành cảm ơn sự hướng dẫn, chỉ bảo tận tình của cô Nguyễn Phạm Thục Anh trong suốt thời gian thực hiện đề tài, cảm ơn cô đã hướng
dẫn tác giả những kiến thức và kỹ năng cơ bản nhất để có thể hoàn thành được đề tài, cảm ơn cô đã luôn dành thời gian giúp đỡ tác giả, và cảm ơn cô vì tác giả đã học được nhiều điều từ cô
Xin chân thành cảm ơn thầy Giang Hồng bắc, thầy Phạm Đức Hùng, các đồng nghiệp khác công tác tại khoa điện - điện tử trường Đại học Sư phạm Kỹ thuật Hưng Yên đã ủng hộ, giúp đỡ và tạo điều kiện giúp đỡ tác giả hoàn thành luận văn
Xin cảm ơn các anh chị, bạn bè học viên lớp TĐH nghành ĐKTĐ khóa 2009-2011 đã ủng hộ, giúp đỡ và động viên trong thời gian học tập và nghiên cứu
Cuối cùng xin cảm ơn những người thân yêu, những người luôn động viên giúp đỡ về mọi mặt mọi hoàn cảnh
Qua một thời gian tác giả đã hoàn thành luận văn tốt nghiệp với đề tài
“Nghiên cứu thuật toán thích nghi ứng dụng cho điều khiển chuyển động robot trong không gian khớp và không gian Decac” Tuy đã cố gắng nhưng
chắc chắn vẫn không tránh khỏi những thiếu sót Kính mong nhận được sự thông cảm và đóng góp ý kiến của quý Thầy Cô và các bạn
Trang 5Mục lục
Lời cam đoan 1
Mục lục 2
Lời cảm ơn 4
Danh mục các ký hiệu, các chữ viết tắt 5
Danh mục các bảng 6
Danh mục các hình vẽ, đồ thị 7
MỞ ĐẦU 9
Chương 1 XÂY DỰNG MÔ HÌNH TOÁN HỌC CỦA ROBOT BA BẬC TỰ DO 11
1.1 Sơ lược quá trình phát triển của Robot công nghiệp 11
1.2 Ứng dụng Robot công nghiệp trong sản xuất 12
1.3 Các định nghĩa về Robot công nghiệp 12
1.3.3 Hệ toạ độ 14
1.3.4 Trường công tác của Robot 16
1.4 Lựa chọn Robot 16
1.5 Xác định động học thuận tau máy theo bảng Đ-H 17
1.6 Xác định điểm tác động cuối của tay máy 20
1.7 Bài toán động học ngược của tay máy Robot RRT 22
1.8 Xây dựng phương trình động lực học Robot RRT 23
1.8.1 Thiết lập phương trình động lực học 23
1.8.2 Mô phỏng phương trình động lực học Robot sử dụng Simulink 31
Chương 2: CÁC THUẬT TOÁN THÔNG DỤNG ĐIỀU KHIỂN ROBOT ROBOT 33
2.1 Phương pháp PD bù trọng trường 33
2.1.1 Cơ sở lý thuyết 33
2.1.2 Mô phỏng điều khiển Robot sử dụng phương pháp PD bù trọng trường 34 2.2 Phương pháp tuyến tính hóa chính xác 37
Trang 62.2.1 Cơ sở lý thuyết 37
2.2.2 Mô phỏng điều khiển Robot sử dụng phương pháp tuyến tính hóa chính xác 38
2.3 Điều khiển bền vững 42
2.3.1 Cơ sở lý thuyết phương pháp điều khiển trượt 42
2.3.2 Mô phỏng điều khiển Robot sử dụng phương pháp điều khiển trượt 43
2.4 Điều khiển thích nghi 46
2.4.1 Cơ sở lý thuyết phương pháp Slotine- Li 46
2.4.2 Mô phỏng điều khiển Robot sử dụng phương pháp Slotine-Li 48
Chương 3: THUẬT TOÁN ĐIỀU KHIỂN THÍCH NGHI VÀ ỨNG DỤNG CHO ĐIỀU KHIỂN CHUYỂN ĐỘNG CỦA ROBOT TRONG KHÔNG GIAN DECAC 53
3.1 Cơ sở lý thuyết 53
3.2 Thiết kế bộ điều khiển 55
3.3 Kết quả mô phỏng tín hiệu điều kihiển và tín hiẹu đầu ra của robot 56
TÀI LIỆU THAM KHẢO 58
Trang 7Danh mục các ký hiệu, các chữ viết tắt
IR Industrial Robot Người máy công nghiệp
hay Robot công nghiệp
AMF American Machine and
Foundry Company Công ty đúc và chế tạo máy của Mỹ DOF Degrees Of Freedom Bậc tự do của Robot hay tay máy
phân
RRT Ro bot RRT Một loại robot có ba bậc tự do sử
dụng trong công nghiệp
D-H Bảng tham số D-H của robot Biểu diễn tham số động học của
robot
Trang 8Danh mục các bảng
Bảng 1 Bảng D-H của robot RRT 19
Trang 9Danh mục các hình vẽ, đồ thị
Hình 1 Robot công nghiệp Kuka (Đức) 13
Hình 2 Minh họa tay máy 15
Hình 3 Quy tắc bàn tay phải 15
Hình 4 Trường công tác của Robot 16
Hình 5 Robot RRT 17
Hình 6 Các khâu của Robot RRT 17
Hình 7 Xác định tọa độ của Robot RRT 18
Hình 8 Các tham số hệ trục tọa độ Robot RRT 19
Hình 9 Mô hình Robot sử dụng simulink 31
Hình 10 Xác định G(Q) 32
Hình 11 Xác định V Q Q&( , ) 32
Hình 12 Xác định H(Q) 32
Hình 13 Sơ đồ khối hệ thống điều khiển Robot theo phương pháp PD bù trọng trường 35 Hình 14 Bộ điều khiển PD bù trọng trường 35
Hình 15 Đáp ứng đầu ra hệ thống 36
Hình 16 Đáp ứng đầu ra hệ thống 36
Hình 17 Đáp ứng đầu ra hệ thống với sự thay đổi tham số m2=0.5 và m3= 0.5 37
Hình 18 Sơ đồ khối hệ thống điều khiển Robot theo phương pháp tuyến tính hóa chính xác 39 Hình 19 Bộ điều khiển tuyến tính hóa chính xác 39
Hình 20 Khâu tạo tín hiệu đặt và giá trị đặt các khớp Qd, Qd' và Qd'' 40
Hình 21 Đáp ứng đầu ra bộ điều khiển tuyến tính hóa chính xác 40
Hình 22 Đáp ứng đầu ra hệ thống 41
Hình 23 Đáp ứng đầu ra hệ thống khi thay đổi tham số m2=0.5 và m3= 0.5 41 Hình 24 Sơ đồ khối hệ thống điều khiển Robot theo phương pháp điều khiển trượt
43
Trang 10Hình 25 Bộ điều khiển trượt 44
Hình 26 Khâu tạo tín hiệu đặt và giá trị đặt các khớp Qd, Qd' và Qd'' 44
Hình 27 Đáp ứng đầu ra bộ điều khiển trượt 45
Hình 28 Đáp ứng đầu ra bộ điều khiển trượt 45
Hình 29 Đáp ứng đầu ra bộ điều khiển trượt khi thay đổi tham số m2=0.5 và m3= 0.5 46 Hình 30 Sơ đồ khối hệ thống điều khiển Robot theo phương pháp điều khiển thích nghi 48 Hình 31 Bộ điều khiển thích nghi 49
Hình 32 Khâu cập nhật tham số 49
Hình 33 Khâu tạo tín hiệu đặt và giá trị đặt các khớp Qd, Qd' và Qd'' 50
Hình 34 Quá trình cập nhật tham số 50
Hình 35 Đáp ứng đầu ra bộ điều khiển thích nghi 51
Hình 36 Quá trình cập nhật tham số 51
Hình 37 Đáp ứng đầu ra bộ điều khiển thích nghi 52
Hình 38 Mô hình Điều khiển chuyển động của robot trong không gian Decac dùng thuật toán điều khiển thích nghi 55
Hình 39 Hinh Bộ điều khiển thích nghi điều khiển chuyển động robot trong không gian Decac 55
Hình 40 Hình Bộ cập tham số 56
Hình 41 Hình Quá trình cập nhập tham số cho bộ điều khiển 56
Hình 42 Hình Tín hiệu đầu ra của của robot 57
Hình 43 Hình Dáp ứng đầu ra bộ điều khiển thích nghi 57
Trang 11MỞ ĐẦU
Đặt vấn đề
Trong nền sản xuất công nghiệp hiện nay của nước ta cũng như của các nước khác trên thế giới, mức độ tự động hóa càng cao thì robot công nghiệp càng đóng vai trò quan trọng trong việc giảm cường độ lao động cho người lao động, tăng năng suất, độ chính xác của sản phẩm góp phần năng cao chất lượng, số lượng và giảm giá thành sản phẩm
Để tiếp cận, nghiên cứu và ứng dụng các loại robot công nghiệp trong sản xuất đòi hỏi người ký sư, kỹ thuật viên phải hiểu và nắm vững được kết cấu cơ khí, tham số động học của robot và các bộ điều khiển được ứng dụng trong các robot công nghiệp Chọn và thực hiện đề tài luật văn thạc sĩ “Nghiên cứu thuật toán thích nghi ứng dụng cho điều khiển chuyển động robot trong không gian khớp và không gian Decac” là một phương pháp tốt nhất hà hiệu qủa nhất để
tôi thực hiện được mục tiêu trên
Các hướng tiếp cận chính để thực hiện đề tài trên là áp dụng các kiến thức cơ
sở về lý thuyết điều khiển, động học và các phương pháp điều khiển về robot công nghiệp đã được trang bị trong quá trình học thạc sĩ
Mục đích của luận văn
Mục đích của luận văn là nghiên cứu về động học robot, các phương pháp điều khiển và phát triển thuật toán điều khiển thích nghi trong không gian Decac
Trong phạm vi của luận văn này tác giả tập trung nghiên cứu động học của robot ba bậc tự do RRT và ứng dụng các thuật toán điều khiển để điều khiển robot RRT, phát triển thật toán điều khiển thích nghi để điều khiển robot RRT trong không gian Decac
Trang 12Phương pháp thực hiện và bố cục
Để đạt được mục đích trên, trình tự nội dung nghiên cứu theo tuần tự từ xây dựng mô hình toán học của robot RRT, nghiên cứu các thuật toán thông dụng để điều khiển robot và ứng dụng vào điều khiển robot RRT, phát triển thuật toán điều khiển thích nghi để điều khiển robot RRT trong không gian làm việc
Nội dung của luận văn được tổ chức như sau:
Chương 1 xây dựng mô hình toán học của robot ba bậc tư do RRT
Chương 2 các thuật toán thông dụng điều khiển robot, ứng dụng để điều khiển robot RRTvà các kết quả mo phỏng
Chương 3 tập trung trình bày thuật toán điều khiển thích nghi và ứng dụng cho điều khiển chuyển động của robot trong không gian Decac
Trang 13Chương 1 XÂY DỰNG MÔ HÌNH TOÁN HỌC CỦA
ROBOT BA BẬC TỰ DO
1.1 Sơ lược quá trình phát triển của Robot công nghiệp
Thuật ngữ “Robot” xuất phát từ tiếng Séc “Robota” có nghĩa là công việc tạp
dịch trong vở kịch Rossum’s Universal Robots của Karel Capek (1921) Đến đầu thập kỷ 60, công ty Mỹ AMF (American Machine and Foundry Company) quảng cáo một loại máy tự động vạn năng và gọi là “Người máy công nghiệp” (Industrial Robot) Ngày nay người ta đặt tên người máy công nghiệp (hay Robot công nghiệp) cho những loại thiết bị có dáng dấp và một vài chức năng như tay người được điều khiển tự động để thực hiện một số thao tác sản xuất
Tiếp theo Mỹ, các nước khác bắt đầu sản xuất Robot công nghiệp: Anh (1967), Thuỵ Điển và Nhật (1968) theo bản quyền của Mỹ; CHLB Đức (1971); Pháp (1972); ở Ý (1973)
Có thể nói, Robot là sự tổ hợp khả năng hoạt động linh hoạt của các cơ cấu điều khiển từ xa với mức độ “tri thức” ngày càng phong phú của hệ thống điều khiển theo chương trình số cũng như kỹ thuật chế tạo các bộ cảm biến, công nghệ lập trình và các phát triển của trí khôn nhân tạo, hệ chuyên gia
Trong những năm sau này, việc nâng cao tính năng hoạt động của Robot không ngừng phát triển Các Robot được trang bị thêm các loại cảm biến khác nhau
để nhận biết môi trường đã có vị trí quan trọng trong chung quanh, cùng với những thành tựu to lớn trong lĩnh vực Tin học - Điện tử đã tạo ra các thế hệ Robot với nhiều tính năng đặc biệt, số lượng Robot ngày càng gia tăng, giá thành ngày càng giảm Nhờ vậy, Robot công nghiệp các dây chuyền sản xuất hiện đại
Mỹ là nước đầu tiên phát minh ra Robot, nhưng nước phát triển cao nhất trong lĩnh vực nghiên cứu chế tạo và sử dụng robot tại Nhật
Trang 141.2 Ứng dụng Robot công nghiệp trong sản xuất
Từ khi mới ra đời, Robot công nghiệp được áp dụng trong nhiều lĩnh vực dưới góc độ thay thế sức người Nhờ vậy các dây chuyền sản xuất được tổ chức lại, năng suất vỡ hiệu quả sản xuất tăng lên rõ rệt
Mục tiêu ứng dụng Robot công nghiệp nhằm góp phần nâng cao năng suất dây chuyền công nghệ, giảm giá thành, nâng cao chất lượng vỡ khả năng cạnh tranh của sản phẩm đồng thời cải thiện điều kiện lao động Đạt được các mục tiêu trên lỡ nhờ vỡ những khả năng to lớn của Robot như: làm việc không biết mệt mỏi, rất dễ dàng chuyển nghề một cách thành thạo, chịu được phóng xạ vỡ các môi trường làm việc độc hại, nhiệt độ cao, “cảm thấy” được cả từ trường và “nghe” được cả siêu âm Robot được dùng thay thế con người trong các trường hợp trên hoặc thực hiện các công việc tuy không nặng nhọc nhưng đơn điệu, dễ gây mệt mỏi, nhầm lẫn
Trong ngành cơ khí, Robot được sử dụng nhiều trong công nghệ đúc, công nghệ hàn, cắt kim loại, sơn, phun phủ kim loại, tháo lắp vận chuyển phôi, lắp ráp sản phẩm
1.3 Các định nghĩa về Robot công nghiệp
1.3.1 Định nghĩa Robot công nghiệp
Hiện nay có nhiều định nghĩa về Robot, có thể điểm qua một số định nghĩa như sau:
Định nghĩa Robot công nghiệp (theo tiêu chuẩn AFNOR (Pháp))
Robot công nghiệp lỡ một cơ cấu chuyển động tự động có thể lập trình, lặp lại các chương trình, tổng hợp các chương trình đặt ra trên các trục toạ độ; có khả năng định vị, định hướng, di chuyển các đối tượng vật chất: chi tiết, dao cụ, gá lắp theo những hành trình thay đổi đã chương trình hoá nhằm thực hiện các nhiệm vụ công nghệ khác nhau
Trang 15Định nghĩa Robot công nghiệp (theo RIA (Robot institute of America))
Robot là một tay máy vạn năng có thể lặp lại các chương trình được thiết kế
để di chuyển vật liệu, chi tiết, dụng cụ hoặc các thiết bị chuyên dùng thông qua các chương trình chuyển động có thể thay đổi để hoàn thành các nhiệm vụ khác nhau
Định nghĩa Robot công nghiệp (theo TOCT 25686-85 (Nga))
Robot công nghiệp lỡ một máy tự động, được đặt cố định hoặc di động được, liên kết giữa một tay máy vỡ một hệ thống điều khiển theo chương trình, có thể lập trình lại để hoàn thành các chức năng vận động vỡ điều khiển trong quá trình sản xuất
Hình 1 Robot công nghiệp Kuka (Đức)
Trang 161.3.2 Bậc tự do của Robot
Bậc tự do lỡ số khả năng chuyển động của một cơ cấu (chuyển động quay hoặc tịnh tiến) Để dịch chuyển được một vật thể trong không gian, cơ cấu chấp hành của Robot phải đạt được một số bậc tự do Nói chung cơ hệ của Robot là một
cơ cấu hở, do đó bậc tự do của nó có thể tính theo công thức:
5 1
suy rộng Trong từng thời điểm hoạt động, các toạ độ suy rộng xác định cấu hình
của Robot bằng các chuyển dịch dài hoặc các chuyển dịch góc cuả các khớp tịnh tiến hoặc khớp quay (hình 1.1) Các toạ độ suy rộng còn được gọi là biến khớp
Trang 17Hình 2 Minh họa tay máy
Các hệ toạ độ gắn trên các khâu của Robot phải tuân theo qui tắc bàn tay phải: Dùng tay phải, nắm hai ngón tay út và áp út vào lòng bàn tay, xoè 3 ngón: cái, trỏ
và giữa theo 3 phương vuông góc nhau, nếu chọn ngón cái là phương và chiều của trục z, thì ngón trỏ chỉ phương, chiều của trục x và ngón giữa sẽ biểu thị phương, chiều của trục y (hình 1.2)
Hình 3 Quy tắc bàn tay phải
Trong Robot ta thường dựng chữ O và chỉ số n để chỉ hệ toạ độ gắn trờn khâu thứ n Như vậy hệ toạ độ cơ bản (Hệ toạ độ gắn với khâu cố định) sẽ được ký hiệu
là O0; hệ toạ độ gắn trên các khâu trung gian tương ứng sẽ là O1, O2, , On-1, hệ toạ
độ gắn trên khâu chấp hành cuối ký hiệu lỡ On
Trang 181.3.4 Trường công tác của Robot
Trường công tác (Workspace or Range of motion) hay vùng làm việc - không gian công tác của Robot là toàn bộ thể tích được quét bởi khâu chấp hành cuối khi Robot thực hiện tất cả các chuyển động có thể Trường công tác bị ràng buộc bởi các thông số hình học của Robot cũng như các ràng buộc cơ học của các khớp; ví
dụ, một khớp quay có chuyển động nhỏ hơn một góc 3600 Người ta thường dùng hai hình chiếu để mô tả trường công tác của một Robot (hình 1.3)
Hình 4 Trường công tác của Robot
1.4 Lựa chọn Robot
Robot được lựa chọn trong bài tập này là Robot RRT với 2 khớp quay và một khớp tịnh tiến Với trường tương tác rộng có thể được sử dụng trong các ứng dụng thực tế:
Trang 19Khớp 3 là khớp tịnh tiến dọc theo trục Z2 một đoạn r3
1.5 Xác định động học thuận tau máy theo bảng Đ-H
Robot RRT có ba khâu như hình vẽ:
Hình 6 Các khâu của Robot RRT
Trang 20Gắn hệ trục tọa độ lên các khâu
Giả định vị trí ban đầu và chọn gốc tọa độ O0 của Robot như hình vẽ Các trục Z đặt cùng phương với các trục khớp
Hình 7 Xác định tọa độ của Robot RRT
Chọn Z1 quay tương đối một góc 90o so với trục Z0, đây chính là góc quay quanh trục X0 một góc α1 (phép biến đổi ROT(X0, α1) trong biểu thức tính An) Nghĩa là trục x0 vuông góc với z0 và trục z1 Ta chọn chiều của x0 từ trái qua phải thì góc quay α1 = 900(chiều dương ngược chiều kim đồng hồ) Đồng thời ta cũng thấy O1 tịnh tiến một đoạn dọc theo z0 so với O0, đó chính là phép đổi Trans(0,0,d1), tịnh tiến dọc treo z0 một đoạn d1, các trục y0 và y1= xác định theo quy tắc bàn tay phải như hình vẽ
Tiếp tục ta chọn gốc tọa độ O2 đặt trùng với O1 vì trục khớp thứ ba và trục khớp thứ 2 cắt nhau tại O1 Trục z2 trùng phương với trục khớp thứ ba, tức là đã quay đi một góc 900 so với z1 quanh trục y1 Phép biến đổi này không có trong biểu thức tính An nên không dung được, ta phải chọn lại vị trí ban đầu của Robot nghĩa là
ta chỉ cần thay đổi vị trí của khâu thứ ba
Trang 21Hình 8 Các tham số hệ trục tọa độ Robot RRT
Theo như hình vẽ, O2 vẫn được đặt trùng với O1, trục z2 có phương thẳng đứng, tức là ta phải quay trục z1 thành trục z2 quanh trục x1 một goác -900 (α2 = -
900)
Đầu cuối của khâu thứ ba không có khớp, ta đặt O3 tại điểm giữa các ngón tay, trục x3 và trục z3 chọn như hình vẽ, như vậy, ta tịnh tiến gốc tọa độ theo z2 một đoạn d3 (phép Trans(0,0,d3)) suy ra đây là khâu tịnh tiến và d3 là biến
Như vậy, đã gắn ba hệ tọa độ nên ba khớp của Robot và bảng Đ-H được xác định như sau:
Trang 23%Toa do diem tac dong cuoi cua tay may
%so voi he toa do O2(T32)
T32=A3
%Toa do diem tac dong cuoi cua tay may
%so voi he toa do O1(T31)
T31=A2*T32
%Toa do diem tac dong cuoi cua tay may
%so voi he toa do O(T3)
Với tọa độ của điểm tác động cuối được khai báo trong Matlab như sau:
Te=[Nx Ox Ax Px;Ny Oy Ay Py;Nz Oz Az Pz;0 0 0 1]%diemtacdongcuoi
Trang 24Ta có hệ phương trình động học của Robot RRT như sau:
n S o
1.7 Bài toán động học ngược của tay máy Robot RRT
Xuất phát từ xây dựng hệ pt động học của Robot RRT ta đã có:
A11=inv(A1)%Ma tran nghich dao cua A1
VP=A2*A3 %ve phai
Te=[Nx Ox Ax Px;Ny Oy Ay Py;Nz Oz Az Pz;0 0 0 1]
%diem tac dong cuo
VT=A11*Te %ve trai
Trang 25ATAN o o ATAN o o
θθ
Trang 26Các biến được khai trong Matlab là
Tọa độ điểm C1 trên thanh nối 1 là PC1, Pc2 Pc3 được khai báo trong Matab là
%toa do C1 tren he toa do{0}
Trang 27PC1 11
1
0 0 1/ 2
c c c
x y
2
2 0
%van toc tai C1 (Vc1)
Vc1=diff(Pc1,t);
Vc1=Vc1(1:3)
Trang 28%van toc tai C2(Vc2)
& & &
& & & &
& & & & & &
& & & & & &
5.3
Vận tốc góc tại Pc1 được khai báo trong Matlab như sau
%van toc goc tai Pc1(w1)
w1=[0 0 diff(q1,t)]
Trang 29%van toc goc tai Pc2(w2)
Động năng của của các khâu lần lượt được khai báo trong Matlab như sau
%dong nang khau 1(K1)
(m2*((d2*cos(q1(t))*sin(q2(t))*diff(q1(t), t))/2 -
(d2*cos(q2(t))*sin(q1(t))*diff(q1(t), t))/2 - (d2*cos(q1(t))*sin(q2(t))*diff(q2(t), t))/2 + (d2*cos(q2(t))*sin(q1(t))*diff(q2(t), t))/2)^2)/2 + (Iyy2*diff(q2(t), t)^2)/2
K3 = (m3*(cos(q1(t))*cos(q2(t))*diff(q3(t), t) +
sin(q1(t))*sin(q2(t))*diff(q3(t), t) + cos(q1(t))*sin(q2(t))*q3(t)*diff(q1(t), t) -
Trang 30cos(q2(t))*sin(q1(t))*q3(t)*diff(q1(t), t) - cos(q1(t))*sin(q2(t))*q3(t)*diff(q2(t), t) + cos(q2(t))*sin(q1(t))*q3(t)*diff(q2(t), t))^2)/2 +
(m3*(cos(q1(t))*sin(q2(t))*diff(q3(t), t) - cos(q2(t))*sin(q1(t))*diff(q3(t), t) - cos(q1(t))*cos(q2(t))*q3(t)*diff(q1(t), t) + cos(q1(t))*cos(q2(t))*q3(t)*diff(q2(t), t)
- sin(q1(t))*sin(q2(t))*q3(t)*diff(q1(t), t) + sin(q1(t))*sin(q2(t))*q3(t)*diff(q2(t), t))^2)/2 + (Iyy3*diff(q2(t), t)^2)/2 + (Izz3*diff(q1(t), t)^2)/2
Thế năng của các khâu được khai báo trong Matlab như sau:
%the nang cua khau1(P1)