219
với
23
2 2
2
3 3
3
2 2
2
3 3
3
cos
cos
sin
sin
q
q l
q
q l
q
q l
q
q l
q
−
=
−
J
ɺ
ɺ
ɺ
ɺ
ɺ
,
1
1
1
1
1
1
cos
sin
q
l
q
q
l
q
−
=
−
J
ɺ
ɺ
Triển khai trong Matlab với các m-file
1. Các phương trình liên kết
function f = bkbl_ ptlk(q1, q2, q3)
global l1 l2 l3 dx dy
f1 = l1*cos(q1) + l2*cos(q2) - l3*cos(q3) - dx;
f2 = l1*sin(q1) + l2*sin(q2) - l3*sin(q3) - dy;
f=[f1; f2];
2. Các ma trận Jacobi
function [J1, J23] = bkbl_Jacob(q1,q2,q3)
global l1 l2 l3 dx dy
J1 = [-l1*sin(q1); l1*cos(q1)];
J23 = [-l2*sin(q2), l3*sin(q3);
l2*cos(q2), -l3*cos(q3)];
3. ðạo hàm của ma trận Jacobi
function [dJ1, dJ23] = bkbl_Jacdot(q1,q2,q3, dq1, dq2,dq3)
global l1 l2 l3 dx dy
dJ1 = -l1*dq1*[cos(q1); sin(q1)];
dJ23 = [-dq2*l2*cos(q2), dq3*l3*cos(q3);
-dq3*l2*sin(q2), dq3*l3*sin(q3)];
4. Chương trình chính
function bkbl_main
global l1 l2 l3 dx dy
l1 = 0.12; l2 = 0.70; l3 = 0.40; dx = 0.50; dy = 0.10;
%cho goc cua tay quay OA
q10 = 0.5; % rad
omega = 1.0; % rad/s
t = linspace(0, 5*pi/omega, 360);
a(1) = 0.2; b(1) = 0.542; % chon xap xi ban dau cho q2, q3
for i=1:length(t)
q1(i)= q10+omega*t(i);
dq1(i) = omega; ddq1(i) = 0;
f = ptlk_bkbl(q1(i), a(1), b(1));
k = 1;
while (norm(f,2) > 1.0e-10 && k < 30)
[J1, J23] = Jacob_bkbl(q1(i), a(1), b(1));
deltax = inv(J23)*(-f);
a(2) = a(1)+deltax(1);
