5.1.1. Tín hiệu đặt cho phương pháp điều khiển PD
Các tín hiệu tham chiếu này đảm bảo cho dáng đi của robot cân bằng trên mặt phẳng. Giá trị khởi tạo đầu tiên cho cho các tín hiệu tham chiếu là ref = [2.29, - 13.35, 12.60, 5.78] theo thứ tự là . Trạng thái của robot ở giá trị tham chiếu khởi tạo này là trạng thái hai chân chạm đất. Qua khối Create references [phần 3.1.2] thì các tín hiệu sẽ được tạo như hình bên dưới.
Hình 5.1. Các tín hiệu đặt cho phƣơng pháp điều khiển PD
Ta thấy rằng các tín hiệu tham chiếu cho phương pháp điều khiển PD này rất lý tưởng. Ví dụ như đối với tín hiệu tham chiếu cho góc chẳng hạn, tín hiệu tham
chiếu cho góc này là một đường thẳng và không thay đổi trong suốt thời gian mà robot đi. Điều đó là không đúng với thực tế, bởi vì, cơ thể con người đi thì góc của thân người so với trục thẳng đứng thì phải có độ dịch chuyển. Có nghĩa là dáng người đi sẽ có sự chuyển động của thân, nên sẽ có sự thay đổi trong góc , cho dù sự thay đổi đó có nhỏ. Đối với các tín hiệu tham chiếu cho các góc khác cũng như vậy, các tín hiệu không có sự thay đổi nào trong suốt quá trình di chuyển của robot.
5.1.2. Tín hiệu đặt cho phương pháp điều khiển mômen tính
Hình 5.2. Các tín hiệu đặt cho phƣơng pháp điều khiển mômen tính
Với các tín hiệu tham chiếu sử dụng trong phương pháp điều khiển mômen tính, ta thấy các tín hiệu tham chiếu này “thực tế” hơn so với tín hiệu tham chiếu dành cho phương pháp điều khiển PD. Chẳng hạn với tín hiệu tham chiếu dành cho góc , tín hiệu tham chiếu dành cho góc này có sự thay đổi tăng giảm trong quá
trình di chuyển của robot. Điều này hợp lý bởi vì dáng thân người của robot khi đi phải có sự lắc lư trong di chuyển. Với các tín hiệu khác cũng có sự thay đổi. Ta có thể dùng tín hiệu tham chiếu này cho quá trình di chuyển của robot.
5.2. Kết quả mô phỏng bằng phƣơng pháp điều khiển PD
Đáp ứng tín hiệu thực so với tín hiệu tham chiếu
Để đảm bảo cho robot có thể đi cân bằng trên một mặt phẳng, đòi hỏi các tín hiệu đặt vào cho hệ robot phải có độ bám dính tốt với các tín hiệu tham chiếu. Khảo sát và rút trích giá trị các tọa độ suy rộng cần thiết từ ngõ ra của hệ thống, sau đó vẽ lên cùng hệ trục các tọa độ suy rộng này cùng với các tín hiệu tham chiếu, ta thấy đáp ứng các tín hiệu thực của hệ thống bám dính khá tốt với tín hiệu tham chiếu.
Đáp ứng tín hiệu ra của hệ thống
Hình 5.3 Tín hiệu ra của hệ thống khi dùng phƣơng pháp PD
Hình 5.4. So sánh tín hiệu góc thực và tham chiếu theo phƣơng pháp PD
Hình 5.5. So sánh tín hiệu góc thực và tham chiếu theo phƣơng pháp PD
Giá trị tín hiệu góc giữa tín hiệu thực và tín hiệu tham chiếu
Hình 5.6. So sánh tín hiệu góc thực và tham chiếu theo phƣơng pháp PD
Giá trị tín hiệu góc giữa tín hiệu thực và tín hiệu tham chiếu
Hình 5.7. So sánh tín hiệu góc thực và tham chiếu theo phƣơng pháp PD
Hình 5.8. Mô phỏng robot bƣớc đi trong 2 giây Mômen quay tác động lên các động cơ của robot
Hình 5.9. Mômen quay tác động lên các động cơ của robot theo phƣơng pháp PD
5.3. Kết quả mô phỏng bằng phƣơng pháp điều khiển mômen tính
Với phương pháp điều khiển mômen tính và sử dụng các tín hiệu tham chiếu cho phương pháp này, ta thấy đáp ứng của tín hiệu ra so với tín hiệu tham chiếu là khá tốt. Gần như tín hiệu ra là bám dính hoàn toàn vào tín hiệu tham chiếu. Điều này có nghĩa là sử dụng phương pháp này, độ sai biệt giữa tín hiệu thực và tín hiệu tham chiếu là rất nhỏ, gần như là không có. Và với một tín hiệu tham chiếu tốt, điều chắc chắn là robot sẽ di chuyển tốt theo quỹ đạo của tín hiệu tham chiếu. Các hình bên dưới thể hiện việc bám dính của tín hiệu ra thực so với tín hiệu tham chiếu.
Đáp ứng tín hiệu ra của hệ thống
Hình 5.10. Tín hiệu ra của hệ thống khi dùng phƣơng pháp mômen tính
Hình 5.11. So sánh tín hiệu góc thực và tham chiếu theo phƣơng pháp mômen tính
Giá trị tín hiệu góc giữa tín hiệu thực và tín hiệu tham chiếu
Hình 5.12. So sánh tín hiệu góc thực và tham chiếu theo phƣơng pháp mômen tính
Giá trị tín hiệu góc giữa tín hiệu thực và tín hiệu tham chiếu
Hình 5.13. So sánh tín hiệu góc thực và tham chiếu theo phƣơng pháp mômen tính
Giá trị tín hiệu góc giữa tín hiệu thực và tín hiệu tham chiếu
Mô hình robot chuyển động trong 2 giây
Hình 5.15. Mô phỏng robot bƣớc đi trong 2 giây
Mômen quay tác động lên các động cơ của robot
Hình 5.16. Mômen quay tác động lên các động cơ của robot theo phƣơng pháp mômen tính
5.4. Chƣơng trình mô phỏng hoạt hình trên Matlab/Simulink
Hình 5.17. Giao diện chƣơng trình mô phỏng hoạt hình robot đi trên mặt phẳng
Một số chức năng của chương trình:
Tham số chương trình: Ô này dùng để nạp file cấu hình cho robot, đó là file
ThamSo.m, file cấu hình này chứa các thông số cấu hình cho hệ thống như: thời gian lấy mẫu, khối lượng của các thành phần của robot, giá trị các tọa độ khởi động đầu tiên, tốc độ…
Mô hình mô phỏng: Ô này dùng để nạp file mô hình mô phỏng, trong chương trình này có là hai file: PD.mdl (dành cho phương pháp điều khiển PD) và CT.mdl
(dành cho phương pháp điều khiển mômen tính), xây dựng trên các khối trong
Thời gian mô phỏng: Ô này dùng để nhập vào thời gian cần mô phỏng để robot đi trong bao nhiêu giây. Ví dụ ở trên lấy thời gian mô phỏng là 10 giây. Lưu ý là thời gian mô phỏng nhập càng lớn thì thời gian thực thi chương trình mô phỏng bên dưới sẽ càng lâu. Thường thời gian được chọn là từ 0-10 giây.
Nút nhấn “Mô Phỏng”: Nút này sẽ lấy tất cả các tham số cũng như file cấu hình + mô phỏng để tính toán các tín hiệu ra của robot, các tín hiệu này sẽ được lưu vào một biến data tạm thời trên workspace của Matlab.
Hoạt hình: Khi nhấn nút Play (dấu >) thì chương trình sẽ lấy biến data lưu tạm ở workspace để đưa ra, vẽ lên hệ trục mô hình chuyển động của robot. Nhấn nút Pause (dấu ||) thì robot tạm dừng, nhấn nút Stop (dấu ) thì robot sẽ dừng hẳn việc di chuyển. Ngoài ra chúng ta còn có thể điều chỉnh tốc độ mô phỏng (vẽ ảnh) với các độ chậm khác nhau từ một ListBox.
CHƢƠNG 6
KẾT LUẬN VÀ
HƢỚNG PHÁT TRIỂN
6.1. Kết luận
Một cách tổng quát, luận văn này đã xây dựng giải thuật điều khiển mômen tính (computed torque) cho đối tượng là robot đi hai chân (biped robot). Sử dụng mô hình biped robot có sẵn trong thư viện, tác giả đã xây dựng các mô phỏng cho giải thuật điều khiển mômen tính đề nghị. Kết quả mô phỏng cho thấy hệ thống hoạt động tin cậy.
Một cách chi tiết, dựa trên lý thuyết về điều khiển tự động và lý thuyết về robot, áp dụng các phương trình động học cho hệ robot cũng như dựa trên nền tảng thư viện robot đã có, đề tài đã đạt được những điểm sau:
Trình bày một cách tổng quát về điều khiển tự động, những thành phần cơ bản trong một hệ thống điều khiển tự động.
Trình bày về lịch sử hình thành, phát triển và khả năng ứng dụng rộng rãi của ngành công nghiệp robot trong các lĩnh vực khoa học kỹ thuật, nghiên cứu và đời sống. Từ đó cho thấy đây là một trong những nhân tố ảnh hưởng rất lớn đến sự phát triển khoa học kỹ thuật của con người trong tương lai.
Tìm hiểu rõ về mô hình của đối tượng robot đi bằng hai chân (biped robot), đây cũng là một đối tượng quan trọng trong nền công nghiệp robot. Mô hình robot này được xây dựng dựa trên các thành phần như: các liên kết (đầu-thân, đùi, ống chân) và các khớp nối. Sử dụng những thành phần đó để đưa ra phương trình động học cho robot dựa trên cơ hệ Lagrange.
Tìm hiểu phương pháp và cách thức xây dựng bộ điều khiển tỉ lệ - vi phân (PD) trong thư viện có sẵn: về cách tạo các tín hiệu tham chiếu, cách tạo bộ điều khiển
PD dựa trên các thành phần có trong Matlab/Simulink, cách robot hoạt động dưới bộ điều khiển này, các tín hiệu vào-ra của bộ điều khiển – hệ robot.
Dựa vào mô hình động học của robot và những tham số trong hệ robot này, đề tài đã đề xuất giải thuật điều khiển mômen tính (computed torque) cho hệ robot đi hai chân, sơ đồ mô phỏng trên Simulink được xây dựng. Kết quả mô phỏng cho thấy giải thuật đề nghị hoạt động tốt, phù hợp với thiết kế.
Việc mô phỏng các tín hiệu vào-ra của robot cho thấy tín hiệu điều khiển cho robot di chuyển bám tương đối tốt với tín hiệu tham chiếu mong muốn. Đề tài cũng có chương trình hoạt hình để thể hiện cho thấy việc robot di chuyển bước đi như thế nào trong không gian 2 chiều.
Phương pháp điều khiển mômen tính có thể xem như bao gồm 2 bước:
o Bước 1: tuyến tính hóa đối tượng điều khiển bằng hồi tiếp trạng thái
o Bước 2: xây dựng giải thuật điều khiển PD cho đối tượng đã được tuyến tính hóa.
So với việc thiết kế bộ điều khiển PD trực tiếp cho đối tượng phi tuyến ban đầu, phương pháp mômen tính cho phép xác định các thông số của bộ điều khiển một cách trực quan và dễ dàng hơn dựa vào động học mong muốn của hệ thống.
6.2. Hƣớng phát triển
Dựa trên những mặt đạt được và những điểm chưa đạt được, đề tài này có thể được phát triển rộng thêm theo các hướng như sau:
Xây dựng một mô hình robot thực tế chứ không chỉ gói gọn trong việc mô phỏng trên máy tính.
Cải thiện phương pháp điều khiển bằng cách tạo ra các tín hiệu tham chiếu tốt hơn. Có thể xây dựng một mô hình robot với đầy đủ các thành phần: đầu, cánh tay,
chân, bàn chân với một phương trình động học đầy đủ. Mô phỏng robot trên hệ tọa độ 3 chiều.
Xây dựng một bộ điều khiển để không chỉ có thể làm cho robot có thể đi, mà còn có thể chạy, nhảy, leo cầu thang…
TÀI LIỆU THAM KHẢO
TIẾNG ANH
[1] Andrew L. Kun, W. Thomas Miller, Adaptive dynamic balance of a biped robot using neural networks, Robotics Laboratory, ECE Dept., University of New Hampshire, Durham, NH 03824
[2] Codrin Codrin Pasca, History of History of Robotics Robotics, ENRICHMENT MINI ENRICHMENT MINI-COURSE COURSE, Robotics Robotics – Intelligent Connection of the Perception to Action Intelligent Connection of the Perception to Action, University of Ottawa University of Ottawa, 2003
[3] Daniel Zaldivar Navarro, A Biped Robot Design, Freie Universität Berlin, Fachbereich Mathematik und Informatik, 2006
[4] Dusko Kati´c Miomir Vukobratovi´c, Survey of intelligent control algorithms for humanoid robots, Robotics Laboratory, Mihaido Pipin Institute
[5] Duy Nguyen-Tuong, Matthias Seeger, Jan Peters, Computed Torque Control with Nonparametric Regression Models, Max Planck Institute for Biological Cybernetics
[6] Ehsan Azadi Yazdi, Aria Alasty, Stabilization of Biped Walking Robot Using the Energy Shaping Method, Journal of Computational and Nonlinear Dynamics, 2008
[7] Hamid Benbrahim, Biped dynamic walking using reinforcement learning, Doctor of Philosophy in Electrical Engineering, December, 1996
[8] Joan Batlle, Enric Hospital, Jeroni Salellas and Marc Carreras, DEVELOPMENT OF A BIPED ROBOT, Institut d‟Informàtica i Aplicacions - Universitat de Girona
[9] L. C. KWEK, E. K. WONG, C. K. LOO and M. V. C. RAO, Application of Active Force Control and Iterative Learning in a 5-Link Biped Robot, Journal of Intelligent and Robotic Systems, 37: 143–162, 2003.
[10] Miomir Vukobratovic, Humanoid Robotics: Past, Present State, Future , The 4th Serbian-Hungarian Joint Symposium on Intelligent Systems, 2006
[11] Nima Jamshidi and Mostafa Rostami, Gait Optimization of Biped Robot during Double Support Phase by Pure Dynamic Synthesis, American Journal of Applied Sciences, 2008
[12] N. Khraief, N. K. M‟Sirdi and O. Licer, Minimum Energy Control based on Limit Cycles (CLC): Application for walking of a biped robot, Laboratory of Robotics of Versailles, Université de Versailles Saint Quentin en Yvelines, France
[13] Olli Haavisto Heikki Hyötyniemi, SIMULATION TOOL OF A BIPED WALKING ROBOT MODEL, Helsinki University of Technology Control Engineering Laboratory, 2004
[14] Poramate Manoonpong, Tao Geng, Tomas Kulvicius, Bernd Porr, Florentin Wo¨ rgo¨ tter, Adaptive, Fast Walking in a Biped Robot under Neuronal Control and Learning, PLoS Computational Biology, Volume 3, Issue 7, e134
[15] Vishnu Vardhan Madadi, and Sabri Tosunoglu, Member, ASME, Design and Development of a Biped Robot, The 7th IEEE International Symposium on Computational Intelligence in Robotics and Automation, 2007
[16] Yoshihiko Takahashi, and Masayoshi Kohda, Simple Humanoid Biped Robot with PIC Microcomputer for University Education, Journal of Robotics and Mechatronics Vol.17 No.2, 2005
[17] Zuoshi Song, Jianqiang Yi, Dongbin Zhao, Xinchun Li, A computed torque
controller for uncertain robotic manipulator systems: Fuzzy approach, Laboratory of Complex Systems and Intelligence Science, Institute of Automation, Chinese Academy of Sciences, Beijing 100080, P.R.China
TIẾNG VIỆT
[18] Nguyễn Thị Phương Hà, Huỳnh Thái Hoàng, L thuyết điều khiển tự động, Nhà xuất bản ĐHQG Tp. Hồ Chí Minh, 2005
[19] Nguyễn Văn Giáp, Ứng dụng Matlab trong điều khiển tự động, Nhà xuất bản ĐHQG Tp. Hồ Chí Minh, 2000
PHỤ LỤC
A.1. Các hệ số trong ma trận A(q) sử dụng các khối trong Matlab/Simulink để chứa A.1.1. Cột thứ nhất của ma trận A với A.1.1. Cột thứ nhất của ma trận A với
A11=m0+2*m1+2*m2 A21=0 A31=(-2*m1*r0-2*m2*r0)*cos(u(3))+(-l1*m2-m1*r1)*cos(u(3)-u(4))- l1*m2*cos(u(3)-u(5))-m1*r1*cos(u(3)-u(5))-m2*r2*cos(u(3)-u(4)+u(6))- m2*r2*cos(u(3)-u(5)+u(7)) A41=(l1*m2+m1*r1)*cos(u(3)-u(4))+m2*r2*cos(u(3)-u(4)+u(6)) A51=(l1*m2+m1*r1)*cos(u(3)-u(5))+m2*r2*cos(u(3)-u(5)+u(7)) A61=-m2*r2*cos(u(3)-u(4)+u(6)) A71=-m2*r2*cos(u(3)-u(5)+u(7))
A.1.2. Cột thứ hai của ma trận A với
A12=0 A22=m0+2*m1+2*m2 A32=(2*m1*r0+2*m2*r0)*sin(u(3))+(l1*m2+m1*r1)*sin(u(3)- u(4))+l1*m2*sin(u(3)-u(5))+m1*r1*sin(u(3)-u(5))+m2*r2*sin(u(3)- u(4)+u(6))+m2*r2*sin(u(3)-u(5)+u(7)) A42=(-l1*m2-m1*r1)*sin(u(3)-u(4))-m2*r2*sin(u(3)-u(4)+u(6)) A52=(-l1*m2-m1*r1)*sin(u(3)-u(5))-m2*r2*sin(u(3)-u(5)+u(7)) A62=m2*r2*sin(u(3)-u(4)+u(6)) A72=m2*r2*sin(u(3)-u(5)+u(7))
A.1.3. Cột thứ ba của ma trận A với A13=(-2*m1*r0-2*m2*r0)*cos(u(3))+(-l1*m2-m1*r1)*cos(u(3)-u(4))- A13=(-2*m1*r0-2*m2*r0)*cos(u(3))+(-l1*m2-m1*r1)*cos(u(3)-u(4))- l1*m2*cos(u(3)-u(5))-m1*r1*cos(u(3)-u(5))-m2*r2*cos(u(3)-u(4)+u(6))- m2*r2*cos(u(3)-u(5)+u(7)) A23=(2*m1*r0+2*m2*r0)*sin(u(3))+(l1*m2+m1*r1)*sin(u(3)- u(4))+l1*m2*sin(u(3)-u(5))+m1*r1*sin(u(3)-u(5))+m2*r2*sin(u(3)- u(4)+u(6))+m2*r2*sin(u(3)-u(5)+u(7)) A33=J0+2*J1+2*J2+2*l1^2*m2+2*m1*r0^2+2*m2*r0^2+2*m1*r1^2+2*m2*r 2^2+(2*l1*m2*r0+2*m1*r0*r1)*cos(u(4))+(2*l1*m2*r0+2*m1*r0*r1)*cos(u(5 ))+2*m2*r0*r2*cos(u(4)-u(6))+2*l1*m2*r2*cos(u(6))+2*m2*r0*r2*cos(u(5)- u(7))+2*l1*m2*r2*cos(u(7)) A43=-J1-J2-l1^2*m2-m1*r1^2-m2*r2^2+(-l1*m2*r0-m1*r0*r1)*cos(u(4))- m2*r0*r2*cos(u(4)-u(6))-2*l1*m2*r2*cos(u(6)) A53=-J1-J2-l1^2*m2-m1*r1^2-m2*r2^2+(-l1*m2*r0-m1*r0*r1)*cos(u(5))- m2*r0*r2*cos(u(5)-u(7))-2*l1*m2*r2*cos(u(7)) A63=J2+m2*r2^2+m2*r0*r2*cos(u(4)-u(6))+l1*m2*r2*cos(u(6)) A73=J2+m2*r2^2+m2*r0*r2*cos(u(5)-u(7))+l1*m2*r2*cos(u(7))
A.1.4. Cột thứ tƣ của ma trận A với
A14=(l1*m2+m1*r1)*cos(u(3)-u(4))+m2*r2*cos(u(3)-u(4)+u(6)) A24=(-l1*m2-m1*r1)*sin(u(3)-u(4))-m2*r2*sin(u(3)-u(4)+u(6)) A34=-J1-J2-l1^2*m2-m1*r1^2-m2*r2^2+r0*(-l1*m2-m1*r1)*cos(u(4))- m2*r0*r2*cos(u(4)-u(6))-2*l1*m2*r2*cos(u(6)) A44=J1+J2+l1^2*m2+m1*r1^2+m2*r2^2+2*l1*m2*r2*cos(u(6)) A54=0 A64=-J2-m2*r2^2-l1*m2*r2*cos(u(6))
A74=0
A.1.5. Cột thứ năm của ma trận A với
A15=(l1*m2+m1*r1)*cos(u(3)-u(5))+m2*r2*cos(u(3)-u(5)+u(7)) A25=(-l1*m2-m1*r1)*sin(u(3)-u(5))-m2*r2*sin(u(3)-u(5)+u(7)) A35=-J1-J2-l1^2*m2-m1*r1^2-m2*r2^2+r0*(-l1*m2-m1*r1)*cos(u(5))- m2*r0*r2*cos(u(5)-u(7))-2*l1*m2*r2*cos(u(7)) A45=0 A55=J1+J2+l1^2*m2+m1*r1^2+m2*r2^2+2*l1*m2*r2*cos(u(7)) A65=0 A75=-J2-m2*r2^2-l1*m2*r2*cos(u(7))
A.1.6. Cột thứ sáu của ma trận A với
A16=-m2*r2*cos(u(3)-u(4)+u(6)) A26=m2*r2*sin(u(3)-u(4)+u(6)) A36=J2+m2*r2^2+m2*r0*r2*cos(u(4)-u(6))+l1*m2*r2*cos(u(6)) A46=-J2-m2*r2^2-l1*m2*r2*cos(u(6)) A56=0 A66=J2+m2*r2^2 A76=0
A.1.7. Cột thứ bảy của ma trận A với
A17=-m2*r2*cos(u(3)-u(5)+u(7)) A27=m2*r2*sin(u(3)-u(5)+u(7))
A47=0
A57=-J2-m2*r2^2-l1*m2*r2*cos(u(7)) A67=0
A77=J2+m2*r2^2
A.2. Các hệ số trong vector b(q) sử dụng các khối trong Matlab/Simulink để chứa
b1=-(-u(19)- u(21)+2*m1*u(10)^2*r0*sin(u(3))+2*m2*u(10)^2*r0*sin(u(3))+l1*m2*u(10)^2 *sin(u(3)-u(4))-2*l1*m2*u(10)*u(11)*sin(u(3)-u(4))+l1*m2*u(11)^2*sin(u(3)- u(4))+m1*u(10)^2*r1*sin(u(3)-u(4))-2*m1*u(10)*u(11)*r1*sin(u(3)- u(4))+m1*u(11)^2*r1*sin(u(3)-u(4))+l1*m2*u(10)^2*sin(u(3)-u(5))- 2*l1*m2*u(10)*u(12)*sin(u(3)-u(5))+l1*m2*u(12)^2*sin(u(3)- u(5))+m1*u(10)^2*r1*sin(u(3)-u(5))-2*m1*u(10)*u(12)*r1*sin(u(3)- u(5))+m1*u(12)^2*r1*sin(u(3)-u(5))+m2*u(10)^2*r2*sin(u(3)-u(4)+u(6))- 2*m2*u(10)*u(11)*r2*sin(u(3)-u(4)+u(6))+m2*u(11)^2*r2*sin(u(3)- u(4)+u(6))+2*m2*u(10)*u(13)*r2*sin(u(3)-u(4)+u(6))- 2*m2*u(11)*u(13)*r2*sin(u(3)-u(4)+u(6))+m2*u(13)^2*r2*sin(u(3)- u(4)+u(6))+m2*u(10)^2*r2*sin(u(3)-u(5)+u(7))-2*m2*u(10)*u(12)*r2*sin(u(3)- u(5)+u(7))+m2*u(12)^2*r2*sin(u(3)- u(5)+u(7))+2*m2*u(10)*u(14)*r2*sin(u(3)-u(5)+u(7))- 2*m2*u(12)*u(14)*r2*sin(u(3)-u(5)+u(7))+m2*u(14)^2*r2*sin(u(3)-u(5)+u(7))) b2=-(-u(20)- u(22)+g*m0+2.*g*m1+2.*g*m2+(2*m1+2.*m2)*u(10)^2*r0*cos(u(3))+(u(10)^ 2-2.*u(10)*u(11)+u(11)^2)*(l1*m2+m1*r1)*cos(u(3)- u(4))+l1*m2*u(10)^2*cos(u(3)-u(5))-2*l1*m2*u(10)*u(12)*cos(u(3)- u(5))+l1*m2*u(12)^2*cos(u(3)-u(5))+m1*u(10)^2*r1*cos(u(3)-u(5))- 2*m1*u(10)*u(12)*r1*cos(u(3)-u(5))+m1*u(12)^2*r1*cos(u(3)- u(5))+m2*u(10)^2*r2*cos(u(3)-u(4)+u(6))-2*m2*u(10)*u(11)*r2*cos(u(3)- u(4)+u(6))+m2*u(11)^2*r2*cos(u(3)-
2*m2*u(11)*u(13)*r2*cos(u(3)-u(4)+u(6))+m2*u(13)^2*r2*cos(u(3)- u(4)+u(6))+m2*u(10)^2*r2*cos(u(3)-u(5)+u(7))- 2*m2*u(10)*u(12)*r2*cos(u(3)-u(5)+u(7))+m2*u(12)^2*r2*cos(u(3)- u(5)+u(7))+2*m2*u(10)*u(14)*r2*cos(u(3)-u(5)+u(7))- 2*m2*u(12)*u(14)*r2*cos(u(3)-u(5)+u(7))+m2*u(14)^2*r2*cos(u(3)- u(5)+u(7))) b3=-((u(19)*r0+u(21)*r0)*cos(u(3))+u(19)*l1*cos(u(3)- u(4))+u(21)*l1*cos(u(3)-u(5))+u(19)*l2*cos(u(3)- u(4)+u(6))+u(21)*l2*cos(u(3)-u(5)+u(7))-u(20)*r0*sin(u(3))- u(22)*r0*sin(u(3))+2*g*m1*r0*sin(u(3))+2*g*m2*r0*sin(u(3))- u(20)*l1*sin(u(3)-u(4))+g*l1*m2*sin(u(3)-u(4))+g*m1*r1*sin(u(3)-u(4))- 2*l1*m2*u(10)*u(11)*r0*sin(u(4))+l1*m2*u(11)^2*r0*sin(u(4))- 2*m1*u(10)*u(11)*r0*r1*sin(u(4))+m1*u(11)^2*r0*r1*sin(u(4))- u(22)*l1*sin(u(3)-u(5))+g*l1*m2*sin(u(3)-u(5))+g*m1*r1*sin(u(3)-u(5))- 2*l1*m2*u(10)*u(12)*r0*sin(u(5))+l1*m2*u(12)^2*r0*sin(u(5))- 2*m1*u(10)*u(12)*r0*r1*sin(u(5))+m1*u(12)^2*r0*r1*sin(u(5))- 2*m2*u(10)*u(11)*r0*r2*sin(u(4)-u(6))+m2*u(11)^2*r0*r2*sin(u(4)- u(6))+2*m2*u(10)*u(13)*r0*r2*sin(u(4)-u(6))- 2*m2*u(11)*u(13)*r0*r2*sin(u(4)-u(6))+m2*u(13)^2*r0*r2*sin(u(4)-u(6))- 2*l1*m2*u(10)*u(13)*r2*sin(u(6))+2*l1*m2*u(11)*u(13)*r2*sin(u(6))- l1*m2*u(13)^2*r2*sin(u(6))-u(20)*l2*sin(u(3)-u(4)+u(6))+g*m2*r2*sin(u(3)- u(4)+u(6))-2*m2*u(10)*u(12)*r0*r2*sin(u(5)- u(7))+m2*u(12)^2*r0*r2*sin(u(5)-u(7))+2*m2*u(10)*u(14)*r0*r2*sin(u(5)- u(7))-2*m2*u(12)*u(14)*r0*r2*sin(u(5)-u(7))+m2*u(14)^2*r0*r2*sin(u(5)- u(7))-2*l1*m2*u(10)*u(14)*r2*sin(u(7))+2*l1*m2*u(12)*u(14)*r2*sin(u(7))- l1*m2*u(14)^2*r2*sin(u(7))-u(22)*l2*sin(u(3)-u(5)+u(7))+g*m2*r2*sin(u(3)- u(5)+u(7))) b4=-(-u(15)-1.*u(19)*l1*cos(u(3)-u(4))-u(19)*l2*cos(u(3)- u(4)+u(6))+u(20)*l1*sin(u(3)-u(4))-g*l1*m2*sin(u(3)-u(4))-g*m1*r1*sin(u(3)- u(4))+l1*m2*u(10)^2*r0*sin(u(4))+m1*u(10)^2*r0*r1*sin(u(4))+m2*u(10)^2*r
0*r2*sin(u(4)-u(6))+2*l1*m2*u(10)*u(13)*r2*sin(u(6))- 2*l1*m2*u(11)*u(13)*r2*sin(u(6))+l1*m2*u(13)^2*r2*sin(u(6))+u(20)*l2*sin(u (3)-u(4)+u(6))-g*m2*r2*sin(u(3)-u(4)+u(6))) b5=-(-u(16)-1.*u(21)*l1*cos(u(3)-u(5))-u(21)*l2*cos(u(3)- u(5)+u(7))+u(22)*l1*sin(u(3)-u(5))-g*l1*m2*sin(u(3)-u(5))-g*m1*r1*sin(u(3)- u(5))+l1*m2*u(10)^2*r0*sin(u(5))+m1*u(10)^2*r0*r1*sin(u(5))+m2*u(10)^2*r 0*r2*sin(u(5)-u(7))+2*l1*m2*u(10)*u(14)*r2*sin(u(7))- 2*l1*m2*u(12)*u(14)*r2*sin(u(7))+l1*m2*u(14)^2*r2*sin(u(7))+u(22)*l2*sin(u (3)-u(5)+u(7))-g*m2*r2*sin(u(3)-u(5)+u(7))) b6=-(-u(17)+u(19)*l2*cos(u(3)-u(4)+u(6))-m2*u(10)^2*r0*r2*sin(u(4)- u(6))+l1*m2*u(10)^2*r2*sin(u(6))- 2*l1*m2*u(10)*u(11)*r2*sin(u(6))+l1*m2*u(11)^2*r2*sin(u(6))- u(20)*l2*sin(u(3)-u(4)+u(6))+g*m2*r2*sin(u(3)-u(4)+u(6))) b7=-(-u(18)+u(21)*l2*cos(u(3)-u(5)+u(7))-m2*u(10)^2*r0*r2*sin(u(5)- u(7))+l1*m2*u(10)^2*r2*sin(u(7))- 2*l1*m2*u(10)*u(12)*r2*sin(u(7))+l1*m2*u(12)^2*r2*sin(u(7))- u(22)*l2*sin(u(3)-u(5)+u(7))+g*m2*r2*sin(u(3)-u(5)+u(7))) A.3. Hàm Linearization.m: function y = Linearization(u)
zeta= 0.7; zeta1 = zeta; zeta2 = zeta; zeta3 = zeta; zeta4 = zeta; omega1= 10; omega2= 10; omega3= 40; omega4= 40;