Global-Initialization-Based Model Predictive Control for Mobile Robots Navigating Nonconvex Obstacle Environments

Abstract

This paper proposes a nonlinear model predictive control (MPC) framework initialized using an initial-guess particle swarm optimization (IG-PSO) algorithm for mobile robots navigating in environments with nonconvex obstacles. The proposed method is designed to address the local minimum problem inherent in conventional optimization-based MPC by incorporating a PSO-based global search method to generate effective initial guesses. In addition, a grid-based representation of the nonconvex obstacles is implemented to systematically define the collision avoidance constraints within the MPC formulation. The proposed method was validated in real-time simulations using the Robot Operating System (ROS) and the Gazebo physics simulator. The results demonstrate that the proposed MPC initialized by IG-PSO generates collision-free trajectories that avoid local minima and track the desired reference trajectory in environments with nonconvex obstacles. Compared with conventional IPOPT-based MPC, the proposed method exhibited improved performance in the tested scenario. The proposed method also maintains real-time control capabilities by selectively activating the IG-PSO algorithm only as required. The findings of this study demonstrate the potential of the proposed framework for robust and efficient trajectory planning in complex, nonconvex obstacle environments.

1. Introduction

Model predictive control (MPC) [1,2,3,4,5] has been widely employed for trajectory tracking of mobile robots, and its ability to handle system constraints, e.g., obstacle avoidance, kinematics, and velocity limits, over a finite prediction horizon makes it a powerful tool for mobile robot control [6,7]. Typically, to ensure real-time performance, MPC is implemented using local optimization methods to optimize the control inputs to track a given reference trajectory while satisfying all constraints within the prediction horizon.

The growing complexity of unstructured environments has made autonomous navigation an increasingly difficult task. Mobile robots must perceive complex obstacle environments, distinguish between traversable and nontraversable regions, and effectively handle irregular terrain and cluttered obstacles [8]. Safe operation also requires reliable object detection and accurate distance estimation, as perception errors can critically degrade motion planning and control [9]. Moreover, recent technological advances in sensing, data processing, and hardware design have significantly enhanced the perception and mobility capabilities of autonomous platforms, thereby enabling more effective integration with optimization-based control frameworks [10].

However, local optimization faces significant challenges in complex environments, particularly those containing nonconvex obstacles. Nonconvex obstacles are difficult to handle because they render the trajectory optimization problem nonconvex, which increases the computational complexity considerably and introduces the risk of convergence to local minima. Local optimizers only explore a limited region of the solution space; thus, the robot may become trapped between obstacles and fail to find a feasible trajectory that follows the reference path. In addition, in extreme cases, the robot may become stuck in a local minimum without the ability to escape.

Recent developments in hybrid MPC approaches have successfully integrated metaheuristics and learning-based methods to enhance real-time performance and robustness. For example, transformer based models have been used to identify active constraints and generate warm starts for MPC, achieving substantial improvements in runtime [11]. Similarly, self supervised initialization frameworks have been developed to accelerate convergence and improve tracking accuracy in complex driving tasks [12]. In addition, memory based warm starting mechanisms with stability guarantees have been proposed to deliver asymptotically optimal MPC solutions using historical optimization data [13].

Global optimization techniques have been investigated to address the limitations of local optimization methods in cluttered environments. Unlike local methods, which are confined to a narrow neighborhood of the initial guess, global optimization techniques can explore a broader search space, which increases the likelihood of finding a feasible and near-optimal solution, even in the presence of complex and nonconvex obstacles. Among the various global optimization methods, particle swarm optimization (PSO) [14,15] has gained attention for its effectiveness in trajectory planning tasks. PSO is a population-based metaheuristic that simulates the collective behavior of a swarm, and it offers an effective balance between exploration and exploitation by updating a set of candidate solutions iteratively based on both the individual and collective experience. This makes it particularly well-suited for generating initial guesses that avoid local minima and yield more globally optimal trajectories in MPC frameworks.

Recent studies have demonstrated the effectiveness of incorporating PSO into MPC frameworks to improve trajectory tracking and obstacle avoidance in cluttered environments. For example, Zhang and Zhang [16] proposed a distributed MPC scheme for mecanum-wheeled mobile robots, where PSO is employed to optimize each robot’s control sequence and avoid local minima during formation tracking. In addition, Kim et al. [17] proposed a hybrid local planner that combines MPC, potential fields, and PSO to dynamically select optimal parameters for smooth obstacle avoidance in real-world driving scenarios. Liu et al. [18] integrated PSO with nonlinear MPC to enhance the path tracking of underground mining vehicles, and they achieved significant reductions in the lateral tracking error in highly curved tunnels. Dahmad et al. [19] introduced an adaptive MPC controller tuned using the multivariate Gaussian mixture model-ant colony optimization (MGMM-ACO) method, which is a global optimizer that improves on the standard PSO by modeling intervariable dependencies, resulting in robust and collision-free motion in environments containing dynamic obstacles. However, these methods do not explicitly address the challenges posed by nonconvex obstacle representations, which are common in real-world environments and frequently lead to fundamentally nonconvex optimization problems.

To address this limitation, this paper proposes an MPC framework that achieves reliable trajectory tracking in environments containing nonconvex obstacles. The core concept of the proposed framework is to intermittently integrate global optimization into the MPC process, thereby enabling the robot to escape local minima and avoid becoming permanently trapped. By selectively activating the global search only as required, the proposed framework enhances robustness against complex obstacle configurations while maintaining sufficient computational efficiency.

The primary contributions of this study are summarized as follows:

• An MPC framework that incorporates global optimization techniques to improve the trajectory tracking performance in environments with nonconvex obstacles is proposed.
• A selective activation strategy is implemented in the proposed framework to invoke global optimization only when the robot is at risk of becoming trapped, thereby preserving computational efficiency.
• The effectiveness of the proposed framework is demonstrated through real-time simulations in nonconvex obstacle environments, and the results demonstrate successful obstacle avoidance and recovery from local minima.

The remainder of this paper is organized as follows: Section 2 formulates the nonconvex obstacle avoidance problem for nonholonomic mobile robots. The proposed global search-based MPC algorithm is presented in Section 3. Section 4 discusses the real-time simulation results, and the paper is concluded in Section 5.

2. Nonconvex Obstacle Avoidance Problem in the MPC Framework

This section describes the nonconvex obstacle avoidance problem addressed in this paper. Here, we consider a scenario, as illustrated in Figure 1, where a robot is required to move from its initial position on the left to a desired position on the right. In this scenario, a new obstacle appears after the reference trajectory has been generated. This obstacle may not have been observable when the reference trajectory was generated, or it may be a dynamic obstacle that has moved into the vicinity of the reference trajectory. A robot operating under an MPC framework is expected to generate a feasible trajectory that follows the reference trajectory and avoids obstacles over a finite prediction horizon. Local optimization algorithms, e.g., the interior point optimizer (IPOPT) [1,20,21,22], are commonly used to generate the robot trajectory within the prediction horizon. IPOPT is a local optimizer that relies heavily on the initial guess, which is typically obtained by sampling the reference trajectory or using the solution from the previous time step. In the scenario shown in Figure 1, when the optimizer is initialized with the sampled reference trajectory, it tends to converge to a local minimum caused by the nonconvex obstacle configuration. Because IPOPT explores only a limited neighborhood around this initial guess, once it converges to the local minimum, the robot cannot escape and fails to find a feasible, collision-free trajectory. This issue is particularly problematic in conventional MPC frameworks, where the solution from the previous time step is utilized as the initial guess for the current optimization process. Once the optimizer converges to a local minimum, it becomes difficult or even impossible for the robot to escape and find an alternative trajectory.

To overcome this limitation, we propose a method that generates an improved initial guess capable of escaping the local minimum. By exploring a broader solution space through a population-based global search, this approach provides a starting point for the local optimizer that avoids convergence to undesirable local minima, thereby enabling successful obstacle avoidance even in challenging nonconvex environments.

Definition 1.

Let $\delta t>0$ be the discretization time step and $N\in \mathbb{N}$ be the number of prediction steps. The prediction horizon is defined as $T_h=N·\delta t$. Here, assuming that the robot’s translational velocity is bounded by $v_{max}>0$, the reachable region from the robot’s current position $(x_k,y_k)\in {\mathbb{R}}^2$ at the current time step $t_k$ over the prediction horizon is defined as follows:

$${\mathcal{R}}_H(x_k,y_k)=\left\{(x,y)\in {\mathbb{R}}^2|\sqrt{{(x-x_k)}^2+{(y-y_k)}^2}\le v_{max}·T_h\right\}.$$

(1)

To ensure safe motion planning within the prediction horizon of the MPC, we define a conservative estimate of the robot’s reachable space based solely on its maximum translational velocity $v_{max}$. Specifically, we approximate the reachable region as a Euclidean ball centered at the robot’s current configuration $(x_k,y_k)$ with a radius that is proportional to the product of the maximum velocity and the total prediction horizon $T_h$. Here, the following assumptions are considered to define the nonconvex obstacle avoidance problem:

Assumption 1.

A global reference path is generated from the prior map data; however, as the robot tracks this path, the environment may change over time, thereby resulting in discrepancies between the actual environment and the prior map.

Assumption 2.

Despite potential environmental changes, the robot is assumed to obtain an accurate local map of the obstacles within its reachable region ${\mathcal{R}}_H(x_k,y_k)$ at each time step. This local map is constructed through onboard sensing or perception systems, thereby enabling access to reliable obstacle information for collision avoidance during the local trajectory planning process.

Problem 1.

We address the problem of local trajectory planning for a mobile robot following a global path generated from incomplete prior map data. The global path may intersect with previously unknown, nonconvex obstacles; thus, the robot must adapt its motion in real-time to avoid collisions. Here, the robot employs MPC to generate a local trajectory over a fixed prediction horizon, relying on the local obstacle information obtained within its reachable region. In this scenario, the objective is to generate a collision-free trajectory that avoids local minima, respects the robot’s non-holonomic motion constraints and control input limits over the prediction horizon, and closely tracks the global reference path.

The challenge of avoiding nonconvex obstacles occurs frequently in real-world navigation tasks, where obstacles can have complex shapes and irregular boundaries. However, conventional local planners often fail to handle such complexities effectively. Thus, an MPC-based planner is proposed to ensure safe navigation in partially known environments.

3. Proposed Method

3.1. Nonlinear MPC Formulation for Trajectory Tracking

We consider a nonholonomic mobile robot whose pose at time $t_k$ is defined as ${\xi }_k={[x_k,y_k,{\theta }_k]}^T$, where $x_k$ and $y_k$ represent the position of the robot, and ${\theta }_k$ denotes its heading angle. Here, the control input is given by ${\mathbf{u}}_k={[v_k,{\omega }_k]}^T$, where $v_k$ and ${\omega }_k$ denote the translational and rotational velocities, respectively. The kinematic model of the robot is represented by the following discrete-time nonlinear system:

$$\begin{array}{cc}\hfill x_{k+1}=& x_k+v_{k}cos{\theta }_k\delta t,\hfill \end{array}$$

(2)

$$\begin{array}{cc}\hfill y_{k+1}=& y_k+v_{k}sin{\theta }_k\delta t,\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill {\theta }_{k+1}=& {\theta }_k+{\omega }_k\delta t,\hfill \end{array}$$

(4)

where $\delta t$ denotes the sampling interval. This system can be expressed compactly as a nonlinear function f such that  

$${\xi }_{k+1}=f({\xi }_{\mathbf{k}},{\mathbf{u}}_k).$$

(5)

A reference trajectory ${\xi }_{r,k}={[x_{r,k},y_{r,k},{\theta }_{r,k}]}^T$ is given with the reference control input ${\mathbf{u}}_{r,k}={[v_{r,k},{\omega }_{r,k}]}^T\in {\mathbb{R}}^2$. The reference translational and rotational velocities are defined as $v_{r,k}=\sqrt{\Delta x_{r,k}^2+\Delta y_{r,k}^2}/\delta t$ and ${\omega }_{r,k}=({\theta }_{r,k+1}-{\theta }_{r,k})/\delta t$, where ${\theta }_{r,k}=arctan2(\Delta y_{r,k},\Delta x_{r,k})$ and $\Delta x_{r,k}=x_{r,k+1}-x_{r,k}$, $\Delta y_{r,k}=y_{r,k+1}-y_{r,k}$. The trajectory error and control input error are defined as ${\xi }_e={\xi }_r-\xi$ and ${\mathbf{u}}_e={\mathbf{u}}_r-\mathbf{u}$, respectively.

The objective function to be minimized is expressed as follows:

$$J\left({\xi }_e,{\mathbf{u}}_e\right)=\sum _{n=0}^{N-1}\left(\parallel {\xi }_e\left(t_{k+n|k}\right){\parallel }_Q^2+{\parallel {\mathbf{u}}_e\left(t_{k+n|k}\right)\parallel }_R^2\right)+\gamma {\parallel {\xi }_e\left(t_{k+N|k}\right)\parallel }^2,$$

(6)

where $Q>0$ and $R>0$ are positive definite matrices, and $\gamma >0$ is a terminal state weight. Here, ${\xi }_e\left(t_{k+n|k}\right)$ and ${\mathbf{u}}_e(t_{k+n|k})$ represent the predicted state and the control input at time step $t_{k+n}$ based on the prediction made at time $t_k$. Thus, a finite-horizon trajectory optimization problem can be formulated as follows:

$$\underset{\mathbf{u}}{min}J\left({\xi }_e,{\mathbf{u}}_e\right),$$

(7)

subject to

$$\begin{array}{cc}\hfill & \xi \left(t_{k+n+1|k}\right)=f(\xi \left(t_{k+n|k}\right),\mathbf{u}\left(t_{k+n|k}\right)),n=0,1,\dots ,N-1,\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill & 0\le v\left(t_{k+n|k}\right)\le v_{max},n=0,1,\dots ,N-1,\hfill \end{array}$$

(9)

$$\begin{array}{cc}\hfill & |\omega \left(t_{k+n|k}\right)|\le {\omega }_{max},n=0,1,\dots ,N-1,\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill & |d_i\left(t_{k+n|k}\right)|\le D_{\mathrm{safe}},\forall i\in \mathcal{O},n=0,1,\dots ,N-1,\hfill \end{array}$$

(11)

where $v_{max}>0$ and ${\omega }_{max}>0$ are the maximum allowable control inputs and $\mathcal{O}$ is the set of known obstacle positions, with $d_i$ denoting the distance to the i-th obstacle in $\mathcal{O}$.

In a conventional MPC framework, the controller computes an optimal sequence of control inputs over the prediction horizon at each time step by solving an optimization problem. Then, only the first segment of the computed control sequence, typically corresponding to the time interval $[t_k,t_k+\delta t]$, is applied to the robot. After applying the control input, the robot’s state is updated, and a new optimization problem is solved at the next time step using the updated state as the initial condition. This process is repeated at each time step, thereby allowing for the controller to adapt the planned trajectory based on the robot’s current state and any changes in the environment.

The above mathematical formulation represents the typical optimization problem in a standard MPC framework. Local search-based optimizers are commonly employed to solve this problem; however, in environments with nonconvex obstacles, the collision avoidance constraint (11), which ensures that the distance between the robot and the obstacles remains greater than or equal to a specified safety distance, introduces nonconvexity into the optimization. Under these conditions, local search-based optimizers frequently struggle to find feasible solutions because they can become trapped in local minima. Thus, the goal of this study is to develop a solver that can handle such scenarios. Section 3.2, Section 3.3 and Section 3.4 present the proposed solver, which was designed to approach globally optimal solutions efficiently without becoming stuck in local minima, even in environments containing nonconvex obstacles.

3.2. Global Search-Based Initial Guess Selection

In this subsection, we propose a global search method to generate initial guesses that can avoid local minima. Among global optimization algorithms, the PSO algorithm [23] is a promising approach because it can find suboptimal solutions quickly [24,25]. Thus, PSO has been investigated as a global optimizer in MPC frameworks. However, executing the PSO algorithm at each time step is more time-consuming than employing a local optimizer; thus, its applicability is limited in real-time systems. In addition, applying the control inputs obtained from the PSO directly without refinement may result in instability in the robot’s motion due to the lack of fine-tuning in the solution.

To address this issue, we propose an initial guess via particle swarm optimization (IG-PSO), which only activates when the IPOPT-based MPC solver becomes trapped in a local minimum, thereby enabling it to escape and improve the solution quality. In the proposed method, each particle represents a candidate sequence of control inputs for the robot. Specifically, the position of the m-th particle at generation l, denoted ${\mathbf{X}}_l^m$, encodes the planned translational and rotational velocities over the prediction horizon. The phenotype of each particle is defined as follows:

$${\mathbf{X}}_l^m=[v_i\left(t_{k|k}\right),v_i\left(t_{k+1|k}\right),\dots ,v_i\left(t_{k+N-1|k}\right),{\omega }_i\left(t_{k|k}\right),{\omega }_i\left(t_{k+1|k}\right),\dots ,{\omega }_i\left(t_{k+N-1|k}\right)],$$

(12)

where $0\le v_i\left(t_{k+n|k}\right)\le v_{max}$ and ${\omega }_i\left(t_{k+n|k}\right)\le {\omega }_{max}$ for $n=0,1,2,\dots ,N-1$. The position and velocity of each particle are updated as follows in each generation:  

$$\begin{array}{cc}\hfill {\mathbf{X}}_{l+1}^m& ={\mathbf{X}}_l^m+{\mathbf{V}}_{l+1}^m\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill {\mathbf{V}}_{l+1}^m& =w_l{\mathbf{V}}_l^m+c_1{R}_1({\mathbf{P}}^m-{\mathbf{X}}_l^m)+c_2{R}_2(\mathbf{G}-{\mathbf{x}}_l^m),\hfill \end{array}$$

(14)

where ${\mathbf{V}}_l^m$ denotes the velocity of the m-th particle at generation l, $c_1$ and $c_2$ are the acceleration coefficients, and $w_l$ denotes the inertia weight. Here, the inertia weight $w_l$ is updated as follows at each generation [23]:  

$$w_l=w_{max}-{\displaystyle \frac{w_{max}-w_{min}}{L_{max}}}\times l,$$

(15)

where $w_{max}$ and $w_{min}$ denote the maximum and minimum bounds of the inertia weight $w_l$, respectively. This linear decrease is employed to gradually shift the optimization behavior from broader global exploration to more focused local search as the number of generations increases. In addition, each particle maintains its personal best position ${\mathbf{P}}_l^m$, and ${\mathbf{G}}_l$ represents the best position found by the entire swarm at generation l. Furthermore, $R_1$ and $R_2$ are the diagonal matrices conforming to the dimensions of ${\mathbf{X}}_l^m$, where diagonal entry is sampled independently from a uniform distribution over $[0,1]$ in each generation.

The proposed PSO-based initial guess generation method is designed to compute a feasible trajectory over the prediction horizon that enables the robot to reach the desired position while satisfying the specified obstacle avoidance constraints. To this end, it is necessary to compute a near-optimal solution that minimizes the cost function while ensuring collision avoidance. Here, the fitness function $F\left({\mathbf{X}}_l^m\right)$ employed to evaluate each particle is defined by augmenting the conventional MPC cost with an additional potential field term that imposes a penalty for proximity to obstacles, which is expressed as follows:

$$F\left({\mathbf{X}}_l^m\right)=J({\xi }_e,{\mathbf{u}}_e)+\sum _i{\psi }_i\left(d_i\right),$$

(16)

where $d_i$ denotes the relative distance between the robot and obstacle i, and $D_{\mathrm{safe}}$ denotes the safe distance for collision avoidance. In addition, the state error ${\xi }_e$ and control input error ${\mathbf{u}}_e$ are computed by considering each particle ${\mathbf{X}}_l^m$ as a control input and propagating the system kinematics defined in (5). The artificial potential field ${\varphi }_i\left(d_i\right)$ is obtained as follows:

$${\varphi }_i\left(d_i\right)=\left\{\begin{array}{cc}{\displaystyle \frac{\kappa }{2}}\left(1+cos\left(\pi {\displaystyle \frac{d_i}{D_{\mathrm{safe}}}}\right)\right),\hfill & d_i\in [0,D_{\mathrm{safe}})\hfill \\ 0,\hfill & \mathrm{otherwise},\hfill \end{array}\right.$$

(17)

which corresponds to a bump function, as used in the literature [25], that decreases smoothly between 0 and $\kappa$ as the distance increases from 0 to $D_{\mathrm{safe}}$.

The overall procedure of the IG-PSO algorithm is described in Algorithm 1. Here, the collision avoidance strategy is the key process. Based on a previously proposed method [26], the collision avoidance strategy is modified to satisfy the collision avoidance constraint in (11). The proposed strategy operates as follows: At each iteration, the IG-PSO algorithm evaluates the fitness values of multiple particles, updating its personal best ${\mathbf{P}}_l^m$ for each particle, as well as the global best ${\mathbf{G}}_l$ for all particles. After repeating this process for $L_{max}$ iterations, the final global best particle is selected as the initial guess for the IPOPT-based MPC solver. Note that the selected global best particle ${\mathbf{G}}_l$ must always satisfy the collision avoidance constraint. To ensure this condition, only particles that satisfy the collision avoidance constraint (11) are eligible to update the personal best ${\mathbf{P}}_l^m$ or global best ${\mathbf{G}}_l$. Among these feasible candidates, the personal best ${\mathbf{P}}_l^m$ and global best ${\mathbf{G}}_l$ are only updated when a particle has a lower fitness value than the current best, which ensures that the final global best particle always satisfies the constraint. This approach leverages the inherent diversity of PSO, which maintains multiple candidate solutions, to handle the collision avoidance constraint effectively. Here, as long as at least one particle among the population satisfies the constraint, the method can guide the search toward feasible and safe solutions.

Compared to the existing collision avoidance strategy [26], a notable improvement of the proposed method is the explicit incorporation of the artificial potential field in (17) into the fitness function. The original method depends on only the tracking error with respect to the reference trajectory and may fail when no particles satisfy the constraint; however, the proposed formulation encourages more particles to satisfy the collision avoidance condition by introducing obstacle-aware potential penalties. As a result, it is more likely that the search process will escape local minima and converge toward feasible trajectories.

Algorithm 1: Pseudocode of IG-PSO Initializer

3.3. Grid-Based Representation of Nonconvex Obstacles for Collision Avoidance Constraints

In conventional MPC, obstacles are frequently approximated by simple convex shapes, e.g., spheres, cylinders, or ellipsoids, which allow for the collision avoidance constraints to be expressed in a mathematically simple form. However, in real-world environments, obstacles typically have nonconvex shapes. Approximating all obstacles in this manner can overly constrain the search space, thereby resulting in conservative trajectories that prioritize obstacle avoidance over following the desired trajectory.

To ensure collision avoidance with nonconvex obstacles in the MPC framework, the distance between the robot and each obstacle, as defined in (11), must remain greater than or equal to a specified safety margin, and enforcing this constraint requires an explicit representation of obstacle locations.

Thus, a grid-based representation method that uses unit cells is proposed to model arbitrary-shaped nonconvex obstacles. The concept of this representation is shown in Figure 2, where the nonconvex obstacle is overlaid with a grid composed of cells of size $C_0\times C_0$. Each cell in the grid is evaluated to determine whether it is occupied by an obstacle or remains free. Here, the set of grid cells identified as occupied is extracted, and the center of each occupied cell is denoted $(o_{xi},o_{yi})$. For the occupied cells, the obstacle avoidance constraint in (11) is enforced by requiring that the distance between the robot and the center of each cell remains at least $D_{\mathrm{safe}}$. With this approach, obstacles of arbitrary nonconvex shapes can be represented effectively, and the resulting formulation ensures that the predicted trajectory over the prediction horizon avoids approaching the occupied cells within the specified safety distance. Note that although the grid-based representation is standard, the increased complexity of the obstacle avoidance constraints in dense or narrow environments can lead to local minima, which motivates the use of IG-PSO to assist the MPC in escaping cases where the robot becomes trapped in a local minimum.

3.4. IG-PSO-Initialized MPC Framework with Nonconvex Obstacle Avoidance

This subsection presents the overall framework that integrates the proposed IG-PSO method into the nonlinear MPC formulation for collision avoidance in environments with nonconvex obstacles. The goal of the proposed framework is to generate an initial guess that avoids local minima and improves the feasibility of the optimization process. Here, the robot’s current state is obtained at each control step, and a grid-based representation of the environment is updated to identify occupied cells corresponding to nonconvex obstacle regions. The centers of these occupied cells are then employed to define the collision avoidance constraints requiring the robot to maintain a safe distance from the obstacles throughout the prediction horizon.

The IG-PSO algorithm is executed only when a local minimum is detected in the trajectory obtained from the previous time step. The detection of a local minimum is based on the objective function J defined in (6), and is determined as follows:

$$J\left({\xi }_e,{\mathbf{u}}_e\right)>J_{\mathrm{active}}.$$

(18)

Here, $J_{\mathrm{active}}$ is a predefined threshold above which the trajectory is considered to be trapped in a local minimum, and the IG-PSO is activated to generate a new initial guess. Once IG-PSO has been activated, it returns to the inactive mode only when the following condition is satisfied:

$$J\left({\xi }_e,{\mathbf{u}}_e\right)<J_{\mathrm{inactive}},$$

(19)

where $J_{\mathrm{inactive}}<J_{\mathrm{active}}$. This lower threshold $J_{\mathrm{inactive}}$ ensures that IG-PSO deactivates only when the trajectory quality has sufficiently improved, introducing hysteresis to prevent oscillations or instability in switching between the IG-PSO and the standard optimization. This selective activation strategy allows for the proposed framework to balance computational efficiency with the ability to effectively escape local minima.

To generate an initial guess for the MPC optimization process, the IG-PSO algorithm performs a global search over the space of the control input sequences. In this process, each candidate is evaluated using a cost function that combines the standard MPC tracking cost with an additional potential field penalty that increases near obstacles. This process yields an initial guess that satisfies the input constraints and avoids areas near obstacles, thereby reducing the likelihood of convergence to a local minimum. Then, the generated initial guess is employed to initialize the constrained nonlinear MPC optimization. The resulting problem is solved to obtain an optimal control input sequence that satisfies the system dynamics, control bounds, and obstacle avoidance constraints. Note that only the first control input of the optimized sequence is applied to the robot, and then the process is repeated at the subsequent time step.

The overall procedure is summarized in Algorithm 2. By integrating IG-PSO into the MPC framework, the proposed method can generate initial guesses that are less likely to fall into local minima, which can occur in environments with complex nonconvex obstacle geometries. This improves the ability of the controller to compute collision-free trajectories while maintaining computational feasibility.

Algorithm 2: Pseudocode of IG-PSO-initialized MPC Framework

Figure 3 shows the overall process of the proposed framework, which integrates IG-PSO with nonlinear MPC to generate collision-free trajectories in environments with nonconvex obstacles. The top part of the diagram shows the high-level control loop, where the robot’s current state is employed to generate an initial guess via IG-PSO, which is followed by the nonlinear MPC optimization process. The bottom part illustrates the internal process of the IG-PSO algorithm, including the particle initialization, fitness evaluation with collision checking, and iterative updates of the personal and global best solutions until the termination criteria are met. This hierarchical structure highlights how the global initialization process is incorporated into each control step of the MPC framework.

4. Real-Time Simulation with Gazebo and ROS

4.1. Simulation Setup

To evaluate the effectiveness of the proposed method, simulations were performed to compare the conventional IPOPT-based MPC with the proposed MPC initialized by the IG-PSO algorithm in a scenario involving a single nonconvex obstacle. This simulation setup used ROS Foxy [27] and the Gazebo physics simulator to emulate realistic robot motion. Here, the robots were modeled in Gazebo using the Clearpath Jackal platform, and the virtual robots were subjected to disturbances representative of real-world conditions. In addition, Gaussian noise was added to the control input, ${\mathbf{u}}_k={[v_k,{\omega }_k]}^T$ using zero-mean normal distributions with standard deviations of ${\sigma }_v=0.015$ m/s and ${\sigma }_{\omega }=4$ deg/s. Gaussian noise was also added to the robot’s position, with standard deviations of ${\sigma }_x=0.1$ m and ${\sigma }_y=0.1$ m. The parameters used for the proposed IG-PSO algorithm are shown in

Table 1. IG-PSO parameters.

Parameter Description	Symbol	Value
Number of particles	M	50
Maximum generations	$L_{max}$	100
Maximum inertia weight	$w_{max}$	0.9
Minimum inertial weight	$w_{min}$	0.4
Acceleration coefficients	$c_1$, $c_2$	2.0

, and the MPC parameters are given in

Table 2. Simulation parameter values.

Parameter Description	Symbol	Value
Activation threshold	$J_{\mathrm{active}}$	0.5
Deactivation threshold	$J_{\mathrm{inactive}}$	0.1
Safe distance	$D_{\mathrm{safe}}$	1.0 m
Maximum linear velocity	$v_{\max }$	1.5 m/s
Maximum angular velocity	${\omega }_{\max }$	1.5 rad/s
Sampling interval	$\delta t$	0.1 s
Prediction steps	N	70
State error weights	Q	diag (0.1, 0.1, 0.01)
Control input error weights	R	diag (0.04, 0.04)
Terminal state weight	$\gamma$	10
Potential field weight	$\kappa$	40
Grid cell size	$C_0\times C_0$	1 m × 1 m

. The parameters $w_{max}$, $w_{min}$, $c_1$, and $c_2$ are set to values commonly used in standard PSO implementations. However, both the number of particles M and the maximum number of iterations $L_{max}$ are limited to 50 and 100, respectively, which are significantly smaller than typically used in standard PSO. While using more particles or iterations can potentially yield higher-quality solutions, it also increases computation time. In this study, these limits were selected to allow for real-time computation, while introducing a potential field to help guide the IG-PSO toward the global optimum.

To evaluate the performance of the proposed method in an environment with a nonconvex obstacle, a scenario resembling the shape shown in Figure 2 was implemented in the Gazebo physics simulator. Here, a grid with unit cells of size 1 m × 1 m was overlaid on the obstacle, and the centers of nine occupied grid cells were extracted. Then, the collision avoidance constraint in (11) was enforced to ensure that the distance between the robot and each occupied cell center remained at least $D_{\mathrm{safe}}$.

4.2. Results

The results of the real-time simulation of the environment with a nonconvex obstacle using Gazebo and the ROS are shown in Figure 4. Here, the shaded gray regions represent the grid cells occupied by the obstacle, and the dashed line indicates the reference trajectory assigned to the robot, which was designed to lead toward the nonconvex obstacle. The robot’s initial pose was set to ${\xi }_0={[0,0,0]}^T$, and it was instructed to move straight toward the obstacle at $1.0$ m/s along the positive x-axis. The solid blue line, with circular markers sampled every 1.7 s, shows the actual trajectory of the virtual robot in Gazebo. Figure 4a shows that the conventional IPOPT-based MPC becomes trapped in a local minimum due to the nonconvex shape of the obstacle and is unable to escape. In contrast, Figure 4b demonstrates that the proposed MPC method initialized by IG-PSO can find a trajectory that avoids the obstacle and subsequently converges to the reference trajectory, even though the scenario was designed to force the robot into the concave region of the obstacle, which would typically lead to a local minimum.

Figure 5a,b shows snapshots from the ROS 3D visualization tool Rviz, illustrating the Gazebo simulation results for the conventional IPOPT-based MPC and the proposed MPC initialized by IG-PSO, respectively. In both figures, the thick gray line indicates the reference trajectory over the prediction horizon, and the black arrows indicate the planned trajectory computed by the optimizer over the prediction horizon. Figure 5a shows that the conventional IPOPT-based MPC remains stuck in a local minimum and cannot generate an escape trajectory because the initial guess for the optimization is taken directly from the solution found at the previous time step, thereby preventing the generation of trajectories that can escape the local minimum. Figure 5b shows that while the proposed MPC initialized IG-PSO behaves similarly to the conventional MPC up to approximately 3 s, it successfully generates trajectories that avoid the nonconvex obstacle and reduce the terminal state error over the prediction horizon to near zero after 4 s. Note that a terminal state error approaching zero indicates that the controller is stable and ensures that the robot converges to the reference trajectory.

The results shown in Figure 6 demonstrate that the conventional MPC fails to find a trajectory that avoids the local minimum, resulting in the total cost J and terminal state error values diverging over time. Figure 7 shows the total cost J and terminal state error for the proposed MPC initialized by IG-PSO, where the plots mark the time steps at which IG-PSO is activated using red diamonds. Up to approximately 3.5 s, both the total cost and terminal state error increase; however, after 3.5 s, the terminal state error converges to zero. The proposed method activates IG-PSO until the total cost approaches zero, which helps the controller avoid becoming stuck in the local minima. The convergence of the terminal state error to zero indicates that a trajectory avoiding the local minimum has been found, with the total cost gradually converging toward zero over time.

To verify the real-time feasibility of the proposed algorithm, the computation time for the optimization process at each time step was measured, and the results are shown in Figure 8. Here, the computation time was measured using a laptop equipped with an Intel Core i7 processor, 32 GB of RAM, and Ubuntu 20.04 operating system. Figure 8 shows the computation times with and without the IG-PSO algorithm for different grid sizes. In Figure 8a, which corresponds to a grid size of 1.0 m, the blue dots represent the times when IG-PSO was inactive, indicating the use of only IPOPT, and the red dots show the total optimization times when IG-PSO was active, including both IG-PSO and IPOPT. When IG-PSO was active, the average computation time was approximately 50 ms, which confirms that the proposed method satisfies the robot’s real-time control requirements. In addition, IG-PSO is activated only when a trajectory is required to escape the local minima induced by nonconvex obstacles. Thus, the measured computation times are considered reasonable for practical applications. In Figure 8b, which corresponds to a grid size of 0.5 m, the number of collision avoidance constraints increases, leading to higher computational complexity and, consequently, longer computation times. Nevertheless, the average computation time with IG-PSO was measured to be about 65 ms, which is still considered reasonable for practical applications, given that our target platform is an indoor mobile robot operating at relatively low speeds.

To evaluate the capability of the proposed algorithm in escaping local minima, additional simulations were performed in a more challenging scenario. The scenario illustrated in Figure 9 consists of three long obstacles designed to guide the robots through a narrow gap and then block their trajectory with another elongated obstacle, which causes the robots to become trapped in a local minimum. In Figure 9a, the conventional IPOPT-based MPC becomes trapped because a blocking obstacle is placed ahead, preventing the robots from moving forward. In contrast, Figure 9b demonstrates that the proposed IG-PSO-based MPC can still find a path to avoid the blocking obstacle, even after falling into the local minimum within the narrow gap. These results indicate that the proposed method not only handles nonconvex obstacle shapes, but is also capable of escaping from situations where the robots are already trapped in a local minimum.

4.3. Discussion on Map Complexity and Computational Performance

The ability of the proposed algorithm to escape local minima is closely related to both map complexity and computation time. Allowing for more computation time generally increases the chance of finding solutions that escape local minima, since the global search has more time to explore the solution space. However, to ensure applicability in real-time robot control, the exploration time of the global optimizer must be restricted.

In the proposed framework, optimization is performed within the prediction horizon. Thus, only obstacles inside the reachable region ${\mathcal{R}}_H$ defined in (1) are considered. Consequently, the overall map size does not directly affect the computation time. Instead, the level of detail in which obstacles are represented plays a critical role. As each occupied cell adds additional obstacle avoidance constraints, smaller grid sizes increase the overall computational complexity. This consideration is particularly important for real-time control, where computation time must be limited to ensure timely trajectory updates. To maintain real-time feasibility for indoor mobile robot applications, the computation time was limited to a maximum of 100 ms. Accordingly, the number of particles and maximum iterations of the global optimizer were fixed to ensure that the optimization process consistently terminates within this bound. While this setting guarantees real-time control, it also introduces a probability that the algorithm may fail to find an escape trajectory within the limited time.

To analyze this trade-off, repeated experiments were conducted for Scenario 1, described in Figure 4, and Scenario 2, described in Figure 9, each with ten runs. The results are summarized in

Table 3. Number of successful trials out of ten for each scenario.

Scenarios	Conventional IPOPT-Based MPC	Proposed MPC Initialized by IG-PSO
Scenario 1	0/10	8/10
Scenario 2	0/10	10/10

. The conventional IPOPT-based MPC failed in all trials of both scenarios, confirming that, once trapped in a local minimum, the robot cannot recover due to the inherent limitations of local optimization. By contrast, the proposed method succeeded in eight out of ten trials for Scenario 1 and in all ten trials for Scenario 2. The two failures in Scenario 1 occurred due to the stochastic nature of the global search, where insufficient diversity in the particle initialization occasionally prevented the discovery of an escape solution. Nevertheless, this limitation can be mitigated by increasing the number of particles, at the expense of higher computation time, highlighting the trade-off between scalability and real-time performance.

5. Conclusions

This paper proposes a nonlinear MPC framework that is initialized by the IG-PSO algorithm to address the local minimum problem encountered in nonconvex obstacle environments. By integrating a PSO-based global search method to generate effective initial guesses, the proposed method improves the ability of the MPC to find feasible and collision-free robot trajectories. In addition, a grid-based representation of nonconvex obstacles is employed to formulate the collision avoidance constraints within the MPC framework. The results of real-time simulations conducted in the ROS and Gazebo demonstrates that the proposed MPC initialized by the IG-PSO algorithm avoids local minima and generates trajectories that converge to the reference path while also satisfying safety constraints. Comparisons with a conventional IPOPT-based MPC highlight its improved performance in terms of both trajectory feasibility and terminal state error convergence. Furthermore, the results of a computation time analysis demonstrated that the proposed method maintains real-time capability by selectively activating the IG-PSO only as required.

The proposed method has been validated using simulations for indoor mobile robots, demonstrating effective collision-free trajectory planning in environments with nonconvex obstacles. For practical deployment, the approach can be applied to various scenarios, such as indoor environments where obstacles are randomly scattered due to a disaster, or outdoor environments with irregular terrain, such as construction sites, where robust and efficient trajectory planning is required for safe navigation.