Nel presente lavoro sono stati presentati due approcci diversi al problema di pianificazione del moto di un veicolo subacqueo autonomo attraverso metodi ispirati al Model Predictive Control.
Il primo prevede la generazione di una libreria di manovre attraverso l’algoritmo MPC che risulta essere un approccio innovativo. La soluzione trovata risulta più leggera e snella dal punto di vista metodologico di quanto non siano quelle normalmente utilizzate.
Con la seconda soluzione proposta, invece è stato possibile superare i limiti della prima legge di guida. Mantenendo l’utilizzo dell’algoritmo MPC, ma incrementandone le capacità attraverso l’utilizzo delle informazioni derivanti dall’acquisizione dei way-point, è possibile generare un percorso migliore se paragonato al precedente. Inoltre questa seconda legge di guida ha offerto la possibilità di valutare l’effettiva utilità del rollare in curva, che di fatto si è dimostrata svantaggiosa dal punto di vista energetico a basse velocità e utile invece nel caso di ampie virate eseguite ad alta velocità. Il prezzo da pagare in questa seconda legge di guida è la soluzione iterativa di un problema MPC a bordo del veicolo.
BIBLIOGRAFIA
1. Carlo L. Bottasso, Domenico Leonello, Barbara Savini, “Path Planning for Autonomous Vehicles by Trajectory Smoothing using Motion Primitives”, IEEE Transactions on
control systems technology , Vol. 16, n. 6, 2008, pp 1152-1168.
2. Thor I. Fossen, Guidance and Control of Ocean Vehicles, John Wiley & Sons Inc Print on, 1994.
3. Simone Loureiro de Oliveira Kothare, Manfred Morari, “Contractive Model Predictive Control for Constrained Nonlinear Systems”, IEEE Transactions on automatic control, Vol. 45, n. 6, 2000, pp 1053-1071.
4. A.Caiti, “Motion planning in marine system”, in Ballielul and Samar (Eds.), Springer Encyclopedia of Systems and Control 2014
5. William Wong, Fan Han, Stephen Gaukrodger, “3D in 2D planar display project”, Care ino III, 2008
6. Sanghyo Less, Am Cho, Changdon Kee, Integrated waypoint path generation and following of an unmanned aerial vehicle, Emerald Group Publishing Limited.
7. Software disponibile all’indirizzo http://www.mathworks.it/
8. Erik P. Anderson, Randal W. Beard, Timothy W. McLain, “Real-Time Dynamic Trajectory Smoothing for Unmanned Air Vehicles”, IEEE Transactions on control
systems technology, Vol. 13, n. 3, 2005, pp471-477.
9. Frazzoli E, Dahleh MA, Feron E (2005). Maneuver-based Motion Planning for Nonlinear Systems with Symmetries. IEEE Trans. Robotics, 21(6), 1077-1091.
10. Greytak MB, Hover FS (2010). Robust Motion Planning for Marine Vehicle Navigation. In Proc. 18th Int. Offshore & Polar Eng. Conf., Vancouver, BC, Canada
11. Alvarez A, Caiti A, Onken R (2004). Evolutionary Path Planning for Autonomous Underwater Vehicles in a Variable Ocean. IEEE J. Oceanic Eng., 29(2), 418-429.
12. Breivik M, Fossen TI (2008). Guidance Laws for Planar Motion Control. In Proc. 47th
IEEE Conf. Decision & Control, Cancun, Mexico.
13. http://www.rovworld.com/article5669.html
14. http://www.miltechmag.com/2013/08/auvsi-2013-unmanned-naval-systems.html
APPENDICE
Algoritmo MPC
Dimostrazione
Come scritto in [2] prendiamo il sistema: ̇( ) ( ( ) ( ))
Dove
Assumiamo che le variabili di controllo siano soggette al vincolo:
( ) * + , ) Il problema di ottimizzazione al tempo , ( ) è rappresentato da:
( | ) ( | ) ( )
Con la funzione obiettivo definita come:
( ) ∫ ‖ ( )‖ ∑‖ ( | )‖ ∑‖ ( | )‖ Soggetta a:
{
̇ ( ( ) ( )) ( | )
| ( | )| ‖ ( )‖ ‖ ‖
L’algoritmo di controllo usato è il seguente:
Dati: Condizioni iniziali: a ; Parametri del controllore: orizzonti , ( ), pesi , parametro di contrazione , ), tempo di campionamento ( ) e vincoli sul controllo
Passo 0: imposta
Passo 1: risolve ( ) specificato dalle equazioni di formula(9-10). Il risultato di questo passo è la sequenza di controllo
Passo 2: Applica l’intera sequenza di controllo calcolata per , - e misura lo stato al tempo
Passo 3: imposta e torna al Passo 1.
Valutando il probelma di regolazione in cui il punto di lavoro desiderato è l’origine, devono valere le seguenti condizioni per assicurare la locale stabilità:
1- ( ) ( ) è un punto di equilibrio del sistema (2), cioè ( )
2- La linearizzazione della dinamica del modello attorno all’origine è stabilizzabile, ovvero *( ⁄ )( ) ( ⁄ )( )+ è una coppia stabilizzabile.
3- Esiste una costante ( ) tale che per ogni * ‖ ‖ + il problema di ottimizzazione a ( ) è risolvibile per tutti i . In altre parole, per tutti è possibile trovare un parametro di contrazione , ) in modo tale che, con l’orizzonte finito tutti i vincoli su gli ingressi e gli stati possono essere soddisfatti mantenendo limitata la funzione obiettivo.
4- Esiste una costante ( ) tale per cui lo stato del modello durante il transitorio rimane interno a ‖ ‖ * | ‖ ‖ ‖ ‖ +, cioè ‖ ( )‖ ‖ ‖ , - .
Teorema: Supponendo che le condizioni 1-4 siano soddisfatte. Dato , ) e ( )
che soddisfano le condizioni 3-4, l’algoritmo di controllo usato rende il sistema in ciclo chiuso esponenzialmente stabile per l’insieme , ad esempio per ogni la traiettoria risultante ( ) ( ) soddisfa la seguente disuguaglianza:
‖ ( )‖ ‖ ‖ ( )(( ) ) Con ( ).
Non resta quindi che verificare che il nostro sistema rispetti le quattro condizioni.
Il primo punto è verificato, dato che in questo lavoro si opera proprio su traiettorie di equilibrio.
Il secondo punto è verificato perchè linearizzando il sistema si vede che il rango della matrice di controllabilità è massimo per i vari punti di equilibrio usati in questo progetto, anche utilizzando una soglia di tolleranza numerica per lo zero di .
Il terzo punto è verificato dato che siccome il sistema è stabilizzabile punto 2 allora esiste una Q in grado di stabilizzarlo, per la quale esiste un insieme convesso e compatto di condizioni iniziali per cui il problema di ottimizzazione ad ogni istante di tempo N è risolvibile.
Il quarto ed ultimo punto è verificato, dato che, essendo l’ingresso vincolato entro limiti inferiori e superiori noti a priori, e considerato che le equazioni del sistema sono quelle di un corpo rigido soggetto a forze idrodinamiche, idrostatiche e gravitazionali (i.e., non esiste un tempo di fuga finito) lo stato , pesato in norma è necessariamente limitato.
Sotto queste condizioni l’algoritmo di controllo proposto rende il sistema A.S.
Implementazione
L’algoritmo MPC è stato implementato con funzioni MATLAB. È necessario dire che la funzione di ottimizzazione usata, fmincon, può presentare bug a causa probabilmente di incompatibilità fra la versione di MATLAB e quella del sistema operativo. Il bug è stato rilevato su un portatile con Windows 7 e Matlab R2011b, e risolto successivamente col passaggio a MATLAB R2012a. Questo problema si presentava sotto forma di una piccola componente random nei risultati forniti da fmincon. Componente che veniva numericamente ridotta all’aumentare della risoluzione dell’algoritmo. La difficoltà di individuazione del problema è dovuta prima di tutto al fatto che come verrà illustrato, la fmincon restituisce in uscita i valori normalmente trascurati in favore dell’analisi dello stato del sistema, che in quanto modello di un sistema reale, si comporta da filtro passa basso, complicando l’individuazione di piccole variazioni delle forze dei thurster tramite l’osservazione dello stato. Inoltre è piuttosto raro ed inusuale lanciare per due volte consecutive la fmincon inizializzandola con gli stessi parametri, ed è questo l’unico caso in cui si può notare il bug.
Detto ciò, verranno ora illustrate le varie funzioni realizzate e gli accorgimenti adottati per ridurre la quantità di calcoli effettuati.
SolveOptControl
A questa funzione vengono passati tutti i parametri con cui inizializzare l’algoritmo di ottimo. Calcola l’ottimo nella finestra temporale di lunghezza e restituisce il vettore contenente le forze dei thruster relative alla finestra appena calcolata. In pratica inizializza la fmincon
settandone i parametri di risoluzione e massimo numero di iterazioni usando il comando optimset. Fissa i vincoli lineari (nulli) ed i limiti di forza sugli attuatori come nella prima
disequazione di formula (10). Utilizza inoltre una variabile condivisa sharedVar fra due funzioni interne, la costFun che valuta il funzionale di costo e la vincoliNonLin che valuta i vincoli non lineari. Questa variabile è una struttura contenente al suo interno i seguenti campi:
1- sharedVar.Fi,contenente il vettore usato per il calcolo dei due successivi campi
2- sharedVar.cost contentente il valore della funzione di costo 3- sharedVar.c contentente il vettore dei vincoli non lineari
4- sharedVar.flagC, sharedVar.flagV per indicare la validità o meno dei due campi
precedenti.
L’utilizzo di una variabile condivisa è dettato dal fatto che sia la costFun che la
vincoliNonLin chiamano la funzione computeIntegral la quale risolve il sistema contenuto in sistema.m con ode45 utilizzando il vettore scelto dalla fmincon ad ogni iterazione che quindi risulta uguale sia per la costFun che per la vincoliNonLin. La soluzione del sistema (modello) è necessaria infatti sia per calcolare la funzione di costo, sia per valutare il rispetto dei vincoli non lineari. Dato che risolvere il sistema con ode45 comporta il grosso dei calcoli, si è scelto di far valutare alla computeIntegral sia il costo, che i vincoli i quali, una volta salvati nella variabile condivisa sharedVar, fanno in modo che solo la prima ad eseguire fra costFun e vincoliNonLin debba chiamare effettivamente la computeIntegral, mentre invece la seconda può tranquillamente leggere il risultato già calcolato. Questo accorgimento ha portato ad un dimezzamento dei tempi di calcolo.
computeIntegral
Questa funzione prende in ingresso la lunghezza delle finestre di predizione e controllo ed , il tempo di campionamento , il tempo iniziale , lo stato iniziale , il vettore selezionato dalla fmincon per il passo attuale, il vettore contenente l’ultimo vettore forza della precedente finestra di controllo, la matrice di peso dello stato nel contractive contraint, la matrici di peso dello stato nel funzionale di costo, le impostazioni di risoluzione di ode45, i vettori di stato e di controllo della traiettoria di equilibrio.
All’interno della finestra di predizione, trasforma il vettore in un vettore di forze/coppie , come in formula (4).
[Ai,Pi,Ki] = unpk_thr_table(ThrusterTable);
[tau] = vfides_iallocation_map(Ai, Pi, Ki, Fi_tmp);
Utilizza questo nuovo vettore per calcolare l’evoluzione del sistema.
[t_intermediate,q_intermediate] = ode45(@sistema, [t0, t0+T], q(k,:) , options , tau , qs_eq,us_eq,f); q(k+1,:) = q_intermediate(size(q_intermediate,1),:);
A questo punto è disponibile lo stato del sistema che viene usato per il calcolo della funzione di costo, formula (9)
for i = 1 : (size(t_intermediate)-1)
val = val + normaPesata(q_intermediate(i,:),Q) *(t_intermediate(i+1)- t_intermediate(i));
end
e per il calcolo dei vincoli non lineari. Prima il contractive contraint, formula (10) terza disequazione
cnew = sqrt(normaPesata(q(N+1,:),P)) - alfa*sqrt(normaPesata(q(1,:),P));
poi il limite sul delta di forza degli attuatori, formula (10) seconda disequazione
b = 0.5;
cnew = [cnew ; abs(Fi0(:,1) - Fi(:,1))-b*ones(7,1)]; for k = 1 : M-1
cnew = [cnew ; abs(Fi(:,k) - Fi(:,k+1))-b*ones(7,1)]; end
La funzione ritorna il valore della funzione di costo ed il vettore contenente i vincoli non lineari.
Interior point
La funzione fmincon di MATLAB viene eseguita con algoritmo interior point dato che questo viene caldamente consigliato in quanto più rapido e preciso per un ampio range di problemi. La spiegazione sul funzionamento è presa dalla guida di MATLAB e riportata di seguito.
Dato il problema di minimizzazione vincolata:
( ) ( ) ( )
Il metodo dell’interior-point risolve per ogni una sequenza di problemi di minimizzazione approssimati, del tipo:
( ) ( ) ∑ ( ) ( ) ( )
Con tante variabili quanti i vincoli di disuguaglianza ( ). Gli devono essere positivi per mantenere ( ) limitato. All’avvicinarsi di a zero il minimo di si avvicina al minimo di . Il termine logaritmico è chiamato barrier function. In pratica si và a risolvere un problema di minimo che ha per vincoli delle uguaglianze dato che è più semplice rispetto a risolverne uno che ha delle disuguaglianze.
Ad ogni passo può risolvere il problema in due modi diversi. Il primo si chiama Newton step e tenta di risolvere le equazioni di KKT (Karush-Kuhn-Tucker) attraverso una approssimazione lineare. Nel caso in cui questo sistema fallisca (ad esempio per problema non localmente convesso vicino all’attuale iterazione) si esegue un passo di gradiente coniugato. Ad ogni iterazione l’algoritmo decrementa una funzione di merito del tipo:
( ) ‖( ( ) ( ) )‖
Il parametro può incrementare col numero di iterazioni per forzare il problema verso la soluzione. Nel caso in cui un passo non decrementi la funzione di merito, questo viene scartato e si prova un nuovo passo. Nel caso i cui la funzione obiettivo o un vincolo non lineare ritornano un valore complesso o non definito o infinito, l’algoritmo scarta l’iterazione e prova con un passo più corto.
applyControl
Questa funzione prende in ingresso il tempo di campionamento , lo stato attuale ed il tempo attuale, il vettore della variabile di controllo relativo all’intera finestra calcolata dalla SolveOptControl, la risoluzione da usare con ode45 ed i vettori di stato e di controllo della traiettoria di equilibrio. Viene chiamata all’interno di un ciclo, che scorre la finestra di controllo.
Al suo interno dal vettore calcola il vettore di forze/coppie , formula (4)
[Ai,Pi,Ki] = unpk_thr_table(ThrusterTable); [tau] = vfides_iallocation_map(Ai, Pi, Ki, Fi);
Fà evolvere il sistema per il tempo
[t_intermediate,q_intermediate] = ode45(@sistema, [t0, t0+T], q0 , options , tau , qs_eq,us_eq,f);
qapplied = q_intermediate(size(q_intermediate,1),:); tapplied = t0+T;
In pratica quindi applica al sistema ̃̇ di formula (8) la finestra di controllo calcolata dalla SolveOptControl. In questo modo il sistema evolve, la finestra di predizione e controllo scorre in avanti e si può inizializzare la SolveOptControl con il nuovo vettore di stato. Queste due funzioni si alternano fino a quando l’errore è abbastanza piccolo da soddisfare il criterio di stop.
Libreria delle manovre
La libreria è contenuta nei file turn_right.mat, climb.mat, desced.mat, tclimb.mat, tdescend.mat i quali contengono rispettivamente le strutture Virata, Climb, Descend, TClimb, Tdescend. La T sta per turned ad indicare che salgono/scendono virando, cioè contengono le spirali, mentre Climb e Descend contengono le rampe.
Implementazione
Per quanto riguarda l’implentazione pratica delle funzioni che generano la libreria di manovre sono state utilizzati 3 file. Che ora verranno illustrati partendo dal caso di manovre di ingresso/uscita dalla virata. Per quanto riguarda le manovre per rampe di salita/discesa e spirali di salita/discesa, vengono usati altri file analoghi a quelli per la virata, ma con diversi parametri e criteri di arresto dell’algoritmo MPC.
Crea_libreria_virata
Questo script serve ad assegnare i valori che verranno utilizzati per inizializzare i vettori di formula (12-13).
qs_ini = [1.5,0,0 , 0,0,0 , 0,0,0 , 0,0,0];
vel_B_eq = [1.5 0 0];
omega_E_eq = [deg2rad(0) , deg2rad(0) , deg2rad(0.2)]; pos_E_eq = [0 0 0];
assetto_E_eq = [deg2rad(0) , deg2rad(0) , deg2rad(0)];
Imposta la lunghezza della finestra di predizione e di controllo con tempo di campionamento . A questo punto assegna i valori alle matrici di peso e , che ad esempio nel caso di ⁄ , valgono:
Q = diag([10,10,1 ,1,2,0.4 ,0,0,5 ,1.5,10,0])*100000;
Una volta selezionati i precedenti parametri, lancia lo script MPC_Guide_virata.m (spiegato di seguito) il quale restituisce il vettore contenente i valori di controllo dell’intera simulazione.
run('...\MPC_Guide_virata.m')
Il vettore , viene usato dalla applica_u_alSistema.m per calcolare l’intera evoluzione del sistema di formula (2).
run('...\applica_u_alSistema.m')
Si ottiene così l’evoluzione dello stato durante la manovra di ingresso in curva. A questo punto procede al salvataggio dei dati nella libreria, file turn_right.m che, data la simmetria del veicolo, contiene solo virate verso destra.
clear Virata i = 1; if (exist('turn_right.mat','file') == 2) load('turn_right.mat', 'Virata'); i = size(Virata,2) +1; end Virata(i).q_start = q_sist.'; Virata(i).Fi_start = Fi; Virata(i).Fi_start_eq = Fi_eq; Virata(i).T = T; Virata(i).q_ini_start = qs_ini.'; Virata(i).u_trim_start = us_eq; Virata(i).spd_trim_start = vel_B_eq; Virata(i).w_trim_start = omega_E_eq; Virata(i).assetto_trim_start = assetto_E_eq; Virata(i).pos_trim_start = pos_E_eq; save('turn_right.mat', 'Virata'); con
- q_start evoluzione dello stato durante la manovra di ingresso in curva - Fi_start vettore del controllo durante la manovra di ingresso in curva - Fi_eq vettore del controllo durante il trim
- T il tempo di campionamento
- q_ini_start vettore di stato del trim precedente alla manovra di ingresso in curva - u_trim_start vettore delle forze/coppie da mantenere durante il trim, ovvero
- spd_trim_start vettore contenente le velocità lineari da mantenere durante il trim di virata
- assetto_trim_start vettore contenente gli angoli di assetto da mantenere durante il trim virata
- pos_trim_start vettore contenente la posizione da mantenere durante il trim virata (ovvero ).
Dopo il salvataggio ripete le solite operazioni precedenti per generare la manovra di uscita dalla curva ed esegue un nuovo salvataggio:
Virata(i).q_end = q_sist.'; Virata(i).q_ini_end = qs_ini.'; Virata(i).u_trim_end = us_eq; Virata(i).spd_trim_end = vel_B_eq; Virata(i).w_trim_end = omega_E_eq; Virata(i).assetto_trim_end = assetto_E_eq; Virata(i).pos_trim_end = pos_E_eq; save('turn_right.mat', 'Virata');
dove chiaramente il trim è il rettilineo successivo alla virata. Vengono ora illustrati i due script MPC_Guide_virata e applica_u_alSistema.
MPC_Guide_virata
Questo script utilizza i parametri assegnati da Crea_libreria_virata.m per generare tramite algoritmo MPC la sequenza di controllo dell’intera manovra di uscita/ingresso dalla curva. Per prima cosa crea la matrice di rotazione necessaria a portare la da assi Earth, ad assi Body richiesti dal modello.
[ ( ) ( ) ( ) ( ) ( ) ( ) ( )⁄ ( ) ( )⁄ ] ( 58 )
J2 = angles_to_angle_trans( assetto_E_eq(2) , assetto_E_eq(1) ); omega_B_eq = (J2\omega_E_eq.').';
Cosi può ottenere e comporre il di formula (12), necessario per passare al sistema ̃̇ con equilibrio nell’origine.
qs_eq = [vel_B_eq omega_B_eq pos_E_eq assetto_E_eq]; qmeasure = qs_ini - qs_eq;
Per completare l’operazione di spostamento dell’equilibrio nell’origine, la funzione
[ us_eq f ] = q_u_f_eq_virata( qs_eq ); us_eq = double(us_eq);
f = double(f);
Tramite la funzione tauFitau viene infine trasformato in vettore di percentuali di forza dei 7 thruster.
[ tau Fi_eq] = tauFitau( us_eq );
Non resta a questo punto che impostare la risoluzione che la computeIntegral dovrà usare con ode45.
atol_ode = 1e-3; rtol_ode = 1e-3;
options = odeset('AbsTol', atol_ode, 'RelTol', rtol_ode);
Fatto ciò, può entrare nel loop che chiamando le funzioni SolveOptControl e applyControl implementa l’algoritmo MPC. Una volta terminata la simulazione, come già detto, si rende disponibile il vettore contenente i valori di controllo dell’intera manovra.
applica_u_alSistema
Questo script si limita a far evolvere il sistema, partendo dalle condizioni iniziali
q_sist(1,:) = qs_ini;
sottoponendogli tramite ode45, l’intera sequenza di controllo a gradoni , ottenuta dallo script precedente.
for k = 1 : size(u,2)
[t_intermediate,q_intermediate] = ode45(@sistema, [0,T], q_sist(k,:) , options , (u(:,k) + us_eq));
q_sist(k+1,:) = q_intermediate(size(q_intermediate,1),:); end
Composizione del percorso
Implementazione
Lo script Genera_percorso.m implementa il blocchetto “Generatore Percorso” di Figura 11. Inizialmente carica le librerie tramite la funzione load.
load('turn_right.mat'); % libreria manovre di virata load('climb.mat'); % libreria rampe in salita
load('descend.mat'); % libreria rampe in discesa load('tclimb.mat'); % libreria spirali in salita load('tdescend.mat'); % libreria spirali in discesa
poi inizializza il percorso suddiviso in due vettori, il primo contenente posizioni ed angoli, mentre il secondo contiene velocità lineari ed angolari.
trj(:,1) = [start(1:3);0;0;0]; % [x;y;z;roll;pitch;yaw] trj_spd(:,1) = [2;0;0;0;0;0]; % [vx;vy;vz;wx;wy;wz]
con start posizione iniziale. Questi dati ed i way-point vengono passati alle funzioni esegui_virata e chiudi_trj di seguito illustrate.
esegui_virata
Questa funzione prende in ingresso il percorso attuale trj e trj_spd, il way-point da cui passare e quello verso cui puntare wp e la libreria di manovre Virata, Climb, Descend, TClimb,
TDescend. Chiamando le funzioni illustrate di seguito, genera il percorso fino ad arrivare al punto di Figura 21, ovvero il punto in cui il veicolo si trova orientato verso il traguardo. Restituisce il percorso fino a questo punto inserendo in trj posizioni ed assetto, mentre in
trj_spd le velocità lineari ed angolari. Ecco ora una spiegazione più dettagliata delle funzioni
utilizzate dalla esegui_virata.
TrackAngle
Questa funzione prende in ingresso la posizione attuale start, il way-point da cui passare wp1
ed il way-point “traguardo” verso cui puntare wp2.
̅̅̅̅̅̅̅̅̅ ‖⁄ ̅̅̅̅̅̅̅̅̅‖
̅̅̅̅̅̅̅̅̅ ‖⁄ ̅̅̅̅̅̅̅̅̅‖
( 59 )
Ovvero:
t = (wp1(1:3) - start(1:3)) / norm(wp1(1:3) - start(1:3)); t = [t,(wp2(1:3) - wp1(1:3)) / norm(wp2(1:3) - wp1(1:3))];
Una volta noti i due versori è possibile calcolare l’orientazione di ovvero l’attuale orientazione del veicolo, ( ) e l’angolo compreso fra e ovvero l’angolo di virata ( ). ( ) ( ) ( 60 ) Ovvero: dChi(1) = atan2(t(2,1),t(1,1)); dChi(2) = atan2(t(2,2),t(1,2))-dChi(1);
La funzione restituisce questi due angoli.
calcola_t_trim
Questa funzione prende in ingresso (dChi) e la libreria delle virate (Virata). Scorre l’intera libreria e per ogni coppia di manovre legge l’ultimo valore dell’angolo di che coincide con e di Figura 21.
dChi_ij = (Virata(i).q_start(12,size(Virata(i).q_start,2))); dChi_jk = (Virata(i).q_end(12,size(Virata(i).q_end,2)));
A questo punto, per ogni coppia di manovre calcola attraverso formula (24) i tempi di mantenimento del trim , mettendo in t_trim quelli delle virate dirette, in t_trim_fix quelli delle virate con heading fix ed infine in t_trim_inv quelli delle virate che eseguono un loop in
( 61 ) Ovvero:
t_trim(i) = abs( (dChi(2)-( dChi_ij + dChi_jk)) / Virata(i).w_trim_start(3)); t_trim_fix(i) = abs( (dChi(2)) / Virata(i).w_trim_start(3));
t_trim_inv(i) = abs( (2*pi - abs(dChi(2) - ( dChi_ij + dChi_jk))) / Virata(i).w_trim_start(3) );
Dato che la libreria contiene solo virate verso destra, questa operazione viene fatta scorrendola per due volte. La seconda considerando le manovre speculari rispetto al piano ovvero verso sinistra.
La funzione restituisce i 3 vettori dei tempi di mantenimento di tutti i possibili trim (t_trim, t_trim_inv, t_trim_fix).
punti_di innesto e punti di_innesto_fix
Questa funzione prende in ingresso posizione attuale (start), il way-point (wp1), traguardo (wp2) , l’orientazione (dChi) il vettore tempi di mantenimento dei trim (t_trim) e la libreria delle virate (Virata). Viene chiamata due volte, la prima passandogli
t_trim e la seconda t_trim_inv. Per quanto riguarda le virata con heading fix viene usata una variante di questa funzione che tiene conto delle due manovre di pre-virata – e di