Cần giúp đỡ gấp về tính toán động học Robot trong MatLap.

[KLibra]

Như tiêu đề em đã viết thì hiện tại em đăng gặp vấn đề rắc rối với Matlap trong việc tính toán động học cho ra thông số hình học của robot 2 bâc tự do trong matlap.. Mọi người ai biết về phần này thì có thể giúp đỡ em được không ạ.

% Khai báo các biến cùng các điều kiện của biến
syms a1 a2 q1 q2 m1 m2 t g dq1 dq2
% q1, q2, là các biến khớp
% a1, a2 là chiều dài các thanh
% m1, m2 là khối lượng các thanh
% t : Biến thời gian
% g là gia tốc trọng trường
% dq1, dq2 là đạo hàm của q1, q2

assume(a1,'real');assume(a1>0); % a1 là số thực dương
assume(a2,'real');assume(a2>0); % a2 là số thực dương
assume(q1,'real');
assume(q2,'real');
assume(m1,'real');assume(m1>0);
assume(m2,'real');assume(m2>0);
assume(t,'real');assume(t>0);
assume(g,'real');assume(g>0);
assume(dq1,'real');
assume(dq2,'real');
q = [q1,q2]; % Vector các tọa độ suy rộng q
dq = [dq1,dq2]; % Vector các vận tốc dài


% Nhập ma trận D-H
A_01 =[ 1 0 0 a1; 0 0 -1 0; 0 1 0 q1; 0 0 0 1];
A_12 =[ cos(q2) sin(q2) 0 a2*cos(q2); sin(q2) cos(q2) 0 a2*sin(q2); 0 0 1 0; 0 0 0 1];

A_02 = A_01*A_12

% 1. Tìm vi trí và tính vận tốc dài của khâu thao tác
rE = A_02(1:3,4) % Vector tọa độ khâu thao tác
v_qE = diff(rE) % Tính vector vận tốc khâu tác động cuối

% 3. Thay so bai toan dong hoc thuan
disp(' ')
disp('Thay so') % Khi thay số ta sử dụng lệnh subs
disp('Vi tri diem tac dong cuoi')
sub_rE = simplify(subs(rE,{q1 q2 a1 a2},{7*sin(2*t) sin(2*t) 6 5}))
disp('Van toc dai:')
sub_diff_qE = simplify(subs(v_qE,{q1 q2 dq1 dq2 a1 a2 },{7*sin(2*t) sin(2*t) 14*cos(2*t) 2*cos(2*t) 6 5}))
disp('Van toc goc:')
sub_R_0E = simplify(subs(R_0E,{a1 a2},{6 5}));
sub_omega = simplify(subs(omega,{q1 q2 dq1 dq2 a1 a2},{7*sin(2*t) sin(2*t) 14*cos(2*t) 2*cos(2*t) 6 5 }))


% 4. Tính toán và vẽ đồ thị
time=0:0.02:2*pi;
num_rE=zeros(3,length(time));
for j=1:length(time)
num_rE(:,j) = subs(sub_rE,t,time(j));
end
figure(1)
clf
title('Quy dao cua khau tac dong cuoi trong bai toan thuan')
hold on
grid on
for j=1:length(time axis([1, 11, -10, 10 ,-5, 15])
pause
)
plot3(num_rE(1,j),num_rE(2,j),num_rE(3,j),'b+'); % Vẽ quĩ đạo chuyển động bằng MATLAB
plot(rob,[3*time(j),2*time(j),time(j)]); % Vẽ hình ảnh chuyển động 3D của Robot theo quĩ đạo
pause(1/30) % Dùng lệnh pause để tạo cảm giác giống như một đoạn phim
end

Cuối cùng nó bị như vậy :



Đây là bài giải của em. Mọi người xem giúp em ntn với ạ . Em xin cảm ơn trước ( Tập tin ở phần đính kèm)Thiết Kế Robot.doc