Constrained path following and dynamic occlusion avoidance for floating base robotic systems

This work formulates a constrained path following sequential-linear quadratic model predictive control (MPC) problem for floating base manipulators. By augmenting the path's arclength parameter within the system states, we can exercise direct control over the path's rate of progression. We exploit this added degree of freedom to regulate progression along a path so as to accommodate for the various dynamic costs and constraints. Through simulations on a Kinova seven degrees of freedom mobile manipulator, we validate our formulation and demonstrate the solver's plasticity in different scenarios. Several settings are studied and presented, namely a nominal case, the case of avoiding collisions, and the case of recovery from wrong initialization. A brief qualitative and quantitative study on the nominal case results is provided. This work also introduces a novel approach for whole-body motion planning and dynamic occlusion avoidance. The proposed approach reformulates the visibility constraint as a likelihood maximization of visibility probability. In this formulation, we augment the primary cost function of a whole-body MPC scheme through a relaxed log barrier function yielding a relaxed log-likelihood maximization formulation of visibility probability. The visibility probability is computed through a probabilistic emph{shadow field} that quantifies point light source occlusions. We provide the necessary algorithms to obtain such a field for both 2D and 3D cases. We demonstrate 2D implementations of this field in simulation and 3D implementations through real-time hardware experiments. We show that due to the linear complexity of our emph{shadow field} algorithm to the map size, we can achieve high update rates, which facilitates onboard execution on mobile platforms with limited computational power. Lastly, we evaluate the performance of the proposed MPC reformulation in simulation for a quadrupedal mobile manipulator, ALMA.

Questo lavoro formula un problema per la pianificazion di traiettorie con controllo a predizione del modello (MPC) di tipo sequenziale lineare quadratico (SLQ) per manipolatori a base fluttuante. Aggiungendo il parametro della curva agli stati del sistema, possiamo esercitare un controllo diretto sulla velocita' di progressione lungo la traiettoria. Sfruttiamo questo grado di libertà aggiuntivo per regolare la progressione lungo un percorso in modo da rispettare i vari costi e vincoli dinamici. Attraverso simulazioni su un manipolatore mobile Kinova a sette gradi di libertà, validiamo la nostra formulazione e dimostriamo la plasticità del risolutore in diversi scenari. Diverse situazioni sono studiate e presentate: una situazione nominale, una situazione per evitare collisioni e una situazione di recupero da un’inizializzazione errata. Viene inoltre fornito un breve studio qualitativo e quantitativo relativo ai risultati del caso nominale. Questo lavoro introduce anche un nuovo approccio per la pianificazione del moto dell’intero corpo (whole-body motion) e la prevenzione di occlusioni dinamiche. L'approccio proposto riformula il vincolo di visibilità come una massimizzazione della probabilità di visibilità. In questa formulazione, aumentiamo la funzione di costo primaria di uno schema MPC per tutto il corpo attraverso una funzione barriera logaritmica rilassata (relaxed log-barrier) che produce una formulazione di massimizzazione di una probabilità logaritmica rilassata della probabilità di visibilità. La probabilità di visibilità è calcolata attraverso un campo d'ombra probabilistico che quantifica le occlusioni di sorgenti luminose puntuali. Forniamo inoltre gli algoritmi necessari per ottenere tale campo per entrambi i casi 2D e 3D. La tesi reporta implementazioni 2D di questo campo con simulazioni, e implementazioni 3D attraverso esperimenti hardware in tempo reale. Mostriamo che grazie al fatto che il nostro algoritmo di campo d'ombra ha una complessita' lineare rispetto alla dimensione della mappa, possiamo raggiungere una elevata frequenza di aggiornamento, il che facilita l'esecuzione a bordo di piattaforme mobili con potenza di calcolo limitata. Infine, valutiamo le prestazioni della riformulazione MPC proposta nella simulazione per un manipolatore mobile quadrupede, ALMA.