Pianificazione traiettoria di un drone

[dimaios]

Infatti xyz hai perfettamente ragione, e' per questo che ho fatto questa domanda senza entrare nel merito della rappresentazione equivalente.

Come la mettiamo con gli angoli di Eulero o rappresentazione equivalente ?

I quaternioni sono molto impiegati nella rappresentazione delle traiettorie satellitari e hanno dei vantaggi notevoli ...... Hamilton era un grande ed anche in questa occasione non si smentisce.

[sedetiam]

... Hamilton era un grande ed anche in questa occasione non si smentisce.

tratto da Wikipedia:

"Hamilton mostrò il suo immenso talento in età precoce, come notò John Breinkley, astronomo e vescovo di Cloyne, nel 1823 quando Hamilton aveva diciotto anni: “Questo giovane, non dico sarà, ma è, il matematico migliore della sua età."
Il genio di Hamilton si fece notare inizialmente nella capacità di apprendere le lingue. All'età dei sette anni aveva fatto progressi considerabili nella conoscenza dell'ebraico, e prima dei tredici anni aveva imparato, sotto l'istruzione dello zio linguista, molte lingue. Fra queste, oltre alle lingue classiche europee e le lingue romanze, il persiano, l'arabo, l'hindi, il sanscrito, e perfino il malese."

........

[spud]

Uhm ritornando in topoc..
Grazie per la segnalazione xyz, il fatto è che quello che interessa a me è ricavare la posizione in termini delle coordinate x,y,z istante per istante. Sull'orientazione io non ho controllo, se la regola lui in base al movimento che deve fare e una volta finito si riallinea con il suo riferimento per cui azzera gli angoli (a parte l'imbardata su cui ho controllo).
L'orientazione non mi influisce sulla posizione istantanea, se è girato in un certo modo o in un altro il punto in cui è non cambia, ecco perché non capisco la necessità di modellare questo comportamento ai fini del solo inseguimento della traiettoria.

[dimaios]

L'orientazione non mi influisce sulla posizione istantanea, se è girato in un certo modo o in un altro il punto in cui è non cambia, ecco perché non capisco la necessità di modellare questo comportamento ai fini del solo inseguimento della traiettoria.

La dinamica del sistema è rappresentata da una matrice delle inerzie non uno scalare per cui la postura influenza la dinamica della traiettoria per definizione.

[spud]

Ok io dicevo così perché il nostro controllo è più ad alto livello del controllo della velocità della singola elica, per questo non mettiamo dentro il modello completo con matrici di inerzia ecc..

Comunque vediamo cosa viene fuori, posterò i risultati delle prove.

[spud]

Rieccomi, dunque abbiamo parlato un po' col prof e abbiamo tirato fuori un algoritmo per inseguire i punti. Considerando questa situazione:

Sorgente FidoCadJ | Ingrandisci | Cos'è Fidocad

si ha che p(i-1) e p(i) sono due punti consecutivi contenuti in un vettore di punti da inseguire, p è la posizione attuale che non fa parte di questo vettore di punti, le coordinate posx e posy sono calcolate integrando le velocità lette dal drone, considerando che le letture arrivano con frequenza :








e sono le velocità lette dal drone non quelle impostate da noi.
Istante per istante calcoliamo la differenza tra la coordinata attuale e quella desiderata cacendo:




so che sto facendo un po' di confusione, con l'indice i intendo l'elemento dei vettori di punti x e y da inseguire nello spazio operativo, con t intendo la posizione istantanea del drone.

Ora sia la distanza tra due punti consecutivi da raggiungere, la velocità massima raggiungibile dal drone in entrambe le direzioni, possiamo variare la velocità da 0 a 1 ma impostiamo quella massima a 0.7 per evitare accelerazioni eccessive (lo destabilizzano tantissimo).

Usiamo queste informazioni per devinire le velocità in e del drone.





in questo modo la velocità x è massima all'inizio e cala man mano che ci si avvicina al punto, la velocità y varia allo stesso modo.

Ci sono un po di aggiustamenti dovuti ai segni delle posizioni ma il succo è questo, che dite?

Consideriamo raggiunto il punto se posx e posy si trovano all'interno di un cerchio di raggio r che sarà la nostra precisione.

[spud]

Ho provato a fare una simulazione con due soli punti, il punto iniziale e il punto da raggiungere







con matlab ho generato i vettori posx e posy e poi mi sono calcolato l'andamento delle velocità man mano che ci si avvicina al punto desiderato, il codice che ho usato è questo:

Codice: Seleziona tutto

x = 2;
xi = 10;
y = 2;
yi = 5;

posx = x:0.1:xi;
posy = y:0.1:yi;

Dx = -(posx)+xi;
Dy = -(posy)+yi;

vmax = 0.7;
vx = vmax * (Dx / rho);
vy = vmax * (Dy / rho);


poi ho fatto la stampa di vx e vy in funzione rispettivamente di posx e posx, il risultato è questo:

velocita_x.jpg (53.17 KiB) Osservato 3795 volte

velocita_y.jpg (43.86 KiB) Osservato 3795 volte

Si nota che le velocità iniziali sono alte perché il punto è lontano, poi tendono a zero man mano che ci si avvicina alla destinazione.

Una volta raggiunta (a meno della tolleranza) incrementiamo i e passiamo all'inseguimento del punto sucessivo.
Il nuovo viene calcolata come la distanza tra il punto attuale (non si sarà precisamente sul punto desiderato) e il punto sucessivo da raggiungere.

Però pensavo che in questo modo tra un punto e l'altro rischio di avere dei picchi di jerk, pensavo di arrotondare un po' l'andamento in prossimità della partenza e dell arrivo, come potrei fare?

[dimaios]

Interpolando con una spline 3D o 2D a seconda del tipo di traiettoria e minimizzando il jerk con un qualsiasi algoritmo partendo da una interpolazione iniziale che vincola solo il passaggio per i punti. Se conosci in amticipo l'orizzonte del target non devi pianificate utilizzando coppie di punti consecutivi ma devi considerare l'ottimizzatore globale vincolato esteso a tutta la traiettoria.

[spud]

Qualche prova di raggiungimento dei punti, in questa prova gli facciamo raggiungere una sequenza di punti scelta da noi, in particolare percorre i lati di un quadrato (di lunghezza circa due metri) e le diagonali. Questa è la sequenza:

Sorgente FidoCadJ | Ingrandisci | Cos'è Fidocad

si vede che ogni tanto va fuori traiettoria (ad esempio l'ultima diagonale la fa un po curva) ma poi riesce a trovare i punti (abbiamo un raggio di tolleranza di 30 cm), quando facciamo traiettorie con punti intermeti l'andamento è molto piu pulito, posterò il video appena è pronto.
Si nota anche che torna abbastanza vicino al punto di partenza quindi questo ci conferma che il nostro algoritmo per il calcolo della posizione è abbastanza accurato.

In seguito c'è il video, ci vuole un po di fantasia per capire il percorso ma più o meno si intuisce

[spud]

Qua si vede un po' di più il controllo, insegue i vertici di un quadrato ma cerca di mantenere una traiettoria il più possibile rettilinea tra un vertice e l'altro; durante il pen'ultimo lato esce di parecchio (motivi sconosciuti) ma poi riesce a ritrovare la strada. I lati sono lunghi, circa 4 metri, per testare la stabilità dell'algoritmo che abbiamo implementato.