## Abstract

The motions of a formation of mobile robots along predetermined paths are optimized according to a tunable time-energy cost function using the cluster space approach to multiagent system specification and control. Upon path-parameterizing cluster state variables describing the geometry and pose of a multirobot group, an optimal control problem is formulated that incorporates formation dynamics and state constraints. The optimal trajectory is derived numerically via a gradient search, iterating over the initial value of one costate. A multirobot formation control simulation is then used to demonstrate the effectiveness of the technique. Results indicate that a substantial tradeoff is made between energy expenditure and motion time when considered as minimization criteria in varying proportions, allowing the operator to tailor mission trajectories according to desired levels of each.

### Keywords

- time-energy optimization
- motion planning
- multirobot systems
- cluster control

## 1. Introduction

In recent years, multiagent robotic systems have been firmly established as an important topic of research owing to the continued emergence of potential applications. Cooperating teams of robots remotely operated or capable of autonomous navigation and sensing can be used to enhance or extend the functions of single-agent systems in areas such as airborne [1–3] and terrestrial-distributed mobile sensing [4–6], search and rescue [7–9], and intelligent transportation [10–12].

For mobile systems, one of the key technical considerations is the development of a technique to coordinate the motions of individual vehicles. The mutual goal of each agent is typically to establish and maintain a certain spatial configuration or to perform complicated geometrically time-varying maneuvers, a use case of particular interest to this investigation. These desired behaviors lead to a variety of formation control problems, for which a wide range of solutions have been and continue to be explored.

Notable work in this area includes the development of leader-follower strategies in which follower agents control their position relative to a designated leader to meet formation requirements [13–15]. Artificial potential fields have similarly been shown effective as a construct to establish formation-keeping forces between robots within a group [16, 17]. Cluster space, an approach that allows for intuitive specification of formation characteristics and implements control directly on these variables, has also been demonstrated successfully for a number of robotic systems [18]. Current trends in research, however, indicate a focus on the incorporation of formation requirements into the framework of optimal control, and will be discussed in this chapter.

Due to the inherent physical distribution of agents and the potential for limited information exchange, decentralized control protocols for multirobot systems are popular. However, centralized architectures, which exploit global information, are more amenable to executing specific time-varying formation trajectories. The latter pertains to a class of similar methods highly relevant to this chapter that delegate path design to an earlier step, relying on motion planning to avoid inter-agent collisions and achieve varying degrees of coordination [19–24]. Distinguished among these is [25], which frames motion coordination as a velocity-optimization problem. Formations are defined by robot-pair relative geometries as a function of distance along their respective paths, forming a constraint net. Optimal trajectories for each robot are then generated such that these formation errors are minimized. Alternatively, [26] represents the set of robot positions at a particular time as a 2D planar curve, and develops a synchronization controller that regulates robot motions to simultaneously track the prescribed formation boundaries as well as their individual paths. Although an optimal velocity signal is not derived, as in [25], the model allows for complete specification of any formation over time, provided the potentially complicated individual robot paths are attainable. For a number of conceivable formations, this is a non-trivial step and can prove prohibitive. The methods proposed in this chapter, which build from the cluster space approach to multirobot system specification, address this issue.

In contrast to the investigations discussed above, much of current research in multirobot systems is dedicated to the use of distributed optimal control techniques. These are appropriate for applications that permit simple formation specification where robots must operate with communication restrictions and local information such as relative positioning. Consensus algorithms, which draw from concepts in distributed computing and graph theory, are present in many of these approaches [27–29]. In the context of a cooperative multivehicle system, information consensus refers to the convergence of agents in a networked system to a common task or variable such as the center of a formation shape, the rendezvous time, or the direction of formation translation [30]. Also of note are distributed control-based methods to handle swarms, or multiscale dynamical systems, which are comprised of many agents [31–34]. In general, these techniques are only capable of assigning coarse-grained dynamics; specific paths and distributions are not determined a priori. Each unit is held subject to local objectives and constraints, giving rise to certain coherent macroscopic behaviors. For example, Ferrari et al. [35] model global behavior of a multiagent system using PDFs and optimizes subject to coupled local agent dynamics in such a manner that cohesion is achieved and the result requires far less computation than classical optimal control.

Energy efficiency in mobile robotics systems is of great concern given that energy sources are often carried, and practical applications require extended remote operation wherein limited resources must be conserved. Many investigations seek to address this issue through the use of motion planning techniques to reduce the energy consumption in system components. Typically, proposed energy models are incorporated into optimal control frameworks to derive trajectories that minimize energy expenditure [36–40]. For example, in a recent work, Liu and Sun [41] decompose energy consumption into three categories: kinetic energy transformation, overcoming traction resistance, and maintenance of electrical sources for the operation of sensors, on board PCs, and control circuits. This analysis is then used to generate both an energy optimal path and a velocity trajectory, which further conserves energy. While a complete and detailed energy model can be beneficial, it is well known that preventing large torque variations in motors by smoothing velocity profiles is most fundamental to energy conservation [42, 43]. This chapter concentrates on minimizing energy expenditure for a formation of mobile robots in this regard.

The investigations referenced above are geared toward individual robots and therefore do not adequately address the optimal planning of energy-efficient trajectories for multiagent systems where a holistic approach is appropriate. There are, however, a number of methods that include provisions for minimizing aggregate energy consumption. For example, Sieber et al. [44] formulate an LQR-like optimal control problem designed to move a formation of mobile robots to a goal while minimizing input energy and incorporating a provision for formation rigidity into the cost functional. Similarly, Wigstrom and Lennartson [45] generated trajectories that reduce energy consumption using pseudo-spectral optimal control, though paths are considered free. Coverage algorithms such as [40, 46] also implement energy conservation constraints while maximizing the reach of mobile robot networks and sensing. A few swarm-like methods have accounted for power consumption as well, but they minimize global energy in order to achieve stability and cohesion for the group, or to affect the size and internal coverage of the swarm [47, 48]. While these examples address the conservation of group energy, they do not deal with the generation of smooth energy-efficient velocity trajectories along predefined paths, an objective of this chapter.

The concepts of time and energy optimality in the context of trajectory generation for multirobot systems are well represented in the literature. However, they are largely accounted for alongside formation constraints, thus the derived control signals are suboptimal with regard to time and/or energy exclusively. To our knowledge, there are no methods that achieve high precision time-varying maneuvers and successfully separate the generation of optimal trajectories for each robot in a multiagent group from formation requirements. The techniques proposed in this chapter address this shortcoming.

The contribution of this investigation is a method to generate continuous force, acceleration, and velocity profiles for a formation of mobile robots with predetermined paths, while time and energy are minimized in chosen proportions. Previously proposed methods with similar objectives have employed optimization techniques with robot level dynamic constraints and formation level cost functions. In contrast, our treatment uniquely optimizes and imposes constraints at the cluster or formation level, with favorable results. Further advantages of our technique are explained in Section 4.

We first introduce the cluster space control framework, which presents a group of mobile robots as a virtual articulating mechanism in order to facilitate characterization and to implement coordinated control of the system. A parameterization of the cluster dynamic equations is next proposed which results in a reduced second-order state space model. The structure and numerical solution of the optimization are then shown. Finally, a simulation of a three-robot cluster controller is used to verify and validate the solution trajectory, after which analysis and results are presented.

## 2. Cluster space

### 2.1. Cluster space specification of multirobot systems

The *cluster space* control framework conceptualizes a multirobot system as a single entity, a *cluster*, which is described in terms of its global position and orientation, shape, and the relative orientations of individual robots within the cluster. Based on these attributes, a set of independent state variables is defined to specify, control, and monitor the position and motion characteristics of the formation [18]. These quantities can be mathematically related to the positions and velocities of the robots in the group through a formal set of kinematic transforms, much like the end-effector position and velocity of a robotic manipulator can be related to its joint angles and rotational velocities. Similarly, a set of cluster dynamic equations of motion have been defined with coefficients that are a function of dynamic properties of individual robots, allowing generalized forces in cluster space to be related to generalized forces in robot space [49]. These relationships enable the use of a feedback formation control system in which the operator can command the path(s) of a multirobot formation with respect to the cluster states, and be relieved from the task of specifying individual robot motions.

Cluster space represents a natural point of view for the operator suited for specifying well-behaved, smooth, formation state trajectories [50]. Unlike other formation control approaches that must commit to centralized or decentralized protocols, the cluster space framework accommodates a range of (de)-centralization architectures through the selection of cluster state space variables [51]. Cluster control has been experimentally demonstrated with groups of two to six robots, operating on/in land/sea/air, with both piloted and automated controls, and with a variety of compensation strategies. Ongoing work includes use of the technique for real-world applications such as gradient-based adaptive environmental sampling [52], object tracking [53], object transportation [54], and escorting in the presence of long-distance communications [55, 56].

### 2.2. Description of a three-robot cluster

Previous work on the cluster space state representation of a mobile multirobot system presented a generalized framework for a system of *n* robots each with *m* degrees of freedom [1]. In this study, we will consider the implementation of a group of three planar robots each with 3 degrees of freedom: two translational (*x*1, *y*1, *x*2, *y*2, *x*3, *y*3) and one rotational (*θ*1, *θ*2, *θ*3). These variables are referenced to a global frame and constitute the *robot space* description of the pose of the system, *r*. Alternatively, the pose of the system in *cluster space* can be defined by a position vector *c* consisting of variables that characterize the globally referenced cluster location and orientation at the formation centroid (*x*_{c}, *y*_{c}, *θ*_{c}), the geometry of the three-robot triangular formation (*p*, *q*, *β*), and the relative rotations of the individual robots within the cluster (*φ*_{1}, *φ*_{2,} *φ*_{3}). In general, we note that the cluster space control framework provides flexibility in the location of the cluster frame and in the selection of shape variables; for the example used in this study, the shape variables (*p*, *q*, *β*) represent a side-side-angle definition of the three-robot cluster. A depiction of both *robot space* and *cluster space* variables is provided in Figure 1.

### 2.3. Cluster space equations of motion

In previous work, successful cluster space control has been demonstrated through the use of a kinematic, or resolved rate, control approach in which the operator specifies desired cluster velocities, which are then converted into robot velocity commands through an inverse-Jacobian transform. This has been adequate for a number of demonstrations and applications, particularly when the robots have on-board velocity controllers, are heavily damped, and when external disturbances are minor. For more challenging cases, a dynamic cluster space controller was developed, based on the definition of cluster equations of motion. This dynamic model describes the interdependence between the positions, velocities, and accelerations of the cluster state variables and the generalized forces and torques acting on the cluster. It contains coefficients that can be mathematically related to similar coefficients in robot space dynamic equations. The details of this derivation using Lagrange formalism are presented in [54]. The resulting cluster space dynamic equation follows:

where if *n* is the number of robots in the cluster and *m* the number of degrees of freedom of each robot: *F ε R*^{n} is an *nm* × 1 vector of cluster generalized forces and torques belonging to a set defined {*F* | *F*_{imin} ≤ *F*_{i} ≤ *F*_{imax}, *i* = 1 : *nm*}; *Λ*(*c*) is an *nm* × *nm* symmetric, positive definite, cluster mass or inertia matrix of the quadratic form; *nm* × 1 vectors of cluster space state positions, velocities, and accelerations, respectively; *nm* × 1 vector of cluster space Centrifugal and Coriolis forces; *g*(c) is an *nm* × 1 vector of gravity forces in cluster space.

The cluster space equations of motion were derived using a Lagrangian formulation based on the formation’s potential and kinetic energy. An alternate dynamic form for a second-order system can also be realized through a quadratic representation of the first-order cluster velocity product terms [49]:

This version is obtained through the use of Christoffel symbols (*Λ*(*c*), where B is an *nm* × *nm* × *nm* vector of Christoffel symbols:

And ^{Γjkic}is an *nm* x *nm* symmetric matrix for *i*, *j*, *k* = 1 to *nm*:

This form has the desired quality of being conducive to state parameterization, a necessary conversion due to the high dimensionality of the cluster state vector and equations of motion. It does not include a gravity term due to the fact that our analysis is specific to planar rovers whose gravity force is canceled out by the force normal to the surface.

### 2.4. Cluster space parameterization

The cluster space state vector for a formation of *n* robots each with *m* degrees of freedom is of size *nm*, which for the presented three-robot example corresponds to a nine-element state vector. The application of optimal control techniques to a cluster state vector of this size would result in a 2-pt boundary (shooting) problem of large order requiring an unmanageable computational effort. We therefore seek to reduce the dimensionality of the control problem and its numerical solution by defining a one-dimensional path space denoted as *s*(*t*) (where *s* = [0,1]), representing the distance traveled along a specified path. The following draws from the general methodology presented in [58].

The cluster state vector, at a given time, specified as a function of the distance traveled along the path, and its first and second derivatives taken w.r.t. *t* are given by

where *f*_{s} is the unit vector tangent to the path, *f*_{ss} is the curvature, *c*,

If we let *m*(*s*) = Λ(*s*)*f*_{s} and

Using Eq. (8) we have rewritten the cluster dynamics in terms of the velocity *m*(*s*) and *b*(*s*) are the coefficients of acceleration and Coriolis/Centripetal forces, respectively, expressed in cluster path coordinates. It is evident in the transformed dynamics that a linear relationship exists between *F* and *nm*-dimensional state space to two independent states, *m*(*s*) and *b*(*s*) are both of size 9 × 9; each element is an expression of a size that precludes its presentation here.)

Now that we have represented the cluster dynamics in path space, inequality constraints on the magnitude of the cluster force, {*F*_{i min} ≤ *F*_{i} ≤ *F*_{i max}, *i* = 1 : *nm*} can also be converted. Substituting (8) into this expression yields *nm* path space inequality expressions for cluster space velocity and acceleration constraints:

The bounds on the path acceleration and velocity at a given point are

where assuming *m*_{i} ≠ 0 and *m*_{j}*b*_{i} − *m*_{i}*b*_{j} ≠ 0:

Again, these derivations originate in [58, 59]. By virtue of the similarity between cluster space dynamics with that of manipulators, the form of the inequality constraints is identical. They have been presented here for completeness; however, the simulations in Section 4 will not consider trajectories in which they are violated as it is outside the scope of our analysis. Furthermore, a motivating factor behind the inclusion of energy in the minimization is to depart from time-optimal trajectories and reduce the strain on robot actuators. This purpose would be defeated if robot acceleration and velocity bounds were reached.

## 3. Cluster space time-energy optimal control

### 3.1. Problem formulation

To minimize, in varying proportions, the energy and motion time of system (8) along a specified cluster path, we formulate and solve the following optimization problem. The parameterization of the cluster dynamics reduces its state space to the double integrator:

The objective function, denoted as *J*, incorporates free final time and a weighted cluster energy term:

where *L*(*x*, *u*) = 1 + ^{2}*F*^{2}, and*F*:

Subject to the boundary conditions:

The state-dependent control constraints can then be defined as

where

_{max} (*s*) and _{min} (*s*).

And the state inequality constraints:

Computing *u**(*t*), the optimal acceleration along the path gives us access to the optimal trajectory of the full dynamic system, *c**(*t*) . The structure of the optimal control solution of this problem follows from the first-order necessary optimality conditions for the case of state-dependent control constraints and free final time [60]. Due to the reformulation in *s*, the time-energy optimal control problem is convex; therefore it can be concluded that any local optimum of the problem is also globally optimal.

### 3.2. Structure of the optimal control solution

The optimal control (*u*^{*}) and states

For all *t ϵ* [*t*_{0}, *t*_{f}] , where the Hamiltonian and costates are given by

The subscript ‘*s*’ denotes partial derivatives with respect to *s* = *x*_{1}. The state-dependent control constraints,

Solving *H*_{u} = 0 for *u* yields the unconstrained optimal control signal:

This equation is valid if the control constraints are inactive. If they are activated, the optimal control switches to the bounds *u*_{max}(*t*) or *u*_{min}(*t*):

### 3.3. Numerical solution

The control signal *u* = *x*_{2} is found by solving a fourth-order two-point boundary value problem given by Eqs. (17), (28), and (29):

If we assume initial unconstrained control, substitute initial condition *x*_{2}(0) = 0 into (26), and apply conditions (22) and (23), we can solve for the initial value of one of the costates:

We now have initial conditions for all necessary variables with the exception of *λ*_{1}(0). The initial condition for *λ*_{1} must therefore be iterated, or guessed at until the correct final conditions are realized for *x*_{1} and *x*_{2}. This numerical problem can be solved in a computationally inexpensive manner.

## 4. Results and analysis

The numerical methods presented in the previous sections were implemented to obtain time-energy optimal trajectories for the cluster paths presented in this section. Subsequently, a cluster space kinematic controller [18] was used to simulate the tracking of desired cluster paths and motions in the following examples for three-robot cluster rotate and translate as well as rotate and resize maneuvers.

### 4.1. Rotate and translate simulation

This cluster path is achieved by commanding translation from (*x*_{c},*y*_{c}) = (0,0) to (*x*_{c},*y*_{c}) = (10,10) and simultaneous formation rotation from *θ*_{c} = 0 to *π*, with initial (*c*_{init}) and path-parameterized cluster state vector (*f*(*s*)):

The following are depictions of the cluster parameterized phase plane and path velocity vs. time for multiple values of

Upon inspection, it is apparent that the generated plots conform to intuition regarding the expected shape of an energy optimal velocity profile. Given that generally steep acceleration results in excessive energy expenditure one expects that a mobile robot would increase its speed gradually (nonlinearly) up to a peak, after which it should identically decelerate. The velocity profile should therefore be parabolic, symmetric, and continuous, unless state constraints were violated, producing a plateau (trapezoidal) effect with discontinuities at the switching point. In [61], velocity profiles that match this description were generated while minimizing total energy drawn from the batteries of a particular mobile robot. Their results, given different motor characteristics, ranged from the widely used trapezoidal profile to the parabolic profile observed in the simulation.

Comparison of the optimal velocity profiles for different

The trajectory obtained, using *ε* = .5, was executed using the simulation referenced above, to yield the snapshots in Figure 4 of the cluster in 5-s intervals.

Notice that the paths of robots 1 and 2 (the blue and red dots, respectively) and robots 2 and 3 (the red and green dots, respectively) intersect during the course of the maneuver, indicating a need for timing constraints to avoid collision. There are methods that define multirobot coordination in this manner, referenced earlier [19, 39], that generate robot velocity profiles from given paths such that robots avoid collision. Cluster space requires no such direct provision, the timing of individual robot trajectories both to avoid inter-robot collision and maintain formation is handled implicitly. This is evident in Figure 5, which depicts the distinct optimal velocity profiles of each robot obtained by applying the cluster inverse kinematic transformations to the optimal parameterized velocity. They are of equal duration yet their dissimilar contours and therefore areas indicate unequal distances traveled. Additionally, due to the fact that optimizing in cluster space relieves us from imposing an explicit formation constraint in the cost function as in [25, 26], the mathematical treatment can be dedicated exclusively to optimizing time and energy.

### 4.2. Rotate and resize simulation

Another example of a cluster maneuver, rotate and resize, is obtained by commanding rotation from *θ*_{c} = 0 to *π* and a simultaneous resize from *p*, *q* = 10 to *p*, *q* = 20, with initial (*c*_{init}) and path-parameterized cluster state vector (*f*(*s*)):

The following are depictions of the cluster parameterized phase plane and path velocity vs. time for multiple values of

As with a rotate and translate case, increasing

The trajectory simulation for *ε* = . 5 was executed to yield the following snapshots of the cluster in 5-s intervals (Figure 8):

The individual robot velocity profiles for this case (not pictured here) are identical due to the fact that their paths do not intersect and the chosen cluster geometry is an equilateral triangle, implying equidistant paths. Therefore, in contrast to Figure 5, this maneuver does not require a provision for timing to avoid inter-agent collision. A virtue of our technique is that optimizing trajectories in cluster space removes the operator from this consideration and handles both scenarios without intervention.

### 4.3. Time-energy tradeoff

As previously acknowledged, adjusting the weight of the energy term in the objective function results in varying duration times and energy expenditures. In order to thoroughly investigate the trade-off range, optimal solutions were obtained for a number of*T*) and the cluster energy as represented in the objective function, *T*, energy expenditure is reduced by 22.5% (point (a) in Figure 9). As*T* is increased further by 10%, energy is reduced by 19.8% (point (b) in Figure 9). The slope of this curve will of course change with cluster path and trajectory; however, we expect to always observe diminishing reductions in energy expenditure with increments of *T*, as we have with this case. In general, for a given cluster and trajectory, this plot can be generated and used as a tool to inform the appropriate choice of

## 5. Conclusion

This chapter presented a technique to generate a time-energy optimal trajectory for a cluster of mobile robots with predetermined paths. The method is uniquely applied in a parameterized formation state space and incorporates the nonlinear dynamics of the cluster and actuator constraints to enable intuitive optimal trajectory planning for time varying formations. Simulation results verified the generated trajectories and demonstrated the advantages of using a time-energy tunable objective function. The ability to choose more energy-efficient trajectories reduces the strain on actuator components and energy reserves, thereby promoting longevity in both. The tool in Figure 9 also demonstrates that the operator has access to a substantial path-specific time/energy range, within which one must choose a mission-appropriate combination.

We plan to incorporate several extensions to this work in the future. First, we will integrate cluster space obstacle avoidance [62] into the optimal trajectory-tracking controller. In addition, we will develop cluster space velocity and acceleration bounds to include the characterization of a velocity limit curve as well as a phase plane description of the admissible range. With these bounds, which are a function of the cluster states and are therefore variable in the choice of path, one could derive the time optimal cluster trajectory, approximated with small