1 Introduction↩︎

The demand for robust, agile controllers has grown significantly in recent years, as researchers pivot to developing field robotic systems that work outside the laboratory, in unpredictable, real-world environments [1]. The research community has increasingly been adopting agile quadcopter platforms as a benchmark to evaluate new control algorithms, given the requirements for fast, reactive controllers. Navigating challenging obstacle fields in this setting is particularly demanding, making it well-suited to evaluate novel control strategies [2]. While professional quadcopter pilots train extensively to handle these challenging environments [3], classic path planning approaches often fail to handle the fast-moving, changing environments, and frequently fail to operate in real-time [4]. Therefore, the development of controllers that bridge the gap between high-level planning and low-level control is essential for matching human performance in real-world environments. Model Predictive Control (MPC) bridges this gap by leveraging a system model to predict future states and optimize control inputs [5]. The MPC problem is typically solved using gradient-based optimization to satisfy system constraints while minimizing a defined cost function. In recent years, the growing capabilities of parallel computing have enabled the implementation of MPC on high-performance hardware, such as GPUs, allowing for faster and more scalable real-time control in complex robotic systems [6].

Model Predictive Path Integral (MPPI) is one such implementation that uses a sampling-based technique for optimization [7]. MPPI works by sampling small perturbations on an initial control input and calculating the cost relating to some control objective for each perturbation. These perturbed control inputs are then fused with a higher weight given to lower cost perturbations, producing a new optimal control sequence. A strength with sampling-based methods is that they do not require differentiable system dynamics, allowing greater freedom to tailor the cost function to the problem at hand. The magnitude of the perturbations added to the initial control sequence naturally defines a maximum change in the control sequence per iteration. This limits how quickly the control input can be transformed into a new optimal control sequence when the environment changes, which can make the controller unresponsive and slow to adapt to new changes. While proven to be a powerful optimizer, this approach introduces challenges in balancing exploration and exploitation. Increasing the magnitude of the perturbations to increase global optimality often induces instability in the closed-loop system, resulting in a low-performance controller.

Several methods have been proposed to increase the global optimality of the MPPI algorithm, while keeping the local optimization capability and cost function design freedoms. DIAL-MPC [8] and MPOPI [9] use the MPPI algorithm iteratively, from high noise to lower noise, to arrive at an optimal control input. This adds to the computation time and lowers the ability to parallelize effectively. The BiC-MPPI method introduced in [10] uses the MPPI algorithm bidirectionally to optimize the control input but requires invertible dynamics. [11] proposed the o-MPPI approach, which changes the algorithm to sample outputs instead of inputs. This modification introduces a limitation, as it also requires invertible dynamics. Earlier work has also attempted to guide the samples with a higher-level planner, such as the RRT guided MPPI introduced in [12]. However, in such approaches, the path planners often struggle with real-time performance, which makes replanning more difficult in dynamic environments. Some methods further diversified the parallelization of the MPPI algorithm, such as AERO MPPI [13] and U-MPPI [14]. These approaches run multiple parallel instances, optimizing multiple objectives at a time, but this lowers the available samples for each parallel instance, which can lead to lower optimality. Feature-based MPPI introduced in [15] samples around key features such as emergency braking, to allow quicker decisions in safety-critical situations. However, there is a limit to how many samples each feature can receive, which may reduce performance during normal operation. Recent approaches, such as Biased-MPPI [16], have focused on modifying the sampling distribution of the MPPI algorithm to improve performance, which extends the MPPI to allow arbitrary ancillary controllers to be included in the sampling distribution. [17] proposed the C-Uniformity MPPI method, which modifies the sampling distribution to a uniform distribution. Still, extensively altering the sampling distribution may negatively impact the performance and convergence of the MPPI algorithm, as potentially harmful biases can be introduced from increased variability of sampled inputs.

In this paper, we propose Motion-Primitive-guided Model Predictive Path Integral (MP-MPPI). This is a low-cost, high-impact modification to the MPPI method that increases global optimality and exploration. In this proposed method, additional control sequences are generated as feasible lattice state motion primitives [18]. These motion primitives serve as informative samples, improving the exploration of the control space. MP-MPPI samples these precomputed motion primitives alongside the standard MPPI perturbations and fuses them to achieve optimal control. An overview of the proposed method can be seen in Figure 1.

It is shown in the following that the proposed method is: (1) Suited for effective parallel implementation on GPUs. (2) Able to achieve robust control of a simulated quadcopter. (3) Of superior performance compared to the base MPPI algorithm in collision avoidance and obstacle field navigation.

2 Methodology↩︎

The proposed MP-MPPI algorithm was realized by first developing a base MPPI solver. This initial solver was subsequently enhanced through the incorporation of motion primitive samples, resulting in the complete MP-MPPI solver capable of leveraging structured trajectory sampling for improved control performance.

2.1 Optimal Control Problem↩︎

In model predictive control, the following optimal control problem (OCP) is solved [5]: \[\label{OCP} \begin{align} \min_{\{u_t\}_{t=0}^{N-1}} \quad & \sum_{t=0}^{N-1} \ell(x_t, u_t) + \ell_f(x_N) \\ \text{s.t.} \quad & x_{t+1} = f(x_t, u_t), \quad t = 0, \dots, N-1, \\ & x_t \in \mathcal{X}, \quad u_t \in \mathcal{U}, \quad t = 0, \dots, N-1, \\ & x_0 = x_{\text{init}}. \end{align}\tag{1}\] where the goal is to minimize the cost function \(\ell\) over the prediction horizon \(N\), given an initial system state \(x_{\text{init}}\). The states \(x_t\) and inputs \(u_t\) are constrained by the sets \(\mathcal{X}\) , and \(\mathcal{U}\), as well as needing to satisfy the constraints \(x_{t+1} = f(x_t, u_t)\), where \(f(x_t, u_t)\) is the system dynamics. \(\ell_f\) is the final cost term, which only considers the final predicted state \(x_N\) and can be used as a stabilizing cost to enforce a stable final state.

2.2 Model Predictive Path Integral↩︎

Model Predictive Path Integral (MPPI) is an implementation of the MPC method, where a sampling-based method is used to optimize the control inputs [7]. Samples are generated as perturbed trajectories by modifying the nominal control \(u_t\) into the perturbed control variable \(u_t^{(k)} = u_t + \epsilon_t^{(k)}\) for \(k=1,\ldots, K\) where \(\epsilon_t^{(k)}\) is a perturbation that is drawn from \(\mathcal{N}(0,\Sigma)\). The perturbed control variables are then used to generate \(K\) samples in the form of the perturbed trajectories \(x_t^{(k)}\) by the numerical integration scheme \[\begin{align} x_{t+1}^{(k)} &= x_t^{(k)} + f_{\mathrm{RK4}}(x_t^{(k)}, u_t^{(k)}, \Delta t), \end{align}\] where \(f_{\mathrm{RK4}}\) is the Runge-Kutta 4th order integration method. The cost \(S^{(k)}\) for each trajectory \(x_t^{(k)}\) is defined as: \[\begin{align} S^{(k)} = \sum_{t=0}^{N-1} \ell(x_t^{(k)}, u_t^{(k)}) + \ell_f(x_N^j). \end{align}\]

The optimal control sequence \(u_t\) can then be generated by performing a weighted sum of the perturbed controls \(u_t^{(k)}\) following the update law \[\begin{align} \tag{2} w_k &= \exp\!\left(-\frac{1}{\lambda} (S^{(k)}-\rho)\right),\\ \eta &= \sum_{k=1}^{K} \exp\!\left(-\frac{1}{\lambda} (S^{(k)}-\rho)\right),\\ \tag{3} u_t &\leftarrow \sum_{k=1}^{K}\frac{w_k }{\eta}\, u_t^{(k)} , \end{align}\] where \(w_k\) is the weight of the \(k\)-th perturbed control sequence and \(\eta\) is the sum of all weights. The variable \(\rho\) is the minimum of the cost \(S^{(k)}\) over the \(K\) samples, and is included for numerical stability and does not influence the optimality of the solution. The cost \(S^{(k)}\) determines which input sequences are emphasized and which are discarded. The temperature variable \(\lambda\) is a tuning variable that changes the way each perturbation is weighted. A larger \(\lambda\) creates a more uniform weighting, while a smaller \(\lambda\) approaches a hard selection of the least costly perturbed trajectory [7]. The magnitude of the covariance matrix \(\Sigma\) defines the rate of adaptation for the optimizer. A lower covariance matrix will lead to more locally optimal results, but decrease the rate of convergence, while a high magnitude covariance matrix increases the convergence speed, but can decrease the stability of the system.

2.3 MP-MPPI: Motion-Primitive-guided MPPI↩︎

This section presents the proposed MP-MPPI extension to the MPPI method, where \(N_p\) additional samples in the form of motion primitives are added. Motion primitives are sequences of control inputs that represent different types of motion, such as left- and right-turns, changes in velocity, etc. [19].

The motion primitive control sequences, denoted \(u_t^{(p,j)}\) for the \(j\)-th motion primtive, produce the trajectories \[x_{t+1}^{(p,j)} = f_{\mathrm{RK4}}(x_t^{(p,j)}, u_t^{(p,j)}).\] The control inputs \(u_t^{(p,j)}\) are selected from the set of feasible control inputs \(\mathcal{U}\), which ensures the dynamic feasibility of the motion primitives.

State lattice motion primitives are used and are generated by solving the OCP for a change in position to a point reference [18]. The OCP is solved for a set of \(N_p\) point-references, which creates \(N_p\) motion primitive samples \(u_t^{(p,0)},\ldots u_t^{(p,N_p-1)}\). These motion primitive samples are combined with white noise samples \(u_t^{(0)},\ldots, u_t^{(M-1)}\).

In the optimization loop, \(M\) samples \(u_\epsilon^0,\ldots,u_\epsilon^{M-1}\) are generated as perturbed control inputs \(u_t^{(k)} = u_t + \epsilon_t^{(k)}\) with white noise \(\epsilon^k\). \(N_p\) samples are drawn from a library of precomputed motion primitives, denoted \(u_t^{(p,j)}\). Each sample is a column of the matrix \[U_t = \begin{bmatrix} u_t^{(0)}& \ldots& u_t^{(M-1)}& u_t^{(p,0)} & \ldots & u_t^{(p,N_p-1)} \end{bmatrix}.\] The cost function is evaluated in parallel for all elements in the vector, to obtain the costs \(S^{(k)}\). Following the weighted sum update scheme from equations 2 3 , the optimal control sequence is computed as follows: \[\label{MP32step} u_t \leftarrow \frac{\sum_{k=1}^{K} \exp\!\left(-\frac{1}{\lambda} (S^{(k)} - \rho)\right) \, U_t^{(k)}}{\sum_{k=1}^{K} \exp\!\left(-\frac{1}{\lambda} (S^{(k)} - \rho)\right)},\tag{4}\] where \(K=M+N_p\) and \(U^{(k)}\) is column \(k\) of \(U_t\). The full MP-MPPI algorithm can be seen in Algorithm 2.

3 Case Study↩︎

The proposed method was implemented on a custom quadcopter simulator. The MP-MPPI algorithm was compiled using the JAX Python library and executed on an RTX 2000 Ada Laptop GPU. JAX is a Google-developed, JIT (Just-In-Time) compatible library for array-oriented numerical computations [20]. This library was used to effectively implement a GPU-vectorized version of the proposed algorithm. The controller was tuned to run above 100 Hz with satisfactory performance for point reference tracking. To preserve and emphasize the real-time viability of the proposed method, we tuned the controller to be lightweight, based on parameters used in previous real-world MPPI implementations [21]. The system was simulated using a discretized model with a time step of 10 ms. The chosen drone model parameters and the hyperparameters for MPPI are shown in Table 1 and Table 2, respectively. The controller parameters in Table 2 are extended to include Table 3 to complete the MP-MPPI controller implementation. The parameters in Table 3 are used to generate the motion primitive library. For each value in the reference lattice, the OCP in (1 ) is solved, creating the set of motion primitives, shown in Figure 3.

3.1 Quadcopter Dynamics Model↩︎

Quadcopters are actuated by four propellers, producing forces and torques on the rigid quadcopter body (\(u=[f_B, \tau_x, \tau_y, \tau_z]^T\)). In this paper, the dynamics model operates with these forces and torques directly. The dynamics model given by \(\Dot{x} = f(x, u)\) was developed based on rigid body equations of motion, with linear dynamics \[\begin{align} \tag{5} \dot{p} &= v, \\ \tag{6} \dot{v} &= g + \frac{1}{m} R(q)f_B, \end{align}\] which describes linear motion of a system with mass \(m\), where \(p=[x, y, z]^T\) is the position in the inertial frame, \(v\) is the velocity in the inertial frame, \(q = [\eta,\sigma^T]^T\) is the unit quaternion of the body orientation and \(g\) is the acceleration due to gravity. \(f_B\) is the total thrust force in the body frame, which is pre-multiplied by the rotation matrix \(R(q)\) to obtain the forces in the inertial frame. Body torques \(\tau_B\) produced are given by \([\tau_x, \tau_y, \tau_z\)]. The angular dynamics are described by the equations \[\begin{align} \tag{7} \dot{q} &= \tfrac{1}{2} q \otimes \begin{bmatrix} 0 \\ \omega \end{bmatrix} ,\\ \tag{8} \dot{\omega} &= J^{-1}(\tau_B - \omega \times (J\omega)), \end{align}\] where \(\omega\) is the angular velocity in body coordinates, and \(\tau_B\) is a vector of all body torques. \(J= \mathrm{diag}(I_x, I_y, I_z)\) is the inertia matrix in body coordinates. The inertia matrix is diagonal due to the assumption of symmetry around the center of mass. The quaternion product is given by \[\begin{bmatrix} \eta_1\\ \sigma_1 \end{bmatrix} \otimes \begin{bmatrix}\eta_2\\\sigma_2 \end{bmatrix} = \begin{bmatrix} \eta_1\eta_2 - \sigma_1^T\sigma_2 \\ \; \eta_1\sigma_2 + \eta_2\sigma_1 + \sigma_1\times\sigma_2 \end{bmatrix}.\] To enforce these model constraints when calculating the trajectory cost, a 4th order Runge-Kutta method (RK4) was used to integrate the differential equations shown in 5 8 . After each integration, the quaternions were normalized to ensure they represented the rotation in unit quaternions.

3.2 Task Specific Cost Function↩︎

The cost function to be minimized was chosen to have a general quadratic form: \[\begin{align} \label{costfunction} \ell(x, v, \omega, q, u) =& c_p \tilde{x}^T \tilde{x} + c_v v^T v \nonumber\\ &+ c_{\omega} \omega^T \omega + c_{q} d_q(q, q_{\mathrm{ref}})^2 + \tilde{u}^TW\tilde{u} \nonumber\\ &+ \delta u^T W_\Delta \delta u, \\ \tilde{x} =& x - x_{\mathrm{ref}}, \\ \tilde{u} =& u - u_{\mathrm{ref}}, \\ \delta u =& u - u_{\mathrm{prev}}, \end{align}\tag{9}\] where \(\ell\) is the cost at a specific state. \(x\) is the position, \(v\) is the velocity, \(q\) is the orientation and \(\omega\) is the angular velocity. \(x_{\mathrm{ref}}\) and \(q_{\mathrm{ref}}\) are the commanded references. \(c_p\), \(c_v\), \(c_{q}\) and \(c_{\omega}\) are cost coefficients, \(W\) is a diagonal matrix with input cost coefficients. \(u\) is the input, and \(u_{\mathrm{ref}}\) is the reference input required to sustain a stable hover. \(u_{\mathrm{prev}}\) is the previously optimal control sequence, and \(W_\Delta\) is a diagonal matrix with change-in-input cost coefficients. The cost coefficients work as regularization terms that penalize high angular and high linear velocity flight. Equally, a rapid change in inputs, or inputs far away from the reference input, can destabilize the controller, and are therefore penalized. The error function for the unit quaternion is given by [21] \[d_q(q, q_{ref}) = 1 - (q \cdot q_{\mathrm{ref}})^{2}, \] where \([\eta_1, \sigma_1^T]^T\cdot [\eta_2, \sigma_2^T]^T = \eta_1\eta_2 + \sigma_1^T\sigma_2\) is the quaternion inner product. It is noted [22] that \({q \cdot q_{\mathrm{ref}}} = \cos(\tilde{\theta}/2)\) where \(\tilde{\theta}\) is the rotation angle of the quaternion deviation \(\tilde{q} = \bar{q}_{\mathrm{ref}}\otimes q = [\cos(\tilde{\theta}/2),\sin(\tilde{\theta}/2)\tilde{k}^T]^T\) where \(\tilde{k}\) is the unit axis of rotation and \(\bar{q}\) is the conjugate of \(q\). It follows that \(d_q(q, q_{\mathrm{ref}}) = \sin^2(\tilde{\theta}/2)\). The unit quaternion cost term works to encourage upright and forward-facing flight.

3.3 Obstacle Avoidance↩︎

Obstacle avoidance is achieved through a cost modification of 9 . Adding high cost to the trajectories that lead to collision means that those trajectories are effectively disregarded when calculating the updated control sequence in 2 . The new trajectory cost becomes \[S^{(k)}{'} = S^{(k)} + \sum_{j=1}^{N_{\mathrm{obs}}} c_{\text{obs}} \cdot \mathbb{1}(x_j^k \in C_{\text{obs}}), \label{eq:avoidance95cost}\tag{10}\]

where \(c_{obs}\) is a penalty for colliding, \(N_{\mathrm{obs}}\) is the number of obstacles and \(\mathbb{1}\) is the indicator function that returns 1 when \(x_j^k \in C_{\text{obs}}\) and 0 otherwise. This function assumes that the obstacle geometry is accessible. To account for the physical dimensions of the drone, a safety distance \(d_{\text{safe}}\) is included in the collision check.

4 Results↩︎

4.1 Obstacle Field Navigation↩︎

Obstacle avoidance and navigation are evaluated on a dense obstacle field navigation task. An example is shown in Figure 4. The quadcopter has to navigate through 300 randomly placed pillars with a diameter of 1 m, generated within a bounding box of 20 m by 100 m. The drone is commanded to follow a moving point reference 10 m in front of the quadcopter on the center line of the obstacle field. Each simulation lasted for 5 s and started from a hovering state. The obstacle field navigation task was run 100 times, with the end positions, as well as the number of collisions, being recorded.

As seen in Figure 5, the MPPI algorithm navigated the obstacle field effectively with a mean traversal distance of 48.0 m and with a standard deviation of 18.0 m. Adding 27 motion primitives (visualized in Figure 3) to create the MP-MPPI controller, increased the mean traversal distance to 66.6 m, with a standard deviation of 12.6 m. The MPPI controller collided once during the 100 runs, while the MP-MPPI controller managed to avoid any collisions. This demonstrated that the addition of a few motion primitives improved the obstacle field navigation in both total distance traversed and in collisions avoided. The MP-MPPI algorithm discovers trajectories that take it beyond its locally minimum solution, and further progress through the obstacle field.

4.2 Single Update Analysis↩︎

The MP-MPPI algorithm and the MPPI base algorithm are evaluated on reactivity, where both algorithms are compared in a scenario where the drone is moving towards a wall at 10 m s⁻¹ from a distance of 5 m, and suddenly becomes aware of the obstacle. The controller performance can be seen in Figure 6.

The MP-MPPI algorithm generates a trajectory that avoids collision with the wall, while the MPPI algorithm collides with it. This highlights how the motion primitive samples go into effect when the base MPPI algorithm is limited by the magnitude of the perturbations. The additional motion primitives inform the controller of solutions that might not be locally optimal, but represent the needed motion to avoid collision with the wall.

4.3 Controller Update Frequency↩︎

Agile robotic systems rely on rapid feedback for stable control, therefore the update frequency is also benchmarked. Figure 7 shows the update frequencies for a varying number of samples and prediction horizons. This shows that the computation times follow a linear trend with respect to the horizon length. It also shows that the algorithm effectively runs on a GPU, with only small differences in latency between using 1024 samples and 128 samples. This allows for the evaluation of additional motion primitive samples, with a limited effect on the update frequency.

4.4 Discussion and Limitations↩︎

While the proposed MP-MPPI method demonstrates that a small selection of precomputed motion primitives can significantly increase the performance over the base MPPI algorithm, the extent of this effect has not been fully explored. While the convergence has been proven for arbitrary controllers [16], there is still the need to explore if these kinematically feasible building blocks provide better convergence and introduce less bias than an arbitrary controller would.

A main limitation of this approach is that the balance between motion primitives and regular noisy samples becomes a design parameter that must be carefully tuned. Too many motion primitives can cause the controller to weight potentially suboptimal trajectories, thereby lowering performance and optimality. Future work may investigate if there is a method to generate the motion primitive library for specific tasks optimally.

5 Conclusion↩︎

In this work, we propose MP-MPPI, an enhanced version of the MPPI algorithm that improves global optimality by introducing exploratory motion primitives into the MPPI update law. The performance is evaluated in a quadcopter simulator, which indicated the method’s ability to navigate highly cluttered environments. Obtained results demonstrated that in an unpredictable environment, MP-MPPI can adapt quickly and avoid collision, where the MPPI fails to do so.

The proposed MP-MPPI method is shown to be a low-cost, high-impact extension to the MPPI method, capable of running on lower-end parallel computing hardware, and thus it may serve as a low-cost method of achieving robustness in autonomous systems.

References↩︎

---

1. Corresponding author: marlon.g.mathisen@ntnu.no↩︎