regolatore lqr per nxtway

tentativo di trovare il regolatore lqr (wiki) per il nxt di sybille mayer con scilab
attingendo a piene mani (copia-incolla) dal mitico R. Bucher


function [num,den]=tfdata(G)
num=G('num');
den=G('den');
endfunction

A=[0,1,0,0; 94.05,0,0,1.3;0,0,0,1; -544.05,0,0,-14.3]
B=[0;-0.21;0;2.32]
C=[1,0,0,0;0,0,1,0]
D=[0;0]
nxtway_SYB=syslin('c',A,B,C,D)
s=%s;
Ts=0.03
spec(A)
autovalori_di_A=spec(A)
sys=syslin('c',A,B,C,D);
sysd=dscr(sys,Ts);
[ad,bd,cd,dd]=abcd(sysd);

// pesi del controllore LQR

Q=diag([10000,1,10000,1]); // 4 per 4
R=[1]; // 1 per 1

//calcola i guadagni LQR per un sistema discreto
[n1,d1]=size(ad);
big=sysdiag(Q,R);
[w,wp]=fullrf(big ,1e-20);
C1=wp(:,1:n1);
D12=wp(:,n1+1:$);
P=syslin('d',ad,bd,C1,D12);
[k,X]=lqr(P)
k_lqr= -k;

E=spec(ad);
plqr=abs(real(log(E)/Ts))';
pmax=max(plqr);
k_lqr=-k_lqr;
preg=spec(A);
// osservatore di ordine ridotto
poli_oss=exp([real(preg(3)),real(preg(4))]*10*Ts);
T=[0,0,0,1;0,1,0,0];

// trova l'ossrvatore di ordine ridotto per A,B,C,D, con polo di osservatore
// T e' la matrice necessaria per rendere [C;T] invertibile
P=[cd;T]
invP=inv([cd;T])
AA=P * ad * invP
ny=size(cd,1)
nx=size(ad,1)
nu=size(bd,2)
A11=AA(1:ny,1:ny)
A12=AA(1:ny,ny+1:nx)
A21=AA(ny+1:nx,1:ny)
A22=AA(ny+1:nx,ny+1:nx)
L1=ppol (A22',A12',poli_oss)';
nn=nx - ny;
A_redobs=[-L1 eye(nn,nn)]*P*ad*invP*[zeros(ny,nn); eye(nn,nn)];
B_redobs=[-L1 eye(nn,nn)]*[P*bd P*ad*invP*[eye(ny,ny);L1]]*[eye(nu,nu) zeros(nu,ny);-dd, eye(ny,ny)];
C_redobs=invP*[zeros(ny,nx-ny);eye(nn,nn)];
D_redobs=invP*[zeros(ny,nu) eye(ny,ny);zeros(nx-ny,nu) L1]*[eye(nu,nu) zeros(nu,ny);-dd, eye(ny,ny)];
ao=A_redobs
bo=B_redobs
co=C_redobs
do=D_redobs

// Crea la forma compatta dell'osservatore ABCD e il guadagno K
//
//

Bu=bo(:,1);
By=bo(:,2:$);
Du=do(:,1);
Dy=do(:,2:$);
X=inv(1+k_lqr*Du);
Ac=ao - Bu*X*k_lqr*co;
Bc=[Bu*X,By-Bu*X*k_lqr*Dy]
Cc=-X*k_lqr*co;
Dc=[X,-X*k_lqr*Dy]
Greg=syslin('d',Ac,Bc,Cc,Dc)
///////////////////////////////////////////
Gregtf=ss2tf(Greg);
Gregtf( :,2)
Gregtf( :,3)
[g1n,g1d]=tfdata(Gregtf( :,2))
[g2n,g2d]=tfdata(Gregtf( :,3))


questo butta fuori come regolatore il seguente:

Greg =


Greg(1) (state-space system:)

!lss A B C D X0 dt !

Greg(2) = A matrix =

- 1.1704196 - 16.638285
0.1322204 2.048937

Greg(3) = B matrix =

0.0329866 - 559.33453 - 69.773154
- 0.0036029 41.850078 4.3785807

Greg(4) = C matrix =

- 35.525535 - 497.8127

Greg(5) = D matrix =

1. - 16487.356 - 1308.8181

Greg(6) = X0 (initial state) =

0.
0.

Greg(7) = Time domain =

d

regolatore con scilab

Riferimento al problema di regolare il sistema instabile del nxt-way di Sybille Mayer.

Con scilab:

A=[0,1,0,0; 94.05,0,0,1.3;0,0,0,1; -544.05,0,0,-14.3] // pag 24
B=[0;-0.21;0;2.32]
C=[1,0,0,0;0,0,1,0]
D=[0;0]
nxtway_SYB=syslin('c',A,B,C,D) // genera sistema lineare
s=%s; // variabile di laplace
spec(A) // calcola autovalori
autovalori_di_A=spec(A)
poli_desiderati=[-2.6 -3.9 -5.2 -6.5] // poli desiderati
K=ppol(A,B,poli_desiderati) // calcolo della matrice di feedback K

F=A-B*K // sistema in retroazione
g2=syslin('c',F,B,C,D) // genera nuovo sistema lineare
spec(F)
autovalori_di_F=spec(F) //calcola autovalori, per verificare il piazzamento poli

si ottengono i seguenti risultati:

--> A =

0. 1. 0. 0.
94.05 0. 0. 1.3
0. 0. 0. 1.
- 544.05 0. 0. - 14.3
B =

0.
- 0.21
0.
2.32
C =

1. 0. 0. 0.
0. 0. 1. 0.
D =

0.
0.
nxtway_SYB =


nxtway_SYB(1) (state-space system:)

!lss A B C D X0 dt !

nxtway_SYB(2) = A matrix =

0. 1. 0. 0.
94.05 0. 0. 1.3
0. 0. 0. 1.
- 544.05 0. 0. - 14.3

nxtway_SYB(3) = B matrix =

0.
- 0.21
0.
2.32

nxtway_SYB(4) = C matrix =

1. 0. 0. 0.
0. 0. 1. 0.

nxtway_SYB(5) = D matrix =

0.
0.

nxtway_SYB(6) = X0 (initial state) =

0.
0.
0.
0.

nxtway_SYB(7) = Time domain =

c
ans =

0
7.8847523
- 4.598569
- 17.586183
autovalori_di_A =

0
7.8847523
- 4.598569
- 17.586183
poli_desiderati =

- 2.6 - 3.9 - 5.2 - 6.5
K =

- 1063.3266 - 123.77134 - 3.2972279 - 9.5224059
F =

0. 1. 0. 0.
- 129.2486 - 25.991982 - 0.6924179 - 0.6997052
0. 0. 0. 1.
1922.8678 287.14951 7.6495687 7.7919818
g2 =


g2(1) (state-space system:)

!lss A B C D X0 dt !

g2(2) = A matrix =

0. 1. 0. 0.
- 129.2486 - 25.991982 - 0.6924179 - 0.6997052
0. 0. 0. 1.
1922.8678 287.14951 7.6495687 7.7919818

g2(3) = B matrix =

0.
- 0.21
0.
2.32

g2(4) = C matrix =

1. 0. 0. 0.
0. 0. 1. 0.

g2(5) = D matrix =

0.
0.

g2(6) = X0 (initial state) =

0.
0.
0.
0.

g2(7) = Time domain =

c
ans =

- 6.5
- 2.6
- 3.9
- 5.2
autovalori_di_F =

- 6.5
- 2.6
- 3.9
- 5.2


si vede che il sistema retroazionato ha i poli (negativi) del valore che si voleva.

I poli ottenuti da Sybille sono:
[-1051.93 -122.34 -3.32 -9.44]
mentre lo script scilab da'
K = - 1063.3266 - 123.77134 - 3.2972279 - 9.5224059

piu' o meno gli stessi.

risposta all'impulso instabile, nxtway_SYB






















risposta all'impulso stabile, g2























risposta al gradino sistema instabile, nxtway_SYB:

























risposta al gradino sistema stabile, g2:

matlab toolkit

questa e' la notizia del giorno: anche all'universita' di Aachen hanno sviluppato per NXT un toolkit per matlab (opensource GNU GPL scaricabile gratuitamente)

aachen

non mi sembra pero' niente paragonato al ECR-NXT


ps: se si ha matlab basta questo

un sito di appassionati di robotica

http://www.societyofrobots.com/sensors.shtml

contiene un'interessante calcolatore per bracci robotici:
http://www.societyofrobots.com/robot_arm_calculator.shtml
da usare dopo aver letto il tutorial:
http://www.societyofrobots.com/robot_arm_tutorial.shtml#DOF

localizzazione fine col sonar (US) /sliding scale sonar

L'idea e' quella del localizzatore normale che traccia la mappa del territorio circostante come in

meow
oppure
davis
oppure
guy
oppure
xaos69

ma perfezionata da un motore che va avanti-indietro radialmente in modo da determinare la posizione in cui US cambia cifra.

Lasciamo perdere il fatto che quando US dice 27cm (a proposito occorre ricordare di mettere
DefaultUnits=Metric , per avere cm invece di pollici nel file
C:\Program Files\LEGO Software\LEGO MINDSTORMS NXT\MINDSTORMS Options.ini) in realta' possono essere 30cm oppure 24cm. Non e' accurato, si sa.

Parliamo invece di precisione.
Supponiamo che US dica 27cm di distanza. In realta' potrebbe essere 27.1cm oppure 27.9cm.
Montando US su un meccanismo a vite senza fine, si puo' stabilire quanti mm all'interno del cm.
Un motore sposta radialmente (ma si monitorano le rotazioni) US di poco verso l'oggetto finche' la cifra passa a 26cm.
Il motore potrebbe essere spingere un coso cosi':












Il blu viene fatto girare, la vite senza fine ruota il filetto nella scanalatura gialla, la quale spinge il pezzo a "L", dove e' montato il sensore US.
Un giro di motore corrisponde ad una distanza pari a quella del passo del filetto.
In questo modo sappremo quanti mm era oltre i 27cm.

en passant: link meccanica

distribuzione dei visitatori a questo blog

NXT nel mezzo dell'Amazzonia?!?

allargatore di loop

Un problema tipico con NXT-G e' quello di far stare i blocchi nei loop che si stringono attorno al loro contenuto e diventano scomodissimi.
B. Davis ha escogitato il trucco di allargarli artificialmente inserendo di proposito dei blocchi con hub molto lunghi (per poi toglierli alla fine).

Ho preparato con Labview un bel block che non fa nulla, ma ha un hub bello lungo (piu' dell'altezza della pagina).