Inverzní kinematická úloha pro robota BOSH-SCARA

1.část semestrální práce k předmětu 35ROB

ČVUT FEL, 2001

Poslední aktualizace: 20.11.2001

1. Úvod

Vyřešení inverzní kinematické úlohy pro robota BOSH spočívá v nalezení transformace prostorových souřadnic do natočení úhlů a nastavení polohy svislého ramene a dále předpokládá programovou kontrolu skutečnosti že se zadané požadované souřadnice nacházaj ív akční oblasti robota.

2. Popis zařízení

Průmyslový robot BOSH SR 450

Jedná se o koncepci robota SCARA (Selective Compliance Assembly Robot Arm). SCARA byla navržena Japoncem Hiroshi Makino, který definoval mechanické uspořádání robota vhodného pro sestavovací linky. Staví na potřebě přesného pohybu ve vertikálním směru a kde horizontální směr není na přesnost tolik náročný.

Bokorys s vyznačením základních rozměrů a vyznačením funkčních částí
BOSH1.gif (43045 bytes)

Půdorys s vyznačeným akčním prostorem

BOSH2.gif (36042 bytes)

3. Použité rozhraní
Ke komunikaci s robotem bylo použito již existujícího modulu v napojení s PC kartou a řídícím software MATLAB 5.2.
Jednotlivé skripty a funkce sestavené v prostředí MatLab jsou popsány v dokumentačním manuálu který je přílohou tohoto dokumentu.

4. Programové řešení

4.1 Kontrola souřadnic

Ke kontrole souřadnic byl použit algoritmus pracující ve dvou fázích. Nejprve zkontroluje příslušnost souřadnic nyní už ve 2-D do toroidu o vnitřním průměru l0, což je délka vnitřního ramene a o vnějším průměru o velikosti l0+l1 (součet délek obou ramen) což představuje polohu kdy jsou obě ramena natažena.

Výsledek generování akčního prostoru pomocí průchodu kontrolním algoritmem
akcni_radius.gif (14798 bytes)

Tentýž výsledek zahrnující i polohu ramen:
akcni_radius_2.gif (14008 bytes)

Pozn.:
Při generování 10000 náhodných poloh ve čtvercové oblasti 450x450mm došlo k 7762 zásahům v oblasti toroidu (250,450) a z toho 3560 v toroidu, ale mimo akcni oblast robota.
Tedy akční plocha robota tvoří pouhých přiblžně 54,1 % z uvažovaného toroidu což může představovat potřebu zvýšené pozornosti při návrhu kompletační linky, kde skutečná aktivní plocha je mnohem menší než by se mohlo zdát předpokládal z uvedené dokumentace
Do akční oblasti robota došlo ke zbývajícím 4202 zásahům, kdy robot byl schopen i za inverze nastavení úhlu Achse2 přestavit rameno do žádané polohy.
Úloha spočívala v náhodném generování souřadnic a tedy četnost přestavování, která byla 50% je maximální hodnotou četnosti inverze kloubu Achse2.
Celý algoritmus inverze je založen na požadavku, kdy k inverzi úhlu dojde až ve chvíli, kdy za současné "polarity" úhlu požadované hodnoty nelze dosáhnout.

Je nutno poznamenat, že na skutečném robotu je povolený rozsah nastavení klubu Achse 1 omezen v rozmezí -85° - +85° oproti deklarovaným +/- 90°.

4.2 Výpočet úhlů natočení

Úhly se spočítají pro sořadnice uvnitř toroidu, kdy má uloha vždy řešení neuvažujeme-li omezení nastavení úhlu. Pokud pokud nalezené řešení nesplňuje tato omezení, zkusíme ještě spočítat druhé řešení..

Postup nalezení řešení IKU:

Výpis části programu v MATLABU (transf_ax.m):

%nejprve se počítá úhel kloubu Achse2 beta=pi-acos((l0^2+l1^2-l^2)/(2*l0*l1))+betacorrection; %a zvolí se jedno z řešení podle aktuálního natočení beta = beta*orientace; %spočtou se souřadnice pro alfa=0 xp=l0+l1*cos(beta); yp=l1*sin(beta); %spočte se úhel spojnic počátku a koncového bodu pro alfa=0 psi=acos(xp/l); if (yp<0) psi=2*pi-psi;end; %a spočte se úhel spojnic počátku a koncového bodu pro požadovaný koncový bod fi=acos(p2(:,1)/l); if (p2(:,2)<0) fi=2*pi-fi;end; %rozdíl je hledaný úhel alfa alfa=fi-psi; %spočtu souřadnice kloubu Achse1 pro zobrazeni p1(:,1)=l0*cos(alfa); p1(:,2)=l0*sin(alfa); %pokud někoho zajímá úhel Achse1 či Achse2 ve stupních, nalezne ho zde betas=360/(2*pi)*beta; alfas=360/(2*pi)*alfa;

4.3 Výpočet polohy vertikálního ramene
Jelikož je rameno rovnoběžné s osou z, získáme polohu ramene řešením lineární rovnice zx = k . z

V našem případě se bylo možno omezit ve vyšších vrstvách na dvě pracovní hladiny PEN UP a PEN DOWN mezi nimiž se vertikální poloha ramena měnila.

4.4 Demostrační program pro simulaci chodu robota

sim_robot.m

sprintf('HURA do vypoctu\n')

%ROBOT

p0=[0,0]; % pocatek souradnic
p1=[0,0]; % poloha kloubu
p2=[2,5]; % cilovy bod

l0=250 % delka prvniho ramene
l1=200 % delka druheho ramene

alfacorrection=0; % korekce uhlu alfa pro nulovou polohu serva
betacorrection=0; % korekce uhlu beta pro nulovou polohu serva

amin=-pi/2; %minimum kloubu alfa
amax=pi/2; %maximum kloubu alfa
bmin=-125*pi/180; %minimum kloubu beta
bmax=125*pi/180; %maximum kloubu beta
cmin=0; %minimum vert.ramene
cmax=240; %maximum vert.ramene

orientace=1; %pocatecni natoceni ramene

maxop=450; % definuje rozmery grafu
radiuswrk=(l0+l1); %vypocet max. pracovni oblasti

% ======================================

init_drob; % vytvor graf
nti=0;ti=0;i=0;
cntmax=5000;

for cnt = -cntmax:cntmax; % multi-simulace

p2(:,1)=2*radiuswrk*(rand-0.5);
p2(:,2)=2*radiuswrk*(rand-0.5);

l=sqrt((p2(:,1)-p0(:,1))^2+(p2(:,2)-p0(:,2))^2);

if ((l>(l0+l1))|(l<abs(l0-l1))) %warning('Possition out of robot''s range.') %Chyba
    else
ti=ti+1;
transf_ax;
%kontrla mezi uhlu
if (((bmin<beta)&(beta<bmax))&((amin<alfa)&(alfa<amax))) ;
drob;
else;
%warning('Possition out of range. Trying to reconfigure arms.')
%drob;
orientace=orientace*-1;
transf_ax;
if (((bmin<beta)&(beta<bmax))&((amin<alfa)&(alfa<amax))) ;
%warning('OK. It works.')
i=i+1;
drob;
                    else;
                %warning('Unable get into inserted possition a unable reconfigure arms too.')
orientace=orientace*-1;
nti=nti+1;
end; %end if
end; %end if

end; %end if v toroidu

end
%pause;
end

drob.m

% vykreslen akt. polohy
figure(21);
line([p2(:,1),p1(:,1)],[p2(:,2),p1(:,2)]);
line([p1(:,1),p0(:,1)],[p1(:,2),p0(:,2)]);

figure(22);
rectangle('Position', [ p2(:,1)-2 p2(:,2)-2 4 4 ]);

init_drob.m

%figura pro kresleni polohy ramen
figure(21);
clf;
cla;
axmax=maxop;
axis([-axmax axmax -axmax axmax]);
corner=l0;
rectangle('Curvature',[1 1],'Position', [-corner -corner 2*corner 2*corner]);
corner=radiuswrk;
rectangle('Curvature',[1 1],'Position', [-corner -corner 2*corner 2*corner]);

% figura pro koncove body
figure(22);
clf;
cla;
axmax=maxop;
axis([-axmax axmax -axmax axmax]);

4.5 Exportovaná konverzní funkce

Pokoušel jsem se implementovat možnost MATLABU zvanou SCRIPT, který se narozdíl od MatLabovské FUNCTION chová s tím rozdílem že po ukončení bloku / resp. M-file nedojde ke ztrátě (ve funkci lokálních) proměnných. Toto jsem ve výpisu musel nahradit vložením volaného kódu přímo do těla hlavní funkce iku.m

Původní myšlenka rozdělení do tří souborů předpokládala samostatný program pro tělo funkce, skript pro výpočet jednoho z řešení a v poslední řadě skript v němž budou pohromadě všechny konstanty skýtající snadné nastavení parametrů řízeného robota na jednom místě.

Funkce iku očekává na vstupu vektor se třemi prvky určující polohu chapadla v prostoru a výstupem je informace o existenci řešení oorr (out of robot range) a hodnoty pro servomotory alfas,betas,verp.

Výpis programu:

function [alfas,betas,verp,oorr] = iku(p2)
%spocte reseni IKU

%sprintf('')

4.5 Upravená koncepce pro další rozšíření

Vzhledem k tomu, že program měl za úkol realizovat krom IKT také kreslící funkce a to od jednoduchých geometrických útvarů jako úsečka a kruhová výseč také kresbu fontu, bylo nutno přepracovat výše uvedené skripty do formy funkcí. Jednou z výhod MatLabu je jeho funkční důslednost, která se projevuje tak že, výstup funkce je definován jen jejími parametry a tělem funkce. Nemožnost použití globálnch proměnných vede k tomu, že je nutno všechny potřebné údaje předávat jako parametr funkce.

Popis parametrů jednotlivých funkcí lze nalézt v příloze.

PŘÍLOHA A

5. Popis navržených funkcí a jejich parametrů

IKU.M

function v = iku(p2,setorient)
p2 ... 3-D vektor požadované polohy
setorient ... {+1,-1} - preferovaná orientace
v(1)=alfas ... nalezený úhel ve stupních
v(2)=betas ... nalezený úhel ve stupních
v(3)=verp ... poloha vertikálního kloubu
v(4)=oorr; ... příznak out-of-robot-range (požadované řešení neexistuje ani v jedné konfiguraci)

Funkce pro řešení inverzní kinematické úlohy

IKU_TEST.M

Funkce testuje exitenci na řešení existence IKT při zadané poloze a orientaci ramen (výběr řešení).
Podobně jako IKU, jen vrací 1 pokud požadované řešení v dané konfiguraci existuje

DROBOT.M

ao=drobot(robot,ao,vec);
ao ... vektor pro zápis do robota R/W
robot ... struktura definující robota
vec=iku([actpos],setorient) ... vektor vrácený funkcí IKU

Funkce.Zapíše vektor pro pozici do výstupního vektoru fronty určeného pro robota.

DROB.M

Skript. Zapíše v případě existence rešení aktuální polohu do grafu v MatLabu

RSEGMENT.M

function ao=rsegment(robot,ai,actpos,testpos,setorient);
ao,ai ... W/R matice s vektory pro robota
robot ... struktura definující robota
actpos,testpos ... 3-D vektory pro definici segmentu od/do
setorient ... preferovaná orientace, která se jinak zapisuje také do ao(1,1)

Pokud lze, provede přesun z bodu do bodu. Segment je základní geometrický útvar z něhož se skládají úsečky a křivky.

RLINE.M

function setorient=rline(robot,actpos,vec);
robot ... struktura definující robota
actpos ... 3-D vektory pro definici segmentu od/do
setorient ... preferovaná orientace, která se jinak zapisuje také do ao(1,1)
vec ... 1,2-xy-druhá souřadnice, 3-PU/PD po dojetí na konec

Funkce pro nakreslení úsečky z bodu do bodu.

RCIRCLE.M

function rcircle(robot,actpos,abrdp);
robot ... struktura definující robota
actpos ... střed části kružnice
abrdp(1) ... alfa - úhel od (0-2*pi)
abrdp(2) ... beta - úhel do (0-2*pi)
abrdp(3) ... radius - poloměr v mm
abrdp(4) ... dir - směr pro přechod úhlu od->do
abrdp(5) ... pen -poloha pisátka

Funkce pro nakreslení části kružnice.

FONT_B.M

function b(vec);
% x,y,uhel,velikost
x=vec(1);
y=vec(2);
fsize=vec(5);
xr=vec(6);
yr=vec(7);
alfa=vec(8);

Funkce vypíše písmeno B o zadané velikosti, pootoční a na zadanou pozici.

FONT_O.M

Funkce vypíše písmeno O o zadané velikosti, pootoční a na zadanou pozici.

FONT_S.M

Funkce vypíše písmeno S o zadané velikosti, pootoční a na zadanou pozici.

FONT_C.M

Funkce vypíše písmeno C o zadané velikosti, pootoční a na zadanou pozici.

FONT_H.M

Funkce vypíše písmeno H o zadané velikosti, pootoční a na zadanou pozici.

FONT_ROT.M

function vec2=font_rot(vec);
% x,y,uhel,velikost
%vec2=vec;
% z , do, pu/pd, okolo, okolik
%vec2(1)=vec(6)+((vec(1)-vec(6))*cos(vec(8)*pi/180));
%vec2(2)=vec(7)+((vec(2)-vec(7))*sin(vec(8)*pi/180));
%vec2(3)=vec(6)+((vec(3)-vec(6))*cos(vec(8)*pi/180));
%vec2(4)=vec(7)+((vec(4)-vec(7))*sin(vec(8)*pi/180));

Funkce pro pootočení bodu ve 2-D okolo zadaného počátku.

NAMOC.M

Skript pro namočení štětce v inkoustu. S modrým inkoustem totiž robot napíše jen cca 15cm čáru.

NAPIS.M

Funkce pro výpis textu. Volá jednotlivé funkce typu font_b pro výpis jednotlivých písmen.

INIT_DROB.M

Skript pro inicializaci robota.

ROBOT_SETTINGS.M

Skript v němž jsou soustředěna všechna nastavení a omezení robota. Jeho editací lze ednoduše měnit pracovní parametry.
Využívá hlavně fce. IKU.

%ROBOT up=-8000; down=-10000; p0=[0,0]; % pocatek souradnic p1=[0,0]; % poloha kloubu %p2=[2,5]; % cilovy bod1 l0=250; % delka prvniho ramene l1=200; % delka druheho ramene alfacorrection=0; % korekce uhlu alfa pro nulovou polohu serva betacorrection=0; % korekce uhlu beta pro nulovou polohu serva amin=pi*-85/180; %minimum kloubu alfa amax=pi*85/180; %maximum kloubu alfa bmin=-120*pi/180; %minimum kloubu beta bmax=120*pi/180; %maximum kloubu beta cmin=0; %minimum vert.ramene cmax=240; %maximum vert.ramene zkonst=1; %konstanta vertikalniho ramene ~ poc.kroku na 1mm delky orientace=1; %pocatecni natoceni ramene maxop=450; % definuje rozmery grafu radiuswrk=(l0+l1); %vypocet max. pracovni oblastiROBOT_INIT.M

Skript. Inicializace robota.

FLUSH.M

Skript pro zapsání matice a do robota a vykonání požadované trajektorie. Používá se jen pro ladící účely.

BOSCHDEMO.M

Skript pro demonstraci funkcí použitý při odevzdání úlohy

T.M, TT.M, TTT.M, XICHT.M

Jedná se o další skripty pro demonstraci funkcí a možností robota (TTT.M ukazuje práci na hranicích akční plochy, XICHT.M nakreslí malý ksichtík)

6. Zdrojové soubory

Jsou uloženy v souboru bosch.zip

Originál dokumentu se nachází na adrese https://www.volny.cz/dajvos/FEL/ROB/bosh1.htm

home