Cơ bản về tay máy

[falleaf]

Nguyên văn bởi tuanphan

Em xem qua chương trình Matlab ở đường link của anh thấy chương trình này sử dụng phương pháp lặp (phương pháp số), còn ở đây em sử dụng phương pháp giải tích. Em cũng thử tìm quyển sách của anh (bản soft) (tác giả là Jean Claude Latombe) nhưng chỉ có bản view giới hạn trên google book thôi, xem qua cũng thấy khó hiểu. Có lẽ em nên đưa chương trình của em lên anh xem thế nào, vì ko có chương trình đó ở đây, nên em type lại thôi (chắc là không có lỗi), anh chịu khó copy, paste đoạn script vào trong matlab thì sẽ xem được kết quả chạy. Nếu cần thì em sẽ copy hình sau.
Đây là đoạn chương trình giải động học ngược robot 2 khâu phẳng. Đầu tiên em cho biến q1, q2 (biến khớp) chạy theo 1 hàm, sau đó tính động học thuận, rồi tính lại động học ngược. Có thể thấy tại giá trị q2=pi (điểm kỳ dị) thì đồ thị của q1, q2 (cuối cùng) bị gãy khúc. Nếu tại thời điểm đó trở đi đến giá trị suy biến tiếp theo em thay lại hàm S2=-sqrt(abs(1-C2.^2)); thì đồ thị lại không bị gãy khúc nữa. Nhưng em không biết cách làm sao để chọn 1 trong 2 hàm đó tại mỗi thời điểm t để đồ thị vẫn luôn ổn.

clear all;
hold on; grid on;
t=0:0.1:3.5;  % t là thời gian
l1=300*ones(1,length(t));  %Chiều dài khâu 1
l2=300*ones(1,length(t));  %Chiều dài khâu 2
q1=0.4*cos(t);
q2=1*sin(t);
plot(t,q1,'b');
plot(t,q2,'b');
x=l1.*cos(t)+l2.*cos(q1+q2);
y=l1.*sin(t)+l2.*sin(q1+q2);
C2=(x.^2+y.^2-l1.^2-l2.^2)/2./l1./l2;
S2=sqrt(abs(1-C2.^2));%nghiệm còn lại là S2=-sqrt(abs(1-C2.^2));
q2=atan2(S2,C2);
S1=y.*(l1+l2.*C2)-x.*l2.*S2; %Thực tế S1 và C1 còn có mẫu số nữa nhưng nó trùng %với mẫu số của C1 nên lúc dùng atan2 mình bỏ mẫu số đi để tránh trường hợp mẫu số %nó gần về 0
C1=y.*l2.*S2+x.*(l1+l2.*C2);
q1=atan2(S1,C1);
plot(t,q1,'r');
plot(t,q2,'r');

Em có thể gửi mô hình robot của em lên đây, hình vẽ, để anh xem kiểu của em là kiểu nào. Nhìn phương trình em viết anh không có thời gian xem kỹ.

Chúc vui