Come affrontare la discontinuità dell'angolo di imbardata a 180 gradi

Question

Sto lavorando a un controllo del veicolo e utilizzo di un sensore 9DOF (accelerometro, magnetometro e giroscopio). Per l'angolo di imbardata, ho un problema di discontinuità a pi rad. (180 gradi). Sto controllando il veicolo con un controller PID e quando il veicolo gira oltre 180 gradi, il segnale cambia improvvisamente (da 180 a -180) e ciò rende il controller un comportamento strano. Lo stesso problema si verificherà anche quando gira più di -180 gradi.

Come metodo, sto usando una matrice di coseno di direzione per calcolare gli angoli di Eulero. (metodo consigliato per il sensore sparkfun.)

La mia domanda è che tipo di approccio dovrei usare? Come affrontare questa discontinuità nel caso in cui si utilizzi un controller PID per controllare l'angolo di imbardata.

Answer 1

Ho avuto lo stesso problema e sto facendo il seguente:

#define MOD(a) ((a > 180.0) ? (a - 360.0) : ((a < -180.0) ? (a + 360.0) : a)) 

La differenza di 2 angoli può semplicemente essere prorogato fino al -180d/+ 180d con tale formula.

Answer 2

Questo è il tuo problema?

After desired angle changes it's sign(ie. 180->-180) 
then suddenly control input(ie. P * error) becomes bigger. 

Ad esempio.

-------------------------------- 
| desired | real | error | 
|-----------|--------|---------| 
| 170 | 160 | 10 | 
| -170 | 160 | -330 | 
-------------------------------- 

Se il tuo problema è lo stesso che ho capito, che ne dici di questo? Prima di andare, si noti che Si presume che tutti i valori di angolo rientrino nell'intervallo [-pi, pi].

error = desired - real; 
if(error > 180) 
    error = error - 360; 
else if(error < -180) 
    error = error + 360; 
else 
    error = error;//do nothing 

Questo metodo sceglie sempre una direzione tale che l'ingresso di controllo diventa più piccolo. (Sai ​​che ci sono due scelte per direzione)