Curvature-Constrained and Constant-Speed Distributed Simultaneous Arrival Control for Multi-Robot Systems
July 16, 2026
Abstract
The simultaneous arrival of multiple mobile robots at a target point is crucial for cooperation tasks such as cooperative encirclement, disaster relief, and environmental monitoring. Although the simultaneous arrival problem itself is already complex, the problem becomes more challenging when there are constraints on the robot trajectory curvatures and the speeds are required to be constant (possibly different for different robots), and the control law for robots needs to be distributed. These constraints are typical for a multi-robot system consisting of, e.g., fixed-wing UAVs. To address this challenge, this paper proposes a distributed switching control method based on the maximum consensus protocol. By exploiting the geometric properties of Dubins paths along with optimization principles, a virtual time variable is introduced, and a hybrid control law that combines optimal control with saturated proportional control is designed. Under the proposed control law, each robot is driven to approach the maximum virtual time among its neighbors, thereby achieving simultaneous arrival under some mild conditions. Furthermore, we prove that in certain cases the proposed method attains a theoretically optimal arrival time. The approach is scalable and real-time, with low communication overhead. Its effectiveness and robustness are validated through extensive simulations and experiments.
1 Introduction↩︎
Cooperative motion planning of multi-robot systems represents a challenge in control theory and robotics. Achieving efficient, robust, and distributed coordination becomes particularly difficult in the presence of physical constraints and disturbances from dynamic environments. In particular, the distributed simultaneous arrival problem requires a group of mobile robots, subject to robot trajectory curvature and constant-speed constraints, to reach a target location simultaneously from arbitrary initial states without a centralized coordination mechanism. This problem relates to diverse application scenarios, such as cooperative target interception and cooperative encirclement. Relevant studies on this topic have been reported in [1]–[4] and the references therein.
The simultaneous arrival problem is similar to the multi-robot rendezvous problem [5]–[7], but with stricter requirements on the target points. In the general rendezvous problem [8]–[10], robots are only required to gather at the same location, and such a location is typically not predetermined. In contrast, the simultaneous arrival problem demands that all robots reach a pre-specified target location at exactly the same time, which imposes higher requirements on control strategies in terms of accuracy and temporal consistency [1]. This problem resembles the cooperative missile strike and interception problem; however, unlike missile systems, the multi-robot distributed simultaneous arrival problem places greater emphasis on individual kinematic constraints (e.g., curvature limitation and constant speed), and necessitate much stricter requirements on target arrival precision and time synchronization. This problem even requires precise simultaneous arrival in the undesirable scenario where robots are close to the targets in distance but the their initial headings direct towards the opposite direction to the target, thereby highlighting the value as well as the challenge of this problem in cooperative tasks.
For the distributed simultaneous arrival problem, several cooperative control approaches have been proposed, which can be broadly categorized into three classes: time-to-go estimation guidance laws, leader-following methods, and consensus-based algorithms. Time-to-go estimation guidance laws regulate the impact or arrival time by explicitly estimating the remaining flight time of each robot. For example, [11] combined proportional navigation guidance (PNG) with time-to-go error feedback to achieve the desired impact time, while [12] designed a Lyapunov-based guidance law using the heading error to achieve time control under constrained initial conditions. Leader-following methods achieve coordination by letting the rest of the robots adjust their motion relative to a designated leader or reference trajectory. For instance, [9] proposed a discontinuous time-invariant control strategy, and [13] employed a bearing-based control law to contract the polygon formed by multiple robots’ positions, thereby achieving coordinated rendezvous. However, these methods often rely on centralized information or designated leaders, limiting their scalability and robustness. In contrast, consensus algorithms can achieve global time synchronization through local information exchange and have been extensively studied in the literature [14], [15], with demonstrated effectiveness even under communication delays and switching topologies [4]. In fact, many existing cooperative guidance methods employ consensus-based strategies and have demonstrated strong guidance performance [16]–[18].
Nevertheless, existing studies still exhibit two limitations. First, most studies do not fully account for the physical saturation constraints of actuators in the form of maximum curvature or steering angle limits, which often prevents theoretical control laws from being effectively implemented on real systems. Second, when robots start extremely close to the target, conventional approaches suffer from severely limited maneuvering space and lack effective trajectory generation and time coordination mechanisms, resulting in infeasible paths. For instance, although [2] considers saturation constraints, it has not considered scenarios in which robots originate near the target. Moreover, many theoretical results have been validated only in simulation environments, with limited experimental verification on real robotic platforms.
Dubins path theory offers an important insight for addressing the aforementioned challenges (i.e., distributed simultaneous arrival subject to kinematic and saturation constraints with adverse initial conditions). It provides a geometrically feasible and time-optimal path generation method for mobile systems subject to curvature constraints, such as unmanned ground vehicles and fixed-wing aircraft [19]. Since Dubins introduced the shortest-path construction for such vehicles in 1957, his model has been widely adopted for minimum-time trajectory planning on various nonholonomic platforms [20], [21]. Under the constant-speed assumption, the shortest path coincides with the minimum-time path, making Dubins paths a natural basis for addressing both temporal coordination and geometric constraints in the simultaneous arrival problem. By integrating Dubins paths with consensus protocols, the challenges posed by kinematic and saturation constraints under extreme initial conditions can be effectively addressed within a distributed control framework.
Contributions: To realize curvature-constrained and constant-speed distributed simultaneous arrival control for a heterogeneous nonholonomic multi-robot system, we propose a distributed switching control method based on the maximum consensus protocol. By exploiting the geometric properties of Dubins paths along with optimization principles, a virtual time variable is introduced, and a hybrid control law that combines optimal control with saturated proportional control is designed. Under the proposed control law, each robot is driven to progressively approach the maximum virtual time among its neighbors, thereby achieving simultaneous arrival. The proposed approach exhibits four notable advantages: 1) It is distributed and scalable, making it applicable to multi-robot systems of arbitrary sizes; 2) It provides a rigorous mechanistic analysis of the simultaneous arrival problem; 3) Under a fixed communication frequency, the communication burden is low, as any two neighboring robots need to transmit and receive at most a single scalar, namely the virtual time variable; 4) Extensive simulations of large-scale multi-robot systems and experiments with multiple quadrotors validate the effectiveness, and we demonstrate the method’s capability of collision avoidance. Notations: Let an integer set be denoted by \(\mathbb{Z}_i^j := \{ m \in \mathbb{Z} : i \le m \le j\}\), where \(i,j \in \mathbb{Z}\) and \(i \le j\). We use boldface to represent vectors \(\mathbf{v} \in \mathbb{R}^n\) in an \(n\)-dimensional real vector space, with the \(i\)-th component denoted by \(v_i\), where \(i \in \mathbb{Z}_1^n\). For any set \(\mathcal{S}\), its complement in the universal set \(\mathcal{U}\) is denoted by \(\mathcal{S}^c := \mathcal{U} \setminus \mathcal{S}\).
Graphs: We define the node set as \(\mathcal{V} := \{1, \ldots, N\}\) to represent robots, and the edge set \(\mathcal{E} \subseteq (\mathcal{V} \times \mathcal{V})\) encodes the communication links between neighboring robots. The neighbor set of robot \(i\) is defined as \(\mathcal{N}_i := \{ j \in \mathcal{V} : (i,j) \in \mathcal{E} \}\). In this paper, we consider only undirected graphs, meaning that if \((i,j) \in \mathcal{E}\), then robots \(i\) and \(j\) can share information bidirectionally. Please see [22] for an introduction to graph theory.
2 Preliminaries And Problem Formulation↩︎
Consider a group of \(N\) mobile robots moving in the space \(\mathbb{R}^2\), subject to nonholonomic motion constraints. For the \(i\)-th robot, \(i \in \mathbb{Z}_1^N\), its state is represented as \(\boldsymbol{\xi}_i = [\boldsymbol{p}_i^{\top}, \theta_i ] \in \mathbb{R}^2 \times \mathbb{S}^1\), where \(\boldsymbol{p}_i = [x_i, y_i]^\top\) denotes the position coordinates and \(\theta_i \in [0, 2\pi)\) denotes the heading angle. The \(i\)-th robot’s kinematics are governed by the nonholonomic constraints: \[\label{0001} \begin{align} & \dot{x}_i = v_i \cos \theta_i, \\ & \dot{y}_i = v_i \sin \theta_i, \\ & \dot{\theta}_i = \omega_i, \end{align}\tag{1}\] where \(v_i > 0\) is a constant linear speed and \(\omega_i \in [-\bar{\omega}_i, \bar{\omega}_i]\) is the controllable angular velocity input. Furthermore, due to the limitations of the robot’s steering mechanism, the minimum turning radius is given by \[\label{0002} \rho_i = \frac{v_i}{\bar{\omega}_i}.\tag{2}\]
To describe the relative position of robot \(i\) with respect to a fixed target point \(\boldsymbol{p}_i^d \in \mathbb{R}^2\), we introduce a polar-coordinate representation of the relative state vector \(\boldsymbol{\zeta}_i = [r_i, \phi_i]^\top \in \mathbb{R}_{\ge 0} \times (-\pi, \pi]\), defined as (see Fig. 1): \[\label{0003} \begin{align} & r_i = \| \boldsymbol{p}_i - \boldsymbol{p}_i^d \|, \\ & \phi_i = \arg(\boldsymbol{p}_i - \boldsymbol{p}_i^d) - \theta_i, \end{align}\tag{3}\] where \(\arg(\cdot)\) denotes the angle between a vector in the plane and the positive \(x\)-axis, taking values in \((-\pi, \pi]\).
Problem ().
Consider a group of \(N\) nonholonomic robots whose kinematics and minimum turning radii are given by 1 and 2 , respectively, where each robots move at possibly different constant speeds \(v_i\). Let \(\boldsymbol{p}_i^d\) denote the target position of robot \(i\), and let the inter-robot communication be modeled by an undirected connected graph \(\mathcal{G} = (\mathcal{V}, \mathcal{E})\). The motion planning objective is to design the angular velocity control inputs \(\Omega = [\omega_1, \omega_2, \dots, \omega_N]^\top \in \mathbb{R}^N\) in 1 such that the resulting robot trajectories \(\boldsymbol{p}_i(t)\) satisfy:
1) (Simultaneous Arrival) There exists a time \(t^*\) such that \(\|\boldsymbol{p}_i(t^*) - \boldsymbol{p}_i^d\| = 0\) for all \(i\in\mathbb{Z}_1^N\).
2) (No Premature Arrival) For any time \(t < t^*\), it holds that \(\|\boldsymbol{p}_i(t) - \boldsymbol{p}_i^d\| > 0\) for all \(i\in\mathbb{Z}_1^N\).
3) (Constraint Satisfaction) The robot trajectories satisfy the constant speed constraint in 1 and the minimum turning radius constraint in 2 .
It is noteworthy that each control input \(\omega_i\) depends solely on the state \((r_i, \phi_i)\) of robot \(i\), and the states of its neighbors set \(\mathcal{N}_i\) via communication, and thus the control law is distributed and scalable.
Definition (Optimal Arrival Time).
The optimal arrival time is defined as \(t^* = \min_{t \in \mathcal{T}} t\), where the set \(\mathcal{T} = \{ t\ge 0 : \| \boldsymbol{p}_i(t) - \boldsymbol{p}_i^d \| = 0, \, \forall i \in \mathbb{Z}_1^N \}\).
Remark 1. Compared with single-robot navigation tasks as in [23] and [24], our problem involves multiple robots and imposes a temporal coordination constraint requiring simultaneous arrival. Compared with tasks that guide robots to desired poses as in [25] and [26], although the terminal orientations in our problem are unconstrained, the additional requirement of simultaneous arrival increases the problem’s complexity.
3 Max-Consensus-Driven Simultaneous Arrival Control↩︎
This section introduces a maximum consensus approach by incorporating virtual time variables. The proposed approach not only guarantees that all robots arrive at their respective target positions simultaneously, but also is capable of addressing the simultaneous arrival problem, including undesirable initial states where some robots are initially very close to their targets.
3.1 Virtual Time Variable↩︎
We introduce a virtual time variable \(T(r,\phi):\mathbb{R}^2\to\mathbb{R}\), defined as the theoretically minimal time required for a robot to reach its target under curvature constraints. Given the robot’s constant speed, this variable is in a one-to-one correspondence with the Dubins path length \(L\) without considering the terminal heading, i.e., \(L = v T\), where \(v\) is constant. Although \(T\) does not necessarily equal the actual time spent by the robot, it serves as a metric to characterize the optimal traveling time in trajectory planning and control. Leveraging this variable, the trajectory planning problem boils down to the analysis of time optimality, establishing a direct connection to classical time-optimal control problems. For the case of a single Dubins-car-modelled robot pursuing a stationary target, this reformulation corresponds to the classical minimum-time control problem, which has been systematically studied in [19]. In this section, we omit the subscript \(i\) for variables associated with robot \(i\).
The time optimal control law for the Dubins paths without terminal heading constraints [2] is given by \[\label{eq:0006} \omega^*(r,\phi) = \begin{cases} -\bar{\omega}, & (r,\phi) \in S_- \cup C_-, \\ 0, & (r,\phi) \in S_0, \\ \bar{\omega}, & (r,\phi) \in S_+ \cup C_+, \end{cases}\tag{4}\] where the sets \(C_-\), \(C_+\), \(S_-\), \(S_+\), and \(S_0\) are defined as \[\label{eq:0007} \begin{align} & C_- = \{ (r,\phi) : r \le 2 \rho \sin\phi, \phi > 0 \}, \\ & C_+ = \{ (r,\phi) : r \le -2 \rho \sin\phi, \phi < 0 \}, \\ & S_- = \{ (r,\phi) : r > -2 \rho \sin\phi, \phi < 0 \}, \\ & S_+ = \{ (r,\phi) : r > 2 \rho \sin\phi, \phi > 0 \}, \\ & S_0 = \{ (r,\phi) : \phi = 0 \}. \end{align}\tag{5}\]
Remark 2. The term \(2\rho \sin \phi\) in 5 represents the lateral offset corresponding to the heading error \(\phi\) at the radius of curvature \(\rho\). It is used to define the boundaries of the regions \(C_\pm\) and \(S_\pm\) for the bang-bang angular velocity control \(\omega^*(r,\phi)\)[27]. The sign of \(\phi\) only determines the turning direction, either a left turn (corresponding to \(C_+\), \(S_+\)) or a right turn (corresponding to \(C_-\), \(S_-\)), and does not affect the time required to reach the target. That is, for the two initial states \((r,\phi)\) and \((r,-\phi)\), the time to reach the goal under the control law in (4 ) is equal. Therefore, unless otherwise stated, we may assume \(\phi \geq 0\) in the following discussion.
Based on (4 ) and (5 ), and under the assumption of no terminal heading constraints, a Dubins path differs from the typical three-segment structure in [28], [29] and contains at most two segments: a single circular arc or a straight line (\(C\) or \(S\)), a circular arc followed by a straight line (\(CS\)), or a double circular arc (\(CC\)). Based on the above analysis, we introduce the following definition.
Definition (Workspace Partitioning).
For a robot with minimum turning radius \(\rho\), the curvature-constrained region is defined as \(\mathcal{D}=\{(r,\phi) : r \le 2\rho\sin|\phi|\}\), and the curvature-feasible region as its complement \(\mathcal{D}^c\).
This partition reflects the geometric threshold characteristics of Dubins path types. Within the curvature-constrained region \(\mathcal{D}\), the robot’s optimal Dubins paths are of the \(CC\) or \(C\) type, where the path curvature always reaches the robot’s maximum allowable value. In contrast, within the curvature-feasible region \(\mathcal{D}^c\), the optimal Dubins paths are of the \(CS\) or \(S\) type, where the path can be realized through a combination of straight and arc segments.
The study [2] has pointed out that within the curvature-constrained region \(\mathcal{D}\), the path length \(L\) exhibits nonlinear characteristics as the initial condition changes, whereas in the curvature-feasible region, \(L\) changes approximately linearly. While the study proposed a numerical method to estimate \(L\) in the curvature-feasible region, it lacks an effective analytical approach for the curvature-constrained region. To address this, we leverage the geometric construction rules of relaxed Dubins paths [30] to derive an analytical solution for the Dubins path length as a function of the initial position, excluding terminal heading constraints, as detailed below.
Figure 2: Illustration of four cases for the calculation of the Dubins path lengths without terminal heading constraints. (a) \(\boldsymbol{\zeta}\in\mathcal{D}^c, |\phi|< \frac{\pi}{2}\); (b) \(\boldsymbol{\zeta}\in\mathcal{D}^c, |\phi|\geq\frac{\pi}{2}\); (c) \(\boldsymbol{\zeta}\in\mathcal{D}, |\phi|<\frac{\pi}{2}\); (d) \(\boldsymbol{\zeta}\in\mathcal{D}, |\phi|\geq\frac{\pi}{2}\)..
Case 1: \(\boldsymbol{\zeta}=[r, \phi]^\top \in\mathcal{D}^c\), let \(s=\sqrt{r^{2}+\rho^{2}-2r\rho\sin|\phi|}\), \(\alpha=\arccos{\frac{\rho}{s}}\), \(\beta=\arccos{\frac{\rho-r\sin\phi}{s}}\), The Dubins path length is given by: \[\label{eq:00017} L(r, \phi)= \begin{cases} (\beta-\alpha)\rho+\sqrt{s^2-\rho^2}, & |\phi|< \frac{\pi}{2}, \\ (2\pi-\alpha-\beta)\rho+\sqrt{s^2-\rho^2}, & |\phi|\geq\frac{\pi}{2}. \end{cases}\tag{6}\]
Case 2: \(\boldsymbol{\zeta}=[r, \phi]^\top \in\mathcal{D}\), let \(s=\sqrt{r^{2}+\rho^{2}+2r\rho\sin |\phi|}\), \(\alpha=\arccos{\frac{3\rho^2+s^2}{4\rho s}}\). \(\beta=\arccos{\frac{\rho^2+s^2-r^2}{2\rho s}}\), \(\gamma=\arccos{\frac{5\rho^2-s^2}{4\rho^2}}\), The Dubins path length is given by: \[\label{eq:00018} L(r, \phi)= \begin{cases} (2\pi+\alpha+\beta-\gamma)\rho, & |\phi|< \frac{\pi}{2},\\ (2\pi+\alpha-\beta-\gamma)\rho, & |\phi|\geq\frac{\pi}{2}. \end{cases}\tag{7}\]
The derivation of 6 and 7 follows from the law of cosines applied to the geometric configuration in Fig. 2, and the monotonicity of the Dubins path length function \(L\) is established in the following theorem.
Theorem (Monotonicity). Let the minimum turning radius be \(\rho\). For \((r,\phi)\in\mathcal{D}^c\), the Dubins path length function \(L(r,\phi)\) is strictly increasing in \(\phi\) over \([0,\pi]\).
Proof. Consider \((r,\phi) \in \mathcal{D}^c\), where \(r > 2 \rho \sin\phi\), and define \(A = \sqrt{s^2 - (\rho - r \sin\phi)^2}\), \(B = \sqrt{s^2 - \rho^2}\), and \(C = \rho - r \sin\phi\).
Case 1: \(\phi \in [0, \frac{\pi}{2}]\). Observe that \((s^2 - \rho C)^2 - A^2 B^2 = r^2 s^2 \sin^2\phi \ge 0\), which implies \(\frac{s^2 - \rho C}{A} \ge B\). Therefore, \[\begin{align} \frac{dL}{d\phi} &= \rho \left( \frac{d\beta}{d\phi} - \frac{d\alpha}{d\phi} \right) + \frac{d}{d\phi} \sqrt{s^2 - \rho^2} \\ &= \frac{r \rho \cos\phi (s^2 - \rho C)}{s^2 A} + \frac{r \rho^3 \cos\phi}{s^2 B} - \frac{r \rho \cos\phi}{B} \\ &= \frac{r \rho \cos\phi}{s^2} \left( \frac{s^2 - \rho C}{A} - B \right) \ge 0, \end{align}\] where the equality holds if and only if \(\phi = 0\).
Case 2: \(\phi \in [\frac{\pi}{2}, \pi]\). Observe that \(s^2 - \rho C = r(r - \rho \sin\phi) > r \rho \sin\phi \ge 0\). Therefore, \[\begin{align} \frac{dL}{d\phi} &= -\rho \left( \frac{d\alpha}{d\phi} + \frac{d\beta}{d\phi} \right) + \frac{d}{d\phi} \sqrt{s^2 - \rho^2} \\ &= \frac{r \rho^3 \cos\phi}{s^2 B} - \frac{r \rho \cos\phi (s^2 - \rho C)}{s^2 A} - \frac{r \rho \cos\phi}{B} \\ &= -\frac{r \rho \cos\phi}{s^2} \left( B + \frac{s^2 - \rho C}{A} \right) > 0. \end{align}\]
In conclusion, for all \(\phi \in [0, \pi]\), we have \(\frac{dL}{d\phi} \ge 0\), with equality if and only if \(\phi = 0\). Hence, the Dubins path length function \(L(r,\phi)\) is strictly monotonically increasing with respect to \(\phi\) over the interval \([0, \pi]\). ◻
Remark 3. If \((r,\phi)\in \mathcal{D}\), then \(L(r,\phi)\) is strictly decreasing with respect to \(\phi \in [\arcsin\frac{r}{2\rho}, \pi-\arcsin\frac{r}{2\rho}]\), and is strictly increasing with respect to \(\phi \in [0, \arcsin\frac{r}{2\rho}]\) and \(\phi \in [\pi-\arcsin\frac{r}{2\rho}, \pi]\). The variation of \(L(r,\phi)\) with respect to \(\phi\) is illustrated in Fig. 3.
Figure 3: (a) The surface of \(L(r,\phi)\) for \(\rho=1\). The black dashed lines indicate the cross-sections at \(r=0.5, 1, 1.5, 4, 6\). (b) The corresponding variations of \(L(r,\phi)\) with respect to \(\phi\)..
3.2 Max-Consensus Control Mechanism↩︎
After introducing the virtual time variable \(T\), our objective is to ensure that all robots achieve simultaneous arrival at the target under a unified virtual time scale. To this end, we employ a max-consensus mechanism to drive the virtual times of individual robots toward agreement, thereby achieving global synchronization. Prior work [15] has investigated the use of max-consensus to address simultaneous arrival for multi-agent systems. However, the approach does not account for curvature constraints, and thus it is not readily applicable to nonholonomic mobile robots. Moreover, it cannot guarantee the optimal arrival time. The advantage of adopting the max-consensus mechanism is that even if some robots are initially very close to their targets, they will be regulated by the distributed control law to “wait for" the”slowest" robot in the team by moving more distances, and in this way, the distributed control law can ensure simultaneous arrival for all agents at the same final time.
Specifically, for the \(i\)-th robot, its desired virtual time is defined as \[\label{121212} T_i^d = \max_{j\in \mathcal{N}_i} T_j,\tag{8}\] and note that the maximum is attained over only the virtual time from the neighboring robots via communication. To ensure that the actual virtual time variable of robot \(i\) is equal to the desired one (i.e., \(T_i = T_i^d\)), the control input \(\omega_i\) must be designed to adjust the robot’s state \((r_i,\phi_i)\) so that the discrepancy \(|T_i^d-T_i|\) is minimized. We first consider a simple case: when \(T_i = T_i^d\), the control law given in (4 ) can be directly applied. For the case \(T_i^d > T_i\), we discuss the following two scenarios:
Case 1: \(\boldsymbol{\zeta}_i \in \mathcal{D}^c\). According to Theorem [th:0001], the virtual time variable \(T_i\) is strictly increasing with respect to the bearing angle \(\phi_i \in [0, \pi]\). Treating \(T_i\) as a univariate function of \(\phi_i\), the inverse function theorem guarantees the existence of a strictly monotone inverse function, which preserves the monotonicity of the original function. Therefore, the inverse function \(T_i^{-1}(T_i^d)\) yields the desired bearing angle \(\phi_i^d\). However, since \(\phi_i\) is restricted to the interval \([0, \pi]\), if \(T_i(\pi) < T_i^d\), no solution exists for the inverse function. In this case, we set the desired bearing angle to \(\phi_i^d = \pi\). From (3 ), the corresponding desired heading is \(\theta_i^d = \arg(\boldsymbol{p}_i - \boldsymbol{p}_i^d) - \phi_i^d\), and the heading error is defined as \(\delta_i = \theta_i^d - \theta_i\). A saturated proportional controller is adopted to regulate the heading error, following [31], [32]. The control law is \(\omega_i = \mathrm{Sat}_{a}^{b}(k_\theta \delta_i)\), where \(k_\theta\) is the control gain and \(a=-\bar{\omega}_i,\, b=\bar{\omega}_i\). The saturation function \(\mathrm{Sat}_a^b:\mathbb{R}\to\mathbb{R}\) is defined as \(\mathrm{Sat}_a^b(x)=x\) for \(x\in[a,b]\), \(\mathrm{Sat}_a^b(x)=a\) for \(x<a\), and \(\mathrm{Sat}_a^b(x)=b\) for \(x>b\).
Case 2: \(\boldsymbol{\zeta}_i \in \mathcal{D}\). Within the curvature-constrained region, the virtual time variable \(T_i\) exhibits only local monotonicity, which is undesirable for control design. To prevent potential instability or discontinuous behavior in this region, the control law given in (4 ) is adopted, aiming to drive the robot rapidly out of the curvature-constrained region and thereby restore system stability.
In summary, the distributed simultaneous-arrival control law can be expressed as \[\omega_i = \begin{cases} \mathrm{Sat}_{-\bar{\omega}_i}^{\bar{\omega}_i} \big( k_\theta (\theta_i^d - \theta_i) \big), & \boldsymbol{\zeta}_i \in \mathcal{D}^c \text{ and } T_i \neq T_i^d, \\ \omega^*(r_i, \phi_i), & \text{otherwise}. \end{cases}\]
3.3 Convergence Analysis↩︎
Under the aforementioned hybrid control strategy, which combines the optimal Dubins-based control with the saturated proportional controller, we next show that all robots will arrive at the target simultaneously if at least one robot succeeds in reaching its target.
Theorem (). For any distinct \(i,j\in \mathbb{Z}_1^N\), there does not exist a time \(t\) such that \(\|\boldsymbol{p}_i(t) - \boldsymbol{p}_i^d\| = 0\) while \(\|\boldsymbol{p}_j(t) - \boldsymbol{p}_j^d\|> 0\). In other words, when robot \(i\) reaches its target, robot \(j\) must have also arrived at its respective target for all \(j \ne i\).
Proof. Suppose that at time \(t_1>0\), at least one robot reaches its target, and let \(\mathcal{I}\ne \emptyset\) denote the set of robots that reach their target, and the set of robots that have not reached their targets is denoted by \(\mathcal{J} = \mathcal{I}^c\). We prove this by contradiction. Suppose that at \(t=t_1\), there is at least one robot that has not reached its target; namely, \(\mathcal{J}\) is non-empty. Since the communication graph \(\mathcal{G}\) is connected, there must exist \(i \in \mathcal{I}\) and \(j \in \mathcal{J}\) such that \(j \in \mathcal{N}_i\). Namely, robot \(i\) reaches its target at time \(t_1\) (i.e., \(\|\boldsymbol{p}_i(t_1) - \boldsymbol{p}_i^d\| = 0\)). Then we must have \(\lim_{t\to t_1} \phi_i = 0\), since the bearing angle \(\phi_i\to 0\) as the robot reaches the target. However, robot \(j\) has not yet reached its target, so we have \(T_i^d - T_i \geq T_j - T_i > 0\) by 8 . This implies \(\lim_{t\to t_1} \phi_i^d = \lim_{t\to t_1} T_i^{-1}(T_i^d) > \lim_{t\to t_1} T_i^{-1}(T_i)\) \(=\lim_{t\to t_1} \phi_i\), indicating that the bearing angle of robot \(i\) cannot reach zero as it approaches the target, a contradiction. ◻
Figure 4: Illustration of the max-consensus process among robots. (a)(b) Ideal trajectories of robot \(i\) when the initial state is \(\phi_i=0\), where the red, blue, and green curves correspond to the curves of the same colors in (c); (c) Variation of the virtual times for robots \(i\) and \(j\); (d) Curves showing \(\bar{t}_i\) versus the bearing angle \(\phi_i\) for robot \(i\) with \(r_i=1,2,5,10\), while the initial state of robot \(j\) is (\(r_j=10, \phi_j=\pi\))..
The assumption that both \(\mathcal{I}\) and \(\mathcal{J}\) are non-empty is invalid. It follows that either \(\mathcal{I}\) or \(\mathcal{J}\) must be empty. In general, whether \(\mathcal{I}=\emptyset\) or \(\mathcal{J}=\emptyset\) depends on the initial states of the robots. We present a relatively relaxed theorem here.
Theorem (). Suppose robot \(i^*\) satisfies the initial states \(T_{i^*}=\max_{i\in \mathbb{Z}_1^N } T_i\). If the gain \(k_\theta\to \infty\) and \[T_{i^*}\geq \sum_{i=1,i\neq i^*}^N \Bigg(\frac{T_i^d}{2}-\frac{r_i}{2v_i}-\frac{2\arctan{\frac{\rho_i}{r_i}}}{v_i}\Bigg) + 2\rho_{i^*},\] then all robots can simultaneously reach their target positions at \(t^*=T_{i^*}\), and \(t^*\) is the optimal arrival time.
Proof. The purpose of taking \(k_\theta\to \infty\) is to make the saturated proportional control closely approximate the optimal control, which requires \(k_\theta\geq\frac{\bar{\omega}_i}{\delta_i}\) or \(k_\theta\leq-\frac{\bar{\omega}_i}{\delta_i}\) at all times and facilitates analysis.
First, we show the following result: For robot \(i\), if the neighbor with the largest virtual time is robot \(j\), and \(\boldsymbol{\zeta}_i \in \mathcal{D}^c\) is satisfied before reaching consensus with \(j\), then we have \[\label{1515} \bar{t}_i \leq \frac{T_i^d}{2}-\frac{r_i}{2v_i}-\frac{2\arctan\frac{\rho_i}{r_i}}{v_i},\tag{9}\] where \(\bar{t}_i\) is the time when \(T_i\) reaches \(T^d_i\) under the condition that \(T^d_i = T_{i^*}\). On one hand, robot \(j\) satisfies \(\dot{T}_j = -1\) under optimal control; on the other hand, since \(T_j > T_i\) (if \(T_j = T_i\), then \(\bar{t}_i = 0\), and the result clearly holds), it follows that \(\delta_i > 0\), and the heading rate control is \(\omega_i = \lim_{k_\theta\to \infty}\mathrm{Sat}_{-\bar{\omega}_i}^{\bar{\omega}_i}\left( k_\theta \delta_i \right) = \pm \bar{\omega}_i\). By converting the time relation \(T_i =\frac{L_i}{v_i}\) into a geometric form, \(\bar{t}_i\) can be determined based on the path length before consensus is reached (i.e., the sum of the red circular arc length \(s\) and the blue line segment length \(l\) in Fig. 4 (a)). Specifically, as shown in Fig. 4 (c), \(\bar{t}_i = t_1 + t_2\), where \(t_1 = \frac{s}{v_i}\), and \(t_2 = \frac{l}{v_i} = \frac{T^d_i(t_1) - T_i(t_1)}{2}\). By combining this with equation 6 , an analytical expression for \(\bar{t}_i\) in terms of the initial bearing angle \(\phi_i\) is obtained. Using a differentiation method similar to that in Theorem [th:0001], we derive \(\frac{\partial \bar{t}_i}{\partial \phi_i} \leq 0\), which implies that the mapping from the initial bearing angle \(\phi_i\) to \(\bar{t}_i\) is decreasing over \(\phi_i \in [0, \pi]\) (see Fig. 4 (d)). Therefore, the maximum value of \(\bar{t}_i\) occurs at \(\phi_i = 0\) (see Fig. 4 (b)), yielding the inequality 9 .
Then, since the communication graph \(\mathcal{G}\) is connected, it admits a spanning tree. Let robot \(i^*\) be chosen as the root of this spanning tree, serving as the reference node for propagating information through the network. Then, for each neighbor \(j \in \mathcal{N}_{i^*}\), the time required to reach consensus satisfies \(t_j\leq\bar{t}_j\), and the corresponding estimate can be propagated recursively along the spanning tree. In the worst-case scenario, \(\mathcal{G}\) degenerates into a chain topology in which each robot communicates only with its immediate successor. In this case, the total time required for the network to achieve consensus is bounded by \(t \leq \sum_{i=1,i\neq i^*}^N \bar{t}_i\). Furthermore, since \(T_{i^*} - t \geq 2\rho_{i^*} \geq 2\rho_{i^*}|\sin{\phi_{i^*}}|\), robot \(i^*\) remains in the curvature-feasible region \(\boldsymbol{\zeta}_{i^*} \in \mathcal{D}^c\).
Finally, according to the optimality of Dubins paths, the time for robot \(i^*\) to reach its target from the initial position is equal to \(T_{i^*}\). Combining the above results, \(t^*\) is the optimal arrival time as it satisfies \(t^* \geq T_{i^*}\). ◻
Remark 4. Our work primarily focuses on the distributed control algorithm based on the max-consensus protocol; therefore, collision avoidance among robots is not discussed in detail. Nevertheless, our method can be seamlessly integrated with existing internal collision avoidance strategies [33], [34]. In the third simulation, we implement a simple trigger-based cooperative collision avoidance mechanism to demonstrate the extensibility of the simultaneous arrival algorithm. Each robot is modeled as a disk of radius \(r_c\). For any two distinct robots \(i,j \in \mathbb{Z}_1^N\), collision avoidance is activated if the following conditions are satisfied simultaneously:
1) \(\|\boldsymbol{p}_i - \boldsymbol{p}_j\| < d_s,\) where \(d_s > \sqrt{2\rho_i r_c + r_c^2} + \sqrt{2\rho_j r_c + r_c^2}\) ensures activation only when the robots are sufficiently close.
2) \((\boldsymbol{p}_i - \boldsymbol{p}_j)^\top (\dot{\boldsymbol{p}}_i - \dot{\boldsymbol{p}}_j) < 0,\) indicating that the robots are moving toward each other along their relative velocity.
3) Let the unit heading directions of robots \(i\) and \(j\) be \(\boldsymbol{s}_i = [\cos\theta_i, \sin\theta_i]^\top\) and \(\boldsymbol{s}_j = [\cos\theta_j, \sin\theta_j]^\top\), respectively. Define the safety circle of robot \(j\) as \(\mathcal{C}_j = \{\boldsymbol{q} \in \mathbb{R}^2 : \|\boldsymbol{q} - \boldsymbol{p}_j\| \le r_s\},\) with \(r_s = 2r_c\). The anticipated trajectory of robot \(i\) is given by \(\boldsymbol{r}_i(\lambda) = \boldsymbol{p}_i + \lambda \boldsymbol{s}_i, \lambda \ge 0\). If there exists \(\lambda^* > 0\) such that \(\|\boldsymbol{r}_i(\lambda^*) - \boldsymbol{p}_j\| \le r_s\), the ray intersects the safety circle. Similarly, for trajectory line segments \(\boldsymbol{r}_i(\lambda) = \boldsymbol{p}_i + \lambda \boldsymbol{s}_i\) and \(\boldsymbol{r}_j(\mu) = \boldsymbol{p}_j + \mu \boldsymbol{s}_j, \lambda,\mu \ge 0\), if there exist \((\lambda^*, \mu^*)\) such that \(\boldsymbol{r}_i(\lambda^*) = \boldsymbol{r}_j(\mu^*)\), the trajectories intersect. If either ray–circle or the trajectory intersection holds, a potential collision is predicted.
When all three conditions are satisfied, robots \(i\) and \(j\) adjust their headings in the agreed direction. For implementation simplicity, their angular velocities are set to the maximum values: \(\omega_i = \bar{\omega}_i, \;\omega_j = \bar{\omega}_j.\)
Figure 5: The first simulation results. Initialize \(N=5\) robots at the origin. The initial heading angles \(\theta_i\) are set by a counterclockwise rotation of \(\frac{\pi}{2}\) applied to the vector pointing from the robot to its target: \([\cos\theta_i, \sin\theta_i]^\top = E \frac{\boldsymbol{p}^d_i - \boldsymbol{p}_i}{\|\boldsymbol{p}^d_i - \boldsymbol{p}_i\|},\) where \(E\) is a rotation matrix. Target positions \(\boldsymbol{p}^d_i\) are uniformly distributed along a circle of radius \(d\) and \(\boldsymbol{p}^d_i = [d \cos(\psi i),d \sin(\psi i)]^\top, \psi = \frac{2\pi}{N+1}, i \in \mathbb{Z}_1^5\). Four experimental groups are conducted with circle radius \(d = 5m, 1m, 3m, 1m\), respectively. The control gain is \(k_\theta = 100\) and the arrival threshold is \(\varepsilon = 0.01m\), i.e., the simulation terminated when \(\|\boldsymbol{p}_i(t^*)-\boldsymbol{p}^d_i\|\leq \varepsilon\) for all robots. In the first two groups, the linear velocity is set as \(v_i = 1 + 0.5(i-1)m/s\) and the maximum angular velocity as \(\bar{\omega}_i = 1rad/s\). In the last two groups, the linear velocity is \(v_i = 1m/s\) and the maximum angular velocity is \(\bar{\omega}_i = 1 + 0.25(i-1)rad/s\) for all \(i \in \mathbb{Z}_1^5\)..
4 Simulations And Experiments↩︎
4.1 Simulations↩︎
We adopt a ring graph as the communication topology, in which each Dubins-car-modelled robot only communicates with its two immediate neighbors. This topology ensures very low communication and computational overhead. In the first simulation, we set \(N=5\) robots to reach their respective target positions simultaneously. As shown in Fig. 5 and Fig. 6, regardless of the initial states, all robots successfully achieved consensus on the virtual time variables and arrived at the target points simultaneously. In the second simulation, we increased the number of robots to \(N=50\), requiring them to reach the target point simultaneously. As illustrated in Fig. 7, all robots are still able to arrive at the target point at the same time. In the third simulation, we used \(N=5\) robots to illustrate the effectiveness and safety of the proposed algorithm when integrated with an internal collision avoidance mechanism. As shown in Fig. 8, the robots successfully achieve simultaneous arrival while maintaining collision-free trajectories.
Figure 6: Data from the first simulation. The first, second, and third rows correspond to the time histories of the virtual time variable \(T_i\), the distance to the target \(r_i\), and the control input \(\omega_i\), respectively. From left to right, the columns correspond to the four different initial state settings..
Figure 7: The second simulation results. In a circular region with the origin as the center and a radius of \(10m\), \(N=50\) curvature-constrained robots are randomly generated. The initial heading angles, linear velocities, and maximum angular velocities of the robots are randomly assigned, with \(\theta_i \in [0,2\pi)\), \(v_i \in [1m/s,5m/s]\), and \(\bar{\omega}_i \in [1rad/s,2rad/s]\). The control gain is \(k_\theta = 100\) and the arrival threshold is \(\varepsilon = 0.01m\). The top row of the figure shows the robot position-time trajectories and motion paths, where small squares and stars indicate the initial and final positions, and arrows represent the initial headings. The final time is \(t^*=8.583s\). The bottom row shows the time histories of the virtual time variable \(T_i\), the distances to the target \(r_i\), and the control inputs \(\omega_i\) for \(i\in \mathbb{Z}_1^{50}\).. a — image, b — image, c — image, d — image, e — image
Figure 8: The third simulation results. The initial states of \(N=5\) robots are set as \(\boldsymbol{p}_i = [10 \cos(\psi i), 10 \sin(\psi i)]^\top\), and the target positions are \(\boldsymbol{p}^d_i = [10 \cos(\psi i + \frac{\pi}{2}), 10 \sin(\psi i + \frac{\pi}{2})]^\top\), where \(\psi = \frac{2\pi}{N+1}\) and \(i \in \mathbb{Z}_1^{5}\). The initial heading angles are aligned with the direction of the vector from the robot to its target, i.e., \([\cos\theta_i, \sin\theta_i]^\top = \frac{\boldsymbol{p}^d_i - \boldsymbol{p}_i}{\|\boldsymbol{p}^d_i - \boldsymbol{p}_i\|}\) for \(i \in \mathbb{Z}_1^{5}\). The robots’ linear velocities are \(v_i = 1m/s\), and their maximum angular velocities are \(\bar{\omega}_i = 1 + 0.1 (i-1)rad/s\). The control gain is \(k_\theta = 100\) and the arrival threshold is \(\varepsilon = 0.01m\). The simultaneous arrival time is \(t^* = 21.754s\).. a — Snapshot at \(t=t^*/4\), b — Snapshot at \(t=t^*/3\), c — Snapshot at \(t=t^*/2\), d — Snapshot at \(t=t^*\)
4.2 Real-World Experiments↩︎
| Example | \(\boldsymbol{\xi}_i(0)=(\boldsymbol{p}_i,\theta_i)\) | \((v_i,\bar{\omega}_i)\) | \(\boldsymbol{p}^d_i\) | |
|---|---|---|---|---|
| Set 1 | UAV 1 | \((12.72,-1.59,3.13)\) | \((0.8,2)\) | \((0,-2)\) |
| UAV 2 | \((9.37,1.95,3.12)\) | \((1,1)\) | \((0,2)\) | |
| Set 2 | UAV 1 | \((5.59,-2.15,0.05)\) | \((0.5,0.4)\) | \((0,-2)\) |
| UAV 2 | \((10.68,0.77,3.26)\) | \((1,1)\) | \((0,2)\) | |
Figure 9: Simultaneous arrival of two UAVs. Snapshots are shown at times \(t=it^*/4,i\in\mathbb{Z}_0^{4}\). The upper figure corresponds to the initial condition set 1, while the lower figure corresponds to the initial condition set 2. From left to right, the figures represent the trajectory plot, virtual time evolution, and control input variation, respectively..
In this study, we utilized two quadrotor UAVs, each equipped with PX4 flight controllers. These UAVs are capable of manually adjusting their curvature while maintaining a constant speed, a feature that can be strictly enforced to assess performance under various initial conditions. The UAVs’ positioning is facilitated by an indoor motion capture system, with inter-UAV communication enabled via WiFi. Two distinct sets of initial conditions were configured for the experiment (see TABLE 1, with base units of meters (m), seconds (s), and radians (rad)), and a simple proportional control method was employed to maintain the flight altitude at \(z=1m\). Simultaneous arrival is defined as the condition when the virtual time difference \(|T_1 - T_2| \leq \delta_t = 0.5s\). The experiment concludes once any UAV reaches its target.
The results are presented in Fig. 9. Notably, even in the presence of disturbances such as oscillations in the \(z\)-axis and communication delays, the error in simultaneous arrival can be kept within a small range (specifically, at time \(t^*\), \(\left|\|\boldsymbol{p}_1(t^*)-\boldsymbol{p}^d_1\|-\|\boldsymbol{p}_2(t^*)-\boldsymbol{p}^d_2\|\right| < 0.1m\)). Additionally, the evolution of virtual time, variations in bearing angle, and UAV trajectories collectively provide indirect validation of Theorem [thm:002] and Theorem [thm:003].
5 Conclusion and Future Work↩︎
We present a distributed control method based on a max-consensus protocol for multi-robot systems with curvature constraints and constant speeds, achieving simultaneous arrival at target points. By introducing a virtual time variable and leveraging the geometric optimization properties of Dubins paths, the proposed method ensures global time synchronization while maintaining optimality, scalability, and low communication overhead. In the control design, a hybrid strategy combining saturated proportional control and optimal control effectively addresses the local non-monotonicity in the curvature-constrained region, thereby guaranteeing system stability. Extensive simulations and experiments with multiple quadrotors demonstrate the effectiveness, robustness, and scalability of the algorithm in large-scale robot systems, and verify the safety of the approach when obstacle avoidance strategies are incorporated.
Future work includes a rigorous theoretical analysis of the zero-dynamics behavior and global convergence in curvature-constrained regions, as well as the design of more refined obstacle avoidance strategies, aiming to extend the algorithm to complex environments. Furthermore, we will focus on extending the simultaneous arrival problem to three-dimensional environments and validating it through experiments with multiple fixed-wing UAVs.
References↩︎
\(^{1}\)Zhouru Xiao, Weijia Yao, Min Liu and Yaonan Wang are with the School of Artificial Intelligence and Robotics, Hunan University, China (email: xzr798@hnu.edu.cn,wjyao@hnu.edu.cn) (Corresponding author: Weijia Yao). The work of Yao was supported by the National Natural Science Foundation of China under Grant 62573182.↩︎
\(^{2}\)Yang Lu is with the College of Intelligence Science and Technology, National University of Defense Technology, China (email: luyang18@mail.sdu.edu.cn).↩︎