222
Ở ñây ta sẽ sử dụng phương pháp giải hệ
phương trình phi tuyến, ñể giải bài toán này,
vì lẽ phương pháp có thể ñược mở rộng dễ
dàng cho các rôbốt có số bậc tự do lớn hơn và
có cấu trúc phức tạp hơn.
Mối liên hệ giữa tọa ñộ ñiểm E với các tọa ñộ
suy rộng ñược xác ñịnh bởi
1
1
2
1
2
1
1
2
1
2
cos
cos(
)
sin
sin(
)
E
E
x
l
q
l
q
q
y
l
q
l
q
q
=
+
−
=
+
−
ðây là một hệ hai phương trình phi tuyến ñối
với hai ẩn
1
2
,
q q
. Phương pháp Newton-Raphson ñược sử dụng ñể giải hệ này, với
ma trận Jacobi như sau
1
1
2
1
2
1
1
2
1
2
sin
sin(
)
cos
cos(
)
q
l
q
l
q
q
l
q
l
q
q
−
−
=
−
−
J
Triển khai trong Matlab với các m-file sau:
1. Phương trình liên kết
function f = taymay2dof_bkbl(q1, q2, xE, yE)
global l1 l2
f1 = l1*cos(q1) + l2*cos(q1 - q2) - xE;
f2 = l1*sin(q1) + l2*sin(q1 - q2) - yE;
f=[f1; f2];
2. Ma trận Jacobi
function Jq = taymay2dof_Jacob(q1, q2)
global l1 l2
Jq = [-l1*sin(q1), l2*sin(q1 - q2);
l1*cos(q1), -l2*cos(q1 - q2)];
3. Chương trình chính
function taymay2dof_main
global l1 l2
l1 = 0.50; l2 = 0.70;
n = 20; % so diem chia
xE = linspace(1.1, 0.1, n);
yE = linspace(0.1, 1.1, n);
a(1) = 0.5; b(1) = 0.2; % chon xap xi ban dau cho q1, q2
for i=1:n
f = taymay2dof_ptlk(a(1), b(1), xE(i), yE(i));
k = 1;
while (norm(f,2) > 1.0e-10 && k < 30)
q
1
q
2
O
A
E
y
E
x
E
Hình 9-35
