Efficient kinodynamic motion planning with motion primitives library and online smoothing

In recent years, autonomous systems have gained significant importance across a wide range of applications, including aerial mobility and robotics. A core capability enabling autonomy is motion planning, which aims to generate optimal trajectories that move the system from an initial to a final configuration while avoiding obstacles. A major challenge in this process lies in simultaneously ensuring the dynamic feasibility of the trajectory, which requires satisfying nonlinear system dynamics, input constraints, and state constraints, and handling non-convex obstacle avoidance. This thesis addresses the problem of real-time optimal kinodynamic motion planning in cluttered environments. The proposed method adopts a hybrid strategy that combines optimal control, to handle nonlinear dynamics, with graph-based search methods that efficiently explore non-convex spaces. The method consists of two phases. In the offline phase, a library of motion primitives is constructed by solving a set of Two-Point Boundary Value Problems (TPBVPs). This precomputation significantly reduces the computational burden during the online phase, allowing for the efficient computation of nearly optimal trajectories. To ensure computational efficiency, continuity between successive motion primitives is enforced only on a selected subset of state variables, specifically those included in the discretization process during the library construction. In the online phase, a graph search algorithm selects and concatenates motion primitives from the precomputed library to generate a complete, collision-free trajectory. However, discontinuities in the non-discretized state variables may occur at the junctions between primitives. These discontinuities are repaired in real-time through a smoothing phase, which restores full state continuity and significantly improves trajectory quality and dynamic consistency. The effectiveness of the method is demonstrated through simulations and real-world experiments on a quadrotor. Results show that the smoothing phase significantly enhances tracking performance. An analysis on the computational times confirms that the approach is suitable for dynamic environments where fast, reliable, real-time planning is required.

Negli ultimi anni, i sistemi autonomi hanno acquisito un'importanza significativa in una vasta gamma di applicazioni, tra cui la mobilità aerea e la robotica. Una capacità fondamentale che permette l'autonomia è la pianificazione del moto, volta a generare traiettorie ottimali che conducano il sistema da una configurazione iniziale a una finale evitando gli ostacoli. Una sfida cruciale in questo processo consiste nel garantire simultaneamente la fattibilità dinamica della traiettoria, rispettando le dinamiche non lineari, i vincoli sugli input e sugli stati, e l’evitamento di ostacoli. Questa tesi affronta il problema della pianificazione ottimale del moto cinematico e dinamico in tempo reale in ambienti ricchi di ostacoli. Il metodo proposto adotta una strategia ibrida che combina il controllo ottimo, per gestire dinamiche non lineari, con metodi di ricerca basati su grafi, per esplorare efficientemente spazi non convessi. Il metodo si compone di due fasi. Nella fase offline, si costruisce una libreria di primitive di moto risolvendo un insieme di Problemi ai Valori al Contorno in Due Punti, riducendo così significativamente il carico computazionale nella fase online. Per garantire l’efficienza computazionale, la continuità tra primitive di moto successive viene imposta solo su un sottoinsieme di variabili di stato, in particolare quelle incluse nel processo di discretizzazione durante la costruzione della libreria. Nella fase online, un algoritmo su grafo concatena le primitive dalla libreria per generare una traiettoria completa e senza collisioni. Tuttavia, possono verificarsi discontinuità nelle variabili di stato non discretizzate nei punti di giunzione tra le primitive. Queste discontinuità vengono corrette in tempo reale attraverso una fase di smoothing, che ripristina la continuità completa delle variabili di stato e migliora significativamente la qualità e la coerenza dinamica della traiettoria. L’efficacia del metodo è dimostrata tramite simulazioni ed esperimenti reali condotti su un quadricottero, evidenziando miglioramenti nelle prestazioni di tracking. Un’analisi dei tempi computazionali conferma che l’approccio è adatto ad ambienti dinamici in cui è richiesta una pianificazione rapida, affidabile e in tempo reale.