Materiály k 3. přednášce: Inverzní úloha kinematiky, Jacobián - FBMI

vM0=TT50*simV50S5*r5M;
Inovace výuky předmětu Robotika v lékařství
simvM0=simplify(vM0); % zjednoduseni formalni uprava, odsud beru translacni
cast Jakobianu
% vysledna matice rychlosti soustavy (z telesa 5 do telesa 0 v prostoru
% souradnic 0)
V50S0=ITT10*ITT21*ITT32*ITT43*ITT54*simV50S5*TT54*TT43*TT32*TT21*TT10;
simV50S0=simplify(V50S0);
%Vypocet J
omega500=[simV50S0(3,2);simV50S0(1,3);simV50S0(2,1)]; %odsud beru rotacni
cast Jakobianu
v500=[simvM0(1,1);simvM0(2,1);simvM0(3,1)]; %odsud beru translační cast
Jakobianu
vkr(1,1)=v500(1,1); %sestaveni vektoru kart.rychlosti konc bodu
vkr(2,1)=v500(2,1);
vkr(3,1)=v500(3,1);
vkr(4,1)=omega500(1,1);
vkr(5,1)=omega500(2,1);
vkr(6,1)=omega500(3,1);
%jacobian([x*y*z; y; x+z],[x y z]) vypočet Jakobianu pomocí fce "jacobian"
J = jacobian([vkr(1,1); vkr(2,1); vkr(3,1); vkr(4,1); vkr(5,1);
vkr(6,1)],[om10 om21 om32 om43 om54])
%ověření správnosti jakobiánu, rozdíl mezi kart. rychlostmi konc. bodu a
%Jakobiáne násobeným vektorem zobecněných rychlostí musí být nulový
"overeni=0"
A=vkr-J*[om10;om21;om32;om43;om54];
overeni=simple (A);
Získaný a ověřený Jacobián lze pak implementovat do řídících struktur pro ovládání paže
robota.
Stránka 6 z 6