Design of a motion planner base on rapidly-exploring random tree (RRT) algorithm with online inertial parameter estimation

In this thesis a control logic for autonomous vehicles is designed. The requirement is to compute and follow a trajectory, minimizing the total time of execution, avoiding other cars, seen as obstacles, and staying in between the road boundaries. The goal is to design an algorithm that can be applied in any road scenario, where the number of vehicles and others obstacles is unknown a-priori. Starting from a goal region given by a high level controller, the planner has to apply a time history of steering angle and braking/driving torque to reach the target, being always in a safe state with the opportunity to stop the car in a controlled way and without hitting anything if any feasible trajectory has been planned in a determined range of time. The controller implemented is based on the Rapidly-exploring Random Tree (RRT) algorithm that has good properties to be applicable in real-time. It is a sampling-based motion planner, so it doesn’t require any additional information other than the state of the vehicle and the point to reach: it processes the information from the high level to find a feasible trajectory and determines the input to apply to the car. The prediction of the trajectory is computed integrating models that can describe the non-holonomic constraints, characteristic of the most ground vehicles. A Single Track Vehicle model (STVM) has been chosen for the planner, while the real car is simulated using a 4-points-of-contact model, which is also used by the Extended Kalman Filter, capable of predict the inertial parameters of the car, that is typically unknown, since it depends on several variables (passengers, loads, tank level and so on). This motion planning approach uses two kinds of controllers: a pure-pursuit controller to compute the steering angle and a PID to generate the torque se- quence in order to maintain the desired speed. The algorithm is implemented in Matlab.

In questa tesi si implementa una logica di controllo per veicoli autonomi che riesca a far seguire all’automobile una traiettoria predetta, minimizzando il tempo di percorrenza, evitando le altre macchine presenti sulla strada e restando entro i limiti della carreggiata. L’obiettivo è di ottenere un algoritmo che possa essere applicato in qualsiasi scenario di traffico, senza una conoscenza a-priori del numero e dello stato degli altri veicoli. Il pianificatore di traiettoria, ricevendo la destinazione dal controllore di alto livello, come può essere ad esempio il GPS, dà come risultato una sequenza di azioni, in termini di coppia e angolo di sterzo, da applicare al veicolo in modo tale che raggiunga la posizione desiderata, rispettando sempre una distanza minima dagli ostacoli presenti e riuscendo comunque a garantire una frenata di emergenza qualora ci fosse una situazione di pericolo o il pianificatore non riuscisse a trovare una traiettoria fattibile entro un determinato intervallo di tempo. Il controllore implementato si basa sull’algoritmo denominato Rapidly-exploring Random Tree (RRT) che ha caratteristiche adeguate per essere applicato ad un sistema reale in real-time. Ad ogni iterazione, il controllore fissa nello spazio di fronte a sé un punto campione che cercherà di collegare ad uno dei nodi presenti nella struttura ad albero, ottenuti dalle iterazioni precedenti, utilizzando un metodo di classificazione per determinare quale di essi sia il nodo più vantaggioso in funzione della distanza che la macchina deve coprire per passare per i due punti. La distanza considerata tra due nodi non è quella euclidea, quindi la lunghezza del segmento che ha come estremi i due punti, ma considera il comportamento non olonomo del veicolo che pone dei vincoli nelle manovre che la macchina può svolgere (si pensi al caso in cui la macchina debba trovarsi in una posizione affiancata e parallela a quella di partenza: il sistema non può semplicemente traslare lateralmente ma deve eseguire numerose manovre; questa è una diretta conseguenza della non olonomia caratteristica dei veicoli su ruote). Se la nuova traiettoria calcolata è sicura, allora i punti appartenenti ad essa, o meglio un numero limitato di punti, sono aggiunti alla struttura ad albero aumentando le probabilità di connessione dei nodi con il campione nelle iterazioni successive. Due modelli di veicoli sono stati considerati all’interno dell’algoritmo: il primo, un modello dinamico a bicicletta, è integrato ad ogni iterazione del controllore per ottenere una previsione della traiettoria che la macchina reale può percorrere; il secondo invece, un modello a quattro punti di contatto che considera il rollio, utilizzato (i) per simulare il comportamento del veicolo che riceve in input i comandi derivanti dal RRT, evidenziandone le differenze dal primo modello, e (ii) per stimare i parametri inerziali attraverso l’Extended Kalman Filter (EKF). Quest’ultimo, integrato al RRT, rappresenta il contributo di questa tesi, evidenziando le migliorie apportate nella previsione della traiettoria nel caso si abbia una conoscenza più precisa dello stato del veicolo. La stima dei parametri inerziali parte dal presupposto che al momento della progettazione del controllore, alcune caratteristiche del veicolo sono teoriche in quanto dipendono da fattori imprevedibili o aleatori, come ad esempio il numero di passeggeri, se sono presenti carichi e quant’altro: è compito del EKF fare una stima di questi parametri riducendo le discrepanze tra il modello software del veicolo e il sistema reale. Per quanto riguarda la determinazione delle azioni di controllo, è stato fatto uso di due tipologie di controllo: un semplice inseguitore di traiettoria per ricostruire l’input dell’angolo di sterzo che prende come riferimento la retta di giunzione tra due nodi, e un controllo PID per cercare di seguire e mantenere la velocità determinata attraverso una legge di moto trapezoidale. Uno studio di diverse casistiche è stato effettuato e qui saranno riportate le più significative in termini di performance, dimostrando il livello di utilizzo a cui si è riusciti a giungere con questo algoritmo e quanto si può spingere in condizioni difficili di traffico. L’implementazione software è stata fatta in ambiente Matlab, a partire dalla logica di controllo fino alla verifica delle prestazioni con delle prove virtuali.