Clothoid-SLAM : simultaneous localization and mapping of line markings for autonomous driving HD maps

In autonomous driving, perception of the surroundings and vehicle localization, with high precision and accuracy, are prerequisites for the vehicle trajectory planning in safe conditions. Moreover, to accurately describe the environment around an autonomous vehicle, possible static road features, e.g., lanes, line markings, or traffic signs, are detected and integrated into a High Definition (HD) map, i.e., a map with centimeter-level accuracy employed for the vehicle localization task. Line-markings, which define lanes boundaries, are geometric road features, relevant for precise and accurate vehicle localization. The geometric modeling of the line markings is preferable, in terms of storage efficiency, compared with other methods to cache maps, e.g., point clouds. In literature, generic mathematical curves are employed to this purpose, e.g., polylines, polynomials, and splines. However, they are not consistent with the lane design, which utilizes clothoids, introducing an approximation of the real-world and possible numerical inconsistencies. Clothoids are mathematical curves also used for path planning due to the linear curvature change property, which allows the vehicle to follow smooth trajectories, suitable to reach better comfort of the passengers. To have consistency with the lane design and the control strategies of the model utilized, we construct a lane-level HD map using clothoids, i.e., we define an HD map modeling the line-markings with consecutive, continuous, and differentiable clothoids. In this way, we achieve both high precision and accuracy of the line-markings analytical description, along with the consistency between the used mathematical model and the real-world case. We construct the HD map in two distinct scenarios. In the first case, we build the HD map, under the assumption that the position of the vehicle gathering data is always known by means of high-resolution sensors. Through a Convolutional Neural Network (CNN), from images taken by an RGB camera mounted on the vehicle, we extract sets of 2D points representing the line-markings. We use these points to create a set of consecutive clothoids, each approximately 10 meters long, with continuity and derivability properties at their joining points. We merge the clothoids with similar parameters, under some constraints, to simplify the line-markings model, and we optimize the clothoids parameters using the $g^{2}o$ framework to simultaneously achieve fitting of the line marking points and continuity and derivability between consecutive clothoids. Lastly, with a greedy algorithm, we further remove unnecessary clothoids. In this scenario, after its complete creation, the HD map is employed for the vehicle localization task when the poses of the vehicle estimated by the available sensors are not precise. In the second scenario, we assume that the estimates of the vehicle positions are affected by uncertainty, and the environment has never been explored or is unmapped. In this case, we exploit a Simultaneous Localization and Mapping (SLAM) formulation for real-time HD map creation and simultaneous vehicle localization. Through a low-accuracy and low-frequency GPS, and available odometry, we propose a graph-based Clothoid-SLAM system, in which we model with nodes each GPS vehicle position, along with the corresponding line-markings clothoids, extracted similarly to the previous case. The associated constraints are new graph SLAM edges we use to perform graph-based optimization of the parameters of the nodes. The optimized configuration results in an improved estimated trajectory of the vehicle and a more accurate HD map, both estimated online. Lastly, in an offline setup, we merge the HD map clothoids that satisfy some constraints, to streamline the line-markings model. We validate our work and measure its performance with experimental campaigns, using the real-world datasets, which we make publicly available for further research at \url{https://airlab.deib.polimi.it/datasets-and-tools/}.

Nell’ambito della guida autonoma, la percezione dell’ambiente circostante e la localizzazione del veicolo, con alta precisione e accuratezza, sono prerequisiti per la pianificazione della traiettoria in condizioni di sicurezza. Per descrivere in maniera accurata l’ambiente circostante un veicolo a guida autonoma, caratteristiche statiche della strada, quali, ad esempio, le corsie di marcia, le linee stradali o i cartelli segnaletici, sono individuati e inseriti in una mappa ad alta definizione (High Definition, HD), ovvero una mappa accurata al centimetro utilizzata per la localizzazione del veicolo. Le linee stradali che definiscono i limiti laterali delle corsie di marcia rappresentano caratteristiche geometriche della strada rilevanti per una precisa e accurata localizzazione del veicolo. L’impiego di modelli geometrici per la definizione delle linee stradali è infatti più conveniente in termini di efficienza di archiviazione se confrontato con altri metodi di salvataggio della mappa come, ad esempio, le nuvole di punti. In letteratura, vengono però utilizzate curve matematiche generiche, quali, ad esempio, le polilinee, i polinomi e le splines. Tuttavia, queste non sono coerenti con la progettazione delle corsie di marcia che prevede l’utilizzo delle clotoidi, introducendo così un’approssimazione del caso reale con incongruenze numeriche. Le clotoidi sono curve matematiche utilizzate anche per la pianificazione del percorso grazie alla proprietà della variazione lineare della curvatura lungo la loro lunghezza che permette al veicolo di percorrere delle traiettorie regolari adatte per il raggiungimento di un comfort migliore dei passeggeri. Per avere coerenza con la progettazione delle corsie di marcia e il modello impiegato nelle strategie di controllo, in questa tesi proponiamo un algoritmo per costruire una mappa HD, al livello delle corsie di marcia, utilizzando le clotoidi, ovvero definiamo una mappa HD modellando le linee stradali con clotoidi consecutive, continue e derivabili. In questo modo, otteniamo una descrizione analitica delle linee stradali ad alta precisione e accuratezza, mantenendo al contempo la coerenza tra il modello ottenuto e il caso reale. Il nostro algoritmo affronta due scenari distinti. Nel primo caso, creiamo la mappa HD tramite un’acquisizione preliminare dei dati che rappresentano l’ambiente sotto l’assunzione che la posizione del veicolo che raccoglie i dati sia sempre nota grazie a sensori ad alta precisione. Con una rete convoluzionale (Convolutional Neural Network, CNN), dalle immagini ottenute da una camera RGB montata sul veicolo, estraiamo degli insiemi di punti 2D che rappresentano le linee stradali. Utilizziamo questi punti per creare un insieme di clotoidi consecutive, ognuna lunga approssimativamente 10 metri, che presentano continuità e derivabilità nei punti di giunzione. Uniamo le clotoidi che presentano parametri simili, sotto certi vincoli, per semplificare il modello delle linee stradali. In seguito, ottimizziamo i parametri delle clotoidi utilizzando il framework $g^{2}o$ per ottenere contemporaneamente il fitting dei punti delle linee stradali e la continuità e derivabilità tra le clotoidi consecutive. Infine, con un algoritmo greedy, eliminiamo le clotoidi non necessarie. In questo scenario, dopo la sua completa creazione, la mappa HD viene utilizzata per la localizzazione del veicolo quando le posizioni del veicolo stimate dai sensori a disposizione non sono sufficientemente precise. Nel secondo scenario, assumiamo che le stime delle posizioni del veicolo siano affette da incertezza, e che l’ambiente non sia mai stato esplorato o non sia mai stata creata una mappa di esso. Utilizziamo in questo caso la formulazione di localizzazione e mappatura simultanei (Simultaneous Localization and Mapping, SLAM) per la creazione in tempo reale della mappa HD e la localizzazione simultanea del veicolo. Attraverso un GPS a bassa risoluzione e frequenza, e l’odometria a disposizione, proponiamo Clothoid-SLAM, un sistema basato su grafo, in cui modelliamo tramite dei nodi ogni posizione del veicolo ottenuta dalla misura del GPS, e le clotoidi che rappresentano le linee stradali e che vengono estratte in modo simile al caso precedente. I vincoli associati a questi nodi sono nuovi archi che utilizziamo per eseguire l’ottimizzazione basata sul grafo dei parametri dei nodi. La configurazione del grafo risultante dall'ottimizzazione corrisponde a una migliore stima della traiettoria del veicolo e una mappa HD più accurata. Infine, in un setup offline, uniamo le clotoidi della mappa HD che soddisfano certi vincoli, snellendo il modello delle linee stradali. Abbiamo infine validato il nostro lavoro misurandone le prestazioni con risultati sperimentali, utilizzando dei datasets con dati reali che abbiamo reso pubblicamente disponibili per ulteriori ricerche al sito \url{https://airlab.deib.polimi.it/datasets-and-tools/}.