Optimized Model Predictive Control-Based Path Planning for Multiple Wheeled Mobile Robots in Uncertain Environments

Abstract

Addressing the path planning problem for multiple wheeled mobile robots (WMRs) in uncertain environments, this paper proposes a multi-WMR path planning algorithm based on the fusion of artificial potential field and model predictive control. Firstly, an artificial potential field model for uncertain environments is established based on the APF method. Secondly, an MPC optimal controller that considers the artificial potential field model is designed to ensure the smooth avoidance of moving and concave obstacles by multiple WMRs in uncertain environments. Additionally, a formation control algorithm based on an enhanced APF method and the leader–follower algorithm is proposed to achieve formation maintenance, intra-formation collision avoidance, and obstacle circumvention, thereby ensuring formation stability. Finally, two sets of simulation experiments in uncertain environments demonstrate the effectiveness and superiority of the proposed method compared to the APF-MPC algorithm, enabling the control of multiple WMRs to reach their target positions safely, smoothly, and efficiently. Furthermore, two sets of real-world experiments validate the feasibility of the algorithm proposed in this paper.

1. Introduction

In recent years, with the rapid development of disciplines such as material science and computer science, the demand for wheeled mobile robots (WMRs) has been increasing in various fields including industrial production, aerospace, and healthcare [1]. The Mecanum wheel mobile robot (MWMR) is a type of wheeled robot equipped with four Mecanum wheels [2]. Due to its excellent maneuverability and stability, it has been widely applied in transportation tasks in uncertain environments [3,4].

For multi-MWMR systems, two key issues must be addressed: path planning and formation control [5]. Path planning is primarily responsible for generating a safe and feasible path that connects the initial position of the MWMR to its target location [6]. Formation control methods are primarily used to regulate the relative distances and angles between MWMRs [7]. These methods enable the MWMRs to work collaboratively and complete tasks while maintaining a specific formation. Effective path planning algorithms and formation control algorithms can significantly enhance the efficiency of multi-MWMR systems in completing tasks [8]. They also help reduce the risk of system failures [9].

Path planning algorithms are generally divided into two categories: global path planning algorithms and local path planning algorithms [10]. Global trajectory planning algorithms [11] require prior knowledge of global environmental information. In contrast, local path planning algorithms plan the path based on real-time environmental data detected by sensors. The A-star algorithm is a commonly used global trajectory planning algorithm. Lin et al. [12] proposed an improved version of the A-star algorithm, which introduces redundant safety space and a prediction-based rule strategy. This enhancement accelerates the path planning process, reduces the number of path waypoints, and improves path smoothness. However, it is only applicable to static, known environments. Gan et al. [13] proposed a DP-A * algorithm that integrates reinforcement learning with the A-star algorithm. This approach improves both path quality and convergence speed. However, it does not take into account the avoidance of moving obstacles. Commonly used local path planning methods include algorithms such as the rapidly exploring random tree (RRT) algorithm [14], the artificial potential field (APF) method [15], and model predictive control (MPC) [16]. The RRT algorithm is a path planning method based on random sampling [17]. During the path planning process, it is characterized by relatively long computation times and a high degree of randomness. Meng et al. [18] proposed the NR-RRT algorithm, which combines the RRT algorithm with neural networks. This approach accelerates the convergence to an approximately optimal path. However, it is not suitable for uncertain environments. Qi et al. [19] proposed the MOD-RRT * algorithm. First, the RRT algorithm is used to generate an initial path. Then, the initial path is replanned to further reduce the path planning time and improve the path smoothness. However, this approach overlooks the presence of moving obstacles. The aforementioned methods are effective in solving path planning problems in environments with no obstacles or simple obstacles. However, in uncertain environments, the performance of these algorithms may be constrained. Therefore, there is a need for a more adaptive path planning algorithm. The APF algorithm has the advantages of low computational complexity, fast response, and strong real-time performance [20]. However, it suffers from issues of local minima and unreachable goals [21]. Li et al. [22] proposed a DynEFWA-APF, which improves the expressions for both repulsive and attractive potential fields. The path is then planned using a combination of gradient descent and firework algorithms. This approach effectively addresses the issues of unreachable goals and local minima. MPC is a commonly used path planning algorithm for MWMRs. Its main advantage lies in its ability to address control problems in multi-input, multi-output systems. MPC handles multiple constraints of the model by solving an optimization problem [23]. Hernandez Vicente et al. [24] proposed a TMPC. They introduced a Linear Time-Invariant (LTI) model for lane-keeping, which improved the computational efficiency of the online optimization problem. In traditional MPC path planning problems, obstacles are typically denoted as hard constraints within the optimization problem. This can lead to situations where the optimization problem has no feasible solution. Zuo et al. [25] and Yang et al. [26] developed obstacle potential field models based on the artificial potential field. They incorporated these models into the objective function of the MPC path planning controller. This approach enables the calculation of the optimal path, considering both obstacle avoidance and vehicle kinematic constraints. The proposed algorithms combine the advantages of both the APF algorithm and MPC. However, neither approach addresses the issues of local minima and unreachable goals inherent in the artificial potential field method [27]. Reinforcement learning (RL) algorithms are also commonly used in path planning tasks. They can interact with the external environment, learn from feedback received, and adjust their strategies to reduce dependency on the environment. However, these algorithms face challenges such as slow convergence and the risk of getting stuck in local minima [28]. Wang et al. [29] proposed a deep reinforcement learning (DRL) path planning algorithm for unmanned aerial vehicles (UAVs) based on cumulative rewards and regional segmentation. This algorithm leverages a cumulative reward model and regional segmentation to accelerate convergence and reduce the risk of falling into local minima. However, in complex environments, the algorithm is constrained by the energy and computational limitations of UAV platforms. It is unable to update the network during the path planning process, making it unsuitable for practical applications.

Common formation control methods include leader–follower, virtual structure, and behavior-based approaches [30]. Among them, the leader–follower method has gained widespread attention from researchers due to its simplicity and stability advantages in formation modeling [31]. In practical formation control, in addition to maintaining the formation shape, it is also necessary to consider intra-formation collision avoidance and obstacle avoidance [32]. He et al. [33] proposed a decentralized leader–follower formation control method, which uses adaptive control and neural networks to compensate for uncertain disturbances and nonlinear dynamic equations. The method also limits the position outputs of USVs to avoid potential internal collisions within the formation, but does not address obstacle avoidance. Pan et al. [34] introduced a path planning and formation control algorithm based on an improved APF algorithm. This approach considers the issues of local minima, unreachable goals, as well as intra-formation collision and obstacle avoidance. However, it does not provide the optimal path based on UAV kinematic constraints.

For the path planning and formation control of multi-MWMR systems, this paper proposes a multi-MWMR path planning and formation control method based on the integration of the artificial potential field and model potential control. The main innovations are as follows:

(1)

Based on the MWMR’s driving environment and kinematic model, an artificial potential field model is developed. Compared to the APF-MPC algorithm [35], the artificial potential field model proposed in this paper simultaneously constructs both repulsive and attractive potential fields. Furthermore, it addresses the issues of goal inaccessibility and local minima. The objective function of the MPC controller is incorporated into this model, resulting in a path planning algorithm that combines the artificial potential field and MPC. This algorithm generates smooth, collision-free trajectories for multiple MWMRs in uncertain environments.

(2)

An enhanced APF-based formation control method, integrated with the leader–follower approach, is proposed to ensure that the multi-MWMR system not only maintains the formation but also avoids internal collisions and obstacles while planning the optimal path.

The remainder of this paper is organized as follows: Section 2 develops the MWMR kinematic model for the path planning algorithm design, as well as the multi-MWMR model based on the leader–follower approach, and establishes the artificial potential field model for the driving environment. Section 3 provides a detailed description of the multi-MWMR path planning algorithm based on the integration of the artificial potential field and MPC and presents the design of the leader–follower formation controller based on the enhanced APF algorithm. Section 4 evaluates the effectiveness of the proposed method through simulation experiments in two different driving environments and further validates the feasibility of the algorithm through real-world experiments. Finally, Section 5 concludes the paper.

2. System Description of Multi-MWMR Path Planning Problem

This section provides the system description of the multi-MWMR path planning problem. First, the kinematic model of an individual MWMR in the formation was established. Next, based on the leader–follower method, the kinematic model of the multi-MWMR system was presented. Finally, a potential field model for the target points and obstacles was developed, taking into account the driving environment of the multi-MWMRs. To simplify the path planning problem for multiple MWMRs, we make the following basic assumptions:

(1)

The ground is relatively flat to prevent the MWMR sensors from being obstructed, and the distance between two points is considered as the simple Euclidean distance.

(2)

All MWMRs in the formation have the same model and configuration.

(3)

The MWMRs in the formation can communicate with each other in real time.

2.1. Establishment of the Kinematic Model for an Individual MWMR

In the path planning process, it is essential to study the motion characteristics of the MWMR based on the kinematic model. This includes analyzing state variables such as the MWMR’s velocity and heading angle, which vary with time. A prediction model is then established for the model predictive controller based on the kinematic model. Ultimately, this leads to the determination of the optimal control sequence for the path planning problem.

The commonly used WMR models include the two-wheel differential drive model, the Ackermann steering model, and the Mecanum wheel drive model. This paper adopts the Mecanum wheel drive model shown in Figure 1, where A, B, C, and D are denoted as the four driving wheels responsible for providing power to the MWMR. The velocities of the i-th MWMR’s four Mecanum wheels are denoted as $v_{ai},v_{bi},v_{ci},v_{di}$, respectively. Let the position of the center of the i-th MWMR in the inertial coordinate system be denoted as $q_i=\left(x_i,y_i\right)$. The longitudinal and lateral velocities of the MWMR are $u_i$ and $v_i$, respectively, while the instantaneous angular velocity is ${\omega }_i$, and the yaw angle of the MWMR is ${\theta }_i$.

Given that the distance between the left and right wheels of the MWMR is $W$, and the distance between the front and rear wheels is $H$, the velocities $v_{ji}(j=a,b,c,d)$ of the wheels of the i-th MWMR at a certain moment can be expressed as Equation (1):

$$\left\{\begin{array}{c}{v}_{ai}=u_i+v_i-{\omega }_i\cdot ({\displaystyle \frac{H+W}{2}})\\ v_{bi}=u_i-v_i-{\omega }_i\cdot ({\displaystyle \frac{H+W}{2}})\\ v_{ci}=u_i+v_i+{\omega }_i\cdot ({\displaystyle \frac{H+W}{2}})\\ v_{di}=u_i-v_i+{\omega }_i\cdot ({\displaystyle \frac{H+W}{2}})\end{array}\right.$$

(1)

The longitudinal velocity $u_i$ of the MWMR can be expressed as Equation (2):

$$u_i={\displaystyle \frac{v_{ai}+v_{bi}+v_{\mathit{ci}}+v_{\mathit{di}}}{4}}$$

(2)

The lateral velocity $v_i$ can be expressed as Equation (3):

$$v_i={\displaystyle \frac{v_{\mathit{ai}}-v_{\mathit{bi}}+v_{\mathit{ci}}-v_{\mathit{di}}}{4}}$$

(3)

The angular velocity ${\omega }_i$ can be expressed as Equation (4):

$${\omega }_i={\displaystyle \frac{-v_{\mathit{ai}}-v_{\mathit{bi}}+v_{\mathit{ci}}+v_{\mathit{di}}}{2\cdot (H+W)}}$$

(4)

In summary, the kinematics of the Mecanum wheel MWMR in the inertial coordinate system can be expressed as Equation (5):

$$\left\{\begin{array}{c}\dot{x}=u\cdot \cos \theta +v\cdot \sin \theta \\ \dot{y}=u\cdot \sin \theta -v\cdot \cos \theta \\ \dot{\theta }=\omega \end{array}\right.$$

(5)

The matrix form of Equation (5) is given by Equation (6):

$$\dot{X}(t)=\left[\begin{array}{ccc}\cos \theta & \sin \theta & 0\\ \sin \theta & -\cos \theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{\displaystyle \frac{1}{4}}& {\displaystyle \frac{1}{4}}& {\displaystyle \frac{1}{4}}& {\displaystyle \frac{1}{4}}\\ {\displaystyle \frac{1}{4}}& -{\displaystyle \frac{1}{4}}& {\displaystyle \frac{1}{4}}& -{\displaystyle \frac{1}{4}}\\ -{\displaystyle \frac{1}{2(H+W)}}& -{\displaystyle \frac{1}{2(H+W)}}& {\displaystyle \frac{1}{2(H+W)}}& {\displaystyle \frac{1}{2(H+W)}}\end{array}\right]u(t)$$

(6a)

$$u_{\min }\le u\le u_{\max }$$

(6b)

where $X={\left[\begin{array}{ccc}x& y& \theta \end{array}\right]}^T$ denotes the state variables, and $u={\left[\begin{array}{cccc}{v}_a& v_b& v_c& v_d\end{array}\right]}^T$ denotes the control inputs.

2.2. Kinematic Model Development for Multiple MWMRs Based on the Leader–Follower Method

The leader–follower method is widely used in the field of multi-agent formation control due to its advantages, such as reliability and low communication overhead. It meets the requirements for multi-MWMR path planning tasks in uncertain environments, as outlined in this paper. As shown in Figure 2, A_L denotes the Leader MWMR, and A_F denotes the Follower MWMR. The distance between A_L and A_F is denoted by $d$, and $\phi$ is the angle between the MWMR’s orientation and the distance $d$. According to the leader–follower method, the desired position $(x_{ref},y_{ref})$ of the Follower MWMR A_F can be determined from the state variable X of the Leader MWMR A_L. During the formation process, the Follower MWMR maintains the formation by adjusting its position according to the desired coordinates. The relationship is given by Equation (7):

$$\left\{\begin{array}{c}{x}_F=x_L+d\cdot \cos \left({\theta }_L+\phi \right)\\ y_F=y_L+d\cdot \sin \left({\theta }_L+\phi \right)\end{array}\right.$$

(7a)

$$\left\{\begin{array}{c}{x}_{F,ref}=x_L+d_{ref}\cdot \cos \left({\theta }_L+{\phi }_{ref}\right)\\ y_{F,ref}=y_L+d_{ref}\cdot \sin \left({\theta }_L+{\phi }_{ref}\right)\end{array}\right.$$

(7b)

The kinematic model of the Follower MWMR is shown in Equation (8):

$$\left\{\begin{array}{c}{\dot{x}}_{F,ref}=u_L\cdot \cos {\theta }_L+v_L\cdot \sin {\theta }_L\\ {\dot{y}}_{F,ref}=u_L\cdot \sin {\theta }_L-v_L\cdot \cos {\theta }_L\end{array}\right.$$

(8)

2.3. Establishment of the Driving Environment Model for MWMRs

The construction of the driving environment model is an essential prerequisite for designing MWMR path planning algorithms. In this study, we use the APF algorithm to build the MWMR’s driving environment model, with a focus on solving the problem of MWMR avoidance of concave obstacles and moving obstacles in uncertain environments. In the APF algorithm, obstacles $q_o=(x_o,y_o)$ and the target point $q_t=(x_t,y_t)$ generate repulsive and attractive potential fields, denoted as $P_{rep}$ and $P_{att}$, respectively. The repulsive potential field $P_{rep}$ repels the MWMR, reaching its maximum value at the position of the obstacle. The attractive potential field $P_{att}$ attracts the MWMR, reaching its minimum value at the target location. When the MWMR moves within the resultant potential field $P_{total}$, which is the combination of both attractive and repulsive fields, it moves from the initial position with higher potential to the target position with lower potential, thus planning a collision-free path from the initial to the target position. The resultant potential field $P_{total}$ can be expressed by Equation (9):

$$P_{total}=P_{att}+P_{rep}$$

(9)

In this section, given the known initial position and target point, an improved artificial potential field is proposed for the real-time path planning of MWMRs in a two-dimensional complex environment. The elaborate architecture of the potential field model within the context of the driving environment is delineated hereinafter.

2.3.1. Target Point

In the path planning process, it is necessary to obtain a smooth and collision-free path connecting the initial position to the target position. To guide the MWMR toward the target location, the attractive potential field function (10) is used to describe the target position as follows:

$$P_{att}=K_{att}\cdot d\left(q,q_t\right)$$

(10)

where $K_{att}$ is a parameter of the attractive potential field, which determines the magnitude and shape of the attractive field. $d\left(q,q_t\right)=‖q-q_t‖=\sqrt{{\left(x-x_t\right)}^2+\left({\left(y-y_t\right)}^2\right)}$ denotes the Euclidean distance between the MWMR and the target position. The value of this function decreases as the distance between the MWMR and the target point reduces, ensuring that the MWMR, within the attractive potential field, ultimately reaches the target location.

2.3.2. Uncrossable Obstacle

When the MWMR navigates in an uncertain environment, it may encounter uncrossable obstacles, such as rocks, walls, etc. Forcing the MWMR to cross these obstacles could cause damage to the vehicle. The repulsive potential field function for uncrossable obstacles is defined as Equation (11). The model of the repulsive potential field is shown in Figure 3.

$$P_{rep}=\left\{\begin{array}{cc}{a}_{rep}\cdot {\displaystyle \frac{D_s\cdot \ln \left(1+d{\left(q,q_t\right)}^{b_{rep}}\right)}{d{\left(q-q_o\right)}^{c_{rep}}}}& d\left(q-q_o\right)\le \rho \\ 0& d\left(q-q_o\right)>\rho \end{array}\right.$$

(11)

where $a_{rep}$, $b_{rep}$, and $c_{rep}$ are parameters of the repulsive potential field, which determine the magnitude and shape of the repulsive field. $d\left(q,q_o\right)=‖q-q_o‖=\sqrt{{\left(x-x_o\right)}^2+\left({\left(y-y_o\right)}^2\right)}$ denotes the Euclidean distance between the MWMR and the obstacle. $\rho$ is the maximum detection range of the MWMR’s sensors. $D_s$ is the safety distance between the MWMR and the obstacle, which can be expressed by Equation (12).

$$D_s=D_0+\left({\dot{q}}_o\cdot \cos \left(\beta /2\right)-\dot{q}\cdot \left|\sin \left(\alpha /2\right)\right|\right)\cdot T_0$$

(12)

In this context, $D_0$ denotes the minimum safe distance between the MWMR and the obstacle, while $T_0$ is the safe time interval. ${\dot{q}}_o$ denotes the velocity of the obstacle, and $\dot{q}=\sqrt{v^2+u^2}$ refers to the velocity of the MWMR. $\alpha$ is the angle between the direction of the MWMR’s velocity and the Euclidean distance line connecting the MWMR and the obstacle, denoted as $d\left(q,q_o\right)$. $\beta$ denotes the angle between the velocity direction of the obstacle and the direction of the Euclidean distance between the MWMR and the obstacle, as shown in Figure 4.

For stationary obstacles located far from the target position, the rate of change in the repulsive potential field function denoted by Equation (11) increases as the distance between the MWMR and the obstacle decreases. Ultimately, it tends to infinity, causing the MWMR to move away from the obstacle. For stationary obstacles near the target position, the repulsive potential field function has a global minimum at the target location, which effectively prevents the problem of unreachable targets.

For moving obstacles, the value of the repulsive potential field is influenced by both the relative position and the relative velocity between the MWMR and the obstacle. A typical process for avoiding moving obstacles is shown in Figure 5. As illustrated in Figure 5a, when a moving obstacle appears directly ahead of the MWMR, with $\alpha =\beta =0$, the expression ${\dot{q}}_o\cdot \cos \left(\beta /2\right)-\dot{q}\cdot \left|\sin \left(\alpha /2\right)\right|={\dot{q}}_o$ holds. In this case, the value of the repulsive potential field is determined by the distance between the MWMR and the obstacle. The MWMR moves in the direction away from the obstacle. In Figure 5b, when the MWMR begins to bypass the obstacle, it accelerates in the direction where $\alpha$ and $\beta$ increase; that is, the MWMR accelerates to move around the obstacle. As shown in Figure 5c, when the movement direction of the MWMR is parallel to that of the obstacle, with $\alpha =\beta =\pi /2$, the expression ${\dot{q}}_o\cdot \cos \left(\beta /2\right)-\dot{q}\cdot \left|\sin \left(\alpha /2\right)\right|=\sqrt{2}\left({\dot{q}}_o-\dot{q}\right)/2$ holds. In this situation, the MWMR continues to accelerate in the direction where $\alpha$ and $\beta$ increase, thereby bypassing the obstacle. Finally, in Figure 5d, after obstacle avoidance is complete, the obstacle is positioned directly behind the MWMR, with $\alpha =\beta =\pi$, and the expression ${\dot{q}}_o\cdot \cos \left(\beta /2\right)-\dot{q}\cdot \left|\sin \left(\alpha /2\right)\right|=-\dot{q}$ holds. The faster the MWMR moves, the lower the probability of collision with the obstacle.

For concave obstacles, as shown in Figure 6, the MWMR will move in the direction where $\alpha$ increases. This allows the MWMR to bypass the concave obstacle.

3. Design of Multi-MWMR Path Planning Algorithm

As MWMR application scenarios become more complex, individual MWMRs face issues such as low efficiency and insufficient capabilities. The increasingly uncertain operating environments and complex task requirements make it inevitable for MWMR systems to evolve towards clustering, autonomy, and intelligence. In this section, we designed a multi-MWMR path planning algorithm. To ensure real-time path planning for multiple MWMRs in uncertain environments, we proposed a path planning controller based on the integration of the artificial potential field and MPC, specifically for the path planning of the Leader MWMR. For the formation control problem in multi-MWMR systems, we proposed a leader–follower method based on the enhanced APF algorithm. The desired positions of the Follower MWMRs were treated as the goal points of the enhanced APF algorithm, as determined by Equation (7). The remaining MWMRs in the formation and the obstacles were treated as obstacles in the enhanced APF algorithm. This approach achieves low computational cost while maintaining the multi-MWMR formation, avoiding complex obstacles, and ensuring collision avoidance within the formation.

3.1. Design of Path Planning Controller for MWMRs

In this section, we first linearized and discretized the kinematic model of the MWMR presented in Section 2 to obtain the prediction model. Next, we designed the objective function for the MWMR path planning problem, incorporating both repulsive and attractive potential fields into the objective. We then proposed an optimal path planning controller that comprehensively considers the MWMR’s kinematics and the driving environment.

The MPC algorithm is capable of predicting the MWMR’s response over the next $N_p$ time steps. Based on these predictions, it optimizes factors such as MWMR kinematics and obstacle avoidance, thereby generating an optimal control sequence for the following $N_c$ time steps. To obtain a collision-free path for the MWMR, we designed an objective function for path planning based on the prediction model, which is delineated hereinafter.

3.1.1. Linearization and Discretization of the Kinematic Model

Based on the kinematic model of the MWMR (Equation (6)), the kinematic model can be reformulated as Equation (13):

$$\left\{\begin{array}{c}\dot{X}(t)=f\left(X(t),u(t)\right)\\ y(t)=CX(t)\end{array}\right.$$

(13)

where $y\left(t\right)$ denotes the system output, and $C$ is the output matrix of the system.

We assume that, at time t, there is a small error between the current position $\left(X(t),u(t)\right)$ and the desired position $\left(X_r(t),u_r(t)\right)$. The state Equation (13) at $\left(X(t),u(t)\right)$ can be expanded using a Taylor series, neglecting the quadratic and higher-order terms. This yields Equation (14):

$$\dot{X}(t)\approx {\dot{X}}_r(t)+A_t\left[X(t)-X_r(t)\right]+B_t\left[u(t)-u_r(t)\right]$$

(14)

In this case, $A_t$ and $B_t$ are the partial derivative matrices of the state Equation (11) with respect to the system state variables and input variables, respectively. They can be expressed as Equation (15):

$$A_t=\left[\begin{array}{ccc}0& 0& -v_{ref}\sin {\theta }_{ref}\\ 0& 0& v_{ref}\cos {\theta }_{ref}\\ 0& 0& 0\end{array}\right]$$

(15a)

$$B_t=\left[\begin{array}{ccc}\cos \theta & \sin \theta & 0\\ \sin \theta & -\cos \theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{\displaystyle \frac{1}{4}}& {\displaystyle \frac{1}{4}}& {\displaystyle \frac{1}{4}}& {\displaystyle \frac{1}{4}}\\ {\displaystyle \frac{1}{4}}& -{\displaystyle \frac{1}{4}}& {\displaystyle \frac{1}{4}}& -{\displaystyle \frac{1}{4}}\\ -{\displaystyle \frac{1}{2(H+W)}}& -{\displaystyle \frac{1}{2(H+W)}}& {\displaystyle \frac{1}{2(H+W)}}& {\displaystyle \frac{1}{2(H+W)}}\end{array}\right]$$

(15b)

$$X_r=\left[\begin{array}{c}{x}_{ref}\\ y_{ref}\\ {\theta }_{ref}\end{array}\right]$$

(15c)

Next, we discretize the state Equation (14) using the Euler method. Let the sampling time be denoted as T. The discrete form is then obtained, as shown in Equation (16):

$$\overline{X}(k+1)=\overline{A}\cdot \overline{X}(k)+\overline{B}\cdot \overline{u}(k)$$

(16)

where

$$\begin{array}{ccc}\overline{A}=I_{3\times 3}+T\cdot A_t& ,& \overline{B}=\end{array}T\cdot B_t$$

(17a)

$$\overline{X}=X-X_r,\overline{u}=u-u_r$$

(17b)

To further eliminate the steady-state error, we augment the system variables by incorporating the control input $\overline{u}(k)$ and state variable $\overline{X}(k+1)$, resulting in a new state variable $\xi (k+1)={\left[\begin{array}{cc}\overline{X}(k+1)& \overline{u}(k)\end{array}\right]}^T$. Therefore, the predictive model can be expressed as Equation (18):

$$\left\{\begin{array}{c}\xi (k+1)=\tilde{A}\xi (k)+\tilde{B}\Delta \overline{u}(k)\\ \eta (k+1)=\tilde{C}\xi (k)\end{array}\right.$$

(18)

where $\tilde{A}=\left[\begin{array}{cc}{\overline{A}}_{3\times 3}& {\overline{B}}_{3\times 4}\\ {\mathrm{O}}_{4\times 3}& {\mathrm{I}}_{4\times 4}\end{array}\right]$, $\tilde{B}=\left[\begin{array}{c}{\overline{B}}_{3\times 4}\\ {\mathrm{I}}_{4\times 4}\end{array}\right]$, $\tilde{C}=\left[\begin{array}{cc}{\mathrm{I}}_{3\times 3}& {\mathrm{O}}_{3\times 4}\end{array}\right]$, $\Delta \overline{u}(k)=\overline{u}(k)-\overline{u}(k-1)$.

Based on the prediction model in Equation (18), we can derive a predictive equation to forecast the output over a future time horizon. The predictive equation can be expressed as Equation (19).

$$Y(k)=\mathsf{\Psi }\xi (k)+\mathsf{\Theta }\Delta U(k)+\mathsf{\Gamma }D(k)$$

(19a)

where

$$\begin{array}{l}\begin{array}{ccc}Y(k)=\left[\begin{array}{c}\eta (k+1)\\ \eta (k+2)\\ ⋮\\ \eta (k+N_p)\end{array}\right],& \mathsf{\Psi }=\left[\begin{array}{c}\tilde{C}\tilde{A}\\ \tilde{C}{\tilde{A}}^2\\ ⋮\\ \tilde{C}{\tilde{A}}^{N_p}\end{array}\right],& \Delta U(k)=\left[\begin{array}{c}\Delta \overline{u}(k)\\ \Delta \overline{u}(k+1)\\ ⋮\\ \Delta \overline{u}(k+N_c-1)\end{array}\right],\end{array}\\ \mathsf{\Theta }=\left[\begin{array}{ccccc}\tilde{C}\tilde{B}& {\mathrm{O}}_{3\times 4}& {\mathrm{O}}_{3\times 4}& \cdots & {\mathrm{O}}_{3\times 4}\\ \tilde{C}\tilde{A}\tilde{B}& \tilde{C}\tilde{B}& {\mathrm{O}}_{3\times 4}& \cdots & {\mathrm{O}}_{3\times 4}\\ \tilde{C}{\tilde{A}}^2\tilde{B}& \tilde{C}\tilde{A}\tilde{B}& \tilde{C}\tilde{B}& \cdots & {\mathrm{O}}_{3\times 4}\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ \tilde{C}{\tilde{A}}^{N_p-1}\tilde{B}& \tilde{C}{\tilde{A}}^{N_p-2}\tilde{B}& \tilde{C}{\tilde{A}}^{N_p-3}\tilde{B}& \cdots & \tilde{C}{\tilde{A}}^{N_p-N_c}\tilde{B}\end{array}\right],\end{array}$$

(19b)

where $N_p$ denotes the prediction horizon, and $N_c$ denotes the control horizon. Typically, $N_p\ge N_c$.

3.1.2. Design of the Objective Function for the MPC Path Planning Controller

To generate a smooth, collision-free path for the MWMR, it is necessary to design a reasonable objective function and apply the optimal control increment to the MWMR model. This objective function should be based on the driving environment established in Section 2, ensuring that the output remains as close as possible to the desired value. At the same time, to suppress abrupt changes in the input, it is essential to constrain the magnitude of the control increment. Therefore, we have designed the following objective function:

$$\underset{P}{\min }J(Y,P_{total})$$

(20a)

$$\begin{array}{cc}s.t.& \begin{array}{c}{Y}_{\min }\le Y\le Y_{\max }\\ u_{\min }\le u\le u_{\max }\\ \Delta u_{\min }\le \Delta u\le \Delta u_{\max }\end{array}\end{array}$$

(20b)

The specific objective function $J(k)$ can be expressed as Equation (21):

$$J(k)={{‖Y(k|k)-Y_{ref}(k|k)‖}_Q}^2+{{‖\Delta U(k|k)‖}_R}^2+P\cdot {\displaystyle \sum _{i=1}^{N_p-1}{P}_{total}(k+i|k)}$$

(21)

In this equation, $Q$, $R$, and $P$ are positive definite and symmetric weighting matrices, and $(k+i|k)$ denotes the predicted value at the i-th step in the future, given the current time step k. The objective function $J(k)$ consists of three components: tracking, input variation, and the potential field of the predicted driving environment. The first component aims to ensure that the output tracks the predefined predicted values. The second component provides a soft constraint on the control increment to suppress abrupt changes in control inputs. The third component ensures that the MWMR generates a collision-free path.

3.2. Design of a Formation Controller for Multiple MWMRs

In the multi-MWMR path planning process based on the fusion of the artificial potential field and MPC, the Leader MWMR moves toward the target point solely under the influence of the path planning controller, without considering the Follower MWMRs in the formation. The Follower MWMRs, under the influence of the formation controller, maintain the formation and avoid obstacles and other MWMRs in the formation based on the desired positions obtained from Equation (7). The specific details are given in Algorithm 1. Considering the relative velocities between the Follower MWMRs, obstacles, and the target point, we propose an enhanced APF algorithm for the Follower MWMRs to maintain the formation in real-time and avoid obstacles, which reduces the computational burden. In the enhanced APF algorithm, the desired position of the Follower MWMRs is treated as an attractive target point to ensure the formation, while other MWMRs within the formation and external obstacles are treated as repulsive entities to avoid collisions within the formation and obstacles outside it.

Algorithm 1: Expected Position Generation Function.

Input: The current moment k, current State Variables ${\left(x_L(k+1),y_L(k+1),{\theta }_L(k+1)\right)}^T$     of Leader UGV at time k + 1, the quantity n of Follower UGVs, the expected     distance di,ref between the Leader UGV and i-th Follower UGV, the expected angle     ${\phi }_{i,ref}$ between the Leader UGV and the i-th Follower UGV.

Output: Expected Position of Follower UGV ${\left(x_F(k+1),y_F(k+1)\right)}^T$.  1:    For each Follower UGV j (j < n)  2:    The expected position $x_{F_j,ref}(k+1)$ of the j-th Follower UGV on the X-axis at      time k + 1. $x_{F_j,ref}(k+1)=x_L(k+1)+d_{j,ref}(k+1)\cdot \cos \left({\theta }_L(k+1)+{\phi }_{j,ref}(k+1)\right)$  3:    The expected position $y_{F_j,ref}(k+1)$ of the j-th Follower UGV on the Y-axis at      time k + 1. $y_{F_j,ref}(k+1)=y_L(k+1)+d_{j,ref}(k+1)\cdot \sin \left({\theta }_L(k+1)+{\phi }_{j,ref}(k+1)\right)$  4:    End for  5:    Return ${\left(x_F(k+1),y_F(k+1)\right)}^T$.

3.2.1. Attraction Force Optimization Based on the Enhanced APF Algorithm

The traditional APF algorithm only considers the relative distance between the MWMR and the target point for attraction force, which is not suitable for uncertain environments and frequently moving target points. In the case of the expected position that continuously moves with the Leader MWMR, we take into account the relative velocity between the Follower MWMRs and the target position in the enhanced APF algorithm. Additionally, to limit the maximum velocity of the Follower MWMRs, we introduce the concept of desired velocity. Based on these considerations, we derive the expression for the total attraction force $F_{att}$, which includes the distance attraction force $F_{att\_p}$, the velocity attraction force $F_{att\_v}$, and the desired velocity attraction force $F_{att\_v\_des}$, as shown in Equation (22):

$$F_{att}=F_{att\_p}+F_{att\_v}+F_{att\_v\_des}$$

(22)

(1)

Modeling of Distance Attraction Force $F_{att\_p}$

The distance attraction force ensures that the MWMR moves towards the target. The distance attraction potential field function is given by Equation (23):

$$U_{att\_p}={\displaystyle \frac{1}{2}}{K}_{att\_p}\cdot d{(q_i,q_{i,ref})}^2$$

(23)

where $k_{att\_p}$ is the positive proportional coefficient, $q_i=\left(x_i,y_i\right)$ denotes the position of the i-th Follower MWMR, and $q_{i,ref}=\left(x_{i,ref},y_{i,ref}\right)$ is the desired position of the i-th Follower MWMR.

The distance attraction force $F_{att\_p}$ is defined as the negative gradient of the distance attraction potential field $U_{att\_p}$, as shown in Equation (24):

$$F_{att\_p}=-\nabla U_{att\_p}=K_{att\_p}d(q_i,q_{i,ref})$$

(24)

(2)

Modeling of Velocity Attraction Force $F_{att\_v}$

The velocity attraction force enhances the adaptability of the MWMR to moving targets. The velocity attraction potential field function is given by Equation (25):

$$U_{att\_v}={\displaystyle \frac{1}{2}}{K}_{att\_v}\cdot {‖{\dot{q}}_i-{\dot{q}}_{i,ref}‖}^2$$

(25)

where $k_{att\_v}$ is the positive proportional coefficient.

The velocity attraction force $F_{att\_v}$ is defined as the negative gradient of the velocity attraction potential field $U_{att\_v}$, as shown in Equation (26):

$$F_{att\_v}=-\nabla U_{att\_v}=K_{att\_v}\cdot ‖{\dot{q}}_i-{\dot{q}}_{i,ref}‖$$

(26)

(3)

Modeling of Desired Velocity Attraction Force $F_{att\_v\_des}$

The desired velocity attraction force ensures that the MWMR can move at a predefined desired velocity. The desired velocity attraction potential field function is given by Equation (27):

$$U_{att\_v\_des}={\displaystyle \frac{1}{2}}{K}_{att\_v\_des}\cdot {\left(‖{\dot{q}}_{i,des}‖-‖{\dot{q}}_i‖\right)}^2$$

(27)

where $k_{att\_v\_des}$ is the positive proportional coefficient, and ${\dot{q}}_{i,des}$ denotes the desired velocity of the i-th Follower MWMR.

The desired velocity attraction force $F_{att\_v\_des}$ is defined as the negative gradient of the desired velocity attraction potential field $U_{att\_v\_des}$, as shown in Equation (28):

$$F_{att\_v\_des}=-\nabla U_{att\_v\_des}=K_{att\_v\_des}\cdot \left(‖{\dot{q}}_{i,des}‖-‖{\dot{q}}_i‖\right)$$

(28)

3.2.2. Repulsion Force Optimization Based on the Enhanced APF Algorithm

The traditional APF algorithm considers only the relative distance between the MWMR and obstacles for repulsion force. However, it is not suitable for uncertain environments that include moving and concave obstacles. In the enhanced APF algorithm, we further account for the relative velocity between the Follower MWMRs and the obstacles. This leads to the expression for the repulsion force $F_{rep}$, as shown in Equation (29):

$$F_{rep}=F_{rep\_p}+F_{rep\_v}$$

(29)

where $F_{rep\_p}$ denotes the distance repulsion force, and $F_{rep\_v}$ denotes the velocity repulsion force.

(1)

Modeling of Distance Repulsion Force

To address the unreachable goal issue of the traditional APF algorithm, we propose a distance repulsion potential field $U_{rep\_p}$, where the repulsive potential at the target point is zero. The detailed formulation is given in Equation (30):

$$U_{rep\_p}=\left\{\begin{array}{c}{\displaystyle \frac{1}{2}}{K}_{rep\_p}{({\displaystyle \frac{1}{d(q_i,q_{o,k})}}-{\displaystyle \frac{1}{\rho }})}^2\ln (1+d(q_i,q_{i,ref})),d(q_i,q_{o,k})\le {\rho }_k\\ 0,d(q_i,q_{o,k})>{\rho }_k\end{array}\right.(k=obs,j)$$

(30)

where $k_{rep\_p}$ is the proportional coefficient. $q_{o,obs}$ and $q_{o,j}$ denote the external obstacles relative to the formation and the j-th MWMR ($j\ne i$) within the formation, respectively. For external obstacles, ${\rho }_{o,obs}=\rho$. ${\rho }_{o,j}$ denotes the maximum collision-avoidance distance between MWMRs within the formation, where ${\rho }_{o,j}\le \rho$.

The repulsive force $F_{rep\_p}$ is the negative gradient of $U_{rep\_p}$, as shown in Equation (31):

$$F_{rep\_p}=\left\{\begin{array}{cc}{F}_{rep\_p1}+F_{rep\_p2}& d(q_i,q_{o,k})\le {\rho }_k\\ 0& d(q_i,q_{o,k})>{\rho }_k\end{array}\right.(k=obs,j)$$

(31a)

$$\left\{\begin{array}{c}{F}_{rep\_p1}=-K_{rep\_p}\left({\displaystyle \frac{1}{d(q_i,q_{o,k})}}-{\displaystyle \frac{1}{{\rho }_k}}\right){\displaystyle \frac{\ln \left(1+d(q_i,q_{i,ref})\right)}{d{(q_i,q_{o,k})}^2}}\\ F_{rep\_p2}={\displaystyle \frac{1}{2}}{K}_{rep\_p}{\left({\displaystyle \frac{1}{d(q_i,q_{o,k})}}-{\displaystyle \frac{1}{{\rho }_k}}\right)}^2{\displaystyle \frac{1}{1+d(q_i,q_{i,ref})}}\end{array}\right.(k=obs,j)$$

(31b)

(2)

Modeling of Velocity Repulsive Force $F_{rep\_v}$

For moving obstacles in an uncertain environment, we consider the relative velocity between the MWMR and the obstacles. A velocity repulsive potential field, as shown in Equation (32), is proposed. This field accounts for the dynamic interaction between the MWMR and the moving obstacles.

$$U_{rep\_v}=\left\{\begin{array}{cc}{K}_{rep\_v}‖{\dot{q}}_{o,k}-{\dot{q}}_i‖\cdot \left(\cos \vartheta \right)\cdot \ln \left(1+d\left(q_i,q_{i,ref}\right)\right)& d(q_i,q_{o,k})<{\rho }_k,\vartheta \in (-{\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}})(k=obs,j)\\ 0& otherwise\end{array}\right.$$

(32)

where $k_{rep\_v}$ is the proportional coefficient, and $\vartheta$ denotes the angle between the relative velocity and the relative position of the MWMR and the obstacle, as shown in Figure 7.

The velocity repulsive force $F_{rep\_v}$ is the negative gradient of $U_{rep\_v}$, as shown in Equation (33):

$$F_{rep\_v}=\left\{\begin{array}{cc}{F}_{rep\_v1}+F_{rep\_v2}& if d({\dot{q}}_i,{\dot{q}}_{o.k})<{\rho }_k and \vartheta \in (-{\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}})\\ 0& otherwise\end{array}\right.(k=obs,j)$$

(33a)

$$\begin{array}{l}{F}_{rep\_v1}=-{\nabla }_{p,o}{U}_{rep\_v}\\ =-{\nabla }_{p,o}(K_{rep\_v}‖{\dot{q}}_{o,k}-{\dot{q}}_i‖\cdot \left(\cos \vartheta \right)\cdot \ln \left(1+d\left(q_i,q_{i,ref}\right)\right))\\ =-K_{rep\_v}{\displaystyle \frac{‖\dot{q}-{\dot{q}}_o‖}{p_o}}\cdot \ln \left(1+d\left(q_i,q_{i,ref}\right)\right)\cdot \left(\sin \vartheta \right)\cdot (\sin \vartheta ,-\cos \vartheta )\end{array}$$

(33b)

$$\begin{array}{l}{F}_{rep\_v1}=-{\nabla }_{p,o}{U}_{rep\_v}\\ =-{\nabla }_{p,o}(K_{rep\_v}‖{\dot{q}}_{o,k}-{\dot{q}}_i‖\cdot \left(\cos \vartheta \right)\cdot \ln \left(1+d\left(q_i,q_{i,ref}\right)\right))\\ =-K_{rep\_v}{\displaystyle \frac{‖\dot{q}-{\dot{q}}_o‖}{p_o}}\cdot \ln \left(1+d\left(q_i,q_{i,ref}\right)\right)\cdot \left(\sin \vartheta \right)\cdot (\sin \vartheta ,-\cos \vartheta )\end{array}$$

(33c)

$$\begin{array}{l}{F}_{rep\_v3}=-{\nabla }_{p,t}{U}_{rep\_v}\\ =-{\nabla }_{p,t}(K_{rep\_v}‖{\dot{q}}_{o,k}-{\dot{q}}_i‖\cdot \left(\cos \vartheta \right)\cdot \ln \left(1+d\left(q_i,q_{i,ref}\right)\right))\\ =-{\displaystyle \frac{K_{rep\_v}‖{\dot{q}}_{o,k}-{\dot{q}}_i‖\cdot \cos \vartheta }{1+d\left(q_i,q_{i,ref}\right)}}\end{array}$$

(33d)

4. Simulation and Experiment

In this section, we validate the effectiveness of the proposed path planning algorithm and formation control algorithm in uncertain environments through both simulation and real-world experiments. First, we set up two different simulation scenarios to compare the proposed method with the APF-MPC algorithm [35], in order to verify the effectiveness and superiority of the proposed path planning algorithm. Among them, Scenario 1 is designed to test the performance of the algorithm in a simple environment with only static obstacles. Scenario 2 is used to evaluate the algorithm’s performance in a complex environment with moving and concave-shaped obstacles. The simulations are conducted on a computer with the following hardware: Intel(R) Core(TM) i5-12400F, 2.50 GHz, and 32 GB RAM. In addition, we apply the proposed path planning algorithm to the path planning of a Mecanum wheel robot, to demonstrate the practical applicability and effectiveness of the algorithm in real-world applications.

4.1. Simulation Result

For an MWMR, common task environments can be classified into simple and complex environments based on the level of complexity. In simple environments, obstacles are sparsely distributed, while, in complex environments, obstacles are densely arranged and may include hard-to-avoid concave obstacles. In this study, we select both simple and complex environments as simulation scenarios and use the APF-MPC algorithm as the comparison method. The size of the simulation scenarios is 100 m × 100 m for both cases. The performance evaluation metrics for the simulations include path length, path smoothness, longitudinal distance, velocity, and angular velocity. The detailed initial parameters for the simulation are provided in

Table 1. Initial parameters for the simulation.

Parameter	Value	Parameter	Value
Sampling time T	0.1 s	Minimum safe distance $D_0$	5 m
Prediction horizon $N_p$	40	Safety time interval $T_0$	2 s
Control horizon $N_c$	20	Desired velocity $V_{des}$ of the UGV	1.2 m/s
Maximum detection range $\rho ={\rho }_{obs}$	15 m	Maximum velocity $V_{\max }$ of the UGV	1.4 m/s
Collision-free distance ${\rho }_j$ between UGVs in a formation	7 m	Maximum acceleration $a_{\max }$ of the UGV	1 m/s2

.

The formation of multiple MWMRs is set as shown in Figure 8. The three MWMRs are positioned at the vertices of an equilateral triangle, with the desired distance between each MWMR set to $d_1=d_2=d_3=5 \mathrm{m}$.

Scenario 1: To verify the effectiveness of the algorithm proposed in this paper in simple environments, we set up the simple scenario shown in Figure 9. The Leader MWMR starts at the initial position (0, 50) and moves towards the target position (100, 50) while avoiding static obstacles. Additionally, it must also avoid a moving obstacle, which starts at position (70, 115) and moves with a speed of $V$ = 1 m/s in the negative Y-axis direction.

Figure 10a shows the trajectories of multiple MWMRs under the action of the artificial potential field and the model predictive control fusion algorithm. The red curve represents the trajectory of the Leader MWMR, while the blue and green curves represent the trajectories of the Follower MWMRs. Figure 10b,c illustrates the angular velocities and velocities of the multiple MWMRs. It can be observed that the fluctuations of both quantities remain within normal ranges.

Figure 10d represents the distance between the Leader MWMR and a static obstacle. It can be observed that, under the proposed algorithm, the multi-MWMR system is able to safely avoid static obstacles. The minimum distance between the Leader MWMR and the static obstacle is 8.7 m. At t = 64 s, the multi-MWMR system begins to avoid a moving obstacle. From Figure 10a–c, it is clear that the Leader MWMR first decelerates in the negative direction of the Y-axis. Subsequently, under the influence of the potential field model proposed in this paper, it moves in the positive direction of the Y-axis to avoid the advancing direction of the moving obstacle. Figure 10e represents the distance between the Leader MWMR and the moving obstacle, showing that the minimum distance between them is 3.6 m.

Figure 10f represents the distance between the three MWMRs in the formation. Figure 10g,h represents the errors in the X-axis and Y-axis, respectively, between the Follower MWMRs and the desired positions. From Figure 10f–h, it can be observed that the distances between the MWMRs remain within a normal range and converge after avoiding moving obstacles.

Figure 11 shows the simulation results of multiple MWMRs in Scenario 1 under the APF-MPC algorithm. By comparing Figure 10 and Figure 11, it can be observed that both the artificial potential field and MPC fusion algorithm and the APF-MPC algorithm are able to smoothly avoid static obstacles and safely navigate around moving obstacles. Comparing Figure 10b,c with Figure 11b,c, it is evident that the Leader MWMR under the artificial potential field and MPC fusion algorithm avoids obstacles more smoothly than under the APF-MPC algorithm.

To compare the performance of different path planning algorithms, we established a metric $J_{smoothness}$ to evaluate the smoothness of the path, as described in reference [36]. As shown in Equation (34), N denotes the total number of path points, and $\left(x_i,y_i\right)$ denotes the coordinates of the i-th path point.

$$J_{smoothness}={\displaystyle \sum _{k=1}^{N-1}\left(1-\cos {\theta }_k\right)}$$

(34a)

$$\cos {\theta }_k={\displaystyle \frac{\left(x_{k+1}-x_k,y_{k+1}-y_k\right)•\left(x_k-x_{k-1},y_k-y_{k-1}\right)}{\sqrt{{\left(x_{k+1}-x_k\right)}^2+{\left(y_{k+1}-y_k\right)}^2•\sqrt{{\left(x_k-x_{k-1}\right)}^2+{\left(y_k-y_{k-1}\right)}^2}}}}$$

(34b)

Based on the multi-MWMR path planning simulation experiments in the simple environment described above, the evaluation metrics such as path length, simulation duration, and the path smoothness $J_{smoothness}$ of the Leader MWMR are obtained, as shown in

Table 2. Evaluation metrics.

Method	Path Length of Leader ugv/m	Simulation Time/s	${\mathit{J}}_{\mathit{s}\mathit{m}\mathit{o}\mathit{o}\mathit{t}\mathit{h}\mathit{n}\mathit{e}\mathit{s}\mathit{s}}$
Artificial Potential Field and Model Predictive Control Fusion Algorithm	103.437	208.08	1.1353
APF-MPC Algorithm	105.757	517.93	1.1816

.

The results show that, in Scenario 1, under the influence of the artificial potential field and MPC fusion algorithm, the path length of the Leader MWMR is reduced by 2.19% compared to the APF-MPC algorithm. Additionally, the simulation time is reduced by 59.82%, and the path smoothness $J_{smoothness}$ is smaller, indicating a smoother path. In summary, in a simple environment, the artificial potential field and MPC fusion algorithm improves the stability and feasibility of MWMR path planning compared to the APF-MPC algorithm. It also further enhances the smoothness of the path and reduces the computational burden. Furthermore, the above experiment also validates the effectiveness of the formation control method based on the enhanced APF algorithm and leader–follower method in Scenario 1.

Scenario 2: To validate the effectiveness of the proposed algorithm in complex environments, we set up a complex scenario as shown in Figure 12. The Leader MWMR moves from the initial position (0, 50) to the target position (100, 50), bypassing static obstacles and avoiding a group of three moving obstacles that are moving at 1 m/s in the negative Y-axis direction. Ultimately, the MWMR avoids obstacles near the target position and reaches the target point.

Figure 13a shows the trajectories of multiple MWMRs under the influence of the artificial potential field and model predictive control fusion algorithm. The red curve represents the trajectory of the Leader MWMR, while the blue and green curves represent the trajectories of the Follower MWMRs. Figure 13b,c displays the angular velocity and linear velocity of the multiple MWMRs, respectively. It can be observed that the fluctuations of both velocities remain within a normal range.

Figure 13a shows that, under the proposed algorithm, multiple MWMRs are able to safely reach the target location in the complex environment of Scenario 2. Between t = 35 s and t = 60 s, the MWMRs use the proposed potential field model to safely avoid a concave obstacle. From t = 75 s to t = 115 s, the MWMRs successfully avoid a group of three moving obstacles. At t = 120 s, the MWMRs, influenced by the attractive potential field in the model, safely avoid a static obstacle near the target location and reach the goal point.

Figure 13d represents the distances between the three MWMRs in the formation. Figure 13e,f shows the errors in the X and Y axes, respectively, between the Follower MWMRs and their desired positions. From Figure 13d–f, it can be observed that, in the complex environment of Scenario 2, the distances between the MWMRs fluctuate as a result of avoiding dense obstacles. However, these distances remain within the normal range.

Figure 14 shows the simulation results of the MWMR formation under the APF-MPC algorithm in Scenario 2. By comparing Figure 13 and Figure 14, it can be observed that both the artificial potential field and MPC fusion algorithm and the APF-MPC algorithm are capable of smoothly avoiding static and moving obstacles. However, as shown in Figure 14a, the multiple MWMRs do not reach the target location. Instead, the multi-MWMR is influenced by obstacles near the target and perform oscillatory motion in the vicinity of the target. From the above simulation results, it can be concluded that the artificial potential field and MPC fusion algorithm proposed in this paper address the unreachable target problem that the APF-MPC algorithm cannot resolve. The proposed algorithm demonstrates better feasibility and stability in complex environments. Additionally, it further validates the effectiveness of the enhanced APF algorithm and the leader–follower method in complex environments.

4.2. Real-World Experiment

To further validate the effectiveness of the MWMR path planning algorithm based on the fusion of the artificial potential field and MPC, we conducted two sets of real-world experiments using a robot with Mecanum wheels. The experiments were carried out in both simple and complex environments. The robot is shown in Figure 15. The robot was outfitted with an LSLIDAR N10P LiDAR sensor for perception and detection, a Raspberry Pi 4B for data processing, and an STM32 MCU for controlling the Mecanum wheel chassis.

(1)

Simple environment:

In this experiment, there is only one obstacle. The experimental results are shown in Figure 16. This experiment validates the effectiveness of the proposed fusion of the artificial potential field and MPC for practical applications in simple scenarios.

(2)

Complex environment:

In this experiment, there is a concave obstacle composed of three obstacle blocks. The experimental results are shown in Figure 17. This experiment validates the effectiveness of the proposed fusion of the artificial potential field and MPC for practical applications in complex scenarios.

5. Conclusions

This paper addresses the multi-MWMR path planning and formation control problem in uncertain environments by proposing a fusion algorithm based on the artificial potential field and MPC. The algorithm represents environmental constraints as a potential field model for uncertain environments, integrating both the repulsive potential fields generated by obstacles and the attractive potential fields generated by the target location, which can effectively avoid the issue of local minima. This innovative potential field model is incorporated into the objective function of the MPC path planning controller to obtain a smooth, collision-free trajectory that adheres to the kinematic constraints of the MWMR. Furthermore, a formation control method based on an enhanced APF algorithm and a leader–follower approach is proposed. This method achieves low computational cost formation maintenance, intra-formation collision avoidance, and obstacle avoidance for MWMR formations. The proposed enhanced APF algorithm takes the relative velocities between Follower MWMRs, the target point, and obstacles into account, enabling the stable tracking of moving target points and the safe avoidance of moving obstacles, thus improving the control performance of the multi-MWMR system in uncertain environments. Finally, the simulation results from a simple scenario with only one static obstacle and one moving obstacle, and a complex scenario with concave obstacles and three moving obstacles have verified the stable avoidance capabilities of the method proposed in this paper against static obstacles, moving obstacles, and concave obstacles. Compared to the APF-MPC algorithm, in the simple scenario, the path length of the Leader MWMR under our proposed algorithm decreased by 2.19%, the simulation time was reduced by 59.82%, and the path smoothness was lower, resulting in a smoother path. Furthermore, in the complex environment, our algorithm safely avoided concave obstacles that the APF-MPC algorithm could not. Moreover, the effectiveness of the proposed method in practical applications is further validated through two sets of real experiments. These experiments were conducted in both simple and complex environments. In the future, we will focus on investigating the impact of nonlinear MWMR dynamics on the performance of the path planning algorithm and the interference with formation communications in an uncertain environment and further implement this in real experimental settings.