**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 inde‐ pendent 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 wellbehaved, 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**

trajectories for each robot in a multiagent group from formation requirements. The techniques

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

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

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 inde‐ pendent 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

Cluster space represents a natural point of view for the operator suited for specifying wellbehaved, 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

states, and be relieved from the task of specifying individual robot motions.

proposed in this chapter address this shortcoming.

technique are explained in Section 4.

144 Recent Advances in Robotic Systems

analysis and results are presented.

**2.1. Cluster space specification of multirobot systems**

**2. Cluster space**

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 (*xc*, *yc*, *θ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**.

**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) + \mathcal{g}\left(c\right) \tag{1}$$

where if *n* is the number of robots in the cluster and *m* the number of degrees of freedom of each robot: *F ε R<sup>n</sup>* is an *nm* × 1 vector of cluster generalized forces and torques belonging to a set defined {*F* | *Fimin* ≤ *Fi* ≤ *Fimax*, *i* = 1 : *nm*}; *Λ*(*c*) is an *nm* × *nm* symmetric, positive definite, cluster mass or inertia matrix of the quadratic form; and its first and second derivatives are *nm* × 1 vectors of cluster space state positions, velocities, and accelerations, respectively; 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\begin{pmatrix} c \\ \end{pmatrix} \ddot{c} + \dot{\mathfrak{c}}^T B \dot{c}. \tag{2}$$

This version is obtained through the use of Christoffel symbols (*Γ jk <sup>i</sup>* (*c*)), 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(\mathcal{c}\right)...\Gamma^{\ast m}\left(\mathcal{c}\right)\right] \tag{3}$$

And *Γ jk <sup>i</sup>* (*c*) is an *nm* x *nm* symmetric matrix for *i*, *j*, *k* = 1 to *nm*:

$$\Gamma^{\prime}\_{\mu}\left(c\right) = \frac{1}{2} \ast \left(\frac{\partial \Lambda\_{\boldsymbol{\eta}}\left(q\right)}{\partial c\_{k}} + \frac{\partial \Lambda\_{\boldsymbol{\omega}}\left(q\right)}{\partial c\_{j}} + \frac{\partial \Lambda\_{\boldsymbol{\omega}}\left(q\right)}{\partial c\_{i}}\right) \tag{4}$$

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**

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

> *F cc cc gc* =L + + , ( )&& & m

*nm* × 1 vectors of cluster space state positions, velocities, and accelerations, respectively; is an *nm* × 1 vector of cluster space Centrifugal and Coriolis forces; *g*(c) is an *nm* × 1 vector of

where if *n* is the number of robots in the cluster and *m* the number of degrees of freedom of each robot: *F ε R<sup>n</sup>* is an *nm* × 1 vector of cluster generalized forces and torques belonging to a set defined {*F* | *Fimin* ≤ *Fi* ≤ *Fimax*, *i* = 1 : *nm*}; *Λ*(*c*) is an *nm* × *nm* symmetric, positive definite, cluster mass or inertia matrix of the quadratic form; and its first and second derivatives are

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

=L + . ( ) & &&&

the cluster mass matrix *Λ*(*c*), where B is an *nm* × *nm* × *nm* vector of Christoffel symbols:

( ) æ ö ¶ ¶ ( ) ¶ ( ) ( ) = ++ ç ÷

1 Λ Λ Λ

*ij ik kj i*

¶¶¶ è ø

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

*kji*

*q q q*

This version is obtained through the use of Christoffel symbols (*Γ jk*

is an *nm* x *nm* symmetric matrix for *i*, *j*, *k* = 1 to *nm*:

whose gravity force is canceled out by the force normal to the surface.

Γ \* 2

*c*

*jk*

( ) ( ) (1)

*<sup>T</sup> F c c c Bc* (2)

= G ¼G é ù ( ) ( ) ë û <sup>1</sup> *nm Bc c* (3)

*ccc* (4)

*<sup>i</sup>* (*c*)), which are derived from

[54]. The resulting cluster space dynamic equation follows:

gravity forces in cluster space.

146 Recent Advances in Robotic Systems

product terms [49]:

And *Γ jk <sup>i</sup>* (*c*) 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

$$\mathbf{c} = f\begin{pmatrix} s \\ \end{pmatrix}; \; \dot{\mathbf{c}} = f\_s \dot{s}; \; \ddot{\mathbf{c}} = f\_{ss}\dot{s}^2 + f\_s \ddot{s} \tag{5}$$

where *fs* is the unit vector tangent to the path, *fss* is the curvature, is the speed along the path, and is the acceleration. Substituting these expressions for *c*, , and in Eq. (2), we obtain

$$F = \Lambda\left(\mathbf{s}\right)\left(f\_{ss}\dot{\mathbf{s}}^2 + f\_s\ddot{\mathbf{s}}\right) + \left(f\_s\dot{\mathbf{s}}\right)^T \mathcal{B}\left(f\_s\dot{\mathbf{s}}\right) \tag{6}$$

$$F = \Lambda\begin{pmatrix} \mathbf{s} \\ \end{pmatrix} f\_{ss}\dot{\mathbf{s}}^2 + \Lambda\begin{pmatrix} \mathbf{s} \\ \end{pmatrix} f\_s\ddot{\mathbf{s}} + \begin{pmatrix} \dot{\mathbf{s}}^T f\_s^T \\ \end{pmatrix} \mathbf{B} \begin{pmatrix} f\_s \dot{\mathbf{s}} \\ \end{pmatrix}. \tag{7}$$

If we let *m*(*s*) = Λ(*s*)*fs* and *b*(*s*)=Λ(*s*) *f ss* + *f <sup>s</sup> <sup>T</sup> <sup>B</sup> <sup>f</sup> <sup>s</sup>* :

$$F = \mathbf{m}\left(s\right)\ddot{s} + b\left(s\right)\dot{s}^2. \tag{8}$$

Using Eq. (8) we have rewritten the cluster dynamics in terms of the velocity and acceleration 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 . This mapping is unique; therefore, we can use as the control input, effectively reducing the 2*nm*-dimensional state space to two independent states, and [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, {*Fi* min ≤ *Fi* ≤ *Fi* 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\_{\text{imin}} \le \mathbf{m}\_{\text{i}}\ddot{\mathbf{s}} + \mathbf{b}\_{\text{i}}\dot{\mathbf{s}}^2 \le F\_{\text{imax}}\pi\\where \,\mathbf{i} = \mathbf{1}:\,\mathbf{mm} \tag{9}$$

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

$$
\dot{s} \le \dot{s}\_{\text{max}} \text{(s)}\tag{10}
$$

$$
\ddot{s}\_{\text{min}}\left(\dot{s}\_{\text{\textc}}\mathbf{s}\right) \le \ddot{s} \le \dddot{s}\_{\text{max}}\left(\dot{s}\_{\text{\textc}}\mathbf{s}\right) \tag{11}
$$

where assuming *mi* ≠ 0 and *mj bi* − *mi bj* ≠ 0:

$$\dot{s}\_{\text{max}}^2(s) = \min\_{\dot{\boldsymbol{\eta}}} \left\{ \max\_{\boldsymbol{\bar{v}}\_{\boldsymbol{\bar{v}}}, \boldsymbol{F}\_{\boldsymbol{\bar{v}}}} \left( \frac{m\_{\boldsymbol{\bar{v}}} \boldsymbol{F}\_{\boldsymbol{\bar{v}}} - m\_{\boldsymbol{\bar{v}}} \boldsymbol{F}\_{\boldsymbol{\bar{v}}}}{m\_{\boldsymbol{\bar{v}}} \boldsymbol{b}\_{\boldsymbol{\bar{v}}} - m\_{\boldsymbol{\bar{v}}} \boldsymbol{b}\_{\boldsymbol{\bar{v}}}} \right) \right\} \quad \text{for } \boldsymbol{\bar{v}}\_{\boldsymbol{\bar{v}}} \, \boldsymbol{j} = \mathbf{1}:nm \tag{12}$$

$$\ddot{s}\_{\text{max}}\left(\dot{s},s\right) = \min\_{l} \left\{ \max\_{F\_i} \left( \frac{F\_l - b\_l \dot{s}\_2}{m\_l} \right) \right\}, \text{ for } \dot{\mathbf{i}} = \mathbf{l} : nm \tag{13}$$

$$\ddot{s}\_{\min}\left(\dot{s},s\right) = \max\_{l} \left\{ \min\_{F\_{l}} \left( \frac{F\_{l} - b\_{l}\dot{s}\_{2}}{m\_{l}} \right) \right\}, \text{ for } \dot{\mathbf{i}} = \mathbf{l}:nm \tag{14}$$

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:

Time-Energy Optimal Cluster Space Motion Planning for Mobile Robot Formations http://dx.doi.org/10.5772/64615 149

$$\dot{\vec{\mathbf{x}}} = f\left(\vec{\mathbf{x}}, \boldsymbol{\mu}\right) = \left[\mathbf{x}\_2, \boldsymbol{\mu}\right]^T = \left[\dot{\mathbf{s}}\_\prime \ddot{\mathbf{s}}\right]^T \tag{15}$$

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

$$\min\_{u} \left( f \right) = \bigcup\_{0}^{t\_f} L\left( \mathbf{x}, u \right) dt \tag{16}$$

where *L*(*x*, *u*) = 1 + 2 *F*2 , and ranges from 0 to 1. Or, in cluster path space coordinates, substituting (8) for *F*:

$$L\left(\mathbf{x},\boldsymbol{\mu}\right) = 1 + \boldsymbol{\varepsilon}^{2} \left(\boldsymbol{b}^{\top}\boldsymbol{b}\,\mathbf{x}\_{2}^{4} + 2\boldsymbol{m}^{\top}\boldsymbol{b}\,\mathbf{x}\_{2}^{2}\boldsymbol{\mu} + \boldsymbol{m}^{\top}\boldsymbol{m}\,\boldsymbol{u}^{2}\right). \tag{17}$$

Subject to the boundary conditions:

$$\mathbf{x}\_1(\mathbf{0}) = \mathbf{0}, \mathbf{x}\_1(t\_f) = \mathbf{x}\_f, \mathbf{x}\_2(\mathbf{0}) = \mathbf{0}, \mathbf{x}\_2(t\_f) = \mathbf{0}. \tag{18}$$

The state-dependent control constraints can then be defined as

$$\mathbf{g}\_1\left(\vec{\boldsymbol{x}},\boldsymbol{\mu}\right) = \boldsymbol{\mu} - \boldsymbol{\mu}\_{\text{max}}\left(\vec{\boldsymbol{x}}\right) \le 0 \tag{19}$$

$$\mathbf{g}\_2\left(\vec{\boldsymbol{x}},\boldsymbol{\mu}\right) = \boldsymbol{\mu}\_{\text{min}}\left(\vec{\boldsymbol{x}}\right) - \boldsymbol{\mu} \le 0\tag{20}$$

where

£ +£ = && &<sup>2</sup>

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

where assuming *mi*

148 Recent Advances in Robotic Systems

& 2 ≠ 0 and *mj*

*bi* − *mi bj* ≠ 0:

( ) ì ü

*i F*

( ) <sup>2</sup>

*i F*

would be defeated if robot acceleration and velocity bounds were reached.

**3. Cluster space time-energy optimal control**

**3.1. Problem formulation**

*ij F F*

ï ï æ ö - <sup>=</sup> í ý ç ÷ <sup>=</sup> - ï ï î þ è ø

*ji ij*

*mF mF*

*ji ij*

*i i*

*i i*

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

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:

*i*

*i*

*s s i j nm mb mb* (12)

& && & (13)

&&& & (14)

max min max , , for , 1 : *i j*

( ) <sup>2</sup> max , min max , for <sup>i</sup> 1: *<sup>i</sup>*

min , max min , for i 1: *<sup>i</sup>*

*F bs <sup>s</sup> <sup>s</sup> ns <sup>m</sup> <sup>m</sup>* ì ü ï ï æ ö - <sup>=</sup> í ý ç ÷ <sup>=</sup> ï ï î þ è ø

*F bs <sup>s</sup> <sup>s</sup> ns <sup>m</sup> <sup>m</sup>* ì ü ï ï æ ö - <sup>=</sup> í ý ç ÷ <sup>=</sup> ï ï î þ è ø

min m max , 1 : *i ii i F s b s F where i nm* (9)

*ss s* & & £ max ( ) (10)

&& && &&& *s ss s s ss* min ( , , ) £ £ max (& ) (11)

*u*max(*x*)= max ( , *s*) and *u*min(*x*)= min ( , *s*).

And the state inequality constraints:

$$h\left(\mathbf{x}\right) = \mathbf{x}\_2 - \dot{\mathbf{s}}\_{\text{max}}\left(\mathbf{x}\_1\right) \le 0 \tag{21}$$

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 refor‐ mulation 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 must satisfy the following conditions [59]:

$$H\_{\mu} \left( \mathbf{x}^\*, \mathbb{X}, \boldsymbol{\mu}^\*, t \right) = 0 \tag{22}$$

$$H\left(\mathbf{x}^\*, \mathbb{X}, \boldsymbol{\mu}^\*, \mathbf{t}\right) = \mathbf{0} \tag{23}$$

For all *t*  [*t*0, *tf* ] , where the Hamiltonian and costates are given by

$$H\left(\vec{\boldsymbol{x}},\vec{\boldsymbol{\lambda}},\boldsymbol{\mu}\right) = \mathcal{L}\left(\vec{\boldsymbol{x}},\boldsymbol{\mu}\right) + \vec{\boldsymbol{\lambda}}^{\top}f\left(\vec{\boldsymbol{x}},\boldsymbol{\mu}\right) \tag{24}$$

$$\dot{\overrightarrow{\mathcal{A}}} = -L\_{\text{x}} - \left(f\_{\text{x}}\right)^{T}\overrightarrow{\mathcal{A}}.\tag{25}$$

Expanded using (15) and (17):

$$\mathbf{H} = \mathbf{I} + \left( \mathbf{z}^2 \left( \mathbf{b}^T \mathbf{b} \,\mathbf{x}\_2^4 + 2\boldsymbol{m}^T \boldsymbol{b} \,\mathbf{x}\_2^2 \boldsymbol{\mu} + \boldsymbol{m}^T \boldsymbol{m} \,\boldsymbol{u}^2 \right) + \boldsymbol{\lambda}\_1 \mathbf{x}\_2 + \boldsymbol{\lambda}\_2 \boldsymbol{\mu} + \mu\_1 \mathbf{g}\_1 + \mu\_2 \mathbf{g}\_2 \right) = \mathbf{0} \tag{26}$$

$$H\_{\mu} = 2\varepsilon^{2} \left( m^{T} b \,\mathrm{x}\_{2}^{2} + m^{T} m \,\mathrm{u} \right) + \lambda\_{2} \tag{27}$$

$$\dot{\lambda}\_1 = -2\varepsilon^2 \left( m^T m\_s \mu^2 + b^T b\_s \mathbf{x}\_2^4 \right) \tag{28}$$

++ - ( ) m <sup>2</sup> <sup>2</sup> ) *TT T mb mb xu g ss s*

$$\dot{\mathcal{A}}\_2 = -4\varepsilon^2 \left( \mathbf{b}^T \mathbf{b} \,\mathbf{x}\_2^3 + m^T \mathbf{b} \,\mathbf{x}\_2 \mu \right) - \mathcal{A}\_1 - \mathbf{g}\_{x\_2}^T \mu. \tag{29}$$

The subscript '*s*' denotes partial derivatives with respect to *s* = *x*1. The state-dependent con‐ trol constraints, , are incorporated into the Hamiltonian using the multipliers . 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 *Hu* = 0 for *u* yields the unconstrained optimal control signal:

$$\mu\_{\rm unc} \left( t \right) = - \left( \frac{2 \varepsilon^2 m^T b x\_2^2 + \lambda\_2}{2 \varepsilon^2 m^T m\_s} \right) \tag{30}$$

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

$$\mu^u\left(t\right) = \begin{cases} \mu\_{\text{max}}\left(t\right) \text{if } \mu\_{\text{unc}} > \mu\_{\text{max}}\\ \mu\_{\text{int}}\left(t\right) \text{if } \mu\_{\text{max}} \ge \mu\_{\text{unc}} \ge \mu\_{\text{min}}\\ \mu\_{\text{max}}\left(t\right) \text{if } \mu\_{\text{unc}} > \mu\_{\text{max}} \end{cases} \tag{31}$$

#### **3.3. Numerical solution**

mulation in *s*, the time-energy optimal control problem is convex; therefore it can be concluded

) and states must satisfy the following conditions [59]:

 l

( ) . *<sup>T</sup>*

( ) 24 2 2 2 2 12 2 11 2 2 1 ( 2 ) <sup>0</sup> *TTT H b bx m bx u m mu x u g g* =+ + + + + + + =

> ( ) 2 2 2 2 <sup>2</sup>*T T H m bx m mu <sup>u</sup>* = ++

> > ++ - ( )

<sup>2</sup> 2 21 4 . *TT T*

& =- + - - ( )

224 1 2 2 ( *T T*

<sup>2</sup> ) *TT T mb mb xu g ss s*

The subscript '*s*' denotes partial derivatives with respect to *s* = *x*1. The state-dependent con‐ trol constraints, , are incorporated into the Hamiltonian using the multipliers

 l

l

 lm

 l

m<sup>2</sup>

> m2

*<sup>x</sup> b bx m bx u g* (29)

l

. They are positive if the associated constraint (dictated by (22)) is active and zero

<sup>=</sup> - + *m mu b bx s s* & (28)

) = + ( ) ( ) r r rrr

=- - *L f x x*

,, , 0 *H x ut <sup>u</sup>* (22)

) = \* \* *Hx u t* ,, , 0 (23)

r& uur (25)

 m

(26)

(27)

,, , , *<sup>T</sup> H x u L xu f xu* (24)

that any local optimum of the problem is also globally optimal.

( l) = \* \*

( l

( l

l

e

 e

2 3

l

 e

l

] , where the Hamiltonian and costates are given by

**3.2. Structure of the optimal control solution**

The optimal control (*u*\*

150 Recent Advances in Robotic Systems

For all *t*  [*t*0, *tf*

Expanded using (15) and (17):

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

$$
\dot{\mathbf{x}}\_1 = \mathbf{x}\_2 \tag{32}
$$

$$\dot{\mathbf{x}}\_2 = \frac{\left(2\boldsymbol{\varepsilon}^2 \boldsymbol{m}^T \boldsymbol{b} \,\mathbf{x}\_2^2 + \boldsymbol{\lambda}\_2\right)}{\left(2\boldsymbol{\varepsilon}^2 \boldsymbol{m}^T \boldsymbol{m}\_\*\right)}\tag{33}$$

$$\dot{\lambda}\_1 = -2\boldsymbol{\varepsilon}^2 \left( m^T m\_s \boldsymbol{\mu}^2 + \boldsymbol{b}^T \boldsymbol{b}\_s \mathbf{x}\_2^4 + \left( m\_s^T \boldsymbol{b} + m^T \boldsymbol{b}\_s \right) \mathbf{x}\_2^2 \boldsymbol{\mu} \right) \tag{34}$$

$$\dot{\mathcal{A}}\_2 = -4\varepsilon^2 \left( \mathbf{b}^T \mathbf{b}\_s \mathbf{x}\_2^3 + m^T \mathbf{b} \mathbf{x}\_2 \mu \right) - \mathcal{A}\_{\text{1}} \tag{35}$$

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:

$$
\lambda\_2 \left( 0 \right) = -\frac{1 + s^2 m^\top \left( 0 \right) m \left( 0 \right) \mu\_{\text{max}}^2 \left( 0 \right)}{\mu\_{\text{max}} \left( 0 \right)} \tag{36}
$$

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.
