Progressive Smoothing for Motion Planning in Real-Time NMPC

TL;DR

The real-time iteration scheme applies to the proposed NMPC formulation, i.e., only one quadratic program needs to be solved at each time step, and different formulations are compared in simulation experiments and shown to successfully improve performance without increasing the computational burden.

Abstract

Nonlinear model predictive control (NMPC) is a popular strategy for solving motion planning problems, including obstacle avoidance constraints, in autonomous driving applications. Non-smooth obstacle shapes, such as rectangles, introduce additional local minima in the underlying optimization problem. Smooth over-approximations, e.g., ellipsoidal shapes, limit the performance due to their conservativeness. We propose to vary the smoothness and the related over-approximation by a homotopy. Instead of varying the smoothness in consecutive sequential quadratic programming iterations, we use formulations that decrease the smooth over-approximation from the end towards the beginning of the prediction horizon. Thus, the real-time iterations algorithm is applicable to the proposed NMPC formulation. Different formulations are compared in simulation experiments and shown to successfully improve performance indicators without increasing the computation time.

Figures (7)

• Figure 1: Progressive smoothing with the proposed ScaledNorm of the obstacle shape along the prediction steps $i\in\{0,N/2,N\}$ for an NMPC prediction of $N$ steps at simulation time $t_\mathrm{sim}=t_0$ (first row) and $t_\mathrm{sim}=t_1$ (second row). The ScaledNorm is smoothest for $i=N$ and tightest for $i=0$. Each plot shows the linearization of the ScaledNorm at the related prediction step.
• Figure 2: Sketch of the considered SV in Frenet coordinates. The occupied spaces for the ego vehicle $\mathcal{O}^\mathrm{ego}{}$ and the SV $\mathcal{O}^\mathrm{sv}{}$ are approximated by rectangles that contains all road-aligned heading angle configurations. By taking the Minkowski sum of both, which results in the inflated exact shape $\mathcal{O}^{\mathrm{sv}*}$, the ego vehicle shape can be considered as point with the position $[p_x, \;p_y]^\top$. The set $\mathcal{O}^\mathrm{ell}{}$ over-approximates the exact rectangular shape by an ellipse.
• Figure 3: Obstacle shape smoothing in normalized coordinates for the set $o(\xi,\alpha)=1$. The associated tightening parameters $\alpha_{\{\cdot\}}$ are smoothed from a square shape (black) to a circle (magenta). The values of $\alpha_{\{\cdot\}}$ are chosen, such that the approximated widths and height are equal, measured at the axes. Notably, the non-convexity of the Boltzmann approximation can be seen for high values of $\alpha_\mathrm{bm}$.
• Figure 4: Linearized constraints $o^{\{\cdot\}}_\mathrm{lin}(\cdot)\geq 1\Leftrightarrow\xi\geq\underline{\xi}^{\{\cdot\}}$ for the generalized coordinates $\xi$ at an linearization point $\tilde{\xi}=[\tilde{\xi}_1,0]^\top$ for the smoothest approximation, i.e., $\alpha_\mathrm{p}=2$ and $\alpha_\mathrm{lse}=\alpha_\mathrm{bm}\approx 0$. Related separating hyperplanes, i.e., $o^{\{\cdot\}}_\mathrm{lin}(\cdot)= 1$ are shown in the upper plot and the linearizations of the obstacle shape functions $o^{\{\cdot\}}(\cdot)$ along $\xi_1$ for $\xi_2=0$ are shown in the lower plot. Any p-norm is homogeneous, thus also for the ScaledNorm formulation it is true that $\underline{\xi}=\underline{\xi}^\mathrm{p}$ for any linearization point $\tilde{\xi}$.
• Figure 5: Comparison of two evasion maneuvers for the $2$-norm and the ScaledNorm formulation for a closed-loop simulation of $12s$. The $2$-norm formulation leads to the ego vehicle evading the two static obstacles conservatively due to the inflated shape of the SV. On the other hand, using the ScaledNorm formulation, the smooth transition of conservatively evading planned trajectories towards a tight driven trajectory (black) is visible.

Theorems & Definitions (9)

• Definition III.1
• Definition III.2
• Definition III.3
• Lemma III.1
• proof
• Lemma III.2
• proof
• Corollary III.1
• proof