Luận án Nghiên cứu hệ thống chuyển động và điều khiển tay máy tự động thu hoạch dứa

Nội dung text: Luận án Nghiên cứu hệ thống chuyển động và điều khiển tay máy tự động thu hoạch dứa

1. 109 Hình 4.6. Thiết bị quả rọi được sử dụng trong thực nghiệm Hình 4.7. Thước góc vuông và thước mét được sử dụng trong thực nghiệm Hình 4.8. Đèn Laser được gắn trùng với tâm trục Z của tay máy được sử dụng trong thực nghiệm 4.3.4.2. Dụng cụ và thiết bị đo nghiêng các trục tay máy hái dứa - Thuật toán mômen động cơ tại chương 2 có khả năng bù thành phần trọng trường trong mô hình động lực học khi khung công tác nghiêng. Do vậy, cần phải đo độ nghiêng của khung (hay độ nghiêng tay máy) để đánh giá khả
2. 111 4.3.4.3. Thiết bị đo rung lắc khung công tác - Thuật toán mômen động cơ tại chương 2 có thành phần bù tác động nhiễu do khung công tác bị rung trong mô hình động lực học tay máy. Do vậy, cần phải đo độ rung của khung công tác để đánh giá khả năng bù nhiễu rung lắc của thuật toán mômen tại chương 2 trong quá trình làm việc thực tế. - Để xác định sự nghiêng độ rung lắc, luận án sử dụng máy đo độ rung Fluke 805 của hãng Fluke của Đức với sai số của máy là 2%. Hình 4.11. Máy đo độ rung Fluke 805 4.4. Tổ chức thực nghiệm 4.4.1. Tổ chức thực nghiệm kiểm chứng kết quả đọc tọa độ của camera Vì tọa độ quả dứa là đầu vào của phương trình tính mômen cần cho các động cơ để tay máy di chuyển tới vị trí quả dứa, do vậy độ chính xác của việc đọc tọa độ ảnh hưởng tiên quyết đến kết quả di chuyển của tay máy. Để thuận tiện cho việc kiểm định độ chính xác của thuật toán đọc tọa độ quả dứa, luận án tiến hành quá trình thực nghiệm tại xưởng sản xuất của công ty cổ phần thiết bị chuyên dùng Việt Nam có địa chỉ tại Xuân Mai- Chương Mỹ- Hà Nội, đây là nơi có điều kiện thuận lợi cho việc tiến hành thực nghiệm và đo đọc so với việc thực nghiệm ngoài cánh đồng, đảm bảo đánh giá tính chính xác của bài toán đọc tọa độ được xây dựng tại chương 3. Ngoài ra, việc thực nghiệm không mất đi tính tổng quát của bài toán nhận dạng ảnh và đọc tọa độ vì máy ảnh được gắn cố định tại một điểm trên khung công tác của LHM tự động thu hoạch dứa. Số lần thí nghiệm dựa vào phạm vi hoạt động của tay máy trên khung công tác (mỗi cánh tay máy theo trục X là 1,1m, trục Y là 2,2m) và đặc điểm sinh học của cây dứa được trồng tại vùng Tây Nam Bộ (mỗi cây cách nhau 0,4
3. 113 mô hình động lực học; ảnh hưởng của rung lắc khung tác động lên chuyển động tay máy. Do vậy, kết quả thực nghiệm này đủ tính khách quan để đánh giá hiệu quả và sự chính xác của thuật toán mômen động cơ được xây dựng dựa trên mô hình động lực học tay máy tại chương 2. Hình 4.14. Xác định các yếu tố nhiễu ảnh hưởng tới chuyển động tay máy tại ruộng dứa Tiền Giang Hình 4.15. Xác định sai số chuyển động tay máy hái dứa tại ruộng dứa Tiền Giang
4. 115 Bảng 4.1: Kết quả xác định tọa độ quả dứa của máy ảnh Thứ tự Tọa độ quả dứa Sai lệch Sai số * thí (mm) X X X (%) nghiệm Lý thuyết Vị trí quả dứa * Y Y Y (Tọa độ máy (mm) ảnh đo) Trục Trục Trục Trục Trục Trục X* Y* X Y X Y X (%) Y (%) 1 -197 790 -200 800 -3 10 1,5 1,3 2 195 786 200 800 5 14 2,5 1,8 3 -208 1215 -200 1200 8 -15 4,0 1,3 4 208 1213 200 1200 -8 -13 4,0 1,1 5 -595 781 -600 800 -5 19 0,8 2,4 6 590 782 600 800 10 18 1,7 2,3 7 -605 1215 -600 1200 5 -15 0,8 1,3 8 608 1218 600 1200 -8 -18 1,3 1,5 9 -205 1620 -200 1600 5 -20 2,5 1,3 10 206 1619 200 1600 -6 -19 3,0 1,2 11 -208 1985 -200 2000 8 15 4,0 0,8 12 207 1988 200 2000 -7 12 3,5 0,6 13 -605 1615 -600 1600 5 -15 0,8 0,9 14 604 1618 600 1600 -4 -18 0,7 1,1 15 -607 1987 -600 2000 7 13 1,2 0,7 16 608 1990 600 2000 -8 10 1,3 0,5 17 -195 890 -200 900 -5 10 2,5 1,1 18 198 886 200 900 2 14 1,0 1,6 19 -208 1316 -200 1300 8 -16 4,0 1,2 20 205 1312 200 1300 -5 -12 2,5 0,9 21 -590 883 -600 900 -10 17 1,7 1,9 22 593 882 600 900 7 18 1,2 2,0 23 -605 1318 -600 1300 5 -18 0,8 1,4
5. 117 4.5.2. Kết quả thực nghiệm chuyển động tay máy thu hoạch dứa 4.5.2.1. Số liệu thực nghiệm Luận án tiến hành thực nghiệm với ruộng dứa được trồng tại huyện Tân Phước, tỉnh Tiền Giang. Các cây dứa được người nông dân trồng trên các thửa ruộng tại địa phương, đến kỳ thu hoạch (hình 4.3). Số lượng thí nghiệm được tiến hành là 36 cho kết quả được thể hiện trên bảng 4.2.
6. 119 7 10 8 6 1,0 395 172 395 160 390 165 370 131 385 152 8 12 15 8 1,4 635 271 630 255 635 255 610 228 625 246 9 12 15 8 1,4 333 1455 335 1490 340 1490 366 1457 347 1479 10 12 15 8 1,4 748 1476 750 1510 755 1510 778 1468 761 1496 11 12 15 8 1,4 164 272 190 300 180 300 167 267 179 289 12 12 15 8 1,4 681 801 665 780 680 775 659 803 668 786 13 12 15 8 1,4 116 831 115 805 105 805 86 832 102 814 14 12 15 8 1,4 559 1505 580 1510 580 1510 556 1549 572 1523 15 8 5 5 0,8 187 1601 185 1600 178 1600 162 1555 175 1585 16 8 5 5 0,8 250 623 245 600 245 590 221 625 237 605 17 8 5 5 0,8 311 1305 330 1310 310 1310 335 1340 325 1320 18 8 5 5 0,8 308 1949 330 1980 330 1990 309 1952 323 1974 19 8 5 5 0,8 634 687 660 700 660 710 630 735 650 715 20 8 5 5 0,8 723 1286 710 1270 710 1270 692 1237 704 1259 21 8 5 5 0,8 250 624 235 610 240 610 215 565 230 595 22 8 5 5 0,8 312 1313 335 1350 320 1355 344 1324 333 1343 23 5 8 5 0,9 309 1953 330 1990 330 1995 312 1958 324 1981 24 5 8 5 0,9 643 684 670 690 665 725 648 712 661 709 25 5 8 5 0,9 166 833 155 790 158 790 137 835 150 805 26 5 8 5 0,9 724 1291 715 1275 715 1275 694 1248 708 1266
7. 121 4.5.2.2. Kiểm chứng công thức lý thuyết về điều khiển chuyển động tay máy qua các số liệu đo đạc 1) Kiểm tra chuyển động theo tọa độ X Từ kết quả thí nghiệm được xác định trong bảng 4.2, số liệu di chuyển tay máy theo trục X được thể hiện lại thông qua bảng 4.3 như sau: Bảng 4.3. Số liệu chuyển động tay máy theo trục X Tọa độ X Vị trí theo lý thuyết Kết quả di chuyển thực tế của Phương Phương (vị trí cánh tay máy 2 2 tay cần di TT sai si sai ˆsi chuyển tới) Lần1 Lần 2 Lần 3 T.bình X Đơn vị (mm) (mm) (mm) (mm) (mm) 1 920 890 890 900 300 225 915 2 910 910 874 898 432 256 882 3 635 635 614 628 147 169 615 4 150 155 124 143 277 256 159 5 790 795 755 780 475 196 766 6 210 210 237 219 243 225 204 7 395 390 370 385 175 100 395 8 630 635 610 625 175 100 635 9 335 340 366 347 277 196 333 10 750 755 778 761 223 169 748 11 190 180 167 179 133 225 164 12 665 680 659 668 117 169 681 13 115 105 86 102 217 196 116
8. 123 Tọa độ X Vị trí theo lý thuyết Kết quả di chuyển thực tế của Phương Phương (vị trí cánh tay máy 2 2 tay cần di TT sai si sai ˆsi chuyển tới) Lần1 Lần 2 Lần 3 T.bình X Đơn vị (mm) (mm) (mm) (mm) (mm) 34 195 195 174 188 147 484 166 35 570 580 593 581 133 484 559 36 170 180 157 169 133 324 187 Tổng 6643 10170 Khảo nghiệm tọa độ X của quả dứa: thực nghiệm-lý thuyết 1000 Tọa độ X -vị trí quả dứa 900 800 700 600 500 400 300 200 100 0 0 5 10 15 20 25 30 35 40 Thứ tự quả dứa TN LT Hình 4.17. Kết quả xác định tọa độ X của quả dứa giữa thực nghiệm - lý thuyết Từ bảng 4.3 ta có: số thí nghiệm n = 36; số lần lặp tại mỗi điểm thí nghiệm r = 3; mức ý nghĩa α = 0,05. Từ số liệu trong bảng 4.3 ta được:
9. 125 2) Kiểm tra chuyển động theo tọa độ trục Y Từ kết quả thí nghiệm được xác định trong bảng 4.2, số liệu di chuyển tay máy theo trục Y được thể hiện lại thông qua bảng 4.4 như sau: Bảng 4.4: Các số liệu chuyển động tay máy theo Y Tọa độ Y Vị trí theo lý thuyết T.T Đo thực tế Phương Phương (vị trí cánh sai s 2 sai ˆs 2 tay cần di i i chuyển tới) Lần1 Lần 2 Lần 3 T.bình Y Đơn vị (mm) (mm) (mm) (mm) (mm) 1 850 855 821 842 337 289 859 2 1425 1425 1386 1412 507 196 1398 3 910 910 856 892 972 324 874 4 910 910 946 922 432 361 941 5 1495 1495 1465 1485 300 289 1468 6 1520 1515 1471 1502 727 361 1483 7 160 165 131 152 337 400 172 8 255 255 228 246 243 625 271 9 1490 1490 1457 1479 363 576 1455 10 1510 1510 1468 1496 588 400 1476 11 300 300 267 289 363 289 272 12 780 775 803 786 223 225 801 13 805 805 832 814 243 289 831 14 1510 1510 1549 1523 507 324 1505 15 1600 1600 1555 1585 675 256 1601 16 600 590 625 605 325 324 623 17 1310 1310 1340 1320 300 225 1305
10. 127 Tọa độ Y- vị trí quả dứa 2500 2000 1500 1000 500 0 0 5 10 15 20 25 30 35 40 Thứ tự quả dứa TN LT Hình 4.18. Kết quả xác định tọa độ Y của quả dứa giữa thực nghiệm- lý thuyết Tương tự như so với trục X, từ số liệu trong bảng 4.4 chúng ta có được: n 2 Si 15155 ; (4.18) i1 2 S972max ; (4.19) n 22115155 S.S420,98t.sinhi  ; (4.20) n36i1 n 2 221 22224 S dư = ˆ . (4.21) Sdu . s i 634,98 n 1i1 36 1 Do vậy: 2 Smax 972 G0,064tn n ; (4.22) 2 15155 Si i1 GGCochranlt (r 1),n,(1 ) (2;36; 0,95) 0,17 (4.23) Do đó GGtnlt 0,0640,17 , phương sai các thí nghiệm là đồng nhất 22 Do SSdu t.sinh nên
11. 129 KẾT LUẬN VÀ KIẾN NGHỊ 1. Kết luận Sau khi nghiên cứu lý thuyết và thực nghiệm động lực học và điều khiển tay máy tự động thu hoạch quả dứa, luận án có một số kết luận sau: 1. Luận án đã xây dựng được phương trình động học ngược để xác định lượng dịch chuyển của các khớp tay máy robot. Đã tiến hành xây dựng được mô hình động lực học tay máy trong các trường hợp lý tưởng và trường hợp thực tế thông qua phương pháp Euler-Lagrange. 2. Luận án đã sử dụng camera xử lý hình ảnh và trí tuệ nhân tạo để nhận diện quả dứa chín cần thu hoạch, đã xây dựng mô hình tính toán tọa độ vị trí của quả dứa cần thu hoạch, từ đó để điều khiển tay máy đến đúng vị trí tọa độ xác định để thu hoạch. 3. Đã quy đổi hai mô hình động lực học về đầu trục động cơ truyền động cho hai khớp, sau đó thiết lập mối quan hệ để đưa mô hình động lực học thực tế (không lý tưởng) về dạng mô hình động lực học lý tưởng kết hợp với thành phần nhiễu, xây dựng được thuật toán PDDE, thuật toán này đảm bảo việc điều khiển chuyển động của tay máy tới đúng vị trí quả dứa cần cắt dựa trên lý thuyết ổn định Lyapunov. 4. Luận án đã xây dựng được phương pháp nghiên cứu thực nghiệm tay tự động thu hoạch quả dứa, kết quả thực nghiệm đã xác định được giá trị bằng số tọa độ của các quả dứa thông qua giá trị đo thực tế, sau đó so sánh với giá trị đọc được từ camera hiển thị trên màn hình máy tính. Kết quả so sánh giữa lý thuyết và thực nghiệm với sai số 5% là chấp nhận được, từ đó mô hình tính toán lý thuyết có độ tin cậy cần thiết. 5. Kết quả nghiên cứu của luận án đã được sử dụng để hoàn thiện tay máy thu hoạch quả dứa trên liên hợp máy thu hoạch quả dứa hiện đang được sử dụng ở vùng Tây Nam Bộ, ngoài ra kết quả nghiên cứu của luận án có thể làm cơ sở khoa học để thiết kế chế tạo tay máy thu hoạch một số loại trái cây khác trong nông nghiệp.
12. 130 TUYỂN TẬP CÁC BÀI BÁO ĐÃ CÔNG BỐ VỀ KẾT QUẢ CỦA LUẬN ÁN 1. Nguyen Pham Thuc Anh, Son Hoang, Duong Van Tai, Bui Le Cuong Quoc (2020), “Developing Robotic system for harvesting pineapples”, proceeding of the 2020 international conference on advanced mechatronic system. 2. Bùi Lê Cường Quốc, Hoàng Sơn “Nghiên cứu động lực học chuyển động tay máy robot thu hoạch dứa tự động trên cánh đồng”, Tạp chí khoa học và công nghệ Lâm nghiệp số 3-2022 trang 124/134. 3. Bùi Lê Cường Quốc, Hoàng Sơn, Nguyễn Phạm Thục Anh, Trần Công chi (2022), “Dynamics and Motion Control of a Pineapple Harvesting Robotic System”, 2022 6th International Conference on Robotics and Automation Sciences (ICRAS 2022).
13. 132 Tiếng anh 10. Hannan, M.W.; Burks, T.F (2004). “Current developments in automated citrus harvesting”. 2004 ASAE Annu. Inter. Meet. 2004, doi: 10.13031/2013.16726. 11. Kondo N.(2000), “Strawberry harvesting robot”, Journal of the Japan Society of Mechanical Engineers 103(976): 148-149. 12. Kendo, N.;Ninomoya, K.;Hayashi, S.; Tomohiko, O.;Kubota, K (2005). “A new challenge of robot for harvesting strawberry grown on table top culture”. 2005 ASAE. Meet 2005, doi: 10.13031/2013.19110. 13. Li, Z.; Li, P.; Yang, H.; Wang, Y (2013). “Stability tests of two-finger tomato grasping for harvesting robots”, Biosyst. Eng. 2013, 116, 163-170. 14. Ling, P.P.; Ehsani, R.; Ting, K.C.; Chi, Y.; Ramalingam, N.; Klingman, M.H.; Draper, C (2004). “Sensing and end-effector for a robotic tomato harvester”. 2004 ASAE Annu. Meet. 2004. Doi: 10.13031/2013.16727. 15. Kondo, N.; Yamamoto, K.; Kurita, M (2008). “A machine vision for tomato cluster harvesting robot” ASAE Annu. Inter. Meet. 2008, 5, 3111- 3120. 16. Li, B.; Wang, M.; Wang, N (2010). “Development of a real-time fruit recognition system for pineapple harvesting robots”. In Proceedings of the Annual Meeting of ASABE, Pittsburgh, PA, USA, 20-23 June 2010. 17. Tanagaki, K.; Fujiura, T.; Akase, A.; Imagawa, I (2008). “Cherry- harvesting robot”. Comput. Electr. Agric. 2008, 63, 65-72. 18. Tạp chí khoa học robot (robotics) (2015), số 4: 164-222, 2015 ( 19. Hemming, J.; Bac, C.W.; Bart, A.J.; van Tuijl, B.A.J.; Barth, R.; Bontsema, J.; Pekkeriet, E. A (2014) “robot for harvesting sweet-pepper in greenhouses. In Proceedings of the International Conference of Agricultural” Engineering, Zurich, Switzerland, 6-10 July 2014.
14. 134 28. Stein, M., Bargoti, S., Underwood, J., (2016) “Image Based Mango Fruit Detection, Localisation and Yield Estimation Using Multiple View Geometry”. 29. B. Bandyopadhyay et al. (Eds.). (2013) “Advances in sliding mode control”. Springer-Verlag: Germany. 30. Mezghani Ben Romdhane Neila, Damak Tarak. (2011) “Adaptive terminal sliding mode control for rigid robotic manipulators”. International Journal of Automation and Computing, 2011; 8 (2): 215-220. 31. Sze San Chong, Xinghuo Yu and Man Zhihong. (1997) “A robust adaptive sliding mode controller for robotic manipulators”. IEEE Workshop on Variable Structure Systems, Tokyo, Japan, December 1996; 31-35. 32. Man ZHIHONG, Xinghuo YU. (1997) “Adaptive terminal sliding mode tracking control for rigid robotic manipulators with uncertain dynamics”. The Japan society of mechanical Engineers 1997; 40(3): 493-502. 33. Chun-Yi Su, Tin-Pui Leung. (1993) “A sliding mode controller with bound estimation for robot manipulators”. IEEE transactions on robotics and automation 1993; 9(2): 208-214. 34. Lei-Po Liu, Zhu-Mu Fu, Xiao-Na Song. (2012) “Sliding mode control with disturbance observer for a class of nonlinear systems”. International Journal of Automation and Computing 2012; 9(5): 487-491. 35. Dong Xinzhuang. (2006) “State feedback sliding mode control for a class of systems with mismatched uncertainties and disturbances”. Proceedings of the 25th Chinese Control Conference, Harbin, China, August 2006; 928-933. 36. GUO YI-shen, CHEN Li. (2008) “Terminal sliding mode control for coordinated motion of a space rigid manipulator with external disturbance”. Applied Mathematics and Mechanics 2008; 29(5): 583-590. 37. Liu Hui, Junfeng Li. (2009) “Terminal sliding mode control for spacecraft formation flying”. IEEE Transactions on Aerospace and Electronic
15. 136 Control 2010; 83(9): 1907-1919. 46. Huang, Y.-J., Kuo, T.-C., Chang, S.-H. “Adaptive sliding-mode control for nonlinear systems with uncertain parameters”. IEEE Transactions on System, Man, and Cybernetics-Part B, 2008; 38: 534-539. 47. Dian Songyi, Wu Zhang, Pu Ming. (2014) “Adaptive sliding mode control for gas feeding system of solid oxide fuel cells”. Journal of Sichuan University 2014; 46(5): 148-155. 48. Bartolini, G., Ferrara, A., Usai, E., Utkin, V.I. (2000) “On multi-input chattering-free second-order sliding mode control”. IEEE Transactions on Automatic Control 2000; 45(9): 1711-1717. 49. Levant, A. (2007) “Principles of 2-sliding mode design”. Automatica 2007; 43: 576-586. 50. Richard M. Murray, Zexiang Li, S. Shanhar Sastry. (1994) “A mathematical introduction to robotic manipulation”. CRC Press: USA, 1994. 51. Huu Cong-Nguyen, Woo-Song Lee, Chang-Jae Cho and Sung-Hyun Han. (2009) “Real-time adaptive control of robot manipulator based on neural network compensator”. ICROS-SICE International Joint Conference 2009, Fukuoka, Japan,August 2009; 2091-2096. 52. Liu Jinkun. (2012) “Sliding mode control and matlab simulation”, 2rd ed. Qinghua University Press: China. 53. Hassan K.Khalil. (2002) “Nonlinear systems”, 3rd ed. Prentice-Hall: USA, 2002. 54. Liu Wen Jie, Hu JiCai, Xiao Xiao Hui, Lu Jia Jiang. (2004) “Dynamic simulation of the non-linear vibration of cables based on ADAMS”. Mechanical and Electronic of Chine 2004; 10(2): 64-66. 55. John J Craig. (2005) “Introduction to robot mechanics and control”, 3rd ed. Prentice-Hall: USA, 2005.
16. PHỤ LỤC
17. qf=1; %qf=(150*pi)/180; tf=4; %Thoi gian di chuyen la 4s q0=0; qdd=6*(qf-q0)/tf^2; tb=tf/2-sqrt(qdd^2*tf^2-(4*qdd*(qf-q0)))/(2*qdd); qb=q0+0.5*qdd*tb^2; if t>=0 && t =tb && t =tf-tb && t tf q1_d=qf; dq1_d=0; ddq1_d=0; end sys(1)=q1_d; sys(2)=dq1_d;
18. qf=1.5; %(70*pi)/180; tf=4; %thoi gian di chuyen 4s q0=0; qdd=6*(qf-q0)/tf^2; tb=tf/2-sqrt(qdd^2*tf^2-(4*qdd*(qf-q0)))/(2*qdd); qb=q0+0.5*qdd*tb^2; if t>=0 && t =tb && t =tf-tb && t tf q2_d=qf; dq2_d=0; ddq2_d=0; end sys(1)=q2_d; sys(2)=dq2_d;
19. x0 = []; % bien trang thai bao dau la 0 ( thay doi so voi ban goc, vi goc ko dung bien trang thai) str = []; ts = [0 0]; % % THEM DE TINH UOC LUONG CUA GIOI HAN CHAN d^ % Hàm S-function khối PDcontroller function sys=mdlOutputs(t,x,u) q1_d=u(1); %quy dao dat khop 1 //(t,x,u) dq1_d=u(2);%van toc dat khop 1 %ddq1_d=u(3);%gia toc dat khop 1 q2_d= u(4); % quy dao dat khop 2 dq2_d=u(5);%van toc dat khop 2 %ddq2_d=u(6);% gia toc dat khop 2 q1=u(7);%u(7); % quy dao th?t c?a kh?p 1 dq1=u(8); %u(8); % V?n t?c th?t c?a kh?p 1 q2=u(9);%u(9); %quy ??o th?t c?a kh?p 2 dq2=u(10); %u(10); % V?n t?c th?t c?a kh?p 2 %q=[q1;q2]; % Ma tr?n qu? ??o th?t 2 kh?p %dq=[dq1;dq2];% Ma tran van toc that 2 khop
20. case 1, sys=mdlDerivatives(t,x,u); case 3, sys=mdlOutputs(t,x,u); case {2,4,9} sys=[]; otherwise error(['Unhandled flag = ',num2str(flag)]); end function [sys,x0,str,ts]=mdlInitializeSizes sizes = simsizes; sizes.NumContStates = 2; sizes.NumOutputs = 4; sizes.NumInputs= -1; sizes.DirFeedthrough = 1; sizes.NumSampleTimes = 0;% Thay doi so voi ban goc la 1 sys = simsizes(sizes); x0 = [0;1]; % bien trang thai bao dau la 0 ( thay doi so voi ban goc, vi goc ko dung bien trang thai) str = []; ts = []; % % THEM DE TINH UOC LUONG CUA GIOI HAN CHAN d^ %
21. gama1=0.1;gama2=0.1; sys(1)=gama1*teta*norm(dq); sys(2)=-gama2*x(2); function sys=mdlOutputs(t,x,u) q1_d=u(1); %quy dao dat khop 1 //(t,x,u) dq1_d=u(2);%van toc dat khop 1 %ddq1_d=u(3);%gia toc dat khop 1 q2_d= u(4); % quy dao dat khop 2 dq2_d=u(5);%van toc dat khop 2 %ddq2_d=u(6);% gia toc dat khop 2 q1=u(7);%u(7); % quy dao th?t c?a kh?p 1 dq1=u(8); %u(8); % V?n t?c th?t c?a kh?p 1 q2=u(9);%u(9); %quy ??o th?t c?a kh?p 2 dq2=u(10); %u(10); % V?n t?c th?t c?a kh?p 2 q=[q1;q2]; % Ma tr?n qu? ??o th?t 2 kh?p dq=[dq1;dq2];% Ma tran van toc that 2 khop % Dinh nghia ma tra sai lech- e1=q1_d-q1;% sai lech quy dao khop 1 (chinh la epsilon 1) e2=q2_d-q2;% chinh la epsilon 2 (do matlab khong co ky hieu epsilon, dung bien e thay the) e=[e1;e2];
22. case 0, [sys,x0,str,ts]=mdlInitializeSizes; case 1, sys=mdlDerivatives(t,x,u); %Outputs case 3, sys=mdlOutputs(t,x,u); %Unhandled flags case {2, 4, 9 } sys = []; %Unexpected flags otherwise error(['Unhandled flag = ',num2str(flag)]); end %mdlInitializeSizes function [sys,x0,str,ts]=mdlInitializeSizes sizes = simsizes; sizes.NumContStates = 4; sizes.NumDiscStates = 0; sizes.NumOutputs = 4; sizes.NumInputs= 4;%6 sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 0; sys=simsizes(sizes); x0=[0.0;0;0.0;0];%da sua str=[];
23. %Ma tran nhieu ngau nhien Md=[Md(1);Md(2)];% Ma tran Momen phat ra tai truc dong co S=inv(Hd)*(Md-Gd-Dt); %+fttt+ft sys(1)=x(2); sys(2)=S(1); sys(3)=x(4); sys(4)=S(2); function sys=mdlOutputs(t,x,u) sys(1)=x(1); sys(2)=x(2); sys(3)=x(3); sys(4)=x(4);