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

More documents

Recommendations

Info

Inovace výuky předmětu Robotika v lékařství q k vektor zobecněných souřadnic v posledním kroku iterace, kde platí že f ( qk ) − X = g( q) ≅ 0 g (q) chybová hodnota mezi žádanou polohou konc. bodu řetězce a aktuální hodnotou koncového bodu řetězce při daných zobecněných souřadnicích q . Vstupem pro newtonovu iterační metodu je žádaná poloha konc. bodu řetězce a jejím výstupem je odpovídající vektor zobecněných souřadnic, který této požadované poloze odpovídá. Řešení může být nekonečně mnoho, nebo jen jedno, ale také nemusí existovat, například pokud je žádaná poloha konc. bodu mimo manipulační prostor paže robota. Využití symbolic toolboxu Matlabu pro výpočet <strong>Jacobián</strong>u kinematického řetězce v laboratoři vysvětluje následný okomentovaný kód : clear all clc % konstanty R21,R32,R43,R54,R5M R10 = sym ('R10'); R21 = sym ('R21'); R32 = sym ('R32'); R43 = sym ('R43'); R54 = sym ('R54'); R5M= sym ('R5M'); % promennne % zobecněné polohy v kin. dvojicích fi10 = sym ('fi10'); fi21 = sym ('fi21'); fi32 = sym ('fi32'); fi43 = sym ('fi43'); fi54 = sym ('fi54'); % zobecněné rychlosti v kin. dvojicích om10 = sym ('om10'); om21 = sym ('om21'); om32 = sym ('om32'); om43 = sym ('om43'); om54 = sym ('om54'); r21=[0; R21; 0; 1]; % pruvodice mezi kinem dvojicemi r32=[0; 0; R32; 1]; r43=[0; 0; R43; 1]; r54=[0; 0; R54; 1]; r5M=[0; 0; R5M; 1]; % transformacni matice TT10=[cos(fi10) -sin(fi10) 0 0; sin(fi10) cos(fi10) 0 0; 0 0 1 0; 0 0 0 1]; % rotace kolem z-ove osy o fi10 TT21=[1 0 0 0; 0 cos(fi21) -sin(fi21) 0; 0 sin(fi21) cos(fi21) 0; 0 0 0 1]; % rotace kolem x-ove osy o fi21 Stránka 4 z 6
Inovace výuky předmětu Robotika v lékařství TT32=[1 0 0 0; 0 cos(fi32) -sin(fi32) 0; 0 sin(fi32) cos(fi32) 0; 0 0 0 1]; % rotace kolem x-ove osy o fi32 TT43=[1 0 0 0; 0 cos(fi43) -sin(fi43) 0; 0 sin(fi43) cos(fi43) 0; 0 0 0 1]; % rotace kolem x-ove osy o fi43 TT54=[cos(fi54) -sin(fi54) 0 0; sin(fi54) cos(fi54) 0 0; 0 0 1 0; 0 0 0 1]; % rotace kolem z-ove osy o fi54 TT50=TT10*TT21*TT32*TT43*TT54; % výsledná transformacni matice z prostoru telesa 5 do prostoru telesa 0 % poloha koncového bodu M vůči rámu v homogennim zobrazení rM0=TT50*r5M; % Rychlosti % diferencialni operatory zakladnich pohybu DTX = [0 0 0 1;0 0 0 0;0 0 0 0;0 0 0 0]; DTY = [0 0 0 0;0 0 0 1;0 0 0 0;0 0 0 0]; DTZ = [0 0 0 0;0 0 0 0;0 0 0 1;0 0 0 0]; DRX = [0 0 0 0;0 0 -1 0;0 1 0 0;0 0 0 0]; DRY = [0 0 1 0;0 0 0 0;-1 0 0 0;0 0 0 0]; DRZ = [0 -1 0 0;1 0 0 0;0 0 0 0;0 0 0 0]; % rychlosti zakladnich pohybu V10S1=om10*DRZ; V21S2=om21*DRX; V32S3=om32*DRX; V43S4=om43*DRX; V54S5=om54*DRZ; % inverzni transformacni matice ITT10=inv(TT10); ITT21=inv(TT21); ITT32=inv(TT32); ITT43=inv(TT43); ITT54=inv(TT54); V10S5=ITT54*ITT43*ITT32*ITT21*V10S1*TT21*TT32*TT43*TT54;%transformace rychlosti V10S1 (v prostoru 1) na V10S5 (v prostoru 5) V21S5=ITT54*ITT43*ITT32*V21S2*TT32*TT43*TT54; V32S5=ITT54*ITT43*V32S3*TT43*TT54; V43S5=ITT54*V43S4*TT54; % vysledna matice rychlosti soustavy (z telesa 5 do telesa 0 v prostoru souradnic 5) V50S5=V10S5+V21S5+V32S5+V43S5+V54S5; simV50S5=simplify(V50S5); % zjednoduseni formalni uprava % rychlost bodu M vuci ramu Stránka 5 z 6
Page 1 and 2: Přednáška 3 Inovace výuky před
Page 3: Inovace výuky předmětu Robotika

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

Create successful ePaper yourself

Delete template?

Save as template?