next up previous
Next: About this document ...

A.A. 2000/2001
Università degli studi di Padova Corso
Controllo dei processi
prof. R. Frezza Controllo mediante stimatore del 3-DOF Helicopter e stima dell'inclinazione del piano descritto dallo stesso
SOMMARIO
  1. Il sistema 3-DOF
  2. Il controllo
  3. Stima della normale al piano:
Il sistema 3-DOF
Il sistema 3DOF Helicopter della Quanser consulting, è costituito sostanzialmente da un'asta che può ruotare rispetto a due assi ortogonali (uno normale al pavimento e l'altro sempre normale all'asta stessa).







L'asta si muove spinta da due eliche poste su un supporto che può ruotare rispetto ad un'asse al quale appartiene l'asta stessa. Tali eliche generano una forza sempre ortogonale all'asta.
Per trovare il modello matematico, è stato usato il metodo di Lagrange:

\begin{displaymath}
E_c= \frac{1}{2}J_{pitch} \dot{\theta}_{pitch}^2 + \end{displaymath}


\begin{displaymath}\frac{1}{2}M_{pitch}b_c^2(\dot{\theta}_e^2 + \dot{\theta}_{travel}^2) +\end{displaymath}


\begin{displaymath}\frac{1}{2} J_e (\dot{\theta}_e^2 + \dot{\theta}_{travel}^2)\end{displaymath}


\begin{displaymath}U_p = M_e b_e g \sin{(\theta _e)} + \end{displaymath}


\begin{displaymath}M_{pitch} g d_{pitch} (1-\cos{(\theta _{pitch})})\end{displaymath}

Posto $T=E_c - U_p$, dalle equazioni

\begin{displaymath}\frac{\partial T}{\partial \theta _e} -
\frac{d}{d t} \frac{...
...\theta} _e} =
b_c K_g (V_{rigth} + V_{left}) \cos{(\theta _p)}\end{displaymath}


\begin{displaymath}\frac{\partial T}{\partial \theta _t} -
\frac{d}{d t} \frac{...
...\theta} _t} =
b_c K_g (V_{rigth} + V_{left}) \sin{(\theta _p)}\end{displaymath}


\begin{displaymath}\frac{\partial T}{\partial \theta _p} -
\frac{d}{d t} \frac{...
... T}{\partial \dot{\theta} _p} =
b_p K_g (V_{rigth} - V_{left})\end{displaymath}

si ricava il modello dinamico:

\begin{displaymath}
\dot{\omega} _e = \frac{M_e b_e g}
{M_p b_c^2 + J_e} \cos{(\theta _e)} -\end{displaymath}


\begin{displaymath}\frac{K_g b_c}
{M_p b_c^2 + J_e}(V_{rigth}+V_{left})\cos{(\theta _p)}
\end{displaymath}


\begin{displaymath}
\dot{\omega}_p = \frac{M_p g d_p} {J_p}
\sin{(\theta _p)} -
\frac{K_g b_p}{J_p}(V_{rigth} - V_{left})
\end{displaymath}


\begin{displaymath}
\dot{\omega}_t = -\frac{b_c K_g}{M_p b_c^2 + J_e}
(V_{right} + V_{left})\sin{(\theta _p)}
\end{displaymath}

dove $\omega_e = \dot{\theta} _e$, $\omega_t = \dot{\theta} _t$, $\omega_p =
\dot{\theta} _p$. Inoltre:
$M_e$
è la massa equivalente del sistema mobile (asta e blocco delle eliche), che tiene conto del contrappeso;
$M_p$
è la massa del blocco delle eliche;
$J_e$
è il momento d'inerzia del sistema mobile;
$J_p$
è il momento d'inerzia del blocco delle eliche;
$b_e$
é la distanza del centro di massa (equivalente) del sistema mobile dall'asse di rotazione;
$b_c$
è la lunghezza dell'asta;
$d_p$
è il braccio del momento generato dalla gravità sul blocco delle eliche
$K_g$
è costante di conversione $V \Rightarrow N$
$V_r$ $V_l$
sono le tensioni applicate ai motori.
Linearizzando si ottiene il sistema:

\begin{displaymath}
\frac{d}{dt}\left[ \begin{array}{c}
\theta _{e}\\
\theta...
...ga _{e}\\
\omega _{p}\\
\omega _{t}
\end{array}\right] +
\end{displaymath}


\begin{displaymath}
\left[ \begin{array}{cc}
0 & 0\\
0 & 0\\
0 & 0\\
-\f...
...\left[
\begin{array}{c}
V_{r}\\
V_{l}
\end{array}\right]
\end{displaymath}

Che valutato in $ \left[ \begin{array}{cccccc}
\theta _{e0} & \theta _{p0} & \theta _{t0} & \o...
...ht] =
\left[ \begin{array}{cccccc} 0 & 0 & 0 & 0 & 0 & 0 \end{array} \right] $ dà il seguente sistema lineare:

\begin{displaymath}
\left[
\begin{array}{c}
\dot{\theta}_e \\ \dot{\theta}_p...
...omega _e \\ \omega _p \\
\omega _t
\end{array}
\right] +
\end{displaymath}


\begin{displaymath}
\left[
\begin{array}{c}
0 \\ 0 \\ 0 \\ \frac{K_g b_c}{M_p...
..._c}{M_p b_c^2 + J_e}\theta _p
\end{array}
\right] V_{right}
\end{displaymath}

Il controllo
Il sistema continuo viene controllato con retroazione dallo stato stimato. La matrice KK di retroazione è calcolata usando la funzione LQR di MATLAB: gli autovalori del sistema

\begin{displaymath}A-B*KK\end{displaymath}

sono stabili e la funzione costo

\begin{displaymath}J=\int(x'Qx + u'Ru)dt\end{displaymath}

viene minimizzata. Le matrici Q ed R sono impostate empiricamente in modo tale da assegnare peso opportuno alle diverse variabili di stato: nel nostro caso il controllo privilegia la precisione sugli angoli elevation e travel.

\begin{displaymath}Q=\left[
\begin{array}{cccccc}
1000 & 0 & 0 & 0 & 0 & 0\\ 
...
... 1 & 0\\
0 & 0 & 0 & 0 & 0 & 10
\end{array}
\right],
R=I
\end{displaymath}

Il sistema retroazionato $A-B*KK$ viene discretizzato con periodo di campionamento $T_C=1$ ms, ottenendo il sistema descritto dalle matrici $F$ e $H$. Lo stimatore dello stato è realizzato, usando la funzione place, in modo tale da avere autovalori di modulo inferiore a quelli del sistema. Gli ingressi dello stimatore sono, ovviamente, le tensioni ai motori e le uscite degli encoder. La legge di evoluzione del sistema stimatore è:

\begin{displaymath}
\left\{
\begin{array}{l}
x(t+1)=(F-L*H)x(t)+\left[G L\right]u(t)\\
y(t)=Ix(t)
\end{array}
\right.
\end{displaymath}

Obiettivo del nostro controllo è far descrivere all'elicottero una rotazione su un piano inclinato a piacere: in un sistema di riferimento solidale con il dispositivo e con l'origine posta sul perno dell'asta rotante la posizione dell'elicottero deve obbedire all'equazione

\begin{displaymath}Ax+By+Cz=0\end{displaymath}

Fissati i coefficienti del piano e impostata una velocità di rotazione costante (quindi angolo travel lineare nel tempo), si ricava, dopo alcuni passaggi, la legge di variazione dell'angolo elevation:







\begin{displaymath}\begin{array}{l}
x=R\cos(e)\cos(t)\\
y=R\cos(e)\sin(t)\\ 
...
...sin(e)\\
e=\arctan(-\frac{A\cos(t)+B\sin(t)}{C})
\end{array}\end{displaymath}

Feature tracking
L'elicottero descrive una circonferenza giacente su un piano inclinato: il tracking visuale della feature collocata in prossimità dell'elicottero stesso è ottenuto con l'impiego dell'algoritmo di TOMASI-KANADE. Poichè questo è molto oneroso dal punto di vista computazionale (smoothing e calcolo del gradiente), viene eseguito solo su una piccola finestra nell'intorno della feature selezionata. La posizione della finestra viene aggiornata ad ogni passo in base allo spostamento precedente. In questo modo è possibile un tracking a velocità piuttosto elevate. In caso di perdita della feature viene automaticamente fatta la riselezione.
L'ALGORITMO
L'algoritmo esegue il tracking da un frame al successivo non di singoli pixel o di configurazioni predefinite degli stessi (come, ad es, 'corner') ma di finestre di dimensione molto inferiore a quella del frame (tipicamente, quadrati di 7-10 pixel di lato). Lo spostamento viene calcolato come soluzione di un sistema lineare di due equazioni ottenuto sulla base dell'approssimazione di un frame all'istante $t+\tau$ con lo sviluppo in serie di Taylor arrestato al 1° termine del frame all'istante $t$. Siano:

\begin{displaymath}J(x)=I(x,t+\tau), I(x,t)\end{displaymath}

le funzioni intensità luminosa agli istanti $t+\tau$ e $t$ rispettivamente. Definita la seguente relazione di approssimazione, è possibile calcolare il residuo $\epsilon$:

\begin{displaymath}J(x)=I(x-d)+n(x)\end{displaymath}


\begin{displaymath}I(x-d)=I(x)-\nabla I(x) d\end{displaymath}


\begin{displaymath}\epsilon = \int_\mathcal{W} \left[I(x)-\nabla I(x) d- J(x)\right]^2w dx=\int_\mathcal{W} (h-gd)^2wdx\end{displaymath}

La minimizzazione di questo residuo può essere fatta in forma chiusa differenziando e uguagliando a zero il seguente integrale:

\begin{displaymath}\int_\mathcal{W} (h-gd)gwdA=0\end{displaymath}

Riscrivendo il tutto si perviene al seguente sistema lineare:

\begin{displaymath}\left( \int_\mathcal{W} gg^Twda\right)d = \int_\mathcal{W} hgwdA\end{displaymath}

che diventa, in forma più concisa:

\begin{displaymath}Gd=e\end{displaymath}

dal quale è possibile ottenere il vettore spostamento $d$.
FEATURE SELECTION
In queste note viene brevemente presentato il criterio di selezione delle feature delle quali si andrà ad eseguire il tracking. Il criterio di selezione è ottimo per costruzione: una finestra è "buona" quando può essere inseguita con successo: questo accade quando il sistema lineare è risolvibile in maniera affidabile e cioè quando i coefficienti della matrice $G$ sono entrambi al di sopra del livello di rumore e ben condizionati. In altre parole, entrambi gli autovalori devono essere dello stesso ordine di grandezza e superiori ad una soglia minima $\lambda$ determinata da un'immagine contenente solo rumore.
Fitting dell'ellisse
Input: Output: Algoritmo:
  1. Si costruisce la matrice dei dati

    \begin{displaymath}D=\left[x_1, x_2,\ldots, x_n\right]^T con\end{displaymath}


    \begin{displaymath}x_i=\left[x_i^2, x_i y_i, y_i^2, x_i, y_i, 1\right]^T\end{displaymath}

  2. E' necessario minimizzare la grandezza

    \begin{displaymath}E=\left\Vert Da\right\Vert^2\end{displaymath}

    Derivando e uguagliando a zero si ottiene:

    \begin{displaymath}2D^T Da=0\end{displaymath}

  3. Posto $S=D^T D$ ciò è equivalente a risolvere il problema agli autovalori e autovettori per

    \begin{displaymath}Sa=0\end{displaymath}

  4. E' dimostrato che gli autovalori della matrice $S$ sono tutti positivi. L'autovettore relativo all'autovalore minimo ($\approx 0$) è il vettore $\hat{a}$ stimato dei parametri della conica.
  5. Se i controlli (det. $b^2-4ac<0$) hanno esito positivo $\hat{a}$ è l'output dell'algoritmo e sullo schermo viene visualizzata l'ellisse stimata.
  6. L'algoritmo su cui ci siamo basati proponeva una soluzione basata sulla risoluzione di un problema agli autovalori generalizzati che porterebbe ad ottenere direttamente un vettore di parametri corrispondenti a un'ellisse:

    \begin{displaymath}
\left\{
\begin{array}{l}
2D^TDa\,-\,2 \lambda Ca\,=\,0\\
a^TCa\,=\,1
\end{array}
\right.
\end{displaymath}

    La nostra soluzione si basa invece sulla risoluzione di un più semplice problema agli autovalori e sul controllo della soluzione descritto in precedenza, questo perché sappiamo a priori che i punti si disporranno su un'ellisse, inoltre in questo modo si alleggerisce il calcolo.
Stima dei parametri del piano
Data un'ellisse giacente su un piano bidimensionale è possibile definire una superficie conica avente per base l'ellisse stessa e per vertice un punto nello spazio non appartenente al piano dell'ellisse.
Input Output Algoritmo
  1. Calcolare i coefficienti $\left[a, b, c, f, g, h\right]$ dell'equazione omogenea del cono

    \begin{displaymath}ax^2+by^2+cz^2+2fyz+2gzx+2hxy=0\end{displaymath}

    che può essere scritta nella forma matriciale

    \begin{displaymath}X^T AX=0\end{displaymath}

  2. Data la matrice simmetrica $A$, si calcolano i relativi autovalori $\lambda_i$ e autovettori $\left[l_i, m_i, n_i\right]$, che permettono di eseguire la rototraslazione che riporta nel canonical frame $\{XYZ\}$.
  3. Riportandosi al sistema di riferimento dato dagli autovettori, l`equazione del cono diviene

    \begin{displaymath}\lambda _1 X^2+\lambda _2 Y^2+\lambda _3 Z^2=\mu=0\end{displaymath}

    Data la base ellittica due autovettori sono di segno positivo, mentre il terzo è negativo. Riordinando opportunamente autovalori e autovettori, in modo tale da avere $\lambda _{1,2} >0$ e $\lambda _3 <0$ si ha la coincidenza dell'asse del cono con l'asse $Z$ del sistema di riferimento.
  4. Il piano di equazione

    \begin{displaymath}lX+mY+nZ=p\end{displaymath}

    la cui intersezione con il cono dà un cerchio ha l'orientazione cercata rispetto all'asse Z del riferimento. Per determinare la normale viene definita una nuova rotazione tale da avere il nuovo asse $Z$ ($Z'$) ortogonale al piano: l'equazione diviene pertanto $Z'=p$
  5. Nel nuovo riferimento si impone che l'intersezione cono - piano sia un cerchio. Si ottengono così, nella base data dagli autovettori $\lambda_i$, il vettore $\left[l, m, n\right]$ normale al piano, che è anche l'output richiesto.
Stima dell'inclinazione
La prima operazione da fare, data la variabilità della posizione relativa telecamera - elicottero, è la definizione della normale di riferimento: questa si ottiene stimando la normale al piano descritto dall'elicottero e parallelo al pavimento. Input Output Algoritmo
  1. Le due normali vengono riportati nel canonical frame grazie alla matrice degli autovettori.
  2. L'angolo compreso tra i due vettori è calcolato secondo le relazioni seguenti:

    \begin{displaymath}v \bullet u = \Vert v\Vert \Vert u\Vert cos(\theta)\end{displaymath}


    \begin{displaymath}\theta=\arccos (\frac{\left[l, m, n\right] \bullet \left[l_{r...
... n\right] \Vert \Vert \left[l_ref , m_ref , n_ref\right]\Vert})\end{displaymath}

  3. Poichè le rototraslazioni conservano gli angoli, il valore ottenuto è invariante cambiando sistema di riferimento (dal canonical frame al sistema solidale con l'elicottero).



next up previous
Next: About this document ...
Bert Michele 383516/IF
2001-07-19