BÀI tập lớn môn ROBOT đại học bách khoa BKA (thầy tiến )

Bài toán động học thuận -Mục đích của bài toán động học thuận là tìm ra vị trí và hướng của cơ cấu tác động cuối khi đã biết giá trị các biến khớp.. Gắn hệ tọa độ lên các khâu - Chon

BÀI TẬP LỚN MÔN ROBOT

Cho robot Scalar 2DOF các thông số như sau:

a Thiết kế bộ điều khiển vị trí cho mỗi khớp biết |Ui|≤ 10V, độ quá điều chỉnh

≤5%, thời gian quá độ là nhỏ nhất

b Chọn 2 quỹ đạo thẳng và tròn, tính quỹ đạo cho từng khớp.

c Mô phỏng trên Matlab simulink So sánh quỹ đạo đặt và quỹ đạo thực tế.

2 Yêu cầu:

a.Xây dựng mô hình toán học của robot trên Matlab simulink.

b Thiết kế bộ điều khiển momen tính toán cho robot, mô phỏng trên Matlab simulink, so sánh quỹ đạo đặt và quỹ đạo thực.

c Thiết kế bộ điều khiển PD bù trọng trường cho robot, mô phỏng trên Matlab simulink, so sánh quỹ đạo đặt và quỹ đạo thực.

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:

1 Bài toán động học thuận

-Mục đích của bài toán động học thuận là tìm ra vị trí và hướng của cơ cấu tác động cuối khi đã biết giá trị các biến khớp Gắn hệ tọa độ lên các khâu

- Chon hệ tọa độ gốc Ooxoyozo : Gốc tọa độ Oo trên trục khớp thứ nhất, trục

xo đặt theo phương của khâu 1, chiều hướng từ trục khớp thứ 1 sang trục khớp

thứ 2 (Hình 1.1.1)

- Hệ tọa độ O1x1y1z1 có gốc tọa độ O1 đặt tại tâm trục khớp thứ hai, trục x1

đặt theo phương của 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ại 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 của khớp thứ 2.

Hình 1.1.1 : Sơ đồ động Robot Planar

Để đơn giản cho việc trình bày ta sử dụng các kí hiệu s⍬I = sin(⍬i) và

Trường ĐHBK Hà Nội BTL Robot Cân bằng các phần tử tương ứng của hai ma trận với nhau ta có phương trình động học của Robot Planar là:

Ma trận biểu diễn hướng của tay máy Robot:

Ma trận biểu diễn vị trí của Robot

2 Bài toán động học ngược

Trong bài toán động học ngược ta cần xác định giá trị các biến khớp của Robot khi đãbiết vị trí và hướng của điểm tác động cuối Vị trí và hướng của khâu tác động cuối đượcxác định từ quỹ đạo chuyển đông cho trước của robot, dưới dạng ma trận:

Ta biết phương trình động học của Robot Planar

Ma trận đã biết (biết vị trí và hướng của cơ cấu tác động cuối ) ta cần xác định giá trị các biến khớp tương ứng Bằng cách cân bằng phần tử ở các hàng

1 và hàng 2 cột hai ma trận tương ứng ta xác định được:

Xác định biến khớp

Bình phương hai vế các phương trình (2.9) và cộng lại ta nhận được phương trình:

= atan2 (px, ) - 𐐜 = atan2 (px, ) – atan2 (

Vậy ta có: atan2 (px, ) – atan2 (

atan2[s

Trường ĐHBK Hà Nội BTL Robot

II Tiến hành:

1 Thiết kế bộ điều khiển vị trí:

Hình 1.1.2: Sơ đồ bộ điều khiển vị trí chô mỗi khớp

Hình 1.1.3: Vị trí khớp 1 so với lượng đặt

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);

Hình 1.2.1: Quỹ đạo góc khớp 1 mô phỏng (màu vàng)và tính toán (màu đen)

Hình 1.2.2: Quỹ đạo góc khớp 2 mô phỏng (màu vàng)và tính toán (màu đen)

Hình 1.2.3: Quỹ đạo thẳng của cánh tay robot tính toán và thực tế

Hình 1.2.4: Quỹ đạo tròn của cánh tay robot tính toán và thực tế

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Ù

Điều khiển trong không gian khớp

Đại lượng điều khiển là vị trí của khớp robot : góc quay đối với khớp quay; độ dịch chuyển thẳng đối với khớp tinh tiến Bộ điều khiển được thiết kế đảm bảo vị trí khớp luôn bám theo vị trí đặt, tức là sai lệch vị trí khớp hội tụ về không với thời gian nhỏ nhất.

Điều khiển trong không gian làm việc

Tín hiệu đặt là vị trí bàn tay máy

Thuật toánđiều khiểnchuyển động

Động lực họcRobot

Quỹ đạo của tay máy :

Quỹ đạo đặt cho tay máy X d [ , , ,x y z d d d xd,yd,zd]T

Yêu cầu đặt ra là tìm M để X  X d khi t  .

Theo yêu cầu của đề bài cho ta sẽ thiết kế 2 bộ điều khiển trong không gian khớp

đó là: Điều khiển trực tiếp momen PIDvà bộ điều khiển PD bù trọng trường.

Sau đây em xin được đề cập đến hai phương pháp điều khiển trong không gian khớp Ưu điểm của phương pháp này là bộ điều khiển tác động trực tiếp đến hệ thống truyền động của khớp Tuy nhiên, hệ thống khó đảm bảo độ chính xác vị trí tay khi tồn tại các sai lệch trong cơ cấu cơ khí.

1.2 Tiêu chuẩn Lyapunov

Định nghĩa (Lyapunov) : Một hệ thống được gọi là ổn định tiệm cận Lyapunov tại điểm cân bằng x nếu như hệ đang ở điểm cân bằng x mà có một tác động nhiễu tức thời (hoặc khoảng thời gian đủ ngắn) đánh bật hệ ra khỏi x thì sau đó hệ có khả năng tự quay về điểm cân bằng ban đầu

Tiêu chuẩn Lyapunov là phương pháp xét tính ổn định một cách trực tiếp trong không gian trạng thái, thích hợp cho những hệ thống mô tả bởi mô hình trạng thái.

Hệ ổn định nếu quá tình tự do của nó tăt dần nên phương pháp Lyapunov sẽ kiểm tra xem quỹ đạo trạng thái của hệ thống ở quá trình tự do có hướng tiến về gốc tọa

độ và kết thúc tại đó không.

Từ phương diện năng lượng có thể xem như phương pháp Lyapunov được xây dựng trên cơ sở bảo tồn năng lượng của một hệ vật lý Năng lượng còn tồn tại bên trong hệ vật lý do tác động tức thời bên ngoài đưa vào được đo bởi một hàm không âm.Hệ sẽ ổn định ở trạng thái cân bằng của nó nếu nhưu trong lân cận điểm cân bằng đó hàm do năng lượng này của hệ luôn có xu hướng giảm về 0.

Định lý Lyapunov : Nếu tồn tại hàm Lyapunov V(x) ,thỏa mãn các điều kiện:

Phương trình động lực học :

Trường ĐHBK Hà Nội BTL Robot

M dk K E K E G Q p  d   ( ) (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 :

( ) ( ) ( , ) ( ) (1 3)

1( ) ( , ) ( ) ( ) ( , ) (1 4)

21

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 xác các tham số khối lượng trong G(Q).

1.4 Thuật toán PID

Moment điều khiển :

Mdk =KpE - Kd +KI

(1-8)

K p diag k p1,k p2, k pn

là ma trận đường chéo hệ số khuếch đại

K d diag k d1,k d2, k dn là ma trận đường chéo hệ số đạo hàm

Sơ đồ điều khiển :

Qd + +

_ _

Q

Q

Hình 2.1.2 Sơ đồ điều khiển PID

Nhược điểm của phương pháp PID là các thông số KI, Kd, KP chỉ được xác định bằng thực nghiệm nên rất khó đưa ra được thông số chính xác.

KP

KI

ĐLH robot

KD

Trường ĐHBK Hà Nội BTL Robot

%dq(1) toc do quay khop 1

%dq(2) toc do quay khop 2

%cac thong so robot

end

 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

%dq(1) toc do quay khop 1

%dq(2) toc do quay khop 2

%cac thong so robot

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

c Mô hình robot từ Mđk ra q,dq,ddq:

- Trong đó :

 Các mô hình G,V không đổi

 Mô hình của H−1 như sau:

function y = 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

Trường ĐHBK Hà Nội BTL Robot

%cac thong so robot

d Đáp ứng của 2 khớp trong mô phỏng:

Hình 2.2.1: Đáp ứng của góc khớp q so với giá trị đặt

Hình 2.2.2: Đáp ứng của dq trong mô hình

Trường ĐHBK Hà Nội BTL Robot

Hình 2.2.3: Đáp ứng của ddq trong mô hình

Hình 2.2.4 : Đáp ứng của mô men điều khiển trong mô hình

2.3 Thiết kế bộ điều khiển PD bù trọng trường:

a Mô hình của bộ điều khiển:

- Trong đó các thông số của bộ điều khiển như sau:

Kd= [16 0;0 16]

Kp= [100 0;0 100]

b Các đáp ứng của q,dq,ddq và Mđk:

Hình 2.3.1: đáp ứng của 2 khớp (q)

Trường ĐHBK Hà Nội BTL Robot

Hình 2.3.2: Đáp ứng của Mđk 2 khớp

Hình 2.3.4: Đáp ứng vận tốc 2 khớp

Hình 2.3.5: Đáp ứng gia tốc của 2 khớp

Ghi chú:

Quỹ đạo thẳng của bài toán robot

Thông số tính toán Thông số mô phỏng

35.0826 59.9685 1.04 2 35.0767 59.9585 1.040416 1.99991134.7697 60.012 1.05 1.9942 34.7629 60.002 1.050445 1.99410734.4591 60.0501 1.06 1.9884 34.4536 60.0401 1.060399 1.98834534.1507 60.0831 1.07 1.9826 34.145 60.073 1.070406 1.98255533.8445 60.1107 1.08 1.9769 33.8387 60.1007 1.08041 1.97676433.5407 60.1331 1.09 1.9711 33.5351 60.1231 1.090401 1.97098233.2391 60.1503 1.1 1.9653 33.2335 60.1403 1.100402 1.96519432.9399 60.1622 1.11 1.9595 32.9344 60.1522 1.110397 1.95940932.643 60.1688 1.12 1.9537 32.6376 60.1587 1.120396 1.95362432.3484 60.1702 1.13 1.9479 32.343 60.1602 1.130395 1.94783632.0562 60.1664 1.14 1.9421 32.0508 60.1564 1.140393 1.94204931.7664 60.1573 1.15 1.9364 31.7611 60.1473 1.150388 1.93626531.4789 60.143 1.16 1.9306 31.4737 60.133 1.160386 1.930478

Trường ĐHBK Hà Nội BTL Robot31.1939 60.1234 1.17 1.9248 31.1887 60.1134 1.170384 1.92469230.9113 60.0985 1.18 1.919 30.9061 60.0885 1.180385 1.91890530.6311 60.0684 1.19 1.9132 30.626 60.0584 1.190382 1.91311930.3534 60.0331 1.2 1.9074 30.3483 60.0231 1.200379 1.90733230.0782 59.9924 1.21 1.9017 30.0732 59.9824 1.210376 1.90154829.8054 59.9465 1.22 1.8959 29.8004 59.9365 1.220377 1.89575929.5352 59.8953 1.23 1.8901 29.5302 59.8853 1.230376 1.88997329.2675 59.8389 1.24 1.8843 29.2626 59.8289 1.240371 1.88418729.0024 59.7771 1.25 1.8785 28.9975 59.7671 1.250369 1.87840128.7398 59.71 1.26 1.8727 28.735 59.7006 1.260354 1.87261628.4799 59.6376 1.27 1.8669 28.4751 59.6277 1.270362 1.8668328.2225 59.5598 1.28 1.8612 28.2178 59.5499 1.280361 1.86104427.9678 59.4767 1.29 1.8554 27.9631 59.4668 1.29036 1.85525727.7157 59.3883 1.3 1.8496 27.711 59.3784 1.300359 1.84946927.4664 59.2944 1.31 1.8438 27.4618 59.2845 1.310355 1.84368627.2197 59.1952 1.32 1.838 27.2151 59.1853 1.320354 1.83789826.9757 59.0906 1.33 1.8322 26.9712 59.0807 1.330352 1.83211226.7345 58.9805 1.34 1.8264 26.73 58.9707 1.34035 1.82632526.4961 58.865 1.35 1.8207 26.4917 58.8552 1.350346 1.8205426.2605 58.7441 1.36 1.8149 26.2561 58.7343 1.360343 1.81475426.0277 58.6176 1.37 1.8091 26.0234 58.6078 1.37034 1.80896925.7977 58.4857 1.38 1.8033 25.7934 58.4759 1.38034 1.80318125.5707 58.3482 1.39 1.7975 25.5664 58.3385 1.390335 1.79739625.3465 58.2051 1.4 1.7917 25.3423 58.1954 1.400333 1.79161125.1252 58.0565 1.41 1.786 25.1201 58.0468 1.410361 1.785824.9069 57.9022 1.42 1.7802 24.9027 57.8925 1.420333 1.78003524.6916 57.7423 1.43 1.7744 24.6875 57.7327 1.430327 1.77425124.4793 57.5768 1.44 1.7686 24.4752 57.5672 1.440325 1.76846424.27 57.4055 1.45 1.7628 24.2659 57.3959 1.450326 1.76267624.0639 57.2285 1.46 1.757 24.0599 57.219 1.460317 1.75689423.8608 57.0457 1.47 1.7512 23.8568 57.0362 1.470317 1.75110623.6608 56.8571 1.48 1.7455 23.6568 56.8476 1.480318 1.74531823.464 56.6626 1.49 1.7397 23.4601 56.6532 1.490313 1.73953423.2704 56.4622 1.5 1.7339 23.2665 56.4528 1.500313 1.73374623.0801 56.2559 1.51 1.7281 23.0762 56.2465 1.510311 1.72796122.893 56.0436 1.52 1.7223 22.8892 56.0343 1.520306 1.72217622.7092 55.8253 1.53 1.7165 22.7054 55.816 1.530305 1.71638922.5287 55.6009 1.54 1.7107 22.5249 55.5916 1.540305 1.71060122.3516 55.3704 1.55 1.705 22.3479 55.3612 1.550299 1.70481722.178 55.1337 1.56 1.6992 22.1743 55.1245 1.560296 1.69903222.0077 54.8907 1.57 1.6934 22.004 54.8815 1.570298 1.693242

21.841 54.6414 1.58 1.6876 21.8373 54.6323 1.580295 1.68745621.6778 54.3858 1.59 1.6818 21.6742 54.3767 1.59029 1.68167221.5182 54.1237 1.6 1.676 21.5146 54.1147 1.600287 1.67588621.3622 53.8551 1.61 1.6702 21.3586 53.8461 1.610287 1.67009921.2099 53.5799 1.62 1.6645 21.2063 53.571 1.620283 1.66431321.0613 53.2981 1.63 1.6587 21.0578 53.2892 1.630279 1.65852920.9164 53.0095 1.64 1.6529 20.9129 53.0006 1.64028 1.65274120.7753 52.7141 1.65 1.6471 20.7718 52.7053 1.650279 1.64695320.6382 52.4118 1.66 1.6413 20.6347 52.403 1.660276 1.64116820.5049 52.1025 1.67 1.6355 20.5015 52.0938 1.670271 1.63538420.3756 51.7861 1.68 1.6298 20.3722 51.7775 1.680269 1.62959720.2503 51.4625 1.69 1.624 20.2469 51.4539 1.690269 1.62380920.1292 51.1316 1.7 1.6182 20.1258 51.123 1.700266 1.61802420.0122 50.7933 1.71 1.6124 20.0088 50.7484 1.710983 1.61198719.8994 50.4474 1.72 1.6066 19.896 50.439 1.720261 1.60645119.7909 50.0939 1.73 1.6008 19.7876 50.0855 1.730256 1.60066719.6867 49.7326 1.74 1.595 19.6834 49.7243 1.740254 1.5948819.587 49.3633 1.75 1.5893 19.5837 49.355 1.750254 1.58909319.4918 48.9859 1.76 1.5835 19.4885 48.9777 1.760252 1.58330619.4012 48.6003 1.77 1.5777 19.3979 48.5922 1.770249 1.5775219.3153 48.2063 1.78 1.5719 19.312 48.1983 1.780245 1.57173519.2341 47.8037 1.79 1.5661 19.2309 47.7957 1.790243 1.5659519.1578 47.3924 1.8 1.5603 19.1546 47.3845 1.800239 1.56016419.0865 46.9721 1.81 1.5545 19.0833 46.9643 1.810235 1.55437919.0202 46.5426 1.82 1.5488 19.017 46.5348 1.820235 1.54859118.9591 46.1037 1.83 1.543 18.9559 46.096 1.830233 1.54280518.9033 45.6552 1.84 1.5372 18.9001 45.6476 1.84023 1.53701918.8529 45.1969 1.85 1.5314 18.8497 45.1893 1.850228 1.53123218.8081 44.7284 1.86 1.5256 18.8049 44.7209 1.860224 1.52544818.7689 44.2494 1.87 1.5198 18.7658 44.242 1.870221 1.51966318.7356 43.7598 1.88 1.514 18.7325 43.7525 1.880217 1.51387818.7082 43.2591 1.89 1.5083 18.7051 43.2519 1.890215 1.5080918.6871 42.747 1.9 1.5025 18.684 42.7399 1.90021 1.50230718.6722 42.2231 1.91 1.4967 18.6691 42.216 1.910211 1.49651818.6639 41.687 1.92 1.4909 18.6608 41.68 1.920209 1.49073218.6623 41.1384 1.93 1.4851 18.6592 41.1315 1.930206 1.48494618.6677 40.5766 1.94 1.4793 18.6646 40.5698 1.940204 1.47915918.6803 40.0013 1.95 1.4736 18.6772 39.9946 1.9502 1.47337418.7004 39.4118 1.96 1.4678 18.6973 39.4052 1.960197 1.46758918.7282 38.8076 1.97 1.462 18.725 38.8011 1.970197 1.461818.7641 38.188 1.98 1.4562 18.7609 38.1816 1.980193 1.456016

Trường ĐHBK Hà Nội BTL Robot18.8083 37.5523 1.99 1.4504 18.8051 37.546 1.990191 1.45022818.8613 36.8997 2 1.4446 18.8581 36.8935 2.000189 1.44444218.9235 36.2294 2.01 1.4388 18.9203 36.2233 2.010185 1.43865818.9953 35.5403 2.02 1.4331 18.9921 35.5344 2.02018 1.43287419.0772 34.8315 2.03 1.4273 19.074 34.8257 2.030177 1.42708919.1697 34.1017 2.04 1.4215 19.1665 34.096 2.040176 1.42130119.2735 33.3496 2.05 1.4157 19.2703 33.344 2.050174 1.41551519.3893 32.5738 2.06 1.4099 19.386 32.5683 2.060173 1.40972619.5179 31.7726 2.07 1.4041 19.5146 31.7673 2.070166 1.40394519.66 30.9439 2.08 1.3983 19.6567 30.9387 2.080166 1.39815619.8169 30.0857 2.09 1.3926 19.8136 30.0807 2.09016 1.39237519.9896 29.1954 2.1 1.3868 19.9863 29.1905 2.100157 1.3865920.1795 28.27 2.11 1.381 20.1761 28.2653 2.110155 1.38080320.3882 27.306 2.12 1.3752 20.3848 27.3014 2.120153 1.37501620.6177 26.2992 2.13 1.3694 20.6142 26.2948 2.130152 1.36922720.8704 25.2446 2.14 1.3636 20.8669 25.2404 2.140147 1.36344521.149 24.136 2.15 1.3579 21.1455 24.132 2.150143 1.3576621.4572 22.9657 2.16 1.3521 21.4536 22.9619 2.16014 1.35187521.7995 21.7236 2.17 1.3463 21.7959 21.712 2.170253 1.34596722.182 20.397 2.18 1.3405 22.1783 20.3936 2.180134 1.34030422.6127 18.968 2.19 1.3347 22.6089 18.9648 2.190134 1.33451423.1034 17.4116 2.2 1.3289 23.0995 17.4087 2.200131 1.32872923.6714 15.6901 2.21 1.3231 23.6674 15.6874 2.210128 1.32294224.3452 13.7417 2.22 1.3174 24.3411 13.7394 2.220125 1.31715725.1781 11.4512 2.23 1.3116 25.1739 11.4493 2.23012 1.31137426.2949 8.5481 2.24 1.3058 26.2905 8.5467 2.240118 1.30558728.2512 3.8289 2.25 1.3 28.2465 3.8283 2.250114 1.299803