Time-Energy Optimal Cluster Space Motion Planning for Mobile Robot Formations

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.

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 (x1, y1, x2, y2, x3, y3) 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 (φ₁, φ_2, φ₃). 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.

Figure 1.

State space variables for a three-robot cluster.

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:

$F=\Lambda \left(c\right)\ddot{c}+\mu \left(c,\dot{c}\right)+g\left(c\right)$E1

where if n is the number of robots in the cluster and m the number of degrees of freedom of each robot: F ε Rⁿ 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; $\vec{\mathrm{c}}$

and its first and second derivatives $\left(\dot{\mathrm{c}},\ddot{\mathrm{c}}\right)$

are nm × 1 vectors of cluster space state positions, velocities, and accelerations, respectively; $\mu \left(c,\dot{c}\right)$

is an 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]:

$F=\Lambda \left(c\right)\ddot{c}+{\dot{c}}^{T}B\dot{c}\text{.}$E2

This version is obtained through the use of Christoffel symbols (${\Gamma }_{jk}^i\left(c\right)$), which are derived from the cluster mass matrix Λ(c), where B is an nm × nm × nm vector of Christoffel symbols:

$B=\left[{\Gamma }^1\left(c\right)\dots {\Gamma }^{\mathit{nm}}\left(c\right)\right]$E3

And ^{${\Gamma }_{jk}^i\left(c\right)$} is an nm x nm symmetric matrix for i, j, k = 1 to nm:

${\mathrm{\Gamma }}_{jk}^i\left(c\right)=\frac{1}{2}*\left(\frac{\partial {\mathrm{\Lambda }}_{ij}\left(q\right)}{\partial c_k}+\frac{\partial {\mathrm{\Lambda }}_{ik}\left(q\right)}{\partial c_j}+\frac{\partial {\mathrm{\Lambda }}_{kj}\left(q\right)}{\partial c_i}\right)$E4

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

$\mathrm{c}=f\left(s\right);\dot{\mathrm{c}}=f_s\dot{s};\dot{\mathrm{c}}=f_{ss}{\dot{s}}^2+f_s\ddot{s}$E5

where f_s is the unit vector tangent to the path, f_ss is the curvature, $\dot{s}$

is the speed along the path, and $\ddot{s}$

is the acceleration. Substituting these expressions for c, $\dot{c}$

, and $\ddot{\mathrm{c}}$

in Eq. (2), we obtain

$F=\mathrm{\Lambda }\left(s\right)\left(f_{ss}{\dot{s}}^2+f_s\ddot{s}\right)+{\left(f_s\dot{s}\right)}^{T}B\left(f_s\dot{s}\right)$E6

$F=\mathrm{\Lambda }\left(s\right)f_{ss}{\dot{s}}^2+\mathrm{\Lambda }\left(s\right)f_s\ddot{s}+\left({\dot{s}}^T{f}_s^T\right)B\left(f_s\dot{s}\right)\text{.}$E7

If we let m(s) = Λ(s)f_s and $b\left(s\right)=\mathrm{\Lambda }\left(s\right)f_{ss}+f_s^{T}B{f}_s$ :

$F=\mathrm{m}\left(s\right)\ddot{s}+b\left(s\right){\dot{s}}^2\text{.}$E8

Using Eq. (8) we have rewritten the cluster dynamics in terms of the velocity $\dot{s}$

and acceleration $\ddot{s}$

along the cluster state paths where 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 $\ddot{s}$

. This mapping is unique; therefore, we can use $\dot{s}$

as the control input, effectively reducing the 2nm-dimensional state space to two independent states, $\dot{s}$

and $\ddot{s}$

[59]. (Note: The coefficient matrices 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:

$F_{imin}\le {\mathrm{m}}_i\ddot{s}+b_i{\dot{s}}^2\le F_{imax},\mathit{where}i=1:\mathit{nm}$E9

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

$\dot{s}\le {\dot{s}}_{\text{max}}\left(s\right)$E10

${\ddot{s}}_{\text{min}}\left(\dot{s},s\right)\le \ddot{s}\le {\ddot{s}}_{\text{max}}\left(\dot{s},s\right)$E11

where assuming m_i ≠ 0 and m_jb_i − m_ib_j ≠ 0:

${\dot{s}}_{\text{max}}^2\left(s\right)={min}_{ij}\left\{{max}_{F_i,F_j}\left(\frac{m_j{F}_i-m_i{F}_j}{m_j{b}_i-m_i{b}_j}\right)\right\},\mathrm{f}\mathrm{o}\mathrm{r}i,j=1:\mathit{nm}$E12

${\ddot{s}}_{\text{max}}\left(\dot{s},s\right)={min}_i\left\{{max}_{F_i}\left(\frac{F_i-b_i{\dot{s}}_2}{m_i}\right)\right\},\mathrm{f}\mathrm{o}\mathrm{r}\mathrm{i}=1:\mathit{nm}$E13

${\ddot{s}}_{\text{min}}\left(\dot{s},s\right)={max}_i\left\{{min}_{F_i}\left(\frac{F_i-b_i{\dot{s}}_2}{m_i}\right)\right\},\mathrm{f}\mathrm{o}\mathrm{r}\mathrm{i}=1:\mathit{nm}$E14

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:

$\dot{\stackrel{⃗}{\mathrm{x}}}=f\left(\vec{x},u\right)={\left[x_2,u\right]}^T={\left[\dot{s},\ddot{s}\right]}^T$E15

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

${min}_u\left(J\right)={\int }_0^{t_f}L\left(x,u\right)dt$E16

where L(x, u) = 1 + 

²F², and

ranges from 0 to 1. Or, in cluster path space coordinates, substituting (8) for F:

$L\left(x,u\right)=1+{\epsilon }^2\left(b^{T}b{x}_2^4+2m^{T}b{x}_2^{2}u+m^{T}m{u}^2\right)\text{.}$E17

Subject to the boundary conditions:

$x_1\left(0\right)=0,x_1\left(t_f\right)=x_f,x_2\left(0\right)=0,x_2\left(t_f\right)=0$E18

The state-dependent control constraints can then be defined as

${\mathrm{g}}_1\left(\vec{x},u\right)=u-u_{\text{max}}\left(\vec{x}\right)\le 0$E19

${\mathrm{g}}_2\left(\vec{x},u\right)=u_{\text{min}}\left(\vec{x}\right)-u\le 0$E20

where

$$u_{\text{max}}\left(x\right)=$$$\ddot{s}$

_max ($\dot{s}$

, s) and $$u_{\text{min}}\left(x\right)=$$$\ddot{s}$

_min ($\dot{s}$

, s).

And the state inequality constraints:

$h\left(x\right)=x_2-{\dot{s}}_{\text{max}}\left(x_1\right)\le 0$E21

Computing u*(t), the optimal acceleration along the path gives us access to the optimal trajectory of the full dynamic system,

, and 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 $\left({\vec{x}}^{*}\right)$

must satisfy the following conditions [59]:

$H_u\left(x^{*},\lambda ,u^{*},t\right)=0$E22

$H\left(x^{*},\lambda ,u^{*},t\right)=0$E23

For all t ϵ [t₀, t_f] , where the Hamiltonian and costates are given by

$H\left(\vec{x},\vec{\lambda },u\right)=L\left(\vec{x},u\right)+{\vec{\lambda }}^{T}f\left(\vec{x},u\right)$E24

$\dot{\stackrel{⃗}{\lambda }}=-L_x-{\left(f_x\right)}^T\overrightarrow{\lambda .}$E25

Expanded using (15) and (17):

$H=1+({\epsilon }^2\left(b^{T}b{x}_2^4+2m^{T}b{x}_2^{2}u+m^{T}m{u}^2)+{\lambda }_1{x}_2+{\lambda }_{2}u+{\mu }_1{g}_1+{\mu }_2{g}_2\right)=0$E26

$H_u=2{\epsilon }^2\left(m^{T}b{x}_2^2+m^{T}mu\right)+{\lambda }_2$E27

${\dot{\lambda }}_1=-2{\epsilon }^2(m^T{m}_s{u}^2+b^T{b}_s{x}_2^4$E28

$+\left(m_s^{T}b+m^T{b}_s\right)x_2^{2}u)-g_s^T\mu$E290

${\dot{\lambda }}_2=-4{\epsilon }^2\left(b^{T}b{x}_2^3+m^{T}b{x}_{2}u\right)-{\lambda }_1-g_{x_2}^T\mu \text{.}$E29

The subscript ‘s’ denotes partial derivatives with respect to s = x₁. The state-dependent control constraints, $g\left(\vec{x},u\right)={\left[g_1\left(\vec{x},u\right),g_2\left(\vec{x},u\right)\right]}^T$

, are incorporated into the Hamiltonian using the multipliers $\vec{\mu }={\left[{\mu }_1,{\mu }_2\right]}^{\mathrm{T}}$

. They are positive if the associated constraint (dictated by (22)) is active and zero otherwise, ensuring that the cost function can only be reduced by violating the constraints [59].

Solving H_u = 0 for u yields the unconstrained optimal control signal:

$u_{\mathrm{u}\mathrm{n}\mathrm{c}}\left(t\right)=-\left(\frac{2{\epsilon }^2{m}^{T}b{x}_2^2+{\lambda }_2}{2{\epsilon }^2{m}^T{m}_s}\right)$E30

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):

$u^{\divideontimes }\left(t\right)=\left\{\begin{array}{c}\begin{array}{c}{u}_{\text{max}}\left(t\right)\mathit{if}{u}_{\mathrm{u}\mathrm{n}\mathrm{c}}>u_{\text{max}}\\ u_{\mathrm{i}\mathrm{n}\mathrm{t}}\left(t\right)\mathit{if}{u}_{\text{max}}\ge u_{\mathrm{u}\mathrm{n}\mathrm{c}}\ge u_{\text{min}}\end{array}\\ u_{\text{max}}\left(t\right)\mathit{if}{u}_{\mathrm{u}\mathrm{n}\mathrm{c}}>u_{\text{max}}\end{array}\right.$E31

3.3. Numerical solution

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

${\dot{x}}_1=x_2$E32

${\dot{x}}_2=\frac{\left(2{\epsilon }^2{m}^{T}b{x}_2^2+{\lambda }_2\right)}{\left(2{\epsilon }^2{m}^T{m}_s\right)}$E33

${\dot{\lambda }}_1=-2{\epsilon }^2\left(m^T{m}_s{u}^2+b^T{b}_s{x}_2^4+\left(m_s^{T}b+m^T{b}_s\right)x_2^{2}u\right)$E34

${\dot{\lambda }}_2=-4{\epsilon }^2\left(b^T{b}_s{x}_2^3+m^{T}b{x}_{2}u\right)-{\lambda }_{1\text{.}}$E35

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

${\lambda }_2\left(0\right)=-\frac{1+{\epsilon }^2{m}^T\left(0\right)m\left(0\right)u_{\text{max}}^2\left(0\right)}{u_{\text{max}}\left(0\right)}$E36

We now have initial conditions for all necessary variables with the exception of λ₁(0). The initial condition for λ₁ must therefore be iterated, or guessed at until the correct final conditions are realized for x₁ and x₂. 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)):

$\begin{array}{cc}{c}_{\mathrm{init}}& ={\left[x_c,y_c,\theta ,{\varphi }_1,{\varphi }_2,{\varphi }_3,p,q,\beta \right]}^T\\ & ={\left[0,0,0,0,0,0,10,10,\pi /3\right]}^T\end{array}$E37

$f\left(s\right)={\left[10s,10s,\pi s,0,0,0,10,10,\pi /3\right]}^T$E38

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

(Figure 2):

Figure 2.

Cluster parameterized phase plane for multiple .

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.

Figure 3.

Velocity along the cluster parameterized path for multiple .

Comparison of the optimal velocity profiles for different

in Figure 3 confirms the anticipated result that increasing values of

, and therefore energy contribution to the cost function, results in velocity profiles of reduced average magnitude and longer duration. In Figure 2, phase planes generated from successively smaller

increase in magnitude as they approach the time optimal trajectory. The time-optimal boundary, though not derived as part of this study, can be determined by calculating the maximum (minimum) parameterized cluster velocity and acceleration achievable at each possible cluster configuration along the given path. However, for a small enough choice of

, the generated velocity profile will sufficiently approximate this limit.

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.

Figure 4.

Snapshots of cluster rotate and translate maneuver in 5-s intervals.

Figure 5.

Individual robot velocity profiles.

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)):

$\begin{array}{cc}{c}_{\mathrm{init}}& ={\left[x_c,y_c,\theta ,{\varphi }_1,{\varphi }_2,{\varphi }_3,p,q,\beta \right]}^T\\ & ={\left[5,5,0,0,0,0,10,10,\pi /3\right]}^T\end{array}$E39

$f\left(s\right)={\left[5,5,\pi s,0,0,0,10s,10s,\pi /3\right]}^T\text{.}$E40

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

(Figure 6):

Figure 6.

Cluster parameterized phase plane for multiple .

Figure 7.

Velocity along the cluster parameterized path for multiple .

As with a rotate and translate case, increasing

lengthens the duration of the trajectory and flattens the shape of the optimal velocity profile (Figure 7), suggesting reduced energy expenditure. Additionally, the phase plane of Figure 6 appears to approach a time-optimal limit with decreasing

, as anticipated. However, comparison with Figure 2 of the general shape and relative location of the peak velocity reveals a discernable difference. The rotate and resize profile exhibits a skew toward the velocity axis resulting in a peak magnitude that occurs before the half-time trajectory point, while the rotate and translate profile is approximately symmetric about the half-time point. This effect can be explained by viewing the cluster as a virtual articulating mechanism possessing inertia. Because the cluster inertia grows as the path is followed, optimizing in cluster space produces a velocity profile that favors torque application early in the maneuver. It is also true then that the symmetric nature of the optimal velocity profile in a case such as a cluster rotate and translate often implies a constant or symmetric inertia throughout the trajectory.

Figure 8.

Snapshots of cluster rotate and resize maneuver in 5-s intervals.

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

values in the rotate and translate case. Figure 9 depicts the relationship between trajectory duration (T) and the cluster energy as represented in the objective function, ${\int }_0^{t_f}{F}^{2}dt$, for

ranging from .001 to .01. The results, normalized to

 = .001, show that for a 10% increase in T, energy expenditure is reduced by 22.5% (point (a) in Figure 9). As

gets larger, and 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

given mission requirements such as time to completion or energy limitations.

5. Conclusion

Figure 9.

Energy vs. trajectory duration.

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

in the simulations of the previous section. We will also attempt to address practical issues such as friction, gravity, and motor dynamics into the formulation in order to make the technique more applicable to a wide range of multirobot systems. Finally, we will experimentally verify and validate the technique with several existing multirobot experimental test beds, to include terrestrial rovers, surface and underwater marine vehicles, and aerial vehicles.