Robustified Time-optimal Point-to-point Motion Planning and Control under Uncertainty

TL;DR

The paper tackles time-optimal point-to-point motion planning under process uncertainty by formulating a robust two-stage OCP that couples Stage 1 (fixed grid) and Stage 2 (time-scaled grid). Stage 1 optimizes a nominal trajectory, feedback gains, and state covariances to minimize uncertainty, while Stage 2 minimizes total time using safety margins derived from Stage 1. A tailored iterative solver splits the problem into a Riccati recursion for gains and a nominal time-optimal OCP with updated margins, enabling real-time execution. Timely replanning is achieved through an asynchronous NMPC (ASAP-MPC) loop that continuously stitches new Stage 1 plans into ongoing control, demonstrated on a unicycle model with obstacle avoidance. The numerical example shows comparable motion times to a single planning approach but with markedly reduced computation time, indicating practical applicability for real-time AMR and time-critical tasks.

Abstract

This paper proposes a novel approach to formulate time-optimal point-to-point motion planning and control under uncertainty. The approach defines a robustified two-stage Optimal Control Problem (OCP), in which stage 1, with a fixed time grid, is seamlessly stitched with stage 2, which features a variable time grid. Stage 1 optimizes not only the nominal trajectory, but also feedback gains and corresponding state covariances, which robustify constraints in both stages. The outcome is a minimized uncertainty in stage 1 and a minimized total motion time for stage 2, both contributing to the time optimality and safety of the total motion. A timely replanning strategy is employed to handle changes in constraints and maintain feasibility, while a tailored iterative algorithm is proposed for efficient, real-time OCP execution.

Figures (4)

• Figure 1: Point-to-point nominal trajectory and the corresponding closed-loop (blue ellipses)/open-loop (red ellipses) uncertainty. Top: single planning. Bottom: timely replanning.
• Figure 2: Nominal controls: from top to bottom, linear velocity from single planning and timely replanning, and angular velocity from single planning and timely replanning, respectively. Red lines denote the executed nominal control trajectory, blue lines indicate control limits for the nominal controls considering safety margins, and gray lines represent the remaining planned nominal control trajectory of each replanning.
• Figure 3: Safety margins on the collision avoidance constraint.
• Figure 4: Computation time and iteration number of each replanning.