Model Predictive Control for Formation Placement and Recovery of Traffic Cone Robots

Abstract

The challenge of effectively managing the formation and recovery of traffic cone robots (TCRs) is addressed by proposing a linear time-varying model predictive control (MPC) strategy. This problem involves coordinating multiple TCR formations within a work area to reach a target location, which is a huge challenge due to the complexity of dynamic coordination. Unlike conventional approaches, our method decomposes the formation control problem into two main components: leader TCR motion planning and follower formation tracking control. The motion planning component involves path and velocity planning to achieve leader trajectory control, which serves as a reference trajectory for the follower. The formation tracking task extends to formation control among multiple robots to achieve the traffic cone robot formation placement and recovery task. To address the TCR input limitation problem, input constraints are considered during the design process of the MPC controllers. The effectiveness and practicality of the proposed control strategy are validated through a series of numerical simulations and physical experiments with TCRs.

1. Introduction

With the destruction and erosion by environmental factors and the constant crushing by traveling vehicles, roads are subject to varying degrees of damage, necessitating regular maintenance to prolong their service life. At present, the predominant approaches in the market for traffic cone placement and collection can be classified as manual placement or the use of collection vehicles [1]. However, manual placement is laborious and time-consuming and poses safety risks. Collection vehicles face issues such as high control costs, complex control systems, and the inability to collect abnormal traffic cones. The challenge of managing the formation and recovery of traffic cone robots (TCRs) involves complex coordination tasks, which are further complicated by dynamic work environments. Effective leader TCR motion planning and follower formation tracking control are crucial for ensuring that TCRs can operate efficiently and maintain desired formations. Among them, The motion planning component involves path and velocity planning to achieve leader trajectory control, which serves as a reference trajectory for the follower. The formation tracking task extends to formation control among multiple robots to achieve the traffic cone robot formation placement and recovery task.

With the rapid development of mobile robot technology, it is widely used in various fields [2,3,4]. Therefore, there has been significant interest in formation design methods, including the virtual structure method [5,6], the behavior-based method [7,8,9], the graph-based method [10,11,12], the artificial potential method [13,14,15], and the leader–follower method [16,17,18]. Among them, in the leader–follower method, one or several TCRs are designated as leaders to set the trajectory or target, while the others are followers. Using the designed formation controller, each follower can track their leader’s movement and maintain the desired formation. Desai first proposed two leader–follower control structures [16], namely, distance–angle control ($l-\phi$) and distance–distance control ($l-l$). Moreover, the feedback linearization method is used to design the control law and prove it stable. The $l-\phi$ control pattern requires a desired distance l and an angle $\phi$ between the follower and leader. In the $l-l$ control pattern, the objective is to maintain the desired distance. Due to its straightforward control mechanism and low computational cost, the leader–follower method has become widely used in practice. To address the formation control problem, this paper proposes a novel formation control scheme for TCRs based on the leader–follower method.

In recent years, based on these two classical leader–follower control structures, many advanced control strategies have been used for formation tracking control, such as sliding mode control [19,20], backstepping control [21], fuzzy systems [22], and model predictive control [23,24]. An innovative two-layer control strategy that combines a kinematic controller based on Lyapunov theory (KCLT) with a dynamic controller based on sliding mode (DCSM) is proposed to address the problem of leader–follower formation tracking control for multiple-wheeled mobile robots (M-WMR) [19]. A novel robust control approach utilizing model predictive control (MPC) is proposed by Wang to address the formation control problem of multi-agent mobile robots, subject to physical constraints and model uncertainty [20]. Chwa et al. [21] design backstepping controllers based on feedback linearization in order to improve formation performance. However, this approach can lead to unrealistic speed jumps when the initial position error is large, which is not practical for formations. In addition, a fuzzy neural network control method with impedance learning is designed for coordinating multi-constrained robots carrying common objects [22]. Xiao et al. [23] develop a nonlinear model predictive control (NMPC) based on neurodynamic optimization for mobile robots. Lin et al. [24] propose a composite control strategy for the formation control of wheeled mobile robots (WMRs) using MPC and adaptive terminal sliding mode control (ATSMC) techniques. MPC is widely used as a popular and effective technique among the above methods. It is an advanced control technique that predicts the future system state behavior by stating the optimal input sequences that minimize the open-loop objective function.

In practical scenarios of formation placement and recovery, TCR systems may need to follow specific trajectories or reach fixed targets. Consequently, TCR systems must not only manage formation behavior but also be capable of path tracking. The path tracking control has been extensively studied in the literature. Viadero-Monasterio et al. [25] propose a multi-input multi-output (MIMO) approach for path tracking control of autonomous vehicles. Their method addresses network-induced delays and incorporates roll dynamics to enhance both driving safety and passenger comfort. An MPC method is employed for path tracking with a focus on rollover stability, using front-wheel steering and external yaw moment as control inputs [26]. An enhanced MPC controller incorporating fuzzy adaptive weight control is proposed to address path tracking in autonomous vehicles. This controller improves tracking accuracy while also considering the vehicle’s dynamic stability during the tracking process [27]. According to the IFAC industry committee [28], MPC ranks fourth in the current industry advanced control influence survey and is expected to be the most popular technique in the future. MPC effectively addresses both formation control and trajectory tracking challenges, making it highly applicable to the placement and recovery of TCRs.

Compared to existing work, this paper proposes an MPC method based on the leader–follower paradigm for TCRs, making three primary contributions. Firstly, for TCRs with incomplete constraints, the proposed linear time-varying MPC ensures locally consistent asymptotic stability, addressing the need for robust formation control under realistic placement and recovery conditions. Secondly, the leader–follower formation model is designed using two relatively “decoupled” steps: the leader TCR’s motion planning and the follower TCRs’ formation controller. This approach simplifies the design process, offering greater flexibility and adaptability to different operational scenarios. Finally, the method provides a practical solution for dynamically integrating TCRs into a formation. By adjusting only the leader TCR’s planning signals, new robots can join the formation without altering the original formation controller, ensuring efficiency and minimal disruption to existing configurations.

The structure of this paper is as follows: In Section 2, the problem formulation and preliminaries are described. Section 3 introduces the proposed formation control design based on the model predictive control. In Section 4, the numerical simulations are presented. Section 5 provides the experimental verification. Finally, Section 6 provides conclusions.

2. Problem Formulation and Preliminaries

2.1. Description of the Formation Working Conditions Problem

Figure 1 illustrates the working condition of TCR formation placement (recovery), which consists of three distinct phases. Phase 1 represents the starting position, which is located in the emergency lane. Phase 2 depicts the TCRs entering the formation placement area, which includes a warning zone, two buffer zones, a working zone, and a termination zone. It is worth noting that the warning zone is the primary placement target in this paper as it is where most safety accidents occur. The remaining areas are placed with normal traffic cones, and a favorable combination of the two allows for the placement of traffic cones as well as the reduction of accidents. Phase 3 describes the formation recovery process where the TCRs return from the placement area to the emergency lane upon receiving a recovery command and await further recovery efforts by crews. In addition, the communication towers shown in the figure serve as GPS signal base stations, which are crucial for providing real-time positional information to the TCRs. These towers enable differential positioning by working in conjunction with the GPS mobile stations equipped on each TCR. This setup ensures accurate tracking and coordination of the TCR formation throughout the placement and recovery process. (Note: In Figure 1, A denotes the warning area; B and D are the buffer areas; C refers to the working area; E is the termination area.)

2.2. Kinematic Model of the Traffic Cone Robot

Figure 2 illustrates the configuration of the TCR, which moves along a flat surface. The TCR comprises a front wheel and two back wheels that rotate at different rates to generate linear velocity $v_i$ and angular velocity ${\omega }_i$ as control inputs. The two back wheels are denoted by $2R_i$, and the wheel rolling radius is represented by $2r_i$.

The kinematic equation of TCR is expressed as

$$\begin{array}{c}\hfill \left[\begin{array}{c}{\dot{x}}_i\\ {\dot{y}}_i\\ {\dot{\theta }}_i\end{array}\right]=\left[\begin{array}{cc}cos{\theta }_i& sin{\theta }_i\\ sin{\theta }_i& cos{\theta }_i\\ 0& 1\end{array}\right]\left[\begin{array}{c}{v}_i\hfill \\ {\omega }_i\hfill \end{array}\right],\end{array}$$

(1)

where $(x_i,y_i,{\theta }_i)$ are the coordinate at the center of mass. As a result of the motor’s hardware limitations, the TCR is subject to the following physical speed constraints.

Considering a TCR moving under pure rolling, the kinematic constraint is

$$\begin{array}{c}\hfill {\dot{y}}_{i}cos{\theta }_i-{\dot{x}}_{i}sin{\theta }_i=0.\end{array}$$

(2)

As a result of the motor’s hardware limitations, the TCR is subject to the following physical speed constraints,

$$\begin{array}{c}\hfill v_i\in [\underline{v},\overline{v}],\overline{v}>0>\underline{v},\end{array}$$

(3)

$$\begin{array}{c}\hfill {\omega }_i\in [\underline{\omega },\overline{\omega }],\overline{\omega }>0>\underline{\omega },\end{array}$$

(4)

where $\underline{v}$ and $\overline{v}$ are the maximum and minimum of $v_i$, respectively. Similarly, $\underline{\omega }$ and $\overline{\omega }$ are the maximum and minimum of ${\omega }_i$, respectively. The linear velocity constraints have been adjusted to permit negative values, allowing for backward movement when necessary. This change reflects a more realistic modeling of the robot’s capabilities. The angular velocity constraints now include negative values, enabling counterclockwise rotations as part of the control strategy.

2.3. Formation Model of Traffic Cone Robots

As shown in Figure 3, ${\mathit{q}}_f^d={\left[x_f^d,y_f^d,{\theta }_f^d\right]}^T$ is the desired follower TCR position posture, ${\mathit{q}}_f={\left[x_f,y_f,{\theta }_f\right]}^T$ is the actual follower TCR position posture. From the geometric relationship between leader and follower TCRs, we have

$${\mathit{q}}_f^d=\left[\begin{array}{c}{x}_f^d\\ y_f^d\\ {\theta }_f^d\end{array}\right]=\left[\begin{array}{c}{x}_l+{\rho }_{lf}^{d}cos\left({\psi }_{lf}^d+{\theta }_l\right)\\ y_l+{\rho }_{lf}^{d}sin\left({\psi }_{lf}^d+{\theta }_l\right)\\ {\theta }_f^d\end{array}\right]$$

(5)

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{\rho }_{lfx}=x_l-x_f=-{\rho }_{lf}cos\left({\psi }_{lf}+{\theta }_l\right)\hfill \\ {\rho }_{lfy}=y_l-y_f=-{\rho }_{lf}sin\left({\psi }_{lf}+{\theta }_l\right)\hfill \end{array}\right.,\end{array}$$

(6)

$$\begin{array}{c}\hfill {\rho }_{lf}=\sqrt{{\rho }_{lfx}^2+{\rho }_{lfy}^2},\end{array}$$

(7)

$$\begin{array}{c}\hfill {\psi }_{lf}=arctan\left({\displaystyle \frac{{\rho }_{lfy}}{{\rho }_{lfx}}}\right)-{\theta }_l+\pi ,\end{array}$$

(8)

where $(x_l,y_l)$ is the leader’s coordinate at the center of mass, ${\theta }_l$ is the leader’s heading angle, and $p_{lf}^d$ and $p_{lf}$ are the desired and actual distances between the follower and the leader, respectively. ${\psi }_{lf}^d$ and ${\psi }_{lf}$ are the desired and actual angles between the follower and the leader, respectively.

Taking the derivative of the above Equation (6) concerning time t, we obtain

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{\dot{\rho }}_{lfx}={\dot{x}}_l-{\dot{x}}_f=v_{l}cos{\theta }_l-v_{f}cos{\theta }_f\hfill \\ {\dot{\rho }}_{lfy}={\dot{y}}_l-{\dot{y}}_f=v_{l}sin{\theta }_l-v_{f}sin{\theta }_f\hfill \end{array}\right.\end{array}$$

(9)

Combining Equation (7) with Equation (9), we obtain

$$\begin{array}{c}\hfill {\dot{\rho }}_{lf}=v_{f}cos\left({\psi }_{lf}+{\theta }_l-{\theta }_f\right)-v_{l}cos{\psi }_{lf}.\end{array}$$

(10)

By taking the derivative of Equation (8), we obtain

$$\begin{array}{c}\hfill {\dot{\psi }}_{lf}={\displaystyle \frac{1}{{\rho }_{lf}}}\left(v_{l}sin{\psi }_{lf}-v_{f}sin\left({\psi }_{lf}+{\beta }_{lf}\right)\right)-{\omega }_l,\end{array}$$

(11)

with

$$\begin{array}{c}\hfill {\beta }_{lf}={\theta }_l-{\theta }_f.\end{array}$$

(12)

By taking the derivative of Equation (12), we obtain

$$\begin{array}{c}\hfill {\dot{\beta }}_{lf}={\dot{\theta }}_l-{\dot{\theta }}_f.\end{array}$$

(13)

Finally, the TCR formation model is given as follows:

$$\begin{array}{c}\hfill \left[\begin{array}{c}{\dot{\rho }}_{lf}\\ {\dot{\psi }}_{lf}\\ {\dot{\beta }}_{lf}\end{array}\right]=\left[\begin{array}{c}{v}_{f}cos{\phi }_{lf}-v_{l}cos{\psi }_{lf}\\ {\displaystyle \frac{1}{{\rho }_{lf}}}\left(v_{l}sin{\psi }_{lf}-v_{f}sin\left({\psi }_{lf}+{\beta }_{lf}\right)\right)-{\omega }_l\\ {\omega }_l-{\omega }_f\end{array}\right].\end{array}$$

(14)

2.4. Description of MPC Design Principles

Suppose a nonlinear mechanical system consists of n state variables and m control inputs, the system dynamics can be described as:

$$\begin{array}{c}\hfill \dot{\chi }\left(k\right)=h(\chi \left(k\right),u\left(k\right)),\end{array}$$

(15)

where $\chi \left(k\right)\in {\mathbf{R}}^n$ is the state vector, $h(·)$ is the system dynamics function, k represents the discrete time step, and $u\left(k\right)\in {\mathbf{R}}^m$ is the control vector.

Next, the error $\xi \left(k\right)$ between the system and the reference state vector ${\chi }_r\left(k\right)$ is given,

$$\begin{array}{c}\hfill \xi \left(k\right)=\chi \left(k\right)-{\chi }_r\left(k\right).\end{array}$$

(16)

By deriving Equation (16), the nonlinear error model of the system is obtained as follows:

$$\begin{array}{c}\hfill \dot{\xi }\left(k\right)=f\left(\xi \left(k\right),u\left(k\right)\right).\end{array}$$

(17)

Assuming that the model equilibrium point is (0, $u_r\left(k\right)$) and subjecting it to a Taylor expansion at the equilibrium point, the error model Equation (17) after the expansion is

$$\begin{array}{cc}\hfill \dot{\xi }\left(k\right)=& {\left.{\displaystyle \frac{\partial f\left(\xi \right(k),u(k\left)\right)}{\partial \xi \left(k\right)}}\right|}_{\xi \left(k\right)=0,u\left(k\right)=u_r\left(k\right)}·\xi \left(k\right)\hfill \\ \hfill & +{\left.{\displaystyle \frac{\partial f\left(\xi \right(k),u(k\left)\right)}{\partial u\left(k\right)}}\right|}_{\xi \left(k\right)=0,u\left(k\right)=u_r\left(k\right)}·\left(u\left(k\right)-u_r\left(k\right)\right)+W,\hfill \end{array}$$

(18)

where W is the higher-order term of the Taylor expansion, and when the higher-order term is neglected, Equation (18) can be reduced to

$$\begin{array}{c}\hfill \dot{\xi }\left(k\right)=J_f\left(\xi \right)\xi \left(k\right)+J_f\left(u\right)\left(u\left(k\right)-u_r\left(k\right)\right),\end{array}$$

(19)

where $J_f\left(\xi \right)$ and $J_f\left(u\right)$ are the Jacobi matrices of $f\left(\xi \right(k),u(k\left)\right)$ at the error model equilibrium point (0, $u_r\left(k\right)$) with respect to $\xi \left(k\right),u\left(k\right)$, respectively.

Bringing $\tilde{u}\left(k\right)=u\left(k\right)-u_r\left(k\right)$ into Equation (19), we have

$$\begin{array}{c}\hfill \dot{\xi }\left(k\right)=A\left(k\right)\xi \left(k\right)+B\left(k\right)\tilde{u}\left(k\right),\end{array}$$

(20)

where $\dot{\xi }\left(k\right)=\dot{\chi }\left(k\right)-{\dot{\chi }}_r\left(k\right)$, $A\left(k\right)=J_f\left(\xi \right)$, $B\left(k\right)=J_f\left(u\right)$.

The linear discrete time-varying system is obtained by discretizing (20) as

$$\begin{array}{c}\hfill \xi (k+1\mid k)=A_k\xi (k\mid k)+B_k\tilde{u}(k\mid k),\end{array}$$

(21)

where $A_k=I_{n\times n}+TA\left(k\right),B_k=TB\left(k\right)$, T is the control period.

Next, a state volume expansion is performed and Equation (21) can be rewritten as

$$\begin{array}{c}\hfill \tilde{\xi }(k+1\mid k)={\tilde{A}}_k\tilde{\xi }(k\mid k)+{\tilde{B}}_k\Delta \tilde{u}(k\mid k),\end{array}$$

(22)

$$\begin{array}{c}\hfill \eta (k\mid k)={\tilde{C}}_k\tilde{\xi }(k\mid k),\end{array}$$

(23)

where

$$\begin{array}{c}\hfill \tilde{\xi }(k\mid k)=\left[\begin{array}{c}\xi (k\mid k)\\ \tilde{u}(k-1\mid k)\end{array}\right],\end{array}$$

(24)

$$\begin{array}{c}\hfill {\tilde{A}}_k=\left[\begin{array}{cc}{A}_k& B_k\\ 0_{m\times n}& I_{m\times m}\end{array}\right],\end{array}$$

(25)

$$\begin{array}{c}\hfill {\tilde{B}}_k=\left[\begin{array}{c}{B}_k\\ I_{m\times m}\end{array}\right],\end{array}$$

(26)

$$\begin{array}{c}\hfill {\tilde{C}}_k=\left[\begin{array}{cc}{I}_{n\times n}\hfill & 0_{n\times m}\end{array}\right],\end{array}$$

(27)

$$\begin{array}{c}\hfill \Delta \tilde{u}(k\mid k)=\tilde{u}(k\mid k)-\tilde{u}(k-1\mid k).\end{array}$$

(28)

Finally, the system state prediction model is

$$\begin{array}{c}\hfill Y\left(k\right)={\Phi }_k\tilde{\xi }(k\mid k)+{\Theta }_k\Delta U\left(k\right),\end{array}$$

(29)

where $\Delta U\left(k\right)={\left[\begin{array}{cccc}\Delta \tilde{u}(k\mid k)\hfill & \Delta \tilde{u}(k+1\mid k)\hfill & \dots \hfill & \Delta \tilde{u}\left(k+N_c-1\mid k\right)\hfill \end{array}\right]}^T$ is the control sequence within the control time domain $N_c$, $N_p$ is predicted time domain,

$$\begin{array}{c}\hfill Y\left(k\right)=\left[\begin{array}{c}\eta (k+1\mid k)\\ \eta (k+2\mid k)\\ ⋮\\ \eta \left(k+N_c\mid k\right)\\ ⋮\\ \eta \left(k+N_p\mid k\right)\end{array}\right],\end{array}$$

(30)

$$\begin{array}{c}\hfill {\Phi }_k=\left[\begin{array}{c}{\tilde{C}}_k{\tilde{A}}_k\\ {\tilde{C}}_k{\tilde{A}}_k^2\\ ⋮\\ {\tilde{C}}_k{\tilde{A}}_k^{N_c}\\ ⋮\\ {\tilde{C}}_k{\tilde{A}}_k^{N_p}\end{array}\right],\end{array}$$

(31)

$$\begin{array}{c}\hfill {\Theta }_k=\left[\begin{array}{cccc}{\tilde{C}}_k{\tilde{B}}_k& 0& \dots & 0\\ {\tilde{C}}_k{\tilde{A}}_k& {\tilde{C}}_k{\tilde{B}}_k& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ {\tilde{C}}_k{\tilde{A}}_k^{N_c-1}{\tilde{B}}_k& {\tilde{C}}_k{\tilde{A}}_k^{N_c-2}{\tilde{B}}_k& \dots & {\tilde{C}}_k{\tilde{B}}_k\\ {\tilde{C}}_k{\tilde{A}}_k^{N_p-1}{\tilde{B}}_k& {\tilde{C}}_k{\tilde{A}}_k^{N_p-2}{\tilde{B}}_k& \dots & {\tilde{C}}_k{\tilde{A}}_k^{N_P-N_C}{\tilde{B}}_k\end{array}\right].\end{array}$$

(32)

The MPC principle dictates that the optimal control output produced by the controller is the optimal solution to the objective function subject to constraints. Therefore, the selection of the objective function is a critical step in controller design. The system cost function is presented below.

$$\begin{array}{c}\hfill J\left(k\right)=\sum _{i=0}^{N_p-1}{∥\chi (k+i\mid k)-{\chi }_r(k+i\mid k)∥}_Q^2+\sum _{i=0}^{N_p-1}{∥\Delta U(k+i\mid k)∥}_R^2+\rho {\epsilon }^2,\end{array}$$

(33)

where $\epsilon$ is the relaxation factor, $\rho$ is the penalty weights, and Q, R are state quantity tracking capability and control quantity weight matrices, respectively. It is worth noting that $\epsilon$ is introduced as a slack variable to relax hard constraints, providing a degree of freedom in situations where strict constraints may be infeasible. $\rho$ is a weight that determines the cost associated with violating constraints via the slack variable $\epsilon$. A larger $\rho$ places more emphasis on minimizing constraint violations, effectively discouraging them unless absolutely necessary for feasibility. Conversely, a smaller $\rho$ permits more leniency in constraint violations.

Let $Y\left(k\right)=\chi \left(k\right)-{\chi }_r\left(k\right)$. By bringing Equation (29) into Equation (33) and ignoring the constant term, the optimal control with constraints can be transformed into the standard quadratic form without constraints as follows:

$$\begin{array}{c}\hfill J\left(k\right)={\displaystyle \frac{1}{2}}{\left[\begin{array}{c}\Delta U\left(k\right)\\ \epsilon \end{array}\right]}^{T}H\left(k\right)\left[\begin{array}{c}\Delta U\left(k\right)\\ \epsilon \end{array}\right]+G^T\left(k\right)\left[\begin{array}{c}\Delta U\left(k\right)\\ \epsilon \end{array}\right],\end{array}$$

(34)

where

$$\begin{array}{c}\hfill H\left(k\right)=\left[\begin{array}{cc}2{\Theta }_k^{T}Q{\Theta }_k+2R& 0_{n·N_c}\\ 0_{n·N_c}& 2\rho \end{array}\right],\end{array}$$

(35)

$$\begin{array}{c}\hfill G\left(k\right)=\left[\begin{array}{c}2{\Theta }_k^{T}Q{e}_k\\ 0_{1\times 1}\end{array}\right],\end{array}$$

(36)

$$\begin{array}{c}\hfill e_k={\Phi }_k\left(\chi \left(k\right)-{\chi }_r\left(k\right)\right).\end{array}$$

(37)

Remark 1. 

The first term in Equation (34) represents the system tracking performance, and the second term represents the system constraint on the control increment. During actual system operation, the system state volume constraint and the control volume constraint must also be taken into account. As a result, Equation (34) can be reformulated as a quadratic programming problem with constraints.

$$\begin{array}{c}\hfill J\left(k\right)={\displaystyle \frac{1}{2}}{\left[\begin{array}{c}\Delta U\left(k\right)\\ \epsilon \end{array}\right]}^{T}H\left(k\right)\left[\begin{array}{c}\Delta U\left(k\right)\\ \epsilon \end{array}\right]+G^T\left(k\right)\left[\begin{array}{c}\Delta U\left(k\right)\\ \epsilon \end{array}\right]\\ \hfill \mathrm{s}.\mathrm{t}.\left\{\begin{array}{c}\Delta U_{min}\le \Delta U\left(k\right)\le \Delta U_{min}\\ Y_{min}\le {\Phi }_k\tilde{\xi }(k\mid k)+{\Theta }_k\Delta U\left(k\right)\le Y_{max}\end{array}\right.,\end{array}$$

(38)

where $U_{max}$ and $U_{min}$ are the maximum and minimum boundary values of the MPC feedback control, $\Delta U_{max}$ and $\Delta U_{min}$ are the maximum and minimum boundary values of the control system feedback control increment, and $Y_{max}$ and $Y_{min}$ are the maximum and minimum boundary values of the error between the system output amount and the desired output.

Solving Equation (38), the optimal sequence can be obtained.

$$\begin{array}{c}\hfill \left[\begin{array}{cc}\Delta U^{*}\left(k\right)\hfill & {\epsilon }^{*}\left(k\right)\end{array}\right]={\left[\begin{array}{ccccc}\Delta {\tilde{u}}^{*}\left(k\right)\hfill & \Delta {\tilde{u}}^{*}(k+1)& \cdots & \Delta {\tilde{u}}^{*}\left(k+N_c-1\right)& {\epsilon }^{*}\left(k\right)\end{array}\right]}^{\mathrm{T}},\end{array}$$

(39)

where $\Delta U^{*}$ is optimal feedback control increment.

Next, the first value of the optimal feedback control increment sequence in the control time domain $\left[\begin{array}{ccc}k,\hfill & \cdots ,\hfill & k+N_c-1\hfill \end{array}\right]$ is used in the actual control.

$$\begin{array}{c}\hfill {\tilde{u}}^{*}\left(k\right)=\tilde{u}(k-1)+\Delta {\tilde{u}}^{*}\left(k\right),\end{array}$$

(40)

where ${\tilde{u}}^{*}$ is the difference between the actual control and the reference control. Then, the final control quantity of the system is

$$\begin{array}{c}\hfill u^{*}\left(k\right)=u_r\left(k\right)+{\tilde{u}}^{*}\left(k\right).\end{array}$$

(41)

3. Formation Control Design Based on Model Predictive Control

Path and speed planning is critical for the leader robot before the TCR can form a target formation. Proper planning not only ensures high-quality final placement but also reduces construction time and costs. In this section, we complete leader TCR motion path planning using a fifth-order polynomial curve. Additionally, we utilize the S-shaped velocity planning method to accomplish leader TCR velocity planning.

3.1. The Path Planning of Leader Traffic Cone Robot

Unlike traditional formation design ideas, this paper proposes a path planning strategy for TCRs based on the leader–follower method, where one TCR is designated as the leader and the remaining TCRs are designated as followers. Through the design of the follower TCRs’ formation and the motion planning of the leader TCR, it realizes the whole TCR formation control. As the follower TCRs maintain the same motion as the leader TCRs, the follower TCRs’ path planning can be ensured by the formation design.

During the placement of the TCRs in formation, the leader TCR can obtain the position of the upper and lower boundaries of the warning zone A and B at the moment T via GPS. In Figure 4, the leader TCR starts from point P and moves in formation at an optimal speed, arriving at point E to stop the placement.

From Figure 4, we can obtain information on the path of the leader TCR.

$$\begin{array}{c}\hfill \left\{\begin{array}{c}x=s+x_0\hfill \\ y=y_0\hfill \end{array}\right.,\end{array}$$

(42)

where s is the distance of the placement process of the TCR from the starting point, ($x_0,y_0$) is the horizontal and vertical coordinate of the starting point P.

From Figure 5, the following information is given about the leader’s path during the recovery condition.

$$\begin{array}{c}\hfill \left\{\begin{array}{c}x=s^{\prime }+x_1\hfill \\ y=y_1\hfill \end{array}\right.,\end{array}$$

(43)

where $s^{\prime }$ is the distance of the TCR recovery process from point E($x_1,y_1$) is the horizontal and vertical coordinates of point E.

3.2. The Velocity Planning of Leader Traffic Cone Robot

In the placement (recovery) process, the driving speed of the TCR is a crucial factor for both construction efficiency and traffic safety. If the driving speed is too fast, it may pose safety hazards to passing vehicles, while driving too slow can affect the overall construction process. Therefore, maintaining a stable and appropriate moving speed is essential to ensure the success of the entire placement and recovery process.

In the field of velocity planning, there are several methods available such as trapezoidal velocity planning, five-polynomial S-shaped velocity planning, and seven-polynomial S-shaped velocity planning. In this study, the trapezoidal velocity planning method was adopted to determine the driving speed during the placement (recovery) process.

As shown in Figure 6, a complete trapezoidal velocity planning process contains three phases: $t_0-t_1$ uniform acceleration phase, $t_1-t_2$ uniform velocity phase, and $t_2-t_3$ uniform deceleration phase.

Next, the speed planning of the leader TCR is given as follows:

$$\begin{array}{c}\hfill v_r\left(t\right)=\left\{\begin{array}{ccc}{v}_0+a_{max}\left(t-t_0\right),\hfill & 0\le t\le t_1,\hfill & v_1=a_{max}{t}_1\hfill \\ v_1,\hfill & t_1\le t\le t_2,\hfill & v_1=v_2\hfill \\ v_2-a_{max}\left(t-t_2\right),\hfill & t_2\le t\le t_3,\hfill & v_0=v_3=0\hfill \end{array}\right.,\end{array}$$

(44)

where $v_0$ is the initial velocity, $v_i$ is the velocity at moment $t_i$, i = 1, 2, 3, and $a_{max}$ is maximum acceleration.

From the Figure 6, we can see that the uniform acceleration phase and the uniform deceleration phase are symmetrical, then we have

$$\begin{array}{c}\hfill t_1=t_3-t_2.\end{array}$$

(45)

According to Equation (44), we have

$$\begin{array}{c}\hfill t_1=v_1/a_{max},\end{array}$$

(46)

$$\begin{array}{c}\hfill t_3=v_2/a_{max}+t_2.\end{array}$$

(47)

The uniform acceleration zone length $S_1$, the uniform velocity zone length $S_2$ and the uniform deceleration zone length $S_3$ are given as

$$\begin{array}{c}\hfill S_1=0.5a_{max}+t_1^2,\end{array}$$

(48)

$$\begin{array}{c}\hfill S_2=v_1(t_2-t_1),\end{array}$$

(49)

$$\begin{array}{c}\hfill S_3=v_1^2/\left(2a_{max}\right),\end{array}$$

(50)

$$\begin{array}{c}\hfill S=S_1+S_2+S_3.\end{array}$$

(51)

Let $a_{max}=1.5\mathrm{m}/{\mathrm{s}}^2$, $v_1=v_2=0.3$ m/s, and S = 5 m. Figure 7 shows the velocity plan for the entire formation trajectory.

Note that recovery placement velocity planning is similar to the placement process and is not repeated.

3.3. Linear Time-Varying MPC Design

The TCR formation control problem based on the leader–follower formation model can be decomposed into two sub-problems: trajectory tracking of the leader and formation following of the follower TCRs.

3.3.1. Trajectory Tracking Control Design for Leader TCR

The TCR formation control system is complex and requires high real-time requirements, and the nonlinear model predictive control is computationally intensive and difficult to meet controller real-time requirements. Therefore, this paper uses a linear time-varying model predictive control.

Rewrite the Equation (1) in the following form

$$\begin{array}{c}\hfill \dot{q}\left(t\right):=g_l\left(q\left(t\right),u_l\left(t\right)\right),\end{array}$$

(52)

where $u_l\left(t\right)={[v_l,{\omega }_l]}^T$ is the control input for the leader.

According to Equations (16)–(21), a linear discrete-time error model for the leader TCR can be obtained as follows:

$$\begin{array}{c}\hfill \xi (k+1\mid k)=A_{kl}\xi (k\mid k)+B_{kl}\Delta u_l(k\mid k),\end{array}$$

(53)

where

$$\begin{array}{c}\hfill A_{kl}=\left[\begin{array}{ccc}1& 0& -v_{r}Tsin{\theta }_r\\ 0& 1& v_{r}Tcos{\theta }_r\\ 0& 0& 1\end{array}\right],\end{array}$$

(54)

$$\begin{array}{c}\hfill B_{kl}=\left[\begin{array}{cc}Tcos{\theta }_r\hfill & 0\hfill \\ Tsin{\theta }_r\hfill & 0\hfill \\ 0\hfill & T\hfill \end{array}\right]\end{array}$$

(55)

Subjecting Equation (53) to state volume expansion, it can be transformed into

$$\begin{array}{c}\hfill {\tilde{\xi }}_l(k+1\mid k)={\tilde{A}}_{kl}{\tilde{\xi }}_l(k\mid k)+{\tilde{B}}_{kl}\Delta {\tilde{u}}_l(k\mid k),\end{array}$$

(56)

$$\begin{array}{c}\hfill {\eta }_l(k\mid k)={\tilde{C}}_{kl}{\tilde{\xi }}_l(k\mid k),\end{array}$$

(57)

where

$$\begin{array}{c}\hfill {\tilde{\xi }}_l(k\mid k)=\left[\begin{array}{c}{\xi }_l(k\mid k)\\ {\tilde{u}}_l(k-1\mid k)\end{array}\right],\end{array}$$

(58)

$$\begin{array}{c}\hfill {\tilde{A}}_{kl}=\left[\begin{array}{cc}{A}_{kl}& B_{kl}\\ 0_{2\times 3}& I_{2\times 2}\end{array}\right],\end{array}$$

(59)

$$\begin{array}{c}\hfill {\tilde{B}}_{kl}=\left[\begin{array}{c}{B}_{kl}\\ I_{2\times 2}\end{array}\right],\end{array}$$

(60)

$$\begin{array}{c}\hfill {\tilde{C}}_{kl}=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& 0\end{array}\right],\end{array}$$

(61)

$$\begin{array}{c}\hfill \Delta {\tilde{u}}_l(k\mid k)={\tilde{u}}_l(k\mid k)-{\tilde{u}}_l(k-1\mid k).\end{array}$$

(62)

Next, the optimal control sequence for the trajectory tracking of the leader TCR at k moments is given as follows:

$$\begin{array}{c}\hfill \left[\begin{array}{cc}\Delta U_l^{*}\left(k\right)\hfill & {\epsilon }_l^{*}\left(k\right)\hfill \end{array}\right]={\left[\begin{array}{ccccc}\Delta {\tilde{u}}_l^{*}\left(k\right)\hfill & \Delta {\tilde{u}}_l^{*}(k+1)\hfill & \cdots \hfill & \Delta {\tilde{u}}_l^{*}\left(k+N_c-1\right)\hfill & {\epsilon }_l^{*}\left(k\right)\hfill \end{array}\right]}^T.\end{array}$$

(63)

Using the first value of the optimal feedback control increment sequence in the control time domain $\left[k,\cdots k+N_c-1\right]$, we have

$$\begin{array}{c}\hfill {\tilde{u}}_l^{*}\left(k\right)={\tilde{u}}_l(k-1)+\Delta {\tilde{u}}_l^{*}\left(k\right).\end{array}$$

(64)

The final control of the leader TCR is then as follows:

$$\begin{array}{c}\hfill u_l^{*}\left(k\right)=u_r\left(k\right)+{\tilde{u}}_l^{*}\left(k\right),\end{array}$$

(65)

where $u_r\left(k\right)$ is the k moment to plan the leader TCR velocity.

3.3.2. Formation Tracking Control Design for Follower TCRs

Formation controller based on the leader–follower model ensures that each follower TCR maintains the desired formation with the leader to complete formation placement (recovery). This section presents a controller design based on MPC.

The kinematic Equantion (14) of the i-th follower TCR formation can be translated into the following form

$$\begin{array}{c}\hfill {\dot{N}}_i\left(t\right):=g_i\left(N_i\left(t\right),u_i\left(t\right)\right),\end{array}$$

(66)

where $u_i\left(t\right)={\left[v_i,{\omega }_i\right]}^T$ is the control input of the i-th follower the TCRs.

According to Equations (16)–(21), a linear discrete-time error model for the follower TCRs can be obtained as follows:

$$\begin{array}{c}\hfill {\xi }_i(k+1\mid k)=A_{ki}{\xi }_i(k\mid k)+B_{ki}\Delta u_i(k\mid k),\end{array}$$

(67)

with

$$\begin{array}{c}\hfill {\xi }_i(k\mid k)=\left[\begin{array}{c}{\rho }_{li}(k\mid k)-{{\rho }_{li}}^d\\ {\psi }_{li}(k\mid k)-{{\psi }_{li}}^d\\ {\beta }_{li}(k\mid k)-{{\beta }_{li}}^d\end{array}\right],\end{array}$$

(68)

$$\begin{array}{c}\hfill \Delta u_i(k\mid k)=\left[\begin{array}{c}{v}_i-v_i^r\\ {\omega }_i-{\omega }_i^r\end{array}\right],\end{array}$$

(69)

$$\begin{array}{c}\hfill A_{ki}=\left[\begin{array}{ccc}0& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ 0& 0& 0\end{array}\right],\end{array}$$

(70)

$$\begin{array}{c}\hfill B_{ki}=\left[\begin{array}{cc}{b}_{11}& b_{12}\\ b_{21}& b_{22}\\ 0& -1\end{array}\right],\end{array}$$

(71)

where

$$\begin{array}{c}\hfill a_{12}=v_{l}sin{\psi }_{li}-v_i^{r}sin\left({\psi }_{li}+{\beta }_{li}\right),\end{array}$$

(72)

$$\begin{array}{c}\hfill a_{13}=-v_i^{r}sin\left({\psi }_{li}+{\beta }_{li}\right),\end{array}$$

(73)

$$\begin{array}{c}\hfill a_{21}={\displaystyle \frac{\left(v_{l}sin{\psi }_{li}-v_i^{r}sin\left({\psi }_{li}+{\beta }_{li}\right)\right)}{-{\left({\rho }_{li}^d\right)}^2}},\end{array}$$

(74)

$$\begin{array}{c}\hfill a_{22}={\displaystyle \frac{\left(v_{l}cos{\psi }_{li}-v_i^{r}cos\left({\psi }_{li}+{\beta }_{li}\right)\right)}{{\rho }_{li}^d}},\end{array}$$

(75)

$$\begin{array}{c}\hfill a_{23}={\displaystyle \frac{\left(-v_i^{r}cos\left({\psi }_{li}+{\beta }_{li}\right)\right)}{{\rho }_{li}^d}},\end{array}$$

(76)

$$\begin{array}{c}\hfill b_{11}=-cos\left({\psi }_{li}+{\beta }_{li}\right),\end{array}$$

(77)

$$\begin{array}{c}\hfill b_{21}={\displaystyle \frac{sin\left({\psi }_{li}+{\beta }_{li}\right)}{-{\rho }_{li}^d}},\end{array}$$

(78)

$$\begin{array}{c}\hfill b_{12}=b_{22}=0.\end{array}$$

(79)

Subjecting Equation (67) to state volume expansion, it can be transformed into

$$\begin{array}{c}\hfill {\tilde{\xi }}_i(k+1\mid k)={\tilde{A}}_{ki}{\tilde{\xi }}_i(k\mid k)+{\tilde{B}}_{ki}\Delta {\tilde{u}}_i(k\mid k),\end{array}$$

(80)

$$\begin{array}{c}\hfill {\eta }_i(k\mid k)={\tilde{C}}_{ki}{\tilde{\xi }}_i(k\mid k),\end{array}$$

(81)

where

$$\begin{array}{c}\hfill {\tilde{\xi }}_i(k\mid k)=\left[\begin{array}{c}{\xi }_i(k\mid k)\\ {\tilde{u}}_i(k-1\mid k)\end{array}\right],\end{array}$$

(82)

$$\begin{array}{c}\hfill {\tilde{A}}_{ki}=\left[\begin{array}{cc}{A}_{ki}& B_{ki}\\ 0_{2\times 3}& I_{2\times 2}\end{array}\right],\end{array}$$

(83)

$$\begin{array}{c}\hfill {\tilde{B}}_{ki}=\left[\begin{array}{c}{B}_{ki}\\ I_{2\times 2}\end{array}\right],\end{array}$$

(84)

$$\begin{array}{c}\hfill {\tilde{C}}_{ki}=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& 0\end{array}\right],\end{array}$$

(85)

$$\begin{array}{c}\hfill \Delta {\tilde{u}}_i(k\mid k)={\tilde{u}}_i(k\mid k)-{\tilde{u}}_i(k-1\mid k).\end{array}$$

(86)

Next, the optimal control sequence for the trajectory tracking of the follower TCRs at k moments is given as follows:

$$\begin{array}{c}\hfill \left[\begin{array}{cc}\Delta U_i^{*}\left(k\right)\hfill & {\epsilon }_i^{*}\left(k\right)\hfill \end{array}\right]={\left[\begin{array}{ccccc}\Delta {\tilde{u}}_i^{*}\left(k\right)\hfill & \Delta {\tilde{u}}_i^{*}(k+1)\hfill & \cdots \hfill & \Delta {\tilde{u}}_i^{*}\left(k+N_c-1\right)\hfill & {\epsilon }_i^{*}\left(k\right)\hfill \end{array}\right]}^T.\end{array}$$

(87)

Taking the first element of the control sequence as the feedback increment of the i-th follower TCR formation control, we have

$$\begin{array}{c}\hfill {\tilde{u}}_i^{*}\left(k\right)={\tilde{u}}_i(k-1)+\Delta {\tilde{u}}_i^{*}\left(k\right).\end{array}$$

(88)

The final control of the leader TCR is then as follows:

$$\begin{array}{c}\hfill u_i^{*}\left(k\right)=u_i^r\left(k\right)+{\tilde{u}}_i^{*}\left(k\right),\end{array}$$

(89)

where $u_i^r\left(k\right)$ is the k moment to plan the follower TCRs velocity.

To this end, the MPC controller flowchart is shown in Figure 8.

4. Numerical Simulations

In this section, a pair of case studies are conducted to show the effectiveness of the proposed TCR method.

Case 1: The numerical simulation of the three TCRs under the formation placement condition.

The main parameters of the TCRs used for the simulation are given as follows: ${\rho }_{l2}=1$ m, ${\rho }_{l3}=2$ m, ${\psi }_{l2}$ = $4\pi /3$ rad, ${\psi }_{l3}$ = $4\pi /3$ rad,, ${\beta }_{l2}={\beta }_{l3}$ = 0 rad, $R_i$ = 0.23 m, $r_i$ = 0.036 m, $N_p$ = 40, $N_c$ = 15, $Q=dag\left(\left[\begin{array}{ccc}1\hfill & 10\hfill & 0.1\hfill \end{array}\right]\right)$, $R=dag\left(\left[\begin{array}{ccc}1\hfill & 1\hfill & 1\hfill \end{array}\right]\right)$, $\rho$ = 10, T = 0.05 s. The initial conditions of the TCRs are given as follows: $x_1$ = 0 m, $y_1$ = 0 m, ${\theta }_1$ = 0 m, $x_2=-0.5$ m, $y_2=0$ m, ${\theta }_2$ = 0 m, $x_3=-1$ m, $y_3$ = 0 m, ${\theta }_3$ = 0 m, and the leader TCR expectation position (5 m, 0 m).

Figure 9 illustrates the trajectory profile of the traffic cone robots’ (TCRs’) in-plane formation placement under the MPC controller. The TCRs maintain a diagonal formation from the starting position (0 m, 0 m) to the target position (5 m, 0 m). The speed of the TCRs is presented in Figure 10. The follower TCRs’ linear velocity fluctuates widely in the starting phase due to initial errors, but the velocity stabilizes at approximately 0.3 m/s after 10 s. The angular velocity gradually converges to 0 rad/s with time. Figure 11 presents the leader TCR-1’s tracking error results. The leader TCR-1 exhibited no initial position errors in either the x or y direction, and the initial heading error is 0.0002 rad. The heading error converged to zero within approximately 2 s of the simulation starting, indicating that the leader TCR-1 can accurately follow the desired placement trajectory in real time. Figure 12 shows the formation error of the follower TCRs, and the formation errors (formation distance error and formation angle error) converge to zero in about 10 s and remain stable during subsequent placement operations. These simulation results demonstrate the effectiveness of the control method in formation stabilization.

Case 2: The numerical simulation of the three TCRs under the formation recovery condition. The formation parameters are as follows: ${\rho }_{l2}=0.1$ m, ${\rho }_{l3}=0.2$ m, ${\psi }_{l2}$ = ${\psi }_{l3}$ = $\pi$ rad, ${\beta }_{l2}$ = ${\beta }_{l3}$ = 0 rad.

The initial conditions of the TCRs are specified as follows: $x_1$ = 5 m, $y_1$ = 0 m, ${\theta }_1$ = 0 m, $x_2$ = 4.5 m, $y_2=-0.85$ m, ${\theta }_2$ = 0 m, $x_3$ = 3.59 m, $y_3=-1.38$ m, and ${\theta }_3$ = 0 m, with the leader TCR expected to be positioned at (10 m, 0 m). The resulting trajectories of the TCRs are similar to those of case 1, with the formation recovery trajectories shown in Figure 13. Figure 14 and Figure 15 illustrate the velocities and formation recovery errors of the TCRs, respectively. As depicted in Figure 13, the MPC controller guides the TCRs from the placement position back to the emergency lane position. The TCRs experience some fluctuation in the formation recovery process due to initial position and angle errors, but the entire recovery process is smooth, and the maximum velocities remain within the specified upper limit of 0.3 m/s (0.3 rad/s), as shown in Figure 14. Furthermore, as demonstrated in Figure 15, the distance and angle errors of the leader TCR-1 and follower TCR-2 converge to 0 around 10 s, while the distance and angle errors of follower TCR-3 converge to 0 around 25 s, after which they remain stable. These results confirm that the proposed control method exhibits satisfactory performance even under recovery conditions.

5. Experiments

In this section, MATLAB 2020 b was used for developing and simulating the control algorithms, while ROS (robot operating system) is utilized for real-time communication and control of the TCRs. Specifically, MATLAB’s Robotics System Toolbox facilitated the integration with ROS, allowing us to design and test the MPC algorithms within MATLAB, convert them to C++, and then deploy these C++ algorithms to the TCR via ROS. Finally, these control algorithms are published to the relevant ROS topics. The TCR running ROS subscribes to these topics to receive commands. Additionally, sensor data from the TCRs were published on ROS topics. This enables the traffic cone robot formation placement and recovery task. Next, we demonstrate the effectiveness and practicality of the MPC controller designed in the previous sections by utilizing three TCRs in a formation to showcase its control performance.

5.1. Experiments

Figure 16 illustrates the experimental setup for the TCR, consisting of the mobile robot platform (IRobot), controller (Raspberry Pi), real-time kinematic (RTK) GPS sensors, and power supply. The PC computer serves as the monitoring layer, communicating with the Raspberry Pi through WiFi. The controllers communicate with each other and the computer via WiFi while also communicating with the RTK GPS through universal serial bus (USB) serial ports. The experimental procedure is as follows:

(1)

Connect the Raspberry Pi and PC via WIFI.

(2)

log in to Raspberry Pi remotely on PC through SSH, and start each TCR.

(3)

Start the parameter optimization node through the launch file.

(4)

Start the RTK GPS serial information reading node and start the trajectory display node through the launch file. This will convert the longitude, latitude, heading, speed, and other information output from both to the OXY plane coordinate system.

(5)

Start the TCR formation controller.

(6)

After the placement (recovery) operation is finished, stop the program and save the TCR’s trajectory information.

Figure 17 and Figure 18 show the experimental process diagrams of the TCRs in placement and recovery formations at t = 0 s, t = 10 s, and t = 20 s, respectively.

Figure 16. Schematic diagram of experimental control principle of TCRs.

Figure 16. Schematic diagram of experimental control principle of TCRs.

5.2. Experiment Result

The experimental setup is consistent with the simulation parameters. Figure 19, Figure 20, Figure 21, Figure 22 and Figure 23 display the experimental results for TCR placement and recovery. Figure 19 shows the trajectories of the leader and follower TCRs during formation placement, where the triangle symbol represents the start position and the star symbol represents the target position. Figure 20 shows that the distance error between the robots gradually converges to 0.1 m, achieving the desired formation of the leader–follower TCR system. Figure 21 depicts the time histories of the leader’s control inputs $v_1$ and ${\omega }_1$, which meet the maximum system limits ($v\in$ (−0.5 m/s, 0.5 m/s), $\omega \in$ (−0.5 rad/s, 0.5 rad/s)). Additionally, the recovery trajectories of the TCRs are shown in Figure 22, demonstrating an orderly return to the emergency lane position. Figure 23 illustrates the reasonable convergence of position errors for the three TCRs under the proposed formation tracking control. After 30 s, the error converges to a small neighborhood around −0.1 m due to GPS sensor error. The results demonstrate that the proposed control method still performs well in formation recovery tconditions.

6. Conclusions

In this paper, we investigate the problem of formation placement and recovery in TCR systems. The formation control task is divided into two sub-tasks: motion planning for the lead TCR and formation control for the follower TCRs. The lead TCR undergoes path and velocity planning, and the resulting trajectory serves as a reference for the follower TCRs. Formation control design employs a time-varying MPC approach that linearizes and discretizes the formation kinematic model to maintain the formation while limiting the control inputs to meet the hardware requirements of the chassis ($v\in$ (−0.5 m/s, 0.5 m/s), $\omega \in$ (−0.5 rad/s, 0.5 rad/s)). Simulations and experiments with TCRs demonstrate the effectiveness of the proposed formation placement and recovery control approach. Among them, the simulated distance error converges to 0 m, and the experimental distance error converges to about ±0.1 m (due to GPS accuracy), both of which satisfy the TCR formation requirements. In future work, we will develop a dynamic model for the TCRs based on the current kinematic model and incorporate road friction effects into the TCRs’ motion model. This refinement will involve examining how frictional forces impact formation control and trajectory tracking, thereby enhancing the practical applicability and robustness of our control strategies.