Trường ĐHBK Hà Nội BTL Robot BÀI TẬP LỚN MÔN ROBOT Cho robot Scalar 2DOF thông số sau: l1=0.3; l2=0.3; lg1=0.15; lg2=0.15; m1=7;m2=2; J1=0.0525;J2=0.0525; g=9.81; K1=380; T1=0.02; K2=300; T2=0.02; Yêu cầu: Giả sử khớp điều khiển hệ ộng driver có hàm truyền = a Thiết kế điều khiển vị trí cho khớp biết 10V, độ điều chỉnh 5%, thời gian độ nhỏ b Chọn quỹ đạo thẳng tròn, tính quỹ đạo cho khớp c Mô Matlab simulink So sánh quỹ đạo đặt quỹ đạo thực tế Yêu cầu: a.Xây dựng mô hình toán học robot Matlab simulink b Thiết kế điều khiển momen tính toán cho robot, mô Matlab simulink, so sánh quỹ đạo đặt quỹ đạo thực c Thiết kế điều khiển PD bù trọng trường cho robot, mô Matlab simulink, so sánh quỹ đạo đặt quỹ đạo thực NHÓM 14 Page Trường ĐHBK Hà Nội BTL Robot BÀI 1:THIẾT KẾ BĐK VỊ TRÍ,QUỸ ĐẠO ROBOT I Cơ sở lý thuyết: Bài toán động học thuận -Mục đích toán động học thuận tìm vị trí hướng cấu tác động cuối biết giá trị biến khớp Gắn hệ tọa độ lên khâu - Chon hệ tọa độ gốc Ooxoyozo : Gốc tọa độ Oo trục khớp thứ nhất, trục xo đặt theo phương khâu 1, chiều hướng từ trục khớp thứ sang trục khớp thứ (Hình 1.1.1) - Hệ tọa độ O1x1y1z1 có gốc tọa độ O1 đặt tâm trục khớp thứ hai, trục x đặt theo phương khâu 2, chiều hướng từ trục khớp thứ hai sang trục khớp thứ ba - Đặt hệ tọa độ O2x2y2z2 đặt tâm bàn tay nắm bắt, chiều hướng từ trục khớp thứ hai đến điểm cuối khớp thứ Hình 1.1.1 : Sơ đồ động Robot Planar NHÓM 14 Page Trường ĐHBK Hà Nội BTL Robot Bảng thông số DH Khâu ⍬i αi di Khâu ⍬1 a1 Khâu ⍬2 a2 Bảng1.1.1 : Bảng thông số DH Robot SCARA Các ma trận chuyển đổi Bằng việc thay thông số động học Denavit Hartenberg vào biểu thức tính ma trận ���−1 ta xác định ma trận chuyển đồi Trong ���−1 ma trận chuyển đổi hệ trục tọa độ i-1 với hệ trục tọa độ i Để đơn giản cho việc trình bày ta sử dụng kí hiệu s⍬I = sin(⍬i) c⍬I = cos(⍬i) Ma trận biểu thị mối quan hệ hệ tọa độ cuối so với hệ tọa độ gốc là: NHÓM 14 Page Trường ĐHBK Hà Nội BTL Robot (2.1) Với c(⍬1+⍬2) = cos(⍬1+⍬2) ; s(⍬1+⍬2) = sin(⍬1+⍬2) Cân phần tử tương ứng hai ma trận với ta có phương trình động học Robot Planar là: Ma trận biểu diễn hướng tay máy Robot: Ma trận biểu diễn vị trí Robot Bài toán động học ngược Trong toán động học ngược ta cần xác định giá trị biến khớp Robot biết vị trí hướng điểm tác động cuối Vị trí hướng khâu tác động cuối xác định từ quỹ đạo chuyển đông cho trước robot, dạng ma trận: NHÓM 14 Page Trường ĐHBK Hà Nội BTL Robot Ta biết phương trình động học Robot Planar Ma trận biết (biết vị trí hướng cấu tác động cuối ) ta cần xác định giá trị biến khớp tương ứng Bằng cách cân phần tử hàng hàng cột hai ma trận tương ứng ta xác định được: (2.3) Xác định biến khớp Bình phương hai vế phương trình (2.9) cộng lại ta nhận phương trình:  c => s => NHÓM 14 = atan2[s Page Trường ĐHBK Hà Nội BTL Robot Xác định biến khớp Đặt k1 = k2 = ta có: � = atan2 ( Đặt k1 = r c(�), k2 = r s(�) Với r = < = > => Do đó: = atan2 (px, Vậy ta có: ) - � = atan2 (px, atan2 (px, atan2[s NHÓM 14 Page ) – atan2 ( ) – atan2 ( Trường ĐHBK Hà Nội BTL Robot II Tiến hành: Thiết kế điều khiển vị trí: Hình 1.1.2: Sơ đồ điều khiển vị trí chô khớp Hình 1.1.3: Vị trí khớp so với lượng đặt NHÓM 14 Page Trường ĐHBK Hà Nội BTL Robot Hình 1.1.4: Vị trí khớp so với lượng đặt Thiết kế quỹ đạo thẳng tròn, thiết kế quỹ đạo cho khớp: 2.1: Chọn quỹ đạo thẳng : fid=fopen('quy_dao_thang.txt','w'); for x=1.04:0.01:2.25 y=(3.148-0.7*x)/1.21; m=(x^2+y^2-l1^2-l2^2)/(2*l1*l2); q2=acos(m); MatA=[l1+l2*cos(q2) l2*sin(q2);-l2*sin(q2) l1+l2*cos(q2)]; Q1=[x y]/(MatA); cq1=Q1(1,1);sq1=Q1(1,2); q1=atan2(sq1,cq1); g2=q2*180/pi;g1=q1*180/pi; fprintf(fid,'[ %3.4f %3.4f %3.4f %3.4f ]',x,y,g1, g2); end 2.2 Chọn quỹ đạo tròn: fclose(fid); fid=fopen('quy_dao_tron.txt','w'); for x=-0.99:0.02:0.99 y=sqrt(5.7762-x^2); m=(x^2+y^2-l1^2-l2^2)/(2*l1*l2); NHÓM 14 Page Trường ĐHBK Hà Nội BTL Robot q2=acos(m); MatA=[l1+l2*cos(q2) l2*sin(q2);-l2*sin(q2) l1+l2*cos(q2)]; Q1=[x y]/(MatA); cq1=Q1(1,1);sq1=Q1(1,2); q1=atan2(sq1,cq1); g2=q2*180/pi;g1=q1*180/pi; fprintf(fid,'[ %3.4f %3.4f %3.4f %3.4f ]',x,y,g1, g2); end 2.3 Đồ thị mô phỏng: Hình 1.2.1: Quỹ đạo góc khớp mô (màu vàng)và tính toán (màu đen) Hình 1.2.2: Quỹ đạo góc khớp mô (màu vàng)và tính toán (màu đen) NHÓM 14 Page Trường ĐHBK Hà Nội BTL Robot Hình 1.2.3: Quỹ đạo thẳng cánh tay robot tính toán thực tế Hình 1.2.4: Quỹ đạo tròn cánh tay robot tính toán thực tế NHÓM 14 Page 10 Trường ĐHBK Hà Nội BTL Robot 2.2 Thiết kế điều khiển mô men tính toán cho robot: a Mô hình điều khiển mô men tính toán cho robot Với: KP= [32.65 0;0 32.65] KI= [8 0;0 8] LP1= [16 0;0 16] b Mô hình tính mô men điều khiển Mđk: NHÓM 14 Page 18 Trường ĐHBK Hà Nội BTL Robot c Mô hình robot từ Mđk q,dq,ddq: - Trong : • Các mô hình G,V không đổi • Mô sau: function y = bai2_codeH_1(in) %q(1) goc theta1 %q(2) goc theta2 %dq(1) toc quay khop %dq(2) toc quay khop NHÓM 14 Page 19 Trường ĐHBK Hà Nội BTL Robot %cac thong so robot global m1 m2 l1 l2 lg1 lg2 J1 J2 q(1) =in(1); q(2) =in(2); M01 =in(3); M02 =in(4); % ma tran quan tinh h(1,1) = m1*lg1^2+J1+m2*(l1^2+lg2^2+2*l1*lg2*cos(q(2)))+J2 ; h(1,2) = m2*(lg2^2 + l1*lg2*cos(q(2)))+J2 ; h(2,1) = m2*(lg2^2 + l1*lg2*cos(q(2)))+J2 ; h(2,2) = m2*lg2^2 + J2 ; Hinv=inv(h); % gia toc khop y=Hinv*[M01;M02]; end NHÓM 14 Page 20 Trường ĐHBK Hà Nội BTL Robot d Đáp ứng khớp mô phỏng: Hình 2.2.1: Đáp ứng góc khớp q so với giá trị đặt Hình 2.2.2: Đáp ứng dq mô hình NHÓM 14 Page 21 Trường ĐHBK Hà Nội BTL Robot Hình 2.2.3: Đáp ứng ddq mô hình Hình 2.2.4 : Đáp ứng mô men điều khiển mô hình NHÓM 14 Page 22 Trường ĐHBK Hà Nội BTL Robot 2.3 Thiết kế điều khiển PD bù trọng trường: a Mô hình điều khiển: - Trong thông số điều khiển sau: Kd= [16 0;0 16] Kp= [100 0;0 100] b Các đáp ứng q,dq,ddq Mđk: Hình 2.3.1: đáp ứng khớp (q) NHÓM 14 Page 23 Trường ĐHBK Hà Nội BTL Robot Hình 2.3.2: Đáp ứng Mđk khớp Hình 2.3.4: Đáp ứng vận tốc khớp NHÓM 14 Page 24 Trường ĐHBK Hà Nội BTL Robot Hình 2.3.5: Đáp ứng gia tốc khớp Ghi chú: Quỹ đạo thẳng toán robot Thông số tính toán q1 q2 x y 35.0826 59.9685 1.04 34.7697 60.012 1.05 1.9942 34.4591 60.0501 1.06 1.9884 34.1507 60.0831 1.07 1.9826 33.8445 60.1107 1.08 1.9769 33.5407 60.1331 1.09 1.9711 33.2391 60.1503 1.1 1.9653 32.9399 60.1622 1.11 1.9595 32.643 60.1688 1.12 1.9537 32.3484 60.1702 1.13 1.9479 32.0562 60.1664 1.14 1.9421 31.7664 60.1573 1.15 1.9364 31.4789 60.143 1.16 1.9306 NHÓM 14 Thông số mô q1 q2 35.0767 59.9585 34.7629 60.002 34.4536 60.0401 34.145 60.073 33.8387 60.1007 33.5351 60.1231 33.2335 60.1403 32.9344 60.1522 32.6376 60.1587 32.343 60.1602 32.0508 60.1564 31.7611 60.1473 31.4737 60.133 Page 25 x 1.040416 1.050445 1.060399 1.070406 1.08041 1.090401 1.100402 1.110397 1.120396 1.130395 1.140393 1.150388 1.160386 y 1.999911 1.994107 1.988345 1.982555 1.976764 1.970982 1.965194 1.959409 1.953624 1.947836 1.942049 1.936265 1.930478 Trường ĐHBK Hà Nội 31.1939 30.9113 30.6311 30.3534 30.0782 29.8054 29.5352 29.2675 29.0024 28.7398 28.4799 28.2225 27.9678 27.7157 27.4664 27.2197 26.9757 26.7345 26.4961 26.2605 26.0277 25.7977 25.5707 25.3465 25.1252 24.9069 24.6916 24.4793 24.27 24.0639 23.8608 23.6608 23.464 23.2704 23.0801 22.893 22.7092 22.5287 22.3516 22.178 22.0077 NHÓM 14 60.1234 60.0985 60.0684 60.0331 59.9924 59.9465 59.8953 59.8389 59.7771 59.71 59.6376 59.5598 59.4767 59.3883 59.2944 59.1952 59.0906 58.9805 58.865 58.7441 58.6176 58.4857 58.3482 58.2051 58.0565 57.9022 57.7423 57.5768 57.4055 57.2285 57.0457 56.8571 56.6626 56.4622 56.2559 56.0436 55.8253 55.6009 55.3704 55.1337 54.8907 BTL Robot 1.17 1.18 1.19 1.2 1.21 1.22 1.23 1.24 1.25 1.26 1.27 1.28 1.29 1.3 1.31 1.32 1.33 1.34 1.35 1.36 1.37 1.38 1.39 1.4 1.41 1.42 1.43 1.44 1.45 1.46 1.47 1.48 1.49 1.5 1.51 1.52 1.53 1.54 1.55 1.56 1.57 1.9248 1.919 1.9132 1.9074 1.9017 1.8959 1.8901 1.8843 1.8785 1.8727 1.8669 1.8612 1.8554 1.8496 1.8438 1.838 1.8322 1.8264 1.8207 1.8149 1.8091 1.8033 1.7975 1.7917 1.786 1.7802 1.7744 1.7686 1.7628 1.757 1.7512 1.7455 1.7397 1.7339 1.7281 1.7223 1.7165 1.7107 1.705 1.6992 1.6934 31.1887 30.9061 30.626 30.3483 30.0732 29.8004 29.5302 29.2626 28.9975 28.735 28.4751 28.2178 27.9631 27.711 27.4618 27.2151 26.9712 26.73 26.4917 26.2561 26.0234 25.7934 25.5664 25.3423 25.1201 24.9027 24.6875 24.4752 24.2659 24.0599 23.8568 23.6568 23.4601 23.2665 23.0762 22.8892 22.7054 22.5249 22.3479 22.1743 22.004 Page 26 60.1134 60.0885 60.0584 60.0231 59.9824 59.9365 59.8853 59.8289 59.7671 59.7006 59.6277 59.5499 59.4668 59.3784 59.2845 59.1853 59.0807 58.9707 58.8552 58.7343 58.6078 58.4759 58.3385 58.1954 58.0468 57.8925 57.7327 57.5672 57.3959 57.219 57.0362 56.8476 56.6532 56.4528 56.2465 56.0343 55.816 55.5916 55.3612 55.1245 54.8815 1.170384 1.180385 1.190382 1.200379 1.210376 1.220377 1.230376 1.240371 1.250369 1.260354 1.270362 1.280361 1.29036 1.300359 1.310355 1.320354 1.330352 1.34035 1.350346 1.360343 1.37034 1.38034 1.390335 1.400333 1.410361 1.420333 1.430327 1.440325 1.450326 1.460317 1.470317 1.480318 1.490313 1.500313 1.510311 1.520306 1.530305 1.540305 1.550299 1.560296 1.570298 1.924692 1.918905 1.913119 1.907332 1.901548 1.895759 1.889973 1.884187 1.878401 1.872616 1.86683 1.861044 1.855257 1.849469 1.843686 1.837898 1.832112 1.826325 1.82054 1.814754 1.808969 1.803181 1.797396 1.791611 1.7858 1.780035 1.774251 1.768464 1.762676 1.756894 1.751106 1.745318 1.739534 1.733746 1.727961 1.722176 1.716389 1.710601 1.704817 1.699032 1.693242 Trường ĐHBK Hà Nội 21.841 21.6778 21.5182 21.3622 21.2099 21.0613 20.9164 20.7753 20.6382 20.5049 20.3756 20.2503 20.1292 20.0122 19.8994 19.7909 19.6867 19.587 19.4918 19.4012 19.3153 19.2341 19.1578 19.0865 19.0202 18.9591 18.9033 18.8529 18.8081 18.7689 18.7356 18.7082 18.6871 18.6722 18.6639 18.6623 18.6677 18.6803 18.7004 18.7282 18.7641 NHÓM 14 54.6414 54.3858 54.1237 53.8551 53.5799 53.2981 53.0095 52.7141 52.4118 52.1025 51.7861 51.4625 51.1316 50.7933 50.4474 50.0939 49.7326 49.3633 48.9859 48.6003 48.2063 47.8037 47.3924 46.9721 46.5426 46.1037 45.6552 45.1969 44.7284 44.2494 43.7598 43.2591 42.747 42.2231 41.687 41.1384 40.5766 40.0013 39.4118 38.8076 38.188 BTL Robot 1.58 1.59 1.6 1.61 1.62 1.63 1.64 1.65 1.66 1.67 1.68 1.69 1.7 1.71 1.72 1.73 1.74 1.75 1.76 1.77 1.78 1.79 1.8 1.81 1.82 1.83 1.84 1.85 1.86 1.87 1.88 1.89 1.9 1.91 1.92 1.93 1.94 1.95 1.96 1.97 1.98 1.6876 1.6818 1.676 1.6702 1.6645 1.6587 1.6529 1.6471 1.6413 1.6355 1.6298 1.624 1.6182 1.6124 1.6066 1.6008 1.595 1.5893 1.5835 1.5777 1.5719 1.5661 1.5603 1.5545 1.5488 1.543 1.5372 1.5314 1.5256 1.5198 1.514 1.5083 1.5025 1.4967 1.4909 1.4851 1.4793 1.4736 1.4678 1.462 1.4562 21.8373 21.6742 21.5146 21.3586 21.2063 21.0578 20.9129 20.7718 20.6347 20.5015 20.3722 20.2469 20.1258 20.0088 19.896 19.7876 19.6834 19.5837 19.4885 19.3979 19.312 19.2309 19.1546 19.0833 19.017 18.9559 18.9001 18.8497 18.8049 18.7658 18.7325 18.7051 18.684 18.6691 18.6608 18.6592 18.6646 18.6772 18.6973 18.725 18.7609 Page 27 54.6323 54.3767 54.1147 53.8461 53.571 53.2892 53.0006 52.7053 52.403 52.0938 51.7775 51.4539 51.123 50.7484 50.439 50.0855 49.7243 49.355 48.9777 48.5922 48.1983 47.7957 47.3845 46.9643 46.5348 46.096 45.6476 45.1893 44.7209 44.242 43.7525 43.2519 42.7399 42.216 41.68 41.1315 40.5698 39.9946 39.4052 38.8011 38.1816 1.580295 1.59029 1.600287 1.610287 1.620283 1.630279 1.64028 1.650279 1.660276 1.670271 1.680269 1.690269 1.700266 1.710983 1.720261 1.730256 1.740254 1.750254 1.760252 1.770249 1.780245 1.790243 1.800239 1.810235 1.820235 1.830233 1.84023 1.850228 1.860224 1.870221 1.880217 1.890215 1.90021 1.910211 1.920209 1.930206 1.940204 1.9502 1.960197 1.970197 1.980193 1.687456 1.681672 1.675886 1.670099 1.664313 1.658529 1.652741 1.646953 1.641168 1.635384 1.629597 1.623809 1.618024 1.611987 1.606451 1.600667 1.59488 1.589093 1.583306 1.57752 1.571735 1.56595 1.560164 1.554379 1.548591 1.542805 1.537019 1.531232 1.525448 1.519663 1.513878 1.50809 1.502307 1.496518 1.490732 1.484946 1.479159 1.473374 1.467589 1.4618 1.456016 Trường ĐHBK Hà Nội 18.8083 18.8613 18.9235 18.9953 19.0772 19.1697 19.2735 19.3893 19.5179 19.66 19.8169 19.9896 20.1795 20.3882 20.6177 20.8704 21.149 21.4572 21.7995 22.182 22.6127 23.1034 23.6714 24.3452 25.1781 26.2949 28.2512 NHÓM 14 37.5523 36.8997 36.2294 35.5403 34.8315 34.1017 33.3496 32.5738 31.7726 30.9439 30.0857 29.1954 28.27 27.306 26.2992 25.2446 24.136 22.9657 21.7236 20.397 18.968 17.4116 15.6901 13.7417 11.4512 8.5481 3.8289 BTL Robot 1.99 2.01 2.02 2.03 2.04 2.05 2.06 2.07 2.08 2.09 2.1 2.11 2.12 2.13 2.14 2.15 2.16 2.17 2.18 2.19 2.2 2.21 2.22 2.23 2.24 2.25 1.4504 1.4446 1.4388 1.4331 1.4273 1.4215 1.4157 1.4099 1.4041 1.3983 1.3926 1.3868 1.381 1.3752 1.3694 1.3636 1.3579 1.3521 1.3463 1.3405 1.3347 1.3289 1.3231 1.3174 1.3116 1.3058 1.3 18.8051 18.8581 18.9203 18.9921 19.074 19.1665 19.2703 19.386 19.5146 19.6567 19.8136 19.9863 20.1761 20.3848 20.6142 20.8669 21.1455 21.4536 21.7959 22.1783 22.6089 23.0995 23.6674 24.3411 25.1739 26.2905 28.2465 Page 28 37.546 36.8935 36.2233 35.5344 34.8257 34.096 33.344 32.5683 31.7673 30.9387 30.0807 29.1905 28.2653 27.3014 26.2948 25.2404 24.132 22.9619 21.712 20.3936 18.9648 17.4087 15.6874 13.7394 11.4493 8.5467 3.8283 1.990191 2.000189 2.010185 2.02018 2.030177 2.040176 2.050174 2.060173 2.070166 2.080166 2.09016 2.100157 2.110155 2.120153 2.130152 2.140147 2.150143 2.16014 2.170253 2.180134 2.190134 2.200131 2.210128 2.220125 2.23012 2.240118 2.250114 1.450228 1.444442 1.438658 1.432874 1.427089 1.421301 1.415515 1.409726 1.403945 1.398156 1.392375 1.38659 1.380803 1.375016 1.369227 1.363445 1.35766 1.351875 1.345967 1.340304 1.334514 1.328729 1.322942 1.317157 1.311374 1.305587 1.299803 Trường ĐHBK Hà Nội Quỹ đạo tròn toán robot Thông số tính toán q1 q2 x 93.6541 44.9923 -0.99 93.1319 44.9923 -0.97 92.6118 44.9923 -0.95 92.0937 44.9923 -0.93 91.5776 44.9923 -0.91 91.0634 44.9923 -0.89 90.551 44.9923 -0.87 90.0404 44.9923 -0.85 89.5315 44.9923 -0.83 89.0243 44.9923 -0.81 88.5187 44.9923 -0.79 88.0146 44.9923 -0.77 87.512 44.9923 -0.75 87.0109 44.9923 -0.73 86.5111 44.9923 -0.71 86.0127 44.9923 -0.69 85.5156 44.9923 -0.67 85.0197 44.9923 -0.65 84.5251 44.9923 -0.63 84.0316 44.9923 -0.61 83.5392 44.9923 -0.59 83.0479 44.9923 -0.57 82.5576 44.9923 -0.55 82.0683 44.9923 -0.53 81.5799 44.9923 -0.51 81.0925 44.9923 -0.49 80.6059 44.9923 -0.47 80.1201 44.9923 -0.45 79.6351 44.9923 -0.43 79.1508 44.9923 -0.41 78.6673 44.9923 -0.39 78.1844 44.9923 -0.37 77.7022 44.9923 -0.35 77.2205 44.9923 -0.33 76.7395 44.9923 -0.31 76.2589 44.9923 -0.29 75.7788 44.9923 -0.27 75.2992 44.9923 -0.25 NHÓM 14 BTL Robot y 2.19 2.1989 2.2076 2.2161 2.2244 2.2325 2.2404 2.248 2.2555 2.2628 2.2698 2.2767 2.2834 2.2898 2.2961 2.3022 2.3081 2.3138 2.3193 2.3247 2.3298 2.3348 2.3396 2.3442 2.3486 2.3529 2.357 2.3609 2.3646 2.3681 2.3715 2.3747 2.3778 2.3806 2.3833 2.3858 2.3882 2.3903 Thông số mô q1 q2 93.6385 44.9848 93.1164 44.9848 92.5964 44.9848 92.0783 44.9848 91.5623 44.9848 91.0482 44.9848 90.5359 44.9848 90.0254 44.9848 89.5166 44.9848 89.0095 44.9848 88.5039 44.9848 87.9999 44.9848 87.4974 44.9848 86.9964 44.9848 86.4967 44.9848 85.9984 44.9848 85.5013 44.9848 85.0055 44.9848 85.511 44.9848 84.0176 44.9848 85.5253 44.9848 83.034 44.9848 82.5438 44.9848 82.0546 44.9848 81.5993 44.9848 81.079 44.9848 80.5924 44.9848 80.1067 44.9848 79.6218 44.9848 79.1376 44.9848 78.692 44.9848 78.2089 44.9848 77.7266 44.9848 77.2447 44.9848 76.7267 44.9848 76.2462 44.9848 75.7662 44.9848 75.2866 44.9848 Page 29 x -0.9893 -0.9693 -0.9493 -0.9293 -0.9093 -0.8893 -0.8693 -0.8493 -0.8293 -0.8093 -0.7893 -0.7693 -0.7493 -0.72931 -0.70931 -0.68931 -0.66931 -0.64931 -0.6697 -0.60931 -0.67027 -0.56931 -0.54931 -0.52932 -0.51067 -0.48932 -0.46932 -0.44932 -0.42932 -0.40932 -0.39089 -0.37088 -0.35088 -0.33087 -0.30934 -0.28934 -0.26934 -0.24934 y 2.190386 2.19931 2.208016 2.21651 2.224789 2.232858 2.240721 2.248377 2.25583 2.263081 2.270135 2.27699 2.283649 2.290114 2.296387 2.302469 2.308363 2.314068 2.30825 2.324919 2.308082 2.335036 2.339821 2.344426 2.348558 2.353099 2.357169 2.361063 2.364781 2.368325 2.371437 2.374648 2.377686 2.380553 2.383447 2.385957 2.388298 2.390468 Trường ĐHBK Hà Nội BTL Robot 74.82 74.3412 73.8628 73.3846 72.9068 72.4292 71.9518 71.4746 70.9975 70.5206 70.0437 69.5669 44.9923 44.9923 44.9923 44.9923 44.9923 44.9923 44.9923 44.9923 44.9923 44.9923 44.9923 44.9923 -0.23 -0.21 -0.19 -0.17 -0.15 -0.13 -0.11 -0.09 -0.07 -0.05 -0.03 -0.01 2.3923 2.3942 2.3959 2.3974 2.3987 2.3999 2.4009 2.4017 2.4024 2.4029 2.4032 2.4034 74.8075 74.3288 73.8505 73.3724 72.8947 72.4171 71.9398 71.4627 70.9857 70.5088 70.032 69.5553 44.9848 44.9848 44.9848 44.9848 44.9848 44.9848 44.9848 44.9848 44.9848 44.9848 44.9848 44.9848 69.0901 44.9923 0.01 2.4034 69.0786 44.9848 68.6133 44.9923 0.03 2.4032 68.6019 44.9848 68.1364 44.9923 0.05 2.4029 68.125 44.9848 67.6595 44.9923 0.07 2.4024 67.6482 44.9848 67.1824 44.9923 0.09 2.4017 67.1712 44.9848 66.7052 44.9923 0.11 2.4009 66.6941 44.9848 66.2278 44.9923 0.13 2.3999 66.2168 44.9848 65.7502 44.9923 0.15 2.3987 65.7392 44.9848 65.2724 44.9923 0.17 2.3974 65.2615 44.9848 64.7942 44.9923 0.19 2.3959 64.7834 44.9848 64.3158 44.9923 0.21 2.3942 64.3051 44.9848 63.837 44.9923 0.23 2.3923 63.8264 44.9848 63.3578 44.9923 0.25 2.3903 63.3472 44.9848 62.8782 44.9923 0.27 2.3882 62.8677 44.9848 62.3981 44.9923 0.29 2.3858 62.3877 44.9848 NHÓM 14 Page 30 -0.22934 -0.20934 -0.18935 -0.16935 -0.14936 -0.12936 -0.10936 -0.08936 -0.06936 -0.04937 -0.02937 -0.00937 0.01062 0.03062 0.05062 0.07061 0.09061 0.11061 0.13060 0.15060 0.17060 0.19059 0.21059 0.23058 0.25058 0.27058 0.29058 2.39247 2.394302 2.395967 2.397463 2.398792 2.399953 2.400948 2.401775 2.402436 2.40293 2.403257 2.403419 2.403413 2.403242 2.402904 2.402399 2.401728 2.40089 2.399886 2.398714 2.397374 2.395867 2.394193 2.39235 2.390338 2.388157 2.385806 Trường ĐHBK Hà Nội BTL Robot 61.9175 44.9923 0.31 2.3833 61.9072 44.9848 61.4365 44.9923 0.33 2.3806 61.4263 44.9848 60.9548 44.9923 0.35 2.3778 60.9446 44.9848 60.4726 44.9923 0.37 2.3747 60.4625 44.9848 59.9897 44.9923 0.39 2.3715 59.9797 44.9848 59.5062 44.9923 0.41 2.3681 59.4963 44.9848 59.0219 44.9923 0.43 2.3646 59.012 44.9848 58.5369 44.9923 0.45 2.3609 58.5271 44.9848 58.0511 44.9923 0.47 2.357 58.0414 44.9848 57.5645 44.9923 0.49 2.3529 57.5549 44.9848 57.0771 44.9923 0.51 2.3486 57.0676 44.9848 56.5887 44.9923 0.53 2.3442 56.5793 44.9848 56.0994 44.9923 0.55 2.3396 56.09 44.9848 55.6091 44.9923 0.57 2.3348 55.5998 44.9848 55.1178 44.9923 0.59 2.3298 55.1086 44.9848 54.6254 44.9923 0.61 2.3247 54.6163 44.9848 54.1319 44.9923 0.63 2.3193 54.1229 44.9848 53.6373 44.9923 0.65 2.3138 53.6284 44.9848 53.1414 44.9923 0.67 2.3081 53.1325 44.9848 52.6443 44.9923 0.69 2.3022 52.6355 44.9848 52.1459 44.9923 0.71 2.2961 52.1372 44.9848 NHÓM 14 Page 31 0.31057 0.33057 0.35057 0.37056 0.39056 0.41055 0.43056 0.45055 0.47055 0.49054 0.51054 0.53053 0.55053 0.57053 0.59052 0.61052 0.63051 0.65051 0.67051 0.69050 0.71050 2.383285 2.380595 2.377731 2.374698 2.371491 2.368111 2.364556 2.360828 2.356924 2.352843 2.348586 2.34415 2.339534 2.334738 2.329761 2.324601 2.319257 2.313729 2.308012 2.302109 2.296017 Trường ĐHBK Hà Nội BTL Robot 51.6461 44.9923 0.73 2.2898 51.6375 44.9848 51.145 44.9923 0.75 2.2834 51.1365 44.9848 50.6424 44.9923 0.77 2.2767 50.6339 44.9848 50.1383 44.9923 0.79 2.2698 40.1299 44.9848 49.6327 44.9923 0.81 2.2628 49.6244 44.9848 49.1255 44.9923 0.83 2.2555 49.1173 44.9848 48.6166 44.9923 0.85 2.248 48.6085 44.9848 48.106 44.9923 0.87 2.2404 48.098 44.9848 47.5936 44.9923 0.89 2.2325 47.5858 44.9848 47.0794 44.9923 0.91 2.2244 47.0715 44.9848 46.5633 44.9923 0.93 2.2161 46.5555 44.9848 46.0452 44.9923 0.95 2.2076 46.0375 44.9848 45.5251 44.9923 0.97 2.1989 45.5175 44.9848 45.0029 44.9923 0.99 2.19 44.9954 44.9848 NHÓM 14 Page 32 0.7305 0.75049 0.77049 1.17261 0.81048 0.83047 0.85047 0.87046 0.89045 0.91046 0.93045 0.95045 0.97044 0.99044 2.289733 2.283258 2.276587 2.097972 2.262659 2.255397 2.247934 2.240267 2.232396 2.224313 2.216024 2.207521 2.198804 2.18987 [...]... l1*lg2*cos(q( 2)) )+ J2 ; h(2, 2) = m2*lg2^2 + J2 ; % gia toc khop y=h*[U( 1); U( 2)] ; end • code khối V: function y = bai2_codeV(in) %q( 1) goc theta1 %q( 2) goc theta2 %dq( 1) toc do quay khop 1 %dq( 2) toc do quay khop 2 %cac thong so robot global m2 l1 lg2 q( 1) = in( 1); q( 2) = in( 2); dq( 1) =in( 3); dq( 2) =in( 4); % Detailed explanation goes here y(1, 1) = -m2*l1*lg2*sin(q( 2)) *(2*dq( 1)+ dq( 2)) ; y(2, 1) = m2*l1*lg2*sin(q( 2)) *dq( 1)^ 2... ĐHBK Hà Nội BTL Robot II Tiến hành: 2.1 Mô hình robot trên matlab: Trong đó: • Code khối G: function y =bai2_codeG(in) %q( 1) goc theta1 %q( 2) goc theta2 %dq( 1) toc do quay khop 1 %dq( 2) toc do quay khop 2 %cac thong so robot global m1 m2 l1 lg1 lg2 g q( 1) =in( 1); q( 2) = in( 2); % trong luc y(1, 1)= g*m1*lg1*cos(q( 1)) +g*m2*(l1*cos(q( 1)) +lg2*cos(q( 1)+ q( 2)) ); y(2, 1)= g*m2*lg2*cos(q( 1)+ q( 2)) ; end NHÓM 14... Robot • Code khối H: function y = bai2_codeH(in) %q( 1) goc theta1 %q( 2) goc theta2 %dq( 1) toc do quay khop 1 %dq( 2) toc do quay khop 2 %cac thong so robot global m1 m2 l1 l2 lg1 lg2 J1 J2 U( 1) =in( 1); U( 2) =in( 2); q( 1) =in( 3); q( 2) =in( 4); % ma tran quan tinh h(1, 1) = m1*lg1^2+J1+m2*(l1^2+lg2^2+2*l1*lg2*cos(q( 2)) )+ J2 ; h(1, 2) = m2*(lg2^2 + l1*lg2*cos(q( 2)) )+ J2 ; h(2, 1) = m2*(lg2^2 + l1*lg2*cos(q( 2)) )+ J2... bai2_codeH_1(in) %q( 1) goc theta1 %q( 2) goc theta2 %dq( 1) toc do quay khop 1 %dq( 2) toc do quay khop 2 NHÓM 14 Page 19 Trường ĐHBK Hà Nội BTL Robot %cac thong so robot global m1 m2 l1 l2 lg1 lg2 J1 J2 q( 1) =in( 1); q( 2) =in( 2); M01 =in( 3); M02 =in( 4); % ma tran quan tinh h(1, 1) = m1*lg1^2+J1+m2*(l1^2+lg2^2+2*l1*lg2*cos(q( 2)) )+ J2 ; h(1, 2) = m2*(lg2^2 + l1*lg2*cos(q( 2)) )+ J2 ; h(2, 1) = m2*(lg2^2 + l1*lg2*cos(q( 2)) )+ J2... lực học : & &+ V (Q, Q &) + G (Q ) M = H (Q )Q & &+  1 H&(Q) + S (Q, Q &)  Q & = H (Q )Q  2  + G (Q ) (1- 1) Phương trình điều khiển M dk = K p E − K d E&+ G (Q ) (1- 2) Trong đó : E = Qd – Q : sai số vị trí khớp robot E&= Q&d − Q& : sai số tốc độ khớp robot Thay (1- 2) vào (1- 1) ta được phương trình động lực học kín : & &+ V (Q, Q &) + G (Q) K p E − K d E&+ G (Q ) = H (Q )Q & &+ V (Q, Q & &) = H... (Q )Q & &+  1 H&(Q) + S (Q, Q &)  Q& ↔ K p E − K d E&= H (Q)Q  2  & &+  1 H&(Q ) + S (Q, Q &)  Q&− K E = − K Q& ↔ H (Q)Q p d  2  & &+ 1 Q&T H&(Q)Q&+ Q&T S (q, q&)Q&− Q&T K E = −Q&T K Q& ↔ Q&T H (Q)Q p d 2 1 d &T Q H (Q)Q&+ E T K p E  = −Q&T K d Q& ( do Q&T S (Q, Q&)Q&= 0 ) → 2 dt Chọn hàm Lyapunov : V(Q, Ta thấy rằng : NHÓM 14 )= 1 &T Q H (Q)Q&+ E T K p E  2 V( E ) ≥ 0  & V( E ). .. ) = 0 ↔ E = 0 và Q=0 && V (Q) ≤ 0 V&(Q &) = 0 ↔ Q&= 0  Page 13 (1 − 3) (1 − 4) (1 − 5) (1 − 6) (1 − 7) Trường ĐHBK Hà Nội Như vậy Q& → K pE → 0 0 khi khi BTL Robot t →∞ thay vào phương trình (1- 5) ta có : t →∞ ⇒ E → 0 khi t → ∞ Sơ đồ điều khiển : Qd + KP _ Q + + _ ĐLH robot KD Q G(Q) G( .) Hình 2.1.1 Sơ đồ điều khiển PD bù trọng trường Ưu điểm : đơn giản về mặt toán học Nhược điểm : Phải biết chính...Trường ĐHBK Hà Nội BTL Robot BÀI 2: XÂY DỰNG MÔ HÌNH ROBOT, BỘ DDIEUF KHIỂN MÔ MEN VÀ BỘ ĐIỀU KHIỂN PD BÙ TRỌNG TRƯỜNG I Cơ sở lý thuyết: 1.1 Tổng quan về phương pháp điều khiển robot Phương trình động lực học robot : & &+ V (Q, Q &) + G (Q) M = H (Q)Q Chức năng của hệ thống điều khiển chuyển động là tìm M để cánh tay có quỹ t→∞ đạo bám... -m2*l1*lg2*sin(q( 2)) *(2*dq( 1)+ dq( 2)) ; y(2, 1) = m2*l1*lg2*sin(q( 2)) *dq( 1)^ 2 ; end NHÓM 14 Page 17 Trường ĐHBK Hà Nội BTL Robot 2.2 Thiết kế bộ điều khiển mô men tính toán cho robot: a Mô hình bộ điều khiển mô men tính toán cho robot Với: KP= [32.65 0;0 32.65] KI= [8 0;0 8] LP1= [16 0;0 16] b Mô hình tính mô men điều khiển Mđk: NHÓM 14 Page 18 Trường ĐHBK Hà Nội BTL Robot c Mô hình robot từ Mđk ra q,dq,ddq: - Trong đó : • Các... Nếu tồn tại hàm Lyapunov V(x) ,thỏa mãn các điều kiện: a, Xác định dương, tức là V(x)> 0 với x ≠ 0 và V(x)= 0 ↔ b, dV(x)/dt < 0 với x là nghiệm tự do của hệ thống thì hệ sẽ ổn định NHÓM 14 Page 12 Trường ĐHBK Hà Nội 1.3 Phương pháp PD bù trọng trường BTL Robot Yêu cầu đặt ra là xác định cấu trúc bộ điều khiển đảm bảo hệ thống ổn định tuyệt đối xung quanh điểm cân bằng ( Q= Qd ), không phụ thuộc vào khối ... lg2 g q( 1) =in( 1); q( 2) = in( 2); % luc y(1, 1)= g*m1*lg1*cos(q( 1)) +g*m2*(l1*cos(q( 1)) +lg2*cos(q( 1)+ q( 2)) ); y(2, 1)= g*m2*lg2*cos(q( 1)+ q( 2)) ; end NHÓM 14 Page 16 Trường ĐHBK Hà Nội BTL Robot • Code... q( 1) = in( 1); q( 2) = in( 2); dq( 1) =in( 3); dq( 2) =in( 4); % Detailed explanation goes here y(1, 1) = -m2*l1*lg2*sin(q( 2)) *(2*dq( 1)+ dq( 2)) ; y(2, 1) = m2*l1*lg2*sin(q( 2)) *dq( 1)^ 2 ; end NHÓM 14 Page 17... bai2_codeH(in) %q( 1) goc theta1 %q( 2) goc theta2 %dq( 1) toc quay khop %dq( 2) toc quay khop %cac thong so robot global m1 m2 l1 l2 lg1 lg2 J1 J2 U( 1) =in( 1); U( 2) =in( 2); q( 1) =in( 3); q( 2) =in( 4); % ma