Time-optimal Point-to-point Motion Planning: A Two-stage Approach

TL;DR

The paper addresses the challenge of time-optimal point-to-point motion planning for nonlinear systems under NMPC. It introduces a two-stage OCP that first uses a fixed-time-grid stage and then a time-scaled second stage, stitched together to minimize total travel time with robustness to computation delays. The ASAP-MPC integration enables online replanning despite varying solver times, demonstrated on a unicycle model navigating around an elliptical obstacle with collision constraints. Results show the two-stage method achieves comparable trajectory times to direct time-scaling while reducing computational load and ensuring feasibility, highlighting its potential for real-time autonomous navigation.

Abstract

This paper proposes a two-stage approach to formulate the time-optimal point-to-point motion planning problem, involving a first stage with a fixed time grid and a second stage with a variable time grid. The proposed approach brings benefits through its straightforward optimal control problem formulation with a fixed and low number of control steps for manageable computational complexity and the avoidance of interpolation errors associated with time scaling, especially when aiming to reach a distant goal. Additionally, an asynchronous nonlinear model predictive control (NMPC) update scheme is integrated with this two-stage approach to address delayed and fluctuating computation times, facilitating online replanning. The effectiveness of the proposed two-stage approach and NMPC implementation is demonstrated through numerical examples centered on autonomous navigation with collision avoidance.

Figures (5)

• Figure 1: $x-y$ position trajectories, obtained by the three approaches.
• Figure 2: Compare the satisfaction of collision avoidance constraint. It is considered feasible when the value of the collision avoidance constraint does not exceed zero.
• Figure 3: Four instances of the NMPC example. The red cross, the blue star, the black line, the red and the blue dot lines denote the initial and desired terminal positions, the executed trajectory, the remaining stage 1 trajectory, and the stage 2 trajectory, respectively.
• Figure 4: The control inputs of stage 1 obtained from the 56th replanning, with black dashed lines denoting the control limits.
• Figure 5: Number of control sampling time $t_s$ spent for each solve. The black dashed line denotes the upper bound.