Optimal kinodynamic planning for autonomous vehicles

Motion planning is one of the most important factors affecting the functioning of an autonomous vehicle and aims to find a collision-free motion guiding the vehicle from an initial configuration to a final one. In particular, kinodynamic motion planning addresses this very issue, taking also into account the dynamics of the system so that the resulting solution would be executable by the vehicle. This thesis addresses the problem of optimal motion planning with particular emphasis on developing efficient planners while considering underlying realistic dynamic model. The first contribution is a resolution optimal motion planner, RRT* with Motion Primitives, that samples the state space in a grid representation and generates a tree of trajectories using a database of motion primitives. Therefore, the computationally intensive part of solving for a steering action is carried to the preliminary phase of database generation by alleviating the computational load during planning. Furthermore, it allows to compute the true cost-to-go in order to guide the expansion of the tree, improving the convergence properties. The algorithm is proven to be asymptotically optimal as the grid resolution goes to zero and number of nodes goes to infinity. The (sub)optimality caused by the gridding can be tuned to compromise between the size of the database and this performance degradation. The second part of the thesis focuses on the topological constraints on the kinodynamic motion planning. In particular, elaborates on the decoupled approach of systematically generating homotopy class constraints and imposing these constraints during the planning phase to obtain a set of distinct local optimal trajectories belonging to different homotopy classes. The contributions in that case is twofold: first, an exact optimal planner based on homotopy class constraints and optimal control is proposed that to decompose the overall trajectory generation problem into simpler sub-problems and, second RRT*_MotionPrimitives is enhanced with homotopy awareness that allow generating distinct good trajectories. Finally, the proposed algorithms are applied to a particular problem of landmark aware motion planning. For this, the motion planning problem is reformulated as a multi-objective planning problem, where the robot has to keep its connection with at least one landmark at all times, without explicitly interpolating a set of selected ones.

La pianificazione del moto è uno dei fattori più importanti che influenzano il funzionamento di un veicolo autonomo ed è finalizzata a trovare un movimento privo di collisioni che guidi il veicolo da una configurazione iniziale a una finale. In particolare, la pianificazione del moto con vincoli cinematici e dinamici affronta proprio questo problema, tenendo conto anche della dinamica del sistema in modo che la soluzione risultante possa essere efefttivamente eseguita dal veicolo. Questa tesi affronta il problema della pianificazione del movimento ottimale con particolare attenzione allo sviluppo di pianificatori efficienti, considerando al contempo un modello dinamico realistico del veicolo. Il primo contributo è un pianificatore del moto a risoluzione ottimale, RRT * con Motion Primitives, che campiona lo spazio degli stati utilizzando una rappresentazione a griglia e generando un albero di traiettorie usando un database di primitive di movimento. Pertanto, la parte computazionalmente intensiva della risoluzione di un'azione di guida viene riportata alla fase preliminare della generazione del database, riducendo il carico computazionale durante la pianificazione in linea. Inoltre, l’approccio proposto consente di calcolare il vero “cost-to-go” e migliora la convergenza, guidando l'espansione dell'albero. L'algoritmo risulta essere asintoticamente ottimale al tendere a zero della risoluzione della griglia e al tendere all’infinito del numero di nodi. La (sub) ottimalità causata dalla griglia può essere regolata per ottenere un compromesso tra la dimensione del database ed il peggioramento delle prestazioni. La seconda parte della tesi si concentra sui vincoli topologici nella pianificazione del moto. In particolare, elabora un approccio disaccoppiato che consiste nel generare sistematicamente vincoli di classe di omotopia e nell’imporre questi vincoli durante la fase di pianificazione, per ottenere traiettorie ottimali locali distinte appartenenti a classi di omotopia diverse. Il contributo in questo caso è duplice: in primo luogo, viene proposto un pianificatore ottimale basato su vincoli di classe di omotopia e su un controllo ottimale, che decompone il problema generale della generazione di traiettorie in sottosistemi più semplici e, in secondo luogo, RRT*_MotionPrimitives è potenziato con l’applicazione dell'omotopia, che consente di generare buone traiettorie. Infine, gli algoritmi proposti sono applicati a un particolare problema di pianificazione del moto che tenga conto di specifici punti di riferimento attraverso cui la traiettoria deve passare. Per questo, il problema di pianificazione del moto viene riformulato come un problema di pianificazione multi-obiettivo, in cui il veicolo deve mantenere la sua connessione con almeno un punto di riferimento in ogni momento, senza interpolare esplicitamente un insieme di quelli selezionati.