Joint Synthesis of Trajectory and Controlled Invariant Funnel for Discrete-time Systems with Locally Lipschitz Nonlinearities

TL;DR

This paper introduces a joint optimization method for trajectory planning and controlled invariant funnel synthesis in nonlinear systems, enhancing feasibility and cost-effectiveness under disturbances.

Contribution

It proposes a recursive scheme that jointly optimizes the nominal trajectory and the invariant funnel, unlike existing methods that treat them separately.

Findings

Improved feasibility of trajectories under disturbances.

Joint optimization yields more cost-effective trajectories.

Validated on robotics applications.

Abstract

This paper presents a joint synthesis algorithm of trajectory and controlled invariant funnel (CIF) for locally Lipschitz nonlinear systems subject to bounded disturbances. The CIF synthesis refers to a procedure of computing controlled invariance sets and corresponding feedback gains. In contrast to existing CIF synthesis methods that compute the CIF with a pre-defined nominal trajectory, our work aims to optimize the nominal trajectory and the CIF jointly to satisfy feasibility conditions without the relaxation of constraints and obtain a more cost-optimal nominal trajectory. The proposed work has a recursive scheme that mainly optimize trajectory update and funnel update. The trajectory update step optimizes the nominal trajectory while ensuring the feasibility of the CIF. Then, the funnel update step computes the funnel around the nominal trajectory so that the CIF guarantees an invariance property. As a result, with the optimized trajectory and CIF, any resulting trajectory propagated from an initial set by the control law with the computed feedback gain remains within the feasible region around the nominal trajectory under the presence of bounded disturbances. We validate the proposed method via two applications from robotics.