4 Xây dựng quan hệ tốc độ của các khớp và tốc độ của tay robot
Tải bản đầy đủ - 0trang
GVHD:TS. NGUYỄN CAO CƯỜNG
840Nghiên
cứu Động lực học Robot là giai đoạn cần thiết trong việc phân tích cũng
841như tổng hợp quá trình điều khiển chuyển động. Trong nghiên cứu Động lực học
842Robot thường giải quyết 2 nhiệm vụ sau đây :
843+
Nhiệm vụ thứ nhất là xác định momen và lực động xuất hiện trong q trình
844chuyển động. Khi đó quy luật biến đổi của biến khớp qi(t) xem như đã biết.
+ Nhiệm vụ thứ hai là xác định các sai số động. Lúc này phải khảo sát các phương
846trình chuyển động của cơ cấu tay máy đồng thời xem xét các đặc tính động lực của
847động cơ truyền động.
845
848Có
nhiều phương pháp nghiên cứu Động lực học Robot nhưng thường dùng hơn cả
849là phương pháp Lagrange bậc 2 vì khi kết hợp với mơ hình Động lực học kiểu DH
850(Denavit-Hartenberg) ta sẽ được các phương trình Động lực học ở dạng vector ma
851trận, rất gọn nhẹ và thuận tiện cho việc nghiên cứu giải tích và tính tốn trên máy
852tính.
853
854
Các phương pháp xây dựng:
855
856
857
858
Phương pháp động tĩnh học: Xây dựng trên nguyên lý D’Alembert
cho phép xác định các lực truyền dẫn để thực hiện chuyển động của bàn kẹp
cùng vật kẹp, đồng thời cho phép xác định lực quán tính trong các khớp.
- Phương pháp dùng phương trình Lagrange: Dựa trên sự cân bằng về năng
lượng. Phương trình hàn Lagrange biểu diễn quan hệ giữa momen góc quay
q, tốc độ và gia tốc .
Phương pháp Lagrange:
Momen đặt lên trục khớp i được xác định theo phương trình Lagrange
bậc 2 có dạng:
859
860
861
862
863
864
=
865
866
867
868
869
870
871
872
-
Trong đó:
L = K – P: Hàm Lagrange.
K: Tổng động năng của thanh nối.
P: Tổng thế năng của thanh nối.
: Momen đặt lên khớp i khi thực hiện chuyển động quay.
SVTH:Bùi Anh Tiến-TĐH9a2
36
GVHD:TS. NGUYỄN CAO CƯỜNG
Biến khớp.
: Vận tốc quay của biến khớp.
873
874
875
876
Các bước thành lập phương trình động lực học:
Bước 1: Tính động năng và thế năng của các thanh nối:
- Động năng: = . + . .
877
878
879
880
881
882
- Thế năng:
883
Pi = -mi.GiT.0Pci
884
885
886
887
Bước 2: Thiết lập hàm Lagrange:
888
L=K–P
889
890
Tổng động năng và thế năng: K =
891
892
893
P=
Bước 3: Thiết lập phương trình Lagrange:
894
895
896
i
897
Bước 4: Viết phương trình động lực học:
= M(q) . + H(q, ) + G(q) + Fd() + Fs() + d
898
899
= -
Các thành phần trong phương trình động lực học:
900
901
902
903
904
905
= M(q). + H(q, ) + G(q) + Fd() + Fs() + d
– Momen sinh ra bởi cơ cấu chấp hành đặt lên khớp. Vecto (n×1)
(t) = [ 1(t), 2(t), … n(t) ]T
– Các giá trị biến khớp. (n×1)
q(t) = [ q1(t), q2(t), … qn(t) ]T
SVTH:Bùi Anh Tiến-TĐH9a2
37
GVHD:TS. NGUYỄN CAO CƯỜNG
– Tốc độ của các biến khớp ( vận tốc góc, thẳng ). (n×1)
(t) = [ 1(t), 2(t), … n(t) ]T
– Gia tốc biến khớp. (n×1)
(t) = [ 1(t), 2(t), … n(t) ]T
Các thành phần trong phương trình động lực học:
= M(q). + H(q, ) + G(q) + Fd() + Fs() + d
(q) – Ma trận qn tính. (n×m)
(q, ) – Vecto tương hỗ và ly tâm. (n×1)
(q) – Vecto trọng trường. (n×1)
s – Vecto ma sát nhớt.
d – Vecto lực ma sát động.
d – Vecto mô tả nhiễu ngoại chưa biết.
Nếu bỏ qua các thành phần lực ma sát và nhiễu:
= M(q). + H(q,) + G(q)
Phương trình động lực học có thể được viết dưới dạng:
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
= M(q) + N(q, ) + d
Trong đó:
N(q, ) = H(q, ) + G(q) + F()
F() = Fd() + Fs()
922
923
924
925
926
927
Xác định phương trình động lực của cánh tay robot:
928
929Trong
930
931
đó:
Өi – Góc quay của khớp quay i.
mi – Khối lượng của thanh nối i.
li – Chiều dài của thanh nối i.
SVTH:Bùi Anh Tiến-TĐH9a2
38
GVHD:TS. NGUYỄN CAO CƯỜNG
lci – Chiều dài từ khớp i đến tâm thanh nối i.
Ii – Momen quán tính của thanh nối i đối với trục qua tâm khối
của thanh nối.
932
933
934
935:
9362.4.1
937
Động năng thành nối 1:
938Ki=v2ci.mi+
939
940Mà:
941
0
wi.Ii.w
0
A1=
942
943
Pci = => (Đạo hàm cấp 1)
944
(Đạo hàm cấp 2)
Bình phương tốc độ độ dài tâm khối ta có:
V2c1=
945
946
947
948
Động năng thanh nối 1:
K1 .m1+I1.
9492.4.2 Thế năng thanh nối 1 :
950Thế năng của thanh nối thứ i được xác định bởi
951Pi = -mi.gT.0Ai.ir với gT là véc tơ trọng trường
952mi là khối lương của thanh nối thứ i
953
1.1.4 Thế năng thanh nối 1:
954P1=-m1..g.
955
P1=m1.g.
956
957
958
Tọa độ tâm khối thanh nối 2:
�
�x2 l1cos1 lg 2cos(1 + 2 )
�
�y2 l1 sin 1 lg 2sin(1 + 2 )
Tốc độ tâm khối thanh nối 2:
�x. l sin . l sin( + )(. . )
1
2
1
1 1
g2
1
2
�2
�.
.
.
.
�y 2 l1cos1 1 l g 2cos(1 + 2 )( 1 2 )
�
959
960→Bình
961
biểu thức:
phương tốc độ dài tâm thanh nối 2:
. 2
. 2
. 2
v x2 y2 l 1 l
2
2
2
1
SVTH:Bùi Anh Tiến-TĐH9a2
2
g2
.
.
. 2
.
.
( 1 2 ) 2l1lg 2cos 2 ( 1 1 2 )
2
39
GVHD:TS. NGUYỄN CAO CƯỜNG
9622.4.3
963
Động năng thanh nối 2:
Pc2 =
964
965
966Đạo hàm hai vế ta được:
967
968
969
970Bình phương 2 vế ta được:
971
972
973
974
975
976
=
T
2 = [0; 0; + ]
977
978
I2 =
979Vậy
980
981
động năng của thanh nối 2 là:
K2 =
9822.4.4 Thế năng thanh
983P2 = -m2 [0; -1]g
984
985
= m2 g.
nối 2:
9862.4.5 Thiết lập hàm Lagrange của robot:
987L=K-P
988L=(K1+K2)-(P1+P2)
989L= .m1+I1.+K2 m2.(l12lg22.(2+2l1.lc2(+ I2)-(m1.g.+m2.g.+lc2.sin())
990*Lấy đạo hàm lần lượt theo các biến ta có :
991m1+I1+m2+(l12+lc22+2l1lc2cos)+I2].+[m2.(lc22+l1lc2.cos+I2)-m2l1lc2.sin(2+)
992=-m1.-m2.g(l1coslc2.cos()
993*Lấy đạo hàm lần lượt theo các biến và ta có :
994m2(lc22+l1lc2cos+I2]m2lc22+I2)-m2l1lc2sin
995-m2glc2cos()-m2lc2l1(+)sin
996Thiết lập phương trình Lagrange ta có :
997=998Từ phương trình tổng quát trên ta có:
999Moment của khớp 1 :
SVTH:Bùi Anh Tiến-TĐH9a2
40
GVHD:TS. NGUYỄN CAO CƯỜNG
1000=-=m1+I1+m2+(l12+lc22+2l1lc2cos)+I2].+[m2.(lc22+l1lc2.cos+I2)-m2l1lc2.sin(2+)1001m2.g(l1coslc2.cos()]
1002Moment của khớp 2:
1003=-=m2(lc22+l1lc2cos+I2]m2lc22+I2)-m2l1lc2sin-(-m2glc2cos()-m2lc2l1(+)sin
1004Ta có phương trình tổng quát của phương trình động lực học là:
1005)+H(q,
1006
=++
10072.4.6
1008
1009
1010
1011
1012
1013
[-m1.-
Phương trình động lực:
10142.4.7 Quan hệ giữa tốc độ của khớp và tốc độ của cánh tay robot:
1015
Biểu diễn tay robot trên 2 thanh nối như sau:
i
1016
rtay= T
1017
Vậy vị trí của điểm đó trong hệ tọa độ gốc o0x0y0z0 là:
0
1018
rtay=0A2.2rtay
10190A2=.=
1020
1021Bình phương tốc độ robot khai triển vận tốc của tay robot theo thành hai phần trục
1022x và y ta có thể:
1023V2tay=u2+v2
1024Trong đó:
1025V1=
1026U=
1027Đạo hàm theo thời gian ta được:
1028
()= l1cos+
1029
=l1sin1030Như vậy ta có và chính là các véc tơ vận tốc của tay robot chiếu theo các phương
1031x và y
1032Vx=()= l1cos+
1033Vy=()= l1cos+
1034Từ các vé trên ta có quan hệ vận tốc giữa tốc độ các khớp và cánh tay robot là:
1035=vy+vx
1036=vy -vx
SVTH:Bùi Anh Tiến-TĐH9a2
41
GVHD:TS. NGUYỄN CAO CƯỜNG
1037
Chương 3: Viết hàm Matlab
1038
10393.1.
Tổng quan về Matlab-Simulink:
1040Matlab
là một bộ chương trình phần mềm lớn của lĩnh vực tốn số. Tên của bộ
1041chương trình chính là từ viết tắt của từ Matrix Laboratory, thể hiện định hướng
1042chính của chương trình là các phép tính vectơr và ma trận. Phần cốt lõi của chương
1043trình bao gồm một số hàm toán, các chức năng xuất nhập cũng như các khả năng
1044điều khiển chu trình mà nhờ đó ta có thể dựng nên các Scripts.
1045Thêm
vào phần cốt lõi, có thể dùng các bộ công cụ Toolbox với phạm vi chức năng
1046chuyên dụng mà người sử dụng cần. Simulink là một Toolbox có vai trò đặc biệt
1047quan trọng: vai trò của một bộ cơng cụ mạnh phục vụ mơ hình hố và mơ phỏng
1048các hệ thống kĩ thuật - Vật lý, trên cơ sở sơ đồ cấu trúc dạng khối.
1049Giao
diện đồ họa trên màn hình của Simulink cho phép thể hiện hệ thống dưới
1050dạng sơ đồ tín hiệu với các khối chức năng quen thuộc. Simulink cung cấp cho
1051người dùng một thư viện rất phong phú, có sẵn với số lượng lớn các khối chức
1052năng cho các hệ tuyến tính, phi tuyến và gián đoạn. Hơn thế người sử dụng có thể
1053tạo nên các khối riêng cho mình.
Sau khi đã xây dựng mơ hình của hệ thống cần nghiên cứu, bằng cách ghép các
1055khối cần thiết, thành sơ đồ cấu trúc của hệ, ta có thể khởi động q trình mơ phỏng.
1054
1056Trong
các q trình mơ phỏng ta có thể trích tín hiệu hiện tại vị trí bất kì của sơ đồ
1057cấu trúc và hiển thị đặc tính của tín hiệu đó trên màn hình. Hơn thế nữa, nếu có nhu
1058cầu ta còn có thể cất giữ các đặc tính đó vào môi trường nhớ. Việc nhập hoặc thay
1059đổi tham số của tất cả các khối cũng có thể thực hiện được rất đơn giản bằng cách
1060nhập trực tiếp hay thông qua matlab. Để khảo sát hệ thống, ta có thể sử dụng thêm
1061các Toolbox như Signal Processing (xử lý tín hiệu), Optimization (tối ưu) hay
1062Control System (hệ thống điều khiển).
1063
SVTH:Bùi Anh Tiến-TĐH9a2
42
GVHD:TS. NGUYỄN CAO CƯỜNG
1064
10653.2 Yêu
cầu của đề
1066Viết hàm Matlab thực hiện các phương trình ở chương 2,3 và vẽ đường biểu diễn
1067vị trí và tốc độ khớp khi tay robot di truyển từ vị trí đến theo một đường thẳng.
1068Đồ thị tốc độ đặt trước của tay Robot dọc theo quỹ đạo đã cho
1069
1070
1071
1072
1073
1074
1075
1076
1077
Do tay Robot di chuyển từ [0,0; 0,4] đến [0,0; 0,7] và theo dạng đồ thị tốc độ
ở bài ra ta có:
x(t)=0
y(t)=s(t) =y01 +
s(t) là quãng đường di chuyển của Robot.
Y01 = s01 = 0,4
Với v(t) = (t) =
Vm : vận tốc cực đại của tay Robot.
y(t) =
1078
1079
1080
1081
1082
1083
1084
Với s02 = 0,4+ Vm(0,25)2/0,5 = 0,4 + Vm.0,125
S03 = s02 + Vm.0,5 = 0,4 + Vm.0,125 + Vm.0,5 = 0,4 + Vm.0,625
Khi t = 0, y(t) = y(1) = 0,7 =s03 –Vm.(0,25)2/0,5 + Vm.0,25
= 0,4 + Vm.0,625 – Vm.(0,25)2/0,5 + Vm.0,25
= 0,4 + 0,75.Vm
Vm = 0,3/0,75 = 0,4 (m/s)
1085
1086
1087
Vậy:
y(t) =
1088
v(t) = (t) =
10893.2.1 Hàm dhrobot(t, td): xác định vị trí khớp 1 và khớp 2.
1090
function [pos1,pos2]=dhrobot(t,td)
1091
%chuong trinh tinh toan dong hoc thuan va nguoc cho khop
1092
%cua Robot 2 bac tu do
1093
% x0,y0,xc,yc : Toa do diem dau va cuoi cua tay Robot
1094
% td: Thoi gian chuyen dong
1095
1096
1097
1098
% Khai bao cac thong so cua Robot
l1=0.4;
l2=0.3;
SVTH:Bùi Anh Tiến-TĐH9a2
43
1 va 2
GVHD:TS. NGUYỄN CAO CƯỜNG
1099
1100
1101
t1=0.25;
t2=0.75;
1102
1103
% Tinh vi tri khop 1 va 2
1104
1105
1106
1107
1108
1109
1110
xt=0;
yt=trarb(t,t1,t2,td);
k1=sqrt((xt.^2+yt.^2+l1^2+l2^2).^2-2*((xt.^2+yt.^2)^2+l1^4+l2^4));
k2=xt.^2+yt.^2+l1^2-l2^2;
k3=xt.^2+yt.^2-l1^2-l2^2;
pos1=(atan2(yt,xt)-atan2(k1,k2)) ;
pos2=atan2(k1,k3) ;
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
function trajt=trarb(t,t11,t22,tdd)
%Tinh quy dao chuyen dong cua tay robot (st=yt ; xt=0)
if (t>=0)&(t
trajt=0.4+0.8*t.^2;
else
if (t>=t11)&(t
trajt=0.45+0.4*(t-0.25);
else
if (t>=t22)&(t<=tdd)
trajt=0.65-0.8*(t-0.75).^2+0.4*(t-0.75);
else
if t>tdd
trajt=0.7 ; % gia tri toa do diem cuoi
end
end
end
end
11313.2.3 Hàm veloe để xác định tốc độ tay
1132 function velo=veloe(t,t11,t22,tdd)
1133 %chuong trinh tinh van toc tay Robot
1134 % td: Thoi gian chuyen dong
1135
1136
1137
Robot
if (t>=0)&(t
velo=1.6*t;
SVTH:Bùi Anh Tiến-TĐH9a2
44
