Guaranteed collision-free trajectory tracking of multiple nonholonomic WMRs subject to multiple constraints

Abstract

The ability to avoid the possible impending collision should be essential for a team of wheeled mobile robots (WMRs) while they execute the trajectory tracking task. Conflict and congestion induced by the same path competition are major challenges in collision avoidance. To cope with these challenges, this paper proposes a time-varying position-based simultaneous obstacle avoidance and trajectory tracking (TV-SOATT) method that coordinates WMRs’ velocities. The proposed TV-SOATT method provides the motion-liveness and safety by introducing the radial bounds for reaching the desired trajectory and forecasting a collision. The pause or wander problem of the WMRs resulting from the path conflict is solved. Multiple illustration examples verify that all WMRs can adaptively adjust their respective positions to satisfy various constraints, achieving collision-free trajectory tracking. Extensive comparison illustrates effectiveness and superiority of our method.

Keywords

Collision avoidance trajectory tracking mobile robots optimization

Introduction

Recently, wheeled mobile robots (WMRs) have obtained compelling attention from scholars and have been applied in search and rescue (Krzysiak and Butail, 2022), warehousing (Draganjac et al., 2016), and care (Mišeikis et al., 2020). As one of the fundamental control problems in robot community, trajectory tracking (TT) is required to follow a time-related geometry path called reference trajectory. TT control methods include: backstepping (Binh et al., 2019; Reis et al., 2023; Zhang and Yang, 2022); dynamic surface control (DSC) (Elhaki and Shojaei, 2022); sliding mode control (SMC) (Moudoud et al., 2023; Zhu et al., 2020); model predictive control (MPC) (krjanc and Klanar, 2017; Zhu et al., 2020); neural network (NN)-based adaptive control (Chen et al., 2021; Elhaki and Shojaei, 2022); fuzzy logic control (FLC) (Alouache and Wu, 2017); and feedback linearization control (Majd et al., 2020; Pliego-Jiménez et al., 2021). Table 1 compares various control methods around TT. In a word, the TT control methods have mostly followed either of the two following strategies. The first strategy is to define the state-tracking error in a coordinate frame fixed to the WMR (e.g., Binh et al. (2019); krjanc and Klanar (2017)). However, linearizing the TT error easily results in local stability from the aspect of the continuous control. The second strategy is to use the linearization feedback technique which defines the linear relationship from the inputs to the outputs (Majd et al., 2020). This strategy guarantees the global stability. Our control method follows the second strategy, where a proportional gain feedback is designed to stabilize the TT error.

Table 1.

Comparison between various TT control methods.

Methods	Advantages	Disadvantages
Backstepping (Binh et al., 2019; Reis et al., 2023; Zhang and Yang, 2022)	Backstepping iternative.	The virtual control signal needs to be repeatedly derived, causing complexity expansion.
DSC (Elhaki and Shojaei, 2022)	The problem of the frequent differential in Backstepping method is solved.	To guarantee stability, dynamic surface gain parameters in control orders need to be chosen carefully.
SMC (Moudoud et al., 2023)	Strong robustness to system uncertainty.	Irremovable chattering.
MPC (krjanc and Klanar, 2017)	MPC generates control action by optimizing a defined objective function repeatedly over a finite moving prediction horizon. It consists of model prediction, roll optimization, and feedback correction.	The method is sensitive to accuracy of the predicted model; High computation complexity; Poor real-time performance; High hardware requirements.
FLC (Alouache and Wu, 2017)	Employing linguistic rules handle uncertainties and imprecise information, and using environmental data can make inferences.	A fuzzy rule base is needed. Maintaining the correctness, consistency, and completeness of the fuzzy rule base is difficult.
NN-based adaptive control (Chen et al., 2021; Elhaki and Shojaei, 2022)	NN can model linear, nonlinear, or complex relationships; unknown parameters can be estimated adaptively; fault-tolerance; real-time control.	Many tunable parameters. Training quanlity depends on the training data.
Feedback linearization control (Li et al., 2021; Pliego-Jiménez et al., 2021)	Simple; global asymptotical stability.	High accuracy requirement for system model. A rapidly increasing velocity component exists at the beginning under large initial position error.

Of interest for this paper are control solutions capable of addressing the collision-free TT problem for multiple WMRs (MWMRs) in a shared 2-D planar environment. Extended to the MWMRs community, the reactive interaction among WMRs enormously increases the difficulty of local trajectory planning. The condition elevates the risk for a collision, path conflict, congestion, and deadlock, so as to the difficulty of the trajectory coordination problem. Although numerous collision avoidance (CA) methods have been proposed, such as a series of variants (Alonso-Mora et al., 2018; Huang et al., 2023) that build on the concept of the velocity obstacle (VO), time-to-collision-based methods (Shahriari and Biglarbegian, 2022b), dynamic vector field (He and Li, 2024), and so on, how to give attention to both accuracy and efficiency while ensuring that each WMR does not collide with other WMRs during the TT still is one of the fundamental challenges in a MWMR’s system. Compared to VO with enlarged conservative bounding volumes and learning-based CA methods with probabilistic safety guarantee, control barrier function (CBF) provides the minimum modification necessary to formally guarantee safety in the context of quadratic programming (QP) and strict safety guarantee for safety-critical systems. Safety barrier certificate (SBC) method is proposed in Borrmann et al. (2015) which extends the CBF to the MWMRs system by incorporating all pairwise collision-free constraints into an admissible control space. The SBC is further applied to a heterogeneous swarm with different maximum accelerations in Wang et al. (2016) and distributes CA responsibilities for WMRs based on their maximum acceleration in Wang et al. (2017). However, the SBC cannot strictly guarantee no collision for all WMRs in a crowed environment because of the introduction of the braking force and may even cause deadlock. Moreover, the SBC solves the synthesized QP equation using the MATLAB quadprog solver which may find a solution slowly and be sensitive to initial values and not suitable to the real-time application. In Li et al. (2021), the control law is built on the Lagrange multipliers method, and bound constraints on optimization variables are included in the piecewise-linear projection function. The controller is globally convergent to a unique equilibrium point and is globally asymptotically stable in the Lyapunov sense. Unlike Wang et al. (2017), the deadlock-avoidance strategy attached as an auxiliary velocity vector is combined with the TT module (Li et al., 2024). The deadlock decision is build on the Lagrange multiplier, ensuring that deadlocks is resolved earlier, before they actually happen.

The WMR inevitably has to pause for a period of time in Grover et al. (2023). The deadlock strategy in Li et al. (2024) is better than that of Grover et al. (2023) in recovery time. However, there are still some open problems that need to be addressed. For example, Figure 1 shows two conflict examples: four WMRs run into the cross point C (top left), and four WMRs are required to follow a same trajectory in the same state (lower left). For the method proposed in Li et al. (2021), WMRs would trap into the deadlock or wander state resulting from the same position competition, generating inefficient paths. Although the article by Li et al. (2024) solves the head-on and crossing conflict, it is inefficient for the merging conflict shown in the lower-left conflict example of Figure 1.

Figure 1.

Two conflict examples: four WMRs, differentiated by colors, run into at the cross point C (top left), and four WMRs are required to follow the same trajectory in the same state (lower left). For the SOATT method (Li et al., 2021), WMRs would pause or wander resulting from the same position competition, generating inefficient paths.

Taking differential-driven MWMRs (can be any type of WMRs) as an illustration example in this study, we focus on addressing the above-mentioned challenges. As an extension of our previous work (Li et al., 2021, 2024), a time-varying position-based simultaneous obstacle avoidance and trajectory tracking (TV-SOATT) scheme is proposed. Different from Li et al. (2021, 2024), a radial bound is introduced for each WMR to reach its goal, instead of tracking a fixed point. This constraint provides a new optimization space for MWMR coordination control. In addition, we found that in a case where no other WMRs request the same position at the same time, the WMR would stop outside the actual reference position. Although the WMR satisfies the TT error bound, the best behavior should be that the WMR reaches the desired position precisely. Consequently, the TT strategy without the error radial bound term, which attaches an auxiliary velocity vector, is chosen as the cost function that is to be minimized. Addition of the auxiliary velocity term addresses the pause and wander problem. Moreover, the constrained optimization problem is usually converted into a unconstrained one by augmenting the cost function using penalty or barrier function (Paden et al., 2016). In this paper, the controller law is built on the Lagrange multipliers on the basis of constructing a QP optimization problem. It can deal with the additional constraints, rather than ways that are based on penalty function/Lagrangian relaxation for constraints fulfillment.

Kinematic model, TV-SOATT scheme design, and unified optimization problem description

In this study, differential-driven WMRs with four wheels (as shown in Figure 2) are employed. We give WMRs’ kinematic model first. Then TT solution, solution for trajectory resource competition, and the obstacle avoidance (OA) solution are investigated in sequence from the perspective of optimization, which generates the TV-SOATT scheme. Finally, they are described as a unified minimization problem-solving methodology at velocity level.

Figure 2.

The WMRs’ kinematic model.

WMRs’ kinematic model

Assume that every employed WMR is symmetrical and both front wheel and rear wheel of each side rotate at the same speed, the WMR’s kinematic can be modeled as (Li et al., 2021)

Ż_{c} = Au \in ℝ^{2 \times 1}

(1)

where $Z_{c} = {[x_{c}, y_{c}]}^{T}$ denotes the coordinate of the control point. $Ż_{c}$ is the time derivative of $Z_{c}$

\begin{matrix} \begin{matrix} A = [\begin{matrix} \frac{r \cos (q)}{2} + \frac{r d_{0} \sin (q)}{L} & \frac{r \cos (q)}{2} - \frac{r d_{0} \sin (q)}{L} \\ \frac{r \sin (q)}{2} - \frac{r d_{0} \cos (q)}{L} & \frac{r \sin (q)}{2} + \frac{r d_{0} \cos (q)}{L} \end{matrix}] \in ℝ^{2 \times 2} \end{matrix} \end{matrix}

(2)

$u = {[u_{l}, u_{r}]}^{T}$ is the control input. r is the wheel radius, q is the heading angle, L is the lateral wheel bases, and $d_{0}$ denotes the perpendicular distance from the midpoint of the wheel axle to the control point. In general, $0 < d_{0} < L ∕ 2$ . In this study, the center of mass (CoM) is chosen as the control point. The nonholonomic constraints inherent to differential-drive WMRs preclude direct full-state control. After feedback linearization, the zero dynamics stability of the system becomes contingent upon $d_{0}$ . When the control point is selected at the CoM, the zero-dynamics stability of the robot is ensured, which is a necessary condition for guaranteeing global asymptotic tracking. Deviation from the CoM introduces centrifugal forces during rotational motion, causing a deviation between the actual posture and the preferred state. Choosing the CoM as the control point offers additional advantages beyond zero-dynamics stability: the kinematic model under CoM control is closer to linear, simplifying the controller design. Additionally, it facilitates measurement and calibration.

TV-SOATT scheme

In this part, we will mathematically describe the TV-SOATT scheme and further describe it as a unified minimization problem.

TT solution

Assume that $Z_{ci}$ and $Z_{di}$ are the actual position and desired position of WMR i, respectively. To achieve $Z_{ci} \to Z_{di}$ , TT scheme is described as (Li et al., 2021)

Ż_{ci} = Ż_{di} - k_{i} ℓ_{i}

(3)

where $Ż_{ci}$ and $Ż_{di}$ are derivative of $Z_{ci}$ and $Z_{di}$ , respectively. $ℓ_{i} = (Z_{ci} - Z_{di})$ denotes the tracking error, and $k_{i}$ is the feedback gain parameter used to improve the tracking accuracy of the WMR i. Li et al. (2021) have proven that introducing error feedback term $- k_{i} ℓ_{i}$ can achieve a promising tracking to the fixed-point target from both theoretical analysis and experiments. Now, consider another scene: a group of WMRs that are not far apart are required to track the same trajectory. Because MWMRs compete for the same position, they will fail to reach the desired position because they repel each other, as shown in the left figure of Figure 3.

Figure 3.

Time-varying trajectory-tracking principle: multiple WMRs (denoted by the circles) are required to reach the seven-pointed star. To address the conflict, as long as the position reached by the WMR is within the given error range from the desired position, trajectory tracking is considered to be successful. The dotted circles in two sub-figures are the permissible error bound.

Trajectory resource competition solution

For this case, we envision to let WMRs track a time-varying position, rather than a fixed point. In other words, redundancy to the TT is imposed on WMRs, and as long as the position reached by the WMR is within the given error range from the desired position, the TT is considered to be successful. Taking Figure 3 as an example, the error bound is set as a circle. On one hand, fixed-point competition happening in the case shown in the left figure of Figure 3 is addressed. On the other hand, all WMRs can keep a user-defined safe distance after introducing the collision-avoidance strategy. Even through a narrow passageway such as a door, WMRs can also adaptively adjust their position to pass the passageway.

Therefore, tracking a time-varying position is interesting for WMRs. In this study, the circle bound is used to test effectiveness of the time-varying-based TT scheme. To keep WMRs’ reachable position within the circle bound whose center is the desired target, it can be mathematically described as $∥ Z_{ci} - Z_{di} ∥ \leq r_{d}$ , where $r_{d}$ denotes the circle bound’s radius, and $Z_{ci}$ is the position of WMR i, $i = 1, 2, \dots, N$ .

OA solution

The OA mechanism that formulates as an inequality, that is, $∥ Z_{ci} - O_{j} ∥ \geq d_{safe}$ , is considered in this study. $O_{j}$ denotes the position of the j-th obstacle, $j = 1, 2, \dots, n$ . WMR senses its surrounding environment relying on the equipped environmental sensing system which consists of sensors. When WMR’s sensing system detects other objects (the detected objects are viewed as obstacles), as shown in case ② and ③ in Figure 4, the control system will compute their relative distances between obstacles and determine whether the computed distances are within the safety threshold. If the distance between obstacles and the WMR is less than the safety threshold $d_{safe}$ , as shown in case ② in Figure 4, OA mechanism will be activated. By always keeping a user-defined safety distance, the collision-free status is ensured.

Figure 4.

When obstacles enter WMR’s detection range, WMR will determine whether distances between obstacles and itself are within the safety threshold. By always keeping distances between the WMR and obstacles outside the safety threshold, the collision-free status is ensured. In case ②, the collision criterion is satisfied, meaning that the collision-avoidance mechanism will be activated in the controller.

Moreover, WMR’s velocity limit is also a non-negligible term. For one thing, limited motor power results in limited velocity for WMR. For another thing, faster velocity may result in a case where the WMR has no ability to respond to the controller’s command in due course of time. Response delay may cause collision. Consequently, WMRs’ velocity limits, $u_{i}^{-} \leq u_{i} \leq u_{i}^{+}$ , should be incorporated into the control law.

Unified optimization problem description

Minimizing the wheel velocity norm of WMR as a cost function, the TV-SOATT scheme can be uniformly descried as a QP optimization problem together with wheel velocity compliance

\begin{matrix} \min_{u_{i}} u_{i}^{T} u_{i} ∕ 2, \end{matrix}

(4a)

\begin{matrix} s.t. u_{i}^{-} \leq u_{i} \leq u_{i}^{+}, \end{matrix}

(4b)

\begin{matrix} ∥ Z_{ci} - Z_{di} ∥ \leq r_{d}, \end{matrix}

(4c)

\begin{matrix} ∥ Z_{ci} - O_{j} ∥ \geq d_{safe} \end{matrix}

(4d)

There are two nodus in solving the aforementioned optimization problem. One is the different-level hybrid optimization problem. Equations (4a) and (4b) are velocity level; however, equations (4c) and (4d) are position level. Directly solving equation (4) is very difficult. The other is the high accuracy problem of inequalities (4c) and (4d) for constraints fulfillment.

Therefore, we need to translate equations (4c) and (4d) into one in the velocity space. To remove the radical sign, equations (4c) and (4d) are first rewritten as

∥ Z_{ci} - Z_{d} ∥^{2} - r_{d}^{2} \leq 0,

(5)

∥ Z_{ci} - O_{j} ∥^{2} - d_{safe}^{2} \geq 0

(6)

Then, differentiating equations (5) and (6) and introducing the error feedback term, we obtain

\partial (∥ Z_{ci} - Z_{d} ∥^{2} - r_{d}^{2}) ∕ ∂t \leq - k_{1} (∥ Z_{ci} - Z_{d} ∥^{2} - r_{d}^{2}),

(7)

\partial (∥ Z_{ci} - O_{j} ∥^{2} - d_{safe}^{2}) ∕ ∂t \geq - k_{2} (∥ Z_{ci} - O_{j} ∥^{2} - d_{safe}^{2})

(8)

where $k_{1}, k_{2} > 0$ are the feedback gain parameters. Furthermore, we have

2 {(Z_{ci} - Z_{d})}^{T} (Ż_{ci} - Ż_{d}) \leq - k_{1} ℓ_{1},

(9)

2 {(Z_{ci} - O_{j})}^{T} (Ż_{ci} - Ȯ_{j}) \geq - k_{2} ℓ_{2}

(10)

with $ℓ_{1_{i}} = ∥ Z_{ci} - Z_{d} ∥^{2} - r_{d}^{2}$ , $ℓ_{2} = ∥ Z_{ci} - O_{j} ∥^{2} - d_{safe}^{2}$ . Combining equations (1), (9), and (10) can be further written as

2 {(Z_{ci} - Z_{d})}^{T} (A_{i} u_{i} - Ż_{d}) \leq - k_{1} ℓ_{1_{i}} .

(11)

2 {(Z_{ci} - O_{j})}^{T} (A_{i} u_{i} - Ȯ_{j}) \geq - k_{2} ℓ_{1_{i}}

(12)

For MWMRs, CA take into account not only the environmental obstacles (static and dynamic) but also the collision between an WMR and other WMRs. To distinguish from both, the collision-avoidance scheme equation (12) is divided into

2 {(Z_{ci} - O_{e_{κ}})}^{T} (A_{i} u_{i}) \geq - k_{2_{1}} ℓ_{e_{i}},

(13)

and

2 {(Z_{ci} - Z_{cj})}^{T} (A_{i} u_{i} - A_{j} u_{j}) \geq - k_{2_{2}} ℓ_{w_{i}}

(14)

Equations (13) and (14) are the OA method between WMR i and its encountered static obstacle $O_{e_{κ}}$ , one between MWMRs, respectively. $O_{e_{κ}}$ denotes the position of the κ-th environmental obstacle, $Z_{cj}$ is the position of the j-th WMR, $j = 1, 2, \dots, N$ , and $j \neq i .$ $ℓ_{e_{i}}$ and $ℓ_{w_{i}}$ are defined as $ℓ_{e_{i}} = ∥ Z_{ci} - O_{e_{κ}} ∥^{2} - d_{e n_{safe}}^{2}$ and $ℓ_{w_{i}} = ∥ Z_{ci} - Z_{cj} ∥^{2} - d_{w_{safe}}^{2}$ , respectively. $k_{2_{1}}$ and $k_{2_{2}}$ are used to adjust the strength of the OA mechanism. $d_{e n_{safe}}$ and $d_{w_{safe}}$ are the safety thresholds determined by users for equations (13) and (14), respectively.

In summary, the minimization optimization scheme described in velocity level for the i-th WMR is as follows

\begin{matrix} \min_{u_{i}} u_{i}^{T} u_{i} ∕ 2, \end{matrix}

(15a)

\begin{matrix} s.t. u_{i}^{-} \leq u_{i} \leq u_{i}^{+}, \end{matrix}

(15b)

\begin{matrix} 2 {(Z_{ci} - Z_{di})}^{T} (Ż_{ci} - Ż_{di}) \leq - k_{1} ℓ_{1_{i}}, \end{matrix}

(15c)

\begin{matrix} 2 {(Z_{ci} - O_{e_{κ}})}^{T} (A_{i} u_{i}) \geq - k_{2_{1}} ℓ_{e_{i}}, \end{matrix}

(15d)

\begin{matrix} 2 {(Z_{ci} - Z_{cj})}^{T} (A_{i} u_{i} - A_{j} u_{j}) \geq - k_{2_{2}} ℓ_{w_{i}} \end{matrix}

(15e)

where equation (15c) represents the TT error constraint. Equations (15d) and (15e) represent the OA constraint and the CA constraint, respectively. In this study, the dynamic environmental obstacle is not considered. The WMR could be viewed as a dynamic obstacle.

However, we found that if employing equation (15), the WMR would stop outside the actual reference position in a case where no other WMRs request the same position at the same time. That is to say, the position tracked by the WMR is $r_{d}$ away from the desired position in a single WMR scenario. Although the WMR satisfies the TT error bound, the best behavior should be that the WMR reaches the desired position precisely. Therefore, following our previous work (Li et al., 2024), the final TV-SOATT method is recast as follows

\begin{matrix} \min_{u} {(A_{i} u_{i} - Ż_{ri})}^{T} (A_{i} u_{i} - Ż_{ri}), \end{matrix}

(16a)

\begin{matrix} s.t. Ż_{ri} = Ż_{di} + k_{i} ℓ_{i} + δ_{i} Q_{i} ℓ_{i}, \end{matrix}

(16b)

\begin{matrix} u_{i}^{-} \leq u_{i} \leq u_{i}^{+}, \end{matrix}

(16c)

\begin{matrix} 2 {(Z_{ci} - Z_{di})}^{T} (Ż_{ci} - Ż_{di}) \leq - k_{1} ℓ_{1_{i}}, \end{matrix}

(16d)

\begin{matrix} 2 {(Z_{ci} - O_{e_{κ}})}^{T} (A_{i} u_{i}) \geq - k_{2_{1}} ℓ_{e_{i}}, \end{matrix}

(16e)

\begin{matrix} 2 {(Z_{ci} - Z_{cj})}^{T} (A_{i} u_{i} - A_{j} u_{j}) \geq - k_{2_{2}} ℓ_{w_{i}} \end{matrix}

(16f)

The TT strategy without the error radial bound term, that is, equation (3), is chosen as the cost function that is to be minimized. Note that an auxiliary velocity vector ( $δ_{i} Q_{i} ℓ_{i}$ ) is additionally considered in equation (3), which is to address the WMR pause problem resulting from the path conflict. Parameters $Q_{i}$ and $δ_{i}$ are explained in our previous work (Li et al., 2024).

Optimization problem reformulation and design of The controller

Optimization problem reformulation

Assume that

\begin{matrix} \begin{matrix} u = [\begin{matrix} u_{1} \\ ⋮ \\ u_{i} \\ ⋮ \\ u_{N} \end{matrix}] \end{matrix} \in ℝ^{2 N}, \begin{matrix} u^{-} = [\begin{matrix} u_{1}^{-} \\ ⋮ \\ u_{i}^{-} \\ ⋮ \\ u_{N}^{-} \end{matrix}] \end{matrix} \in ℝ^{2 N}, \begin{matrix} u^{+} = [\begin{matrix} u_{1}^{+} \\ ⋮ \\ u_{i}^{+} \\ ⋮ \\ u_{N}^{+} \end{matrix}] \end{matrix} \in ℝ^{2 N}, \end{matrix}

(17)

\begin{matrix} \begin{matrix} A = [\begin{matrix} A_{1} & 0 & \dots & 0 & \dots & 0 \\ 0 & A_{2} & \dots & 0 & \dots & 0 \\ ⋮ & ⋮ & ⋱ & 0 & \dots & 0 \\ 0 & 0 & \dots & A_{i} & \dots & 0 \\ ⋮ & ⋮ & \dots & 0 & ⋱ & 0 \\ 0 & 0 & \dots & 0 & \dots & A_{N} \end{matrix}] \end{matrix} \in ℝ^{2 N \times 2 N} \end{matrix}

(18)

$Ż_{r} = [Ż_{r_{1}}, \dots, Ż_{r_{i}}, \dots, Ż_{r_{N}}] \in ℝ^{2 N}$ , where $δ = diag ([δ_{1}, \dots, δ_{N}])$ $\otimes I_{2} \in ℝ^{2 N \times 2 N}$ . Symbol ⊗ denotes Kronecker product. $Q = blkdiag (Q_{i}) \in ℝ^{2 N \times 2 N}$ . For equation (16d)

\begin{matrix} \begin{matrix} M_{1} = [\begin{matrix} {(Z_{c 1} - Z_{d 1})}^{T} & \dots & 0 \\ 0 & \dots & 0 \\ ⋮ & ⋱ & 0 \\ 0 & \dots & {(Z_{cN} - Z_{dN})}^{T} \end{matrix}] \end{matrix} \in ℝ^{N \times 2 N} \end{matrix}

$ℓ_{1} = [ℓ_{1_{1}}, \dots, ℓ_{1_{i}}, \dots, ℓ_{1_{N}}] \in ℝ^{N}$ , we can obtain that

2 M_{1} (Au - H_{1}) \leq - k_{1} ℓ_{1}

(19)

where $H_{1} = [Ż_{d 1}, \dots, Ż_{di}, \dots, Ż_{dN}] \in ℝ^{2 N}$ . Then, equation (19) can be further rewritten as

ωu \leq A_{right}

(20)

where $w = 2 M_{1} A$ , $A_{right} = 2 M_{1} H_{1} - k_{1} ℓ_{1}$ . As for n static obstacles, let

\begin{matrix} \begin{matrix} M_{2} = \\ [\begin{matrix} {(Z_{c 1} - O_{e κ_{1}})}^{T} & 0 & \dots & 0 \\ ⋮ & ⋱ & ⋮ & ⋮ \\ 0 & {(Z_{cN} - O_{e κ_{1}})}^{T} & \dots & 0 \\ ⋮ & ⋮ & ⋱ & 0 \\ 0 & 0 & \dots & {(Z_{cN} - O_{e κ_{n}})}^{T} \end{matrix}] \end{matrix}, \end{matrix}

(21)

\begin{matrix} \begin{matrix} ξ_{2} = [\begin{matrix} ∥ Z_{c 1} O_{e κ_{1}} ∥^{2} \\ ⋮ \\ ∥ Z_{cN} O_{e κ_{1}} ∥^{2} \\ ⋮ \\ ∥ Z_{c 1} O_{e κ_{n}} ∥^{2} \\ ⋮ \\ ∥ Z_{cN} O_{e κ_{n}} ∥^{2} \end{matrix}] \end{matrix} \in ℝ^{Nn} \end{matrix}

(22)

where $M_{2} \in ℝ^{Nn \times 2 Nn}$ . Let $u_{1} = repmat (u, n, 1) \in ℝ^{2 Nn}$ and $A_{blk} = blkdiag (A, n, 1) \in ℝ^{2 Nn \times 2 Nn}$ , MATLAB function $blkdiag (∙)$ denotes a diagonal matrix, then the OA formulation equation (16e) considering N WMRs and n environmental obstacles is rewritten as

B u_{1} \leq B_{right}

(23)

where $B = - 2 M_{2} A_{blk}$ , $B_{right} = k_{2_{1}} (ξ_{2} - d_{e n_{safe}}^{2})$ . Note that for simplicity, we incorporated n environmental obstacles into equation (23) via the replicate matrix function repmat(⋅). $B u_{1} \leq B_{right}$ can be easily divided into n sub-equations related to control variables u. For the inter-MWMRs CA, assume that $H_{2} = blkdiag (M_{3}) \in ℝ^{(N^{2} - N) ∕ 2 \times (N^{2} - N)}$ with $M_{3} = [m_{12}, m_{13}, \dots, m_{1 N}, m_{23}, \dots, m_{ij}, \dots, m_{(N - 1) N}] \in$ $ℝ^{1 \times (N^{2} - N)}$ , $m_{ij} = {(Z_{ci} - Z_{cj})}^{T}$ , $i = 1, \dots, N - 1$ , $j = i + 1, \dots, N$ , $i \neq j$ . $D_{right} = k_{2_{2}} ℓ_{w}$ , and $ℓ_{w} = [ℓ_{w_{1}}, \dots, ℓ_{w_{i}}, \dots, ℓ_{w_{N}}] \in ℝ^{(N^{2} - N) ∕ 2}$ , we have

Du \leq D_{right}

(24)

where $D = - 2 H_{2} CA$ . For C, $C \in ℝ^{(N^{2} - N) \times 2 N}$ , which is composed of 0, 1, and $- 1$ . Pseudo-code of the determination of parameter C is given in Li et al. (2021). Therefore, the QP description of the combined-formed TV-SOATT method is

\begin{matrix} \min_{u} {(Au - Ż_{r})}^{T} (Au - Ż_{r}), \end{matrix}

(25a)

\begin{matrix} s.t. Ż_{r} = Ż_{d} + kℓ + δ Q ℓ, \end{matrix}

(25b)

\begin{matrix} u^{-} \leq u \leq u^{+}, \end{matrix}

(25c)

\begin{matrix} ωu \leq A_{right}, \end{matrix}

(25d)

\begin{matrix} B u_{1} \leq B_{right}, \end{matrix}

(25e)

\begin{matrix} Du \leq D_{right} \end{matrix}

(25f)

Controller design

In this study, the controller is built on the Lagrange multiplier method. Specifically, define the following Lagrange function of equation (25)

\begin{matrix} \begin{matrix} L (u, λ, α, β) = {(Au - Ż_{r})}^{T} (Au - Ż_{r}) + λ^{T} (wu - \\ A_{right}) + α^{T} (B u_{1} - B_{right}) + β^{T} (Du - D_{right}) \end{matrix} \end{matrix}

(26)

where $λ \in ℝ^{N}, α \in ℝ^{Nn}, β \in ℝ^{(N^{2} - N) ∕ 2}$ , are the Lagrange multiplier. Following Li et al. (2021), the controller is obtained

\begin{matrix} ϵ \overset{\cdot}{u} = - u + P_{1} (u - 2 A^{T} (Au - Ż_{r}) - w^{T} λ \\ - [B^{T} α] (1 : 2 N) - \dots - [B^{T} α] (2 N \\ (n - 1) + 1 : 2 Nn) - D^{T} β), \\ ϵ \overset{\cdot}{λ} = P_{2} {(wu - A_{right} + λ)}^{+} - λ, \\ ϵ \overset{\cdot}{α} = P_{3} ({(B u_{1} - B_{right} + α)}^{+} - α, \\ ϵ \overset{\cdot}{β} = P_{4} ({(Du - D_{right} + β)}^{+} - β \end{matrix}

(27)

To limit complexity of the controller, bound constraint equation (25c) is mapped into the piecewise linear projection function $P_{1} (\cdot) : ℝ^{2 N} \to Ω$ . $P_{1} (u) = [{[P_{1} (u)]}_{1}, {[P_{1} (u)]}_{2}, \dots,$ ${[P_{1} (u)]}_{2 N}]$ with its i-th element being

{[P_{1} (u)]}_{i} = {\begin{matrix} u_{i}^{-} if u_{i} < u_{i}^{-}, \\ u_{i} if u_{i}^{-} \leq u_{i} \leq u_{i}^{+}, \\ u_{i}^{+} if u_{i} > u_{i}^{+} . \end{matrix}

(28)

P_{2} {(ψ_{i})}^{+} = {\begin{matrix} 0 if ψ_{i} < 0, \\ ψ_{i} if ψ_{i} \geq 0 \end{matrix}

(29)

$P_{2} {(ψ_{i})}^{+} \in ℝ^{N}$ , $ψ_{i} = {[wu - A_{right} + λ]}_{i}$ is the i-th element of $wu - A_{right} + λ$ . $P_{2} {(\cdot)}^{+}$ ensures that

{\begin{matrix} λ = 0 if wu \leq A_{right}, \\ λ > 0 if wu > A_{right} \end{matrix}

(30)

Similarly, $P_{3} {(\cdot)}^{+} \in ℝ^{Nn}$ and $P_{4} {(\cdot)}^{+} \in ℝ^{(N^{2} - N) ∕ 2}$ ensure

{\begin{matrix} α = 0 if B u_{1} \leq B_{right}, \\ α > 0 if B u_{1} > B_{right}, \end{matrix}

(31)

{\begin{matrix} β = 0 if Du \leq D_{right}, \\ β > 0 if Du > D_{right} \end{matrix}

(32)

The designed controller is globally convergent to a unique equilibrium point. Due to similarity with our previous work (Li et al., 2021), the proof is omitted.

Simulation

Effectiveness of both the TV-SOATT method and the designed controller are shown in this section. A yellow circle is used to show the allowable TT error bound of an WMR. $q (0)$ and $u (0)$ denote the initial heading angle and the initial wheel velocity. $λ (0), α (0), andβ (0)$ are the initial Lagrange multiplier. They are initialized as zero. Other used parameters are $ϵ = 0.1$ , $u^{+} = 4 m ∕ s$ , $u^{-} = - 4 m ∕ s$ .

Single WMR TT

We first consider a scene where a WMR that starts from ${[0, 0.2]}^{T}$ is expected to track a $4 m$ straight line with the reference trajectory being ${[0 + 0.1 t, 0]}^{T}$ . Two static obstacles, $O_{1}$ and $O_{2}$ , are located in ${[1, 0]}^{T}$ and ${[2, 0]}^{T}$ . The obtained TT results are shown in Figure 5(c), where the $d_{e n_{safe}} = 0.3 m$ and $r_{d} = 0.5 m$ , $θ = - 16 6^{\circ}$ .

Figure 5.

Collision avoidance and TT results of a WMR under three control methods: (a) tracking result achieved by method proposed in Li et al. (2021), (b) tracking result achieved by equation (15), (c) tracking result achieved by equation (25), (d) distance profile corresponding to (a), (e) distance profile corresponding to (b), (f) distance profile corresponding to (c), (g) wheel velocity profile corresponding to (a), (h) wheel velocity profile corresponding to (b), (i) wheel velocity profile corresponding to (c).

Effectiveness

Figure 5(c), (f), and (i) show that the WMR successfully avoided the collision with two static obstacles (see Figure 5(c)) and always kept a safe distance $\geq 0.3 m$ from two obstacles $O_{1}$ and $O_{2}$ (see Figure 5(f)). Specifically, outside the non-safety region (i.e. $∥ Z_{ref} - O_{i} ∥ \geq d_{e n_{safe}}$ , $i = 1, 2$ ), which means that the obstacles $O_{1}$ and $O_{2}$ did not threaten the safety of the WMR, the WMR moved along the reference trajectory. Around $t = 10 s$ and $t = 20 s$ , the WMR deviated from the red reference trajectory because $∥ Z_{ref} - O_{i} ∥$ satisfied the collision criteria. The collision-avoidance strategy equation (25e) came into force. Moreover, we can see from Figure 5(i) that the wheel velocity u always satisfies $u > 0$ during exercise.

Comparison

We compared the TV-SOATT method equation (25) with other two methods (the SOATT method proposed in Li et al. (2021) and equation (15)). Figure 5(a) is a result achieved by the SOATT method. Figure 5(b) is one obtained by the equation (15). For the SOATT method, both the collision-avoidance and TT constraints are satisfied (see Figure 5(d)). However, the WMR paused ( $u = 0$ ) around $t = 10 s$ and $t = 20 s$ (see Figure 5(g)). Around $t = 15 s$ and $t = 25 s$ , the WMR was imposed as an excessive instantaneous velocity accessible to the velocity upper bound $u^{+} = 4 m ∕ s$ to reach the desired positions. For the method equation (15), we observed two phenomena from Figure 5(b) and (h): one is that WMR is quiescent within $[0 - 5] s$ (see Figure 5(h)). This is because the initial position of the WMR is within the allowable TT error. The other is that the WMR stopped at ${[3.5, 0]}^{T}$ . The WMR satisfied the TT error bound constraint $∥ Z_{c} - Z_{d} ∥ \leq 0.5 m$ , but the best behavior should be the WMR reaching the desired position precisely.

Note that in this example, we specifically set the WMR to not be in its desired position at the initial moment. Following Figure 5, the WMR successfully moves along its reference trajectory under our TT method, when the CA strategy is not activated. A drawback of the TT controller from Shahriari and Biglarbegian (2022a) is that when the desired heading angle of the WMR is $0^{\circ}$ , the WMR is unable to move along its reference trajectory if the WMR’s initial position is not located at the preferred position but its initial orientation is $0^{\circ}$ . For this case, the WMR is assigned to a zero angular velocity.

MWMR trajectory tracking

Sine TT

In this example, eight WMRs which are located in different initial positions are required to track a same sine trajectory $Z_{d} = {[t, 10 \sin (0.2 t)]}^{T}$ , as shown in Figure 6(a). For a better eye illustration, error threshold of an WMR deviating from the target position is set as $r_{d} = 5 m$ . The inter-MWMRs’ safety threshold is set as $2 π r_{d} ∕ Nm$ . Safety threshold between an WMR and an environmental obstacle is set as $3 m$ . The whole travel time is set as $50 s$ , and the initial heading angles of all WMRs are set as $4 5^{\circ}$ . Note that in this simulation, $ϵ = d_{t} = 0.001$ , $u^{+} = - u^{-} = 30 m ∕ s$ , and $θ = 9 0^{\circ}$ . In this simulation, we considered a high-speed scenario that requires the WMR to move quickly within a short time step. Therefore, a large wheel velocity bound $u^{+} = - u^{-} = 30$ m/s is adopted. Figure 6 gives position snapshots of eight WMRs achieved by the controller equation (27) at different instances. Each WMR is distinguished by different colors. A static point obstacle located in ${[39, 5]}^{T}$ is considered. Figure 6(a) showed the initial condition. It was seen that WMRs 2, 4, and 7 were far from the target position ${[0, 0]}^{T}$ that was to be tracked, and they were not within the allowable TT error bound. Figure 6(b) showed the position of every WMR at $t = 10 s$ and their respective travel trajectory. Under the designed controller, eight WMRs were within the allowable yellow bound. Distances between any two WMRs in the workspace were greater than or equal to the defined safety threshold $10 π ∕ 8 m$ , see Figure 7(a) and (c). Around $t = 40 s$ , all WMRs altered their trajectories (see Figure 6(c)). This is because that distance from the WMR 3 to the red static obstacle O satisfies the inequality constraint $∥ Z_{c_{3}} - O ∥ < d_{e n_{safe}}$ (so did the WMR 8). Therefore, the corresponding OA strategy was activated, $∥ Z_{c_{3}} - O ∥ = 3 m$ (see Figure 7(b)). Trajectory alterations of the WMR 3 and the WMR 8 enabled trajectories of other WMRs to change. Figure 6(d) showed the overall travel trajectory profiles of all WMRs. Combining Figure 6 with Figure 7, it is observed that all WMRs satisfy constraints equations (25d)–(25f). When the relative distance violates the setting threshold, it will be restricted to the bound.

Figure 6.

Position snapshots of eight WMRs achieved by equation (27) at different instances: (a) $t = 0 s$ , (b) $t = 10 s$ , (c) $t = 40 s$ , (d) $t = 50 s$ . Each WMR is distinguished by different colors. A static point obstacle is denoted by a red circle, whose position is ${[39, 5]}^{T}$ .

Figure 7.

Distance profiles corresponding to Figure 6: (a) Distance profiles between MWMRs. (b) Distance profiles between each WMR and the static obstacle O. (c) Distance profiles between each WMR and its desired position. Constraints equations (25d)–(25f) are always satisfied.

Straight TT

We consider a case where eight MWMRs would pass a narrow doorway. We deliberately placed eight WMRs in a circular distribution satisfying all constraints at the initial time, aiming at highlighting the capability of WMRs passing the narrow doorway under control of the proposed TV-SOATT scheme. The overall travelled time is $80 s$ . The error bound on the desired target is set as $r_{d} = 0.7 m$ , and $d_{e n_{safe}} = 0.3 m$ , $d_{w_{safe}} = 2 π r_{d} ∕ Nm$ . The obtained simulation result is shown in Figure 8. Distance profiles between WMRs, ones between WMRs and doorway, and ones between WMRs and the tracking target are given in Figure 9. Obviously, all WMRs pass successfully the narrow doorway while satisfying the constraints equations (25d) and (25e). Constraint equation (25f) is slightly violated during the WMRs passing the door (see Figure 9(a)). This is because the gain parameter in equation (25e) is greater than one in equation (25f) to avoid the narrow doorway. Combining all simulations, it can be concluded that all WMRs are able to adaptively adjust their respective positions while satisfying the constraints. Therefore, the proposed TV-SOATT method is feasible, and the designed controller is effective for MWRs.

Figure 8.

Results achieved by the proposed controller when eight WMRs pass a narrow doorway.

Figure 9.

Distance profiles corresponding to Figure 8: (a) distance profile between WMRs, (b) distance profile between WMRs and doorway, (c) distance profile between WMRs and the desired tracking target.

Conclusions

The TV-SOATT scheme had been designed by employing a quadratic optimization, with an aim of addressing both path conflict problems induced by the same position competition. It introduced radial bounds for tracking a target and forecasting a collision. The addition of the auxiliary velocity term addressed the problem of the pause and wander in place and had no influence on the convergence rate of the tracking error. The controller was built on the Lagrange multiplier method, whose equilibrium point corresponded to the optimal solution of the proposed TV-SOATT scheme. Simulations showed that by coordinating MWMRs’ velocities, all WMRs had the ability to adaptively adjust their respective positions to satisfy multiple constraints and tracked their respective goals on the premise of no collision.

Many challenges remain. Efficiency improvement of the OA strategy in path conflict is being considered deeply. Control law that can lead the WMRs backward or wait is being considered deeply. Experimental validation is also a part of our plans. The additional challenges and issues that arise during the hardware implementation may include:

Sim-to-Real Gap: The current results are obtained based on the ideal conditions. Real-world disturbances resulting from road condition, communication response delay, and so on could amplify the Sim-to-Real gap.

Computational scalability: When the number of WMRs is small, the cooperative control strategy can adopt the master-slave architecture. However, as the number of WMRs increases, the computational burden on the master will greatly increase, even causing crashes.

Footnotes

Declaration of conflicting interests

The authors declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The authors received no financial support for the research, authorship, and/or publication of this article.

ORCID iD

Xiaoxiao Li

Data availability statement

Data sharing not applicable to this article as no datasets were generated or analyzed during the current study.

References

Alonso-Mora

Beardsley

Siegwart

(2018) Cooperative collision avoidance for nonholonomic robots. IEEE Transactions on Robotics 34(2): 404–420.

Alouache

(2017) Fuzzy logic PD controller for trajectory tracking of an autonomous differential drive mobile robot (i.e. Quanser Qbot). Industrial Robot: An International Journal 45: 23–33.

Binh

Nguyen

, et al. (2019) An adaptive backstepping trajectory tracking control of a tractor trailer wheeled mobile robot. International Journal of Control, Automation and Systems 17(2): 465–473.

Borrmann

Wang

Ames

, et al. (2015) Control barrier certificates for safe swarm behavior. IFAC-PapersOnLine 48: 68–73.

Chen

Liu

, et al. (2021) Adaptive-neural-network-based trajectory tracking control for a nonholonomic wheeled mobile robot with velocity constraints. IEEE Transactions on Industrial Electronics 68(6): 5057–5067.

Draganjac

Miklic

Kovacic

, et al. (2016) Decentralized control of multi-AGV systems in autonomous warehousing applications. IEEE Transactions on Automation Science and Engineering 13(4): 1433–1447.

Elhaki

Shojaei

(2022) Observer-based robust platoon formation control of electrically driven car-like mobile robots under collision avoidance and connectivity maintenance with a prescribed performance. Journal of Vibration and Control 28(19-20): 2696–2716.

Grover

Liu

Sycara

(2023) The before, during, and after of multi-robot deadlock. The International Journal of Robotics Research 42(6): 317–336.

(2024) Simultaneous position and orientation planning of nonholonomic multi-robot systems: A dynamic vector field approach. IEEE Transactions on Automatic Control 69(12): 8354–8369.

10.

Huang

Zeng

Chi

, et al. (2023) Velocity obstacle for polytopic collision avoidance for distributed multi-robot systems. IEEE Robotics and Automation Letters 8(6): 3502–3509.

11.

krjanc

Klanar

(2017) A comparison of continuous and discrete tracking-error model-based predictive control for mobile robots. Robotics and Autonomous Systems 87(C): 177–187.

12.

Krzysiak

Butail

(2022) Information-based control of robots in search-and-rescue missions with human prior knowledge. IEEE Transactions on Human-Machine Systems 52(1): 52–63.

13.

, et al. (2021) Simultaneous obstacle avoidance and target tracking of multiple wheeled mobile robots with certified safety. IEEE Transactions on Cybernetics 52(11): 11859–11873.

14.

, et al. (2024) Distance-and velocity-based simultaneous obstacle avoidance and target tracking for multiple wheeled mobile robots. IEEE Transactions on Intelligent Transportation Systems 5(2): 1736–1748.

15.

Majd

Razeghi-Jahromi

Homaifar

(2020) A stable analytical solution method for car-like robot trajectory tracking and optimization. IEEE/CAA Journal of Automatica Sinica 7(1): 39–47.

16.

Mišeikis

Caroni

Duchamp

, et al. (2020) Lio–a personal robot assistant for human-robot interaction and care applications. IEEE Robotics and Automation Letters 5(4): 5339–5346.

17.

Moudoud

Aissaoui

Diany

(2023) Adaptive integral-type terminal sliding mode control: Application to trajectory tracking for mobile robot. International Journal of Adaptive Control and Signal Processing 37: 603–616.

18.

Paden

Čáp

Yong

, et al. (2016) A survey of motion planning and control techniques for self-driving urban vehicles. IEEE Transactions on Intelligent Vehicles 1(1): 33–55.

19.

Pliego-Jiménez

Martínez-Clark

Cruz-Hernández

, et al. (2021) Trajectory tracking of wheeled mobile robots using only Cartesian position measurements. Automatica 133(C): 109756.

20.

Reis

Xie

Cabecinhas

, et al. (2023) Nonlinear backstepping controller for an underactuated ASV with model parametric uncertainty: Design and experimental validation. IEEE Transactions on Intelligent Vehicles 8(3): 2514–2526.

21.

Shahriari

Biglarbegian

(2022a) A novel predictive safety criteria for robust collision avoidance of autonomous robots. IEEE/ASME Transactions on Mechatronics 27(5): 3773–3783.

22.

Shahriari

Biglarbegian

(2022b) Toward safer navigation of heterogeneous mobile robots in distributed scheme: A novel time-to-collision-based method. IEEE Transactions on Cybernetics 52(9): 9302–9315.

23.

Wang

Ames

Egerstedt

(2016) Safety barrier certificates for heterogeneous multi-robot systems. In: 2016 American control conference (ACC), Boston, MA, 6–8 July, pp. 5213–5218. Piscataway, NJ: IEEE.

24.

Wang

Ames

Egerstedt

(2017) Safety barrier certificates for collisions-free multirobot systems. IEEE Transactions on Robotics 33(3): 661–674.

25.

Zhang

Yang

(2022) Secure adaptive trajectory tracking control for nonlinear robot systems under multiple dynamic obstacles: Safety barrier certificates. IEEE Transactions on Industrial Electronics 69(11): 11549–11559.

26.

Zhu

Gan

, et al. (2020) A hybrid control strategy of 7000 m-human occupied vehicle tracking control. IEEE Transactions on Intelligent Vehicles 5(2): 251–264.