Dynamic Objective MPC for Motion Planning of Seamless Docking Maneuvers

TL;DR

The paper addresses precise docking and pose-reaching in narrow environments, where traditional two-stage planning can yield suboptimal trajectories. It introduces dynamic weight allocation and dynamic objective allocation within a unified MPC framework that combines MPCC with Cartesian MPC, enabling seamless progression from path-following to goal-reaching whether the goal lies on-path, inside a corridor, or beyond its end. The key contributions are the state-dependent, sigmoid-based modulation of contouring and Frenet weights, plus a mechanism to switch from Frenet-based to Cartesian objectives for states behind the corridor end, all evaluated across simulations and a CARLA docking scenario. The results show shorter mission times and smoother trajectories compared to baselines, with demonstrated feasibility for docking maneuvers in realistic settings, indicating practical impact for automated docking and constrained motion planning.

Abstract

Automated vehicles and logistics robots must often position themselves in narrow environments with high precision in front of a specific target, such as a package or their charging station. Often, these docking scenarios are solved in two steps: path following and rough positioning followed by a high-precision motion planning algorithm. This can generate suboptimal trajectories caused by bad positioning in the first phase and, therefore, prolong the time it takes to reach the goal. In this work, we propose a unified approach, which is based on a Model Predictive Control (MPC) that unifies the advantages of Model Predictive Contouring Control (MPCC) with a Cartesian MPC to reach a specific goal pose. The paper's main contributions are the adaption of the dynamic weight allocation method to reach path ends and goal poses inside driving corridors, and the development of the so-called dynamic objective MPC. The latter is an improvement of the dynamic weight allocation method, which can inherently switch state-dependent from an MPCC to a Cartesian MPC to solve the path-following problem and the high-precision positioning tasks independently of the location of the goal pose seamlessly by one algorithm. This leads to foresighted, feasible, and safe motion plans, which can decrease the mission time and result in smoother trajectories.

Figures (12)

• Figure 1: Overview of the proposed methods. 1. Precise positioning at switching points by dynamic weight allocation while being able to smooth the rest of the trajectory; 2. Seamless and safe planning to goal poses by dynamic objective allocation while respecting corridor constraints.
• Figure 2: Visualization of the stated problem. Each trajectory state $\boldsymbol{x}_k$ has a reference pose $p_k^\text{r}$ perfectly orthogonally at the reference path. This provides valid frenet coordinates. However, with standard MPCC, the goal pose behind the corridor cannot be reached without switching to another motion planning algorithm, as there is no valid pairing of a state and a reference pose for $\theta > \theta^\text{e}$.
• Figure 3: Schematic visualization of the dynamic weight allocation method to precisely reach the path end at $\theta^\text{e}$: The sigmoid function $\sigma(\theta)$ blends the weights $q^\text{c}$ into $q^\text{c,e}$ for trajectory states approaching the path end at $\theta=30$. This causes an increasing penalty of $e^\text{c}$, which leads to the blue trajectory in Frenet coordinates with minimized $e^\text{c}$ towards the path end.
• Figure 4: Schematic visualization of the dynamic weight allocation to reach a goal pose inside the corridor: The weights $\gamma^\text{eff}$ and $q^\text{c,eff}$ of states close to the projected longitudinal goal position $\theta^\text{g}$ are blended to zero. Now, the penalty $\boldsymbol{q}_N^\text{eff}$ to reach the goal pose dominates the behavior of the MPC. Hence, the algorithm plans precisely to the goal pose while being able to deviate from the reference path which is shown by the blue trajectory in Cartesian coordinates.
• Figure 5: Schematic visualization of the dynamic objective allocation: The purple curve at the top visualizes the drop of $q^\text{l, eff}$ caused by the case distinction from Eq. \ref{['eq:case_dist']}. The other weights are equally blended as in Fig.\ref{['fig:method:dyn_weight_goal']} In the lower part, the trajectory of a vehicle maneuvering to a goal pose is shown. For every state $\boldsymbol{x}$ with a $\theta \le \theta^\text{e}$, a corresponding reference exists on the reference path. For states behind the corridor with $\theta < \theta^\text{e}$, $q^\text{l, eff}$ is set to zero, which allows them to have no valid reference.