{'fraction_non_alphanumeric': 0.05776173285198556, 'fraction_numerical': 0.015862597090033913, 'mean_word_length': 4.420397272457753, 'pattern_counts': {'":': 0, '<': 2, '<?xml version=': 0, '>': 10, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 1, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Energy-Optimal Motion Planning for Agents: Barycentric Motion and Collision Avoidance Constraints

As robotic swarm systems emerge, it is increasingly important to provide strong guarantees on energy consumption and safety to maximize system performance. One approach to achieve these guarantees is through constraint-driven control, where agents seek to minimize energy consumption subject to a set of safety and task constraints. In this paper, we provide a sufficient and necessary condition for an energy-minimizing agent with integrator dynamics to have a continuous control input at the transition between unconstrained and constrained trajectories. In addition, we present and analyze barycentric motion and collision avoidance constraints to be used in constraint-driven control of swarms.


I. INTRODUCTION
Control of swarms systems is an emerging topic in the fields of controls and robotics. Due to their adaptability and flexibility [1], swarm systems have attracted considerable attention in transportation [2], construction [3], and surveillance [4] applications. As we advance to experimental swarm testbeds [5], [6] and outdoor experiments [7], it is critical to minimize the cost per agent in the swarm by considering energy-minimizing algorithms with strong guarantees on safety and performance.
Safety and performance have recently been explored in the context of control barrier functions [8], where agents react to the environment in real-time to satisfy task and safety constraints. In this paper, we seek to advance the state of the art in real-time optimal control by providing: (1) a necessary and sufficient condition for continuity of the control input of an energy-minimizing agent with integrator dynamics, (2) a barycentric motion constraint that guarantees an agent's arrival to a desired set in finite time, and (3) a system of equations involving only the state and control variables that guarantees optimality for the barycentric motion and collision avoidance constraints.
Our first contribution has been explored on a case-by-case basis [9]- [11]; however, there has been no general continuity result reported in the literature. Our main result is applicable as a coarse high-level plan for many systems, particularly with applications in connected and automated vehicles [12] and drone swarming [13]. Our barycentric motion constraint is inspired by the distributed formation control law presented in [14]. In particular, it was shown that swarms of agents constrained to move toward a target point, e.g., a barycenter, are very robust to noise and disturbances. This is partially due to the large space of feasible control inputs. Due to this property, we believe barycentric motion is a natural approach to drive agents into a desired set under the constraint-driven paradigm.
The remainder of the paper is organized as follows. In Section II, we provide our main result that gives sufficient and necessary conditions for our proposed agent to have a continuous control input. In Section III, we propose the barycentric motion constraint, and in Section IV, we derive the corresponding optimal motion primitive. In Section IV-C, we derive the remaining optimality conditions in terms of the state and control of the agent, and in Section IV-D, we present the optimality conditions for the agent to constrain itself to the surface of a closed disk, and equivalently, collision avoidance. Finally, we draw concluding remarks and future work in Section V.
II. MAIN RESULT Consider a dynamical system S(t) with k states in R n at time t ∈ R, where x i (t) ∈ R n , i = 1, . . . , k, k ∈ N. Let the system obey integrator dynamics, i.e., where u(t) ∈ R n is the bounded control input, i.e., ||u(·)|| < ∞. Let e(t) be the rate of energy consumption of the system given by i.e., rate of energy consumption is proportional to the L 2 norm of the control input. Finally, we impose the following constraint, where (4) is a class C q−1 function, and q ∈ N is the minimum number of time derivatives of g(x(t), t) required for u(t) to appear in g (q) (x(t), t).
To compute the energy-optimal control input for the system we follow the standard procedure for constrained optimal control [15]. First we take q − 1 time derivatives of (4) to construct a vector of tangency conditions, where g(x(t), t) is a class C q−1 function. Next, we seek the optimal control input u(t) that minimizes the Hamiltonian, where λ(t) ∈ R k×n are the influence functions, f (x(t), u(t)) are the integrator dynamics defined by (2), and µ(t) ∈ R ≥0 is a Lagrange multiplier where The optimal control input must satisfy ∂H ∂u = 0, thus the optimal unconstrained input is where λ k (t) is the influence function corresponding to the state x k (t) and k = |S(t)|.
Theorem 1. Consider the dynamical system S(t) in (1) with an energy cost (3), and a scalar functional constraint on the state trajectory g(x(t), t). Suppose S(t) transitions between the constrained and unconstrained cases at time t 1 , and let N(x(t), t) denote the tangency conditions (7). If there exists t 2 > t 1 , t 1 , t 2 ∈ R, such that g(x(t), t) = 0 for all t ∈ [t 1 , t 2 ], and N t (x(t 1 ), t 1 ) = 0, then the optimal control input u(t) is continuous at the junction t 1 .
Proof. The jump conditions of the influence functions and Hamiltonian at time t are where t − and t + correspond to the left and right limits of t, respectively, and π is a q × 1 vector of constant Lagrange multipliers. Substituting (8) into (12) yields where the superscripts − and + correspond to variables evaluated at t − and t + , respectively. Note that µ(t − ) = 0 and g (q) (x(t + ), t + ) = 0, thus those terms do not appear in (13). Substituting (11) into (13) yields Next, we simplify the influence functions in (14) using continuity of the states and (10), Substituting (15) into (14) and rearranging yields By Lemma 1, the control input u(t) is continuous at the junction if and only if the right hand side of (16) is zero, hence we may formulate the equivalent condition, Since π is generally nonzero, we seek to satisfy Since g(x(t), t) = 0 for all t ∈ [t 1 , t 2 ], its q derivatives exist and are equal to zero for all t Equating the time derivative of (7) at t + 1 to (18) yields the equivalent condition d dt Eq. (19) can be expanded into a system of equations for the right-hand derivative of each row r, namely where r = 0, 1, 2, ..., q − 1. Condition (20) is the definition of right q-derivatives of g(x, t), which exist by our premise. Thus, (18) is always satisfied and the control input u(t) is continuous. The analysis for the system when exiting the constrained arc to the unconstrained arc is the same, and thus due to space limitations, the proof is omitted. Corollary 1. Consider the dynamical system described in Theorem 1. If the system is traveling along the trajectory imposed by the constraint g(x(t), t) = 0, and the tangency conditions, N(x(t), t), are discontinuous at some time t 1 , then the control input at u(t 1 ) is continuous if and only if a feasible u(t 1 ) exists.
Proof. Let t 1 be the time where any element of N( . Continuity in the system state implies that at least one row of N(x(t + i ), t + 1 ) must be nonzero. To satisfy g(x(t), t) = 0 for t > t 1 requires an infinite impulse control input at t 1 [15], which contradicts the boundedness of u(t). Thus, if a feasible control input exists, the system must transition to an unconstrained arc at t 1 , hence u(t 1 ) is continuous by Theorem 1.
Next, we present a case study for Theorem 1 and Corollary 1 for a double-integrator system in R 2 under a barycentric motion constraint. This is presented in the context of constraint-driven optimal control, where the agent seeks to minimize energy consumption under a path constraint.

III. PROBLEM FORMULATION
As a step toward real-time constraint-driven optimal control of swarm systems, consider a single agent in R 2 with double integrator dynamicṡ where p and v are the agent's position and velocity, respectively. The state of the agent is given by Let p r (t) be a time-varying reference position which the agent seeks to reach in finite time. We denote the relative distance between the agent and reference state as Definition 1. For a desired aggregation distance D ∈ R >0 , we define the barycentric motion constraint where β(·) > 0 imposes decreasing barycentric motion toward the closed disk.We refer to ||r(t)|| > D and ||r(t)|| ≤ D as Case I and Case II of g(x(t), t), respectively.
Finally, we present a constraint-driven optimal control problem to determine the energy-optimal trajectory of the agent under the barycentric motion constraint. given: where [t 0 , t f ] ⊂ R is the planning horizon for the agent.
To solve Problem 1 we impose the following assumptions.
We impose Assumption 1 to ensure that the agent can generate a feasible trajectory at time t 0 . This assumption may be relaxed by extending (25) to include an additional case. However, this would add complexity to the problem without fundamentally changing our analysis.
Assumption 2. There are no disturbances or noise, and the agent is able to track the trajectory generated by Problem 1.
We impose Assumption 2 to analyze the agent's behavior in a deterministic setting. This assumption may be relaxed by imposing some notion of robustness to Problem 1 or reformulating it as a stochastic optimal control problem. through the final steps, which adds complexity without a significant impact on our results.

IV. SOLUTION APPROACH
As a first step, we prove the continuity of control when transitioning between the cases of the barycentric motion constraint (Definition 1). Then, we discuss the constrained motion primitive for Problem 1 and show how to optimally piece together the unconstrained and constrained arcs using only state and control conditions.

A. Properties of the Barycentric Motion Constraint
The constraint g(x(t), t) has two cases (see Definition 1), Case I corresponds to a barycentric spiral, and Case II corresponds to a closed disk centered on the reference state. When an agent transitions between an unconstrained arc and the arc defined by g(x, t) = 0, the control input u(t) is continuous by Theorem 1. Next, we present three lemmas that describe the behavior of the agent while traveling along g(x(t), t) = 0. Lemma 2. If there exist γ ∈ R >0 that lower bounds β(x(t)) for all t ∈ R, then the agent will satisfy ||r(t)|| ≤ D in finite time.
Proof. Let the agent satisfy ||r(t)|| > D. By Definition 1, β x(t) + r(t) ·ṙ(t) ≤ 0. This implies that β x(t) + ||r(t)|| ||ṙ(t)|| cos θ r (t) ≤ 0 by the definition of the dot product, where θ r (t) is the angle between r(t) andṙ(t). Substituting the lower bounds for β x(t) and r(t) and rearranging yields ||ṙ(t)|| cos θ r (t) < − γ D . Thus, the component ofṙ(t) in the direction of r(t) has a negative sign and is upper bounded by a negative constant. This implies that ||r(t)|| will decrease to a distance D in finite time.
Lemma 3. If the agent enters the closed disk of diameter D, as described by Case II of Definition 1, then the agent will remain within the disk for all time.
Lemma 4. If the the agent is travelling along the constrained arc described by Definition 1, and transitions from Case I to Case II at a time t 1 and distance ||r(t 1 )|| = D, then the control input is continuous at the transition.
Proof. When the agent transitions from Case I to Case To stay on the constrained arc requires thaṫ r i (t + 1 ) · r i (t + 1 ) = 0. Thus, agent i must exit the constrained arc at t 1 , and u i (t 1 ) is continuous by Corrolary 1.
By Lemmas 2-4 we have proven that our proposed barycentric motion constraint 1) will drive the agent within a disk of diameter D in finite time, 2) the agent will remain within the disk for all future time, and 3) the discontinuity in g x(t), t does not introduce a discontinuity into the optimal control input. Next, we describe the constrained motion primitive for Case I of the barycentric motion constraint.

B. Constrained Motion Primitive
To solve for the constrained motion of the agent when ||r(t)|| > D, we use Hamiltonian analysis [15]. First, we construct the vector of tangency conditions, and we append the derivative of (29) to the Hamiltonian, The Euler-Lagrange equations are To solve (31) -(33) we follow the method outlined in [13]. Since Problem 1 is a generalization of the problem reported in [13], we impose that ||ṙ(t)|| is a constant. This is the reigning optimal solution [17] for this constrained motion primitive.
For simplicity we will omit the dependence of the unit vectorsp x(t) andq x(t) on x(t) when no ambiguity arises. To guarantee that the basis in Definition 2 is always well defined we select the following functional form for β, where κ ∈ (0, 1) is the cosine of the angle between r andṙ by the definition of the dot product. As b(t) > D, we impose a > 0 when traveling along the barycentric spiral.
Following the procedure of [13], we may projectr(t) onto the unit vectorsp andq, which yields We then seek the time derivatives of (34) and (35). First, It follows thatq and substituting (37) yieldṡ whereβ Substituting (36) into (37) and by the definition ofr, we have Next, we solve each row of (45) by substituting in the Euler-Lagrange equations and taking time derivatives until we have a system of ordinary differential equations that are only a function of a, b(t), µ(t), and their derivatives. We start by decomposing (45) into a system of two equations, where substituting (31) and by rearranging we have which, by (29), simplifies to The next step is to take a time derivative of (50) and (51), then substitute (50) and (51) in for thep andq terms that appear. Then we substitute (32) into the resulting equations and simplify, which yields Finally, we take a time derivative of (52) and (53) and substitute (33). Applying Assumption 3 and simplifying yields Equations (54) and (55) describe the evolution ofμ(t) and b(t) for a given constant speed a and barycentric parameter κ.
To find the optimal control input to the agent we may integrate (39), where b(t 1 ) = b 0 . Finally, substituting (43) and (56) into (37) yields which, by definition of the dot product, is Thus, and the orientation ofr(t) is perpendicular toq. In the next subsection we use (54) and (55) to determine how the agent will optimally transition from the unconstrained to the constrained arc.

C. Barycentric Jump Conditions
Let the agent transition from an unconstrained to barycentric-constrained arc at some time t. The jump conditions for the influence functions are [15], Substituting (31) into (61) and applying continuity of u i (t) implies Next, we project (62) onto the unit vectorsp andq which yields two scalar equations, By definition κ ∈ (0, 1), which implies that µ(t + ) = 0 and π = 0. Thus we may simplify (60) to Finally, the time derivatives of u i (t) arė We may substitute µ(t + ) = 0 into (54) and (55), which yieldsμ Thus, At the junction between the unconstrained and constrained cases, we have four unknowns, p(t), ||v(t)||, and the optimal transition time t. The corresponding four equations are continuity in u(t), by Theorem 1, and (70). We may then apply (29) to find v(t) at the transition, which gives us sufficient boundary conditions to solve for the unconstrained and constrained trajectories. In the next section we provide the optimality conditions when activating the disk constraint given in Case II of Definition 1, which also applies to collision avoidance.

D. Fixed Distance Constraint
Next we consider when ||r(t)|| ≤ D. The corresponding tangency conditions are g (2) x(t), t = 2r(t) ·r(t) + 2ṙ(t) ·ṙ(t), which leads to the same analysis as Section III when N x(t), t) = 0 and g (2) x(t), t = 0 if we impose b(t) = D and κ = 0. In this case (55) implies that µ(t) is a constant, and (54) implies that The derivative of N with respect to the state is and the jump conditions at time t are [15], The condition ∂H which, by Theorem 1, implies Thus, by (76), Combining the time derivative of (31) with (32) and substituting it along with (79) into (75) impliesu(t − ) =u(t + ) + 2Dπ 1 r(t), which, projected ontoq yieldṡ Thus, we have three unknowns: the angle of r(t), the relative speed a = ||ṙ(t)||, and the optimal transition time t. The three corresponding equations are continuity in u(t) by Theorem 1 and the continuity ofu(t) ·q by (80). This is sufficient to determine r(t) andṙ(t) using the tangency conditions (71), which determines the trajectory of the agent along the fixed distance constraint.
Finally, we note that the in a swarm system, for two agents i, j ∈ N, we may write a safe distance constraint for agent i relative to agent j, for some agent radius R. This results in tangency conditions identical to (71) when the constraint is active, i.e., N x(t), t = 0. Thus, the preceding analysis holds for the transition to the collision avoidance constraint if j's trajectory satisfies Assumption 3.

V. CONCLUSIONS AND FUTURE WORK
In this paper, we presented a proof of continuity of the control input for a class of energy-minimizing systems when transitioning between constrained and unconstrained trajectories. We extended this result to include the case where the constraint becomes discontinuous at a point, and finally, we proposed an original barycentric motion for constraintdriven optimal control. We derived the optimal control input when traveling along the constrained arc and derived the optimality conditions for transitioning to the constrained arc as a function of the and control variables. Finally, we extended our analysis to include collision avoidance between agents.
Ongoing research addresses the potential of deriving an efficient shooting method to numerically solve the proposed jump conditions in real-time. Future work should consider extending Theorem 1 for the cases where (1) multiple constraints become active simultaneously, (2) the agent transitions between different constrained arcs, (3) only the right partial derivatives of N(x(t), t) exist, and (4) a constraint becomes active only at a single instant in time. Another potential direction for future research is to include additional agent interactions to achieve a desired emergent behavior from the system [11].