Tải bản đầy đủ
PHẦN 2: XÂY DỰNG MÔ HÌNH TOÁN HỌC

PHẦN 2: XÂY DỰNG MÔ HÌNH TOÁN HỌC

Tải bản đầy đủ

HVTH: Dương Đức Trọng

GVHD: PGS. TS Phan Bùi Khôi

2.2. tham số hóa mô hình , xác định các giá trị số theo thiết kế
Các biến khớp: θ1, θ 2, θ 3.
Cho giá trị đặt trước:d1=200; a2=180; a3=100
Ma trận D-H cho các khâu:
MAPLE :
#--------------Phantich DH TINH DHM--------------------TMat1:=DHMat(0,0,0,0):
nj:=rowdim(DHTB):
for i from 1 by 1 to nj do
temp:=DHMat(DHTB[i,1],DHTB[i,2],DHTB[i,3],DHTB[i,4]);
AIJ[i]:=temp:
TMat1:=multiply(TMat1,temp):
AI[i]:=copy(TMat1):
end do:
print(Matran_KhauI):
for i from 1 by 1 to nj do
print(AIJ[i]):
end do:
print(Matran_KhauI0):
for i from 1 by 1 to nj do
print(AI[i]):
end do:
RE:=subvector(AI[nj],1..3,4):
print(VITRI_XE):
print(RE[1]):
print(VITRI_YE):
print(RE[2]):
print(VITRI_ZE):
print(RE[3]):
9

HVTH: Dương Đức Trọng

GVHD: PGS. TS Phan Bùi Khôi

10

HVTH: Dương Đức Trọng

GVHD: PGS. TS Phan Bùi Khôi

Cho quy luật chuyển động của các khớp
11

HVTH: Dương Đức Trọng

GVHD: PGS. TS Phan Bùi Khôi

qt1:=1.5+0.8*cos(omega*t):
qt2:=0.3+0.4*sin(omega*t):
qt3:=-0.2-0.15*cos(omega1*t):
MAPLE :
DHTB:=matrix([[theta1,d1,a1,90],
[theta2,0,a2,0],
[theta3,0,a3,0]]):
#----Toa do khop----------------------Unknowns:=vector([theta1,theta2,theta3]):
#----Tham so dong hoc----------------prm_:=vector([d1, a1, a2, a3, l3]):
prm0:=vector([0.2, 0, 0.18, 0.1, 0.5]):
#-----quy luat chuyen dong cua cac khau---------------omega:=3:
omega1:=3:
qt1:=1.5+0.8*cos(omega*t):
qt2:=0.3+0.4*sin(omega*t):
qt3:=-0.2-0.15*cos(omega1*t):
qlcd:=vector([qt1,qt2,qt3]):
#----Thoi gian thuc hien-------------------------------t0:=0:
tc:=6.5:
dt:=0.005:
#----so buoc tinh--------------------------------------Num:=round((tc-t0)/dt):
nj:=rowdim(DHTB):
vec_prm:=vectdim(prm_):
xE:=vector(Num):
yE:=vector(Num):
zE:=vector(Num):
time_:=vector(Num):
kqt:=matrix(Num,nj):

12

HVTH: Dương Đức Trọng

GVHD: PGS. TS Phan Bùi Khôi

AE0:=copy(AI[nj]):
for i from 1 by 1 to vec_prm do
AE0:=subsMat(prm_[i]=prm0[i],AE0):
end do:
for i from 1 by 1 to Num do
q1:=evalf(subs(t=t0,qlcd[1])):
q2:=evalf(subs(t=t0,qlcd[2])):
q3:=evalf(subs(t=t0,qlcd[3])):
xE[i]:=evalf(subs([Unknowns[1]=q1,Unknowns[2]=q2,Unknowns[3]=q3],AE0[1,4])):
yE[i]:=evalf(subs([Unknowns[1]=q1,Unknowns[2]=q2,Unknowns[3]=q3],AE0[2,4])):
zE[i]:=evalf(subs([Unknowns[1]=q1,Unknowns[2]=q2,Unknowns[3]=q3],AE0[3,4])):
time_[i]:=t0:
t0:=t0+dt:
kqt[i,1]:=q1:
kqt[i,2]:=q2:
kqt[i,3]:=q3*(-5):
end do:
#---MO PHONG SO DONG HOC ROBOT-----------------------------------vitri_x:=cat(Dothi_VITRI_diem_E_theophuongtruc_,X,_theo_THOIGIAN):
print(vitri_x):
plott(xE,time_,red);
vitri_y:=cat(Dothi_VITRI_diem_E_theophuongtruc_,Y,_theo_THOIGIAN):
print(vitri_y):
plott(yE,time_,blue);
vitri_z:=cat(Dothi_VITRI_diem_E_theophuongtruc_,Z,_theo_THOIGIAN):
print(vitri_z):
plott(zE,time_,black);
print(QUYDAO_CHUYENDONG_CUADIEM_E):
pointplot3d({seq([xE[k],yE[k],zE[k]],k=1..Num)});

13

HVTH: Dương Đức Trọng

GVHD: PGS. TS Phan Bùi Khôi

WriteFile_in2(kqt,"qdt.txt"):

14

HVTH: Dương Đức Trọng

GVHD: PGS. TS Phan Bùi Khôi

15

HVTH: Dương Đức Trọng

GVHD: PGS. TS Phan Bùi Khôi

16

HVTH: Dương Đức Trọng

GVHD: PGS. TS Phan Bùi Khôi

Kết quả chạy MAPLE cho xuất file : qdt.txt (tọa độ điểm E )
Dùng cho mô phỏng
PHẦN 3: MÔ PHỎNG
3.1. Xuất file STL từ Solidwork
Xuất file từ SOLIDWORK các khâu theo định dạng dưới

3.2. Mô phỏng
Khởi tạo RobotSimulator.exe

17

HVTH: Dương Đức Trọng

GVHD: PGS. TS Phan Bùi Khôi

Chọn build robot

Lưu lại thành tên : trong.rb

18

HVTH: Dương Đức Trọng

GVHD: PGS. TS Phan Bùi Khôi

Chọn khâu đế , chọn file stl : k0.stl

Chọn khâu 1 và thông số D-H khâu 1 , biến khớp theta1 để = 0

Chọn khâu 2 và thông số D-H khâu 2 , biến khớp theta2 để = 0

19