Steering a Swarm of Large-Scale Underactuated Mechanical Systems Using a Generalized Coordinates Transformation

Abstract

Steering large-scale particle or robot systems is challenging because of their high dimensionality. We use a centralized stochastic approach that allows for optimal control at the cost of a central element instead of a decentralized approach. Previous works are often restricted to the assumption of fully actuated robots. Here we propose an approach for underactuated robots that allows for energy-efficient control of the robot system. We consider a simple task of gathering the robots (minimizing positional variance) and steering them towards a goal point within a bounded area without obstacles. We make two main contributions. First, we present a generalized coordinate transformation for underactuated robots, whose physical properties should be considered. We choose Euler-Lagrange systems that describe a large class of robot systems. Second, we propose an optimal control mechanism with the prime objective of energy efficiency. We show the feasibility of our approach in robot simulations.

1. Introduction

The steer of large-scale multi-agent particle systems is challenging due to the high degree of freedom in such distributed systems of loosely coupled robots [1]. The published approaches on this subject can roughly be separated into two complementary classes: (A) centralized approaches assuming complete information and focusing on precision and efficiency [2] and (B) decentralized approaches assuming only partial observability and focusing on simple reactive and behavior-based control [3]. While both concepts are generally justified, the centralized approach may be almost unavoidable for certain tasks. Here, we investigate a control problem in robot swarms with minimal hardware [4,5]. In the case of a simple robot, such as the Kilobot robot with its minimal equipment of sensors [6], certain tasks may be infeasible relying on a decentralized approach. The advantage of having simple hardware is, in turn, that possibly many robots can be built to form a large-scale formation with high redundancy. The automatic control problem can be thought of as macroscopic or stochastic control of a cloud of robots determined by a distribution [4,7].

The global input can be, for example, the mean position of all robots and their variance. The output is a global control law that is broadcasted to all robots or that operates as a moment of force on each robot. The variance may be calculated based on robot positions [5], which could be relaxed in a different approach. An option is to exploit the environment by gathering robots at flat obstacles until minimum variance is achieved [8]. The control iterates over measuring robot positions followed by possibly longer periods of not measuring again but relying on the dynamical model of each robot plus adding Brownian noise on positions, velocities, and accelerations. This is generally related to mean-field models of multi-robot systems [9] and, specifically, the concept of assuming microscopically Brownian particles and the resulting macroscopic evolution of a swarm described by a distribution relates directly to known modeling approaches in swarm robotics based on Langevin equations and Fokker-Planck equations [10].

We propose an optimal energy-efficient control mechanism that minimizes positional variance and steers the robot system’s mean position to a target position. In particular, our work starts by showing that it is possible to obtain mathematically a mapping such that underactuated robot systems take a partial form. However, due to the complexity of the dynamics (coupling of the inertia matrix), it is not possible to design a controller. Another challenge is the fact that the control input matrix $G\left(q\right)$ is time-variant. However, in [3,11,12], the authors assumed the input matrix to be in the form of $G={\left[I_m{0}_s\right]}^{\top }$. This assumption on the input matrix G can be applied only to simple robot structures. In this paper, we cover the case where $G\left(q\right)={\left[G_u\left(q\right)G_a\left(q\right)\right]}^{\top }$ has a general form. Therefore, we relax this assumption on the input matrix G differently from what is done in [3,11,12]. Indeed, finding a transformation to have the robot systems take a partial form is not straightforward. Nonetheless, a new generalized coordinate transformation framework is proposed to decouple the system. This allows the development of an optimal control mechanism with the prime objective of energy efficiency. In control theory, several techniques exist to design energy-efficient control laws [13]. However, the state-dependent Riccati equation (SDRE) [14] does not cancel nonlinear terms, which is advantageous because canceling such nonlinearities would significantly increase the control signals [15]. Furthermore, SDRE parameterizes and characterizes the system to a state-dependent coefficient (SDC) form that is useful for immediate stability analysis. Then, we show that our control design provides set point tracking (stabilization) with semi-global properties. Our proof is based on the Lyapunov stability criterion [16].

Recently in [2], the author introduced a centralized control of underactuated nonidentical Euler–Lagrange systems. The methodology is valid based on the assumption of the accessibility of the information of the whole network. In this paper, we develop a novel centralized control of underactuated large-scale multi-agent systems using only the mean position of all agents and their variance. The experiment highlights three advantages: (i) It resolves the limitation of the existing control strategy by introducing a novel two-step methodology to control the swarm, (ii) it increases the performance by exploiting the torque generated for the orientation of particles and providing smoother trajectories, and (iii) it proposes a base performance comparison with the actuated holonomic swarm of particles [5,17].

2. Euler-Lagrange Dynamics

We consider an underactuated robot system with dynamics described by the well-known Euler-Lagrange (EL) equations of motion

$$\begin{array}{c}\hfill M\left(q\right)\ddot{q}+C(q,\dot{q})\dot{q}+\nabla V\left(q\right)=G\left(q\right)u,\end{array}$$

(1)

where $q\in {\mathbb{R}}^n$ are the configuration variables, $u\in {\mathbb{R}}^m$ are the control signals, $M\left(q\right)>0$ is the generalized inertia matrix, $C(q,\dot{q})$ represent the Coriolis and centrifugal forces, $V\left(q\right)$ is the systems potential energy, and $G\left(q\right)$ is the input matrix. First, we make an assumption characterizing the class of generalized coordinate transformation T that we use here.

Assumption 1.

There exists an invertible mapping $\mathsf{\Phi }:{\mathbb{R}}^n\to {\mathbb{R}}^n$, such that

$${\nabla }_q\mathsf{\Phi }\left(q\right)=T^{-1}\left(q\right).$$

(2)

is invertible for all q.

Lemma 1.

Consider a mapping $\mathsf{\Phi }:{\mathbb{R}}^n\to {\mathbb{R}}^n$ that satisfies Assumption 1 and define the generalised coordinate transformation as follows

$$\mathbf{q}=\mathsf{\Phi }\left(q\right).$$

(3)

Then, the EL dynamics (1) can be written as follows

$$\begin{array}{c}\hfill \mathcal{M}\left(\mathbf{q}\right)\ddot{\mathbf{q}}+\mathcal{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}+\nabla \mathcal{V}\left(\mathbf{q}\right)=\mathcal{G}\left(\mathbf{q}\right)u,\end{array}$$

(4)

where

$$\begin{array}{ccc}\hfill \dot{\mathbf{q}}& :=& T^{-1}\left(q\right)\dot{q}\hfill \end{array}$$

(5)

$$\begin{array}{ccc}\hfill \mathcal{M}\left(\mathbf{q}\right)& :=& T^{\top }\left(q\right)M\left(q\right)T\left(q\right){|}_{q={\mathsf{\Phi }}^{-1}\left(\mathbf{q}\right)}\hfill \end{array}$$

(6)

$$\begin{array}{ccc}\hfill \mathcal{V}\left(\mathbf{q}\right)& :=& V\left(q\right){|}_{q={\mathsf{\Phi }}^{-1}\left(\mathbf{q}\right)}\hfill \end{array}$$

(7)

$$\begin{array}{ccc}\hfill \mathcal{G}& :=& T^{\top }\left(q\right)G\left(q\right){|}_{q={\mathsf{\Phi }}^{-1}\left(\mathbf{q}\right)}\hfill \end{array}$$

(8)

and $\mathcal{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}$ are the Coriolis and centrifugal forces associated with mass matrix $\mathcal{M}\left(\mathbf{q}\right)$ that we can compute by

$$\mathcal{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}=\left[{\nabla }_{\mathbf{q}}\left[\mathcal{M}\left(\mathbf{q}\right)\dot{\mathbf{q}}\right]-\frac{1}{2}{\nabla }_{\mathbf{q}}^{\top }\left[\mathcal{M}\left(\mathbf{q}\right)\dot{\mathbf{q}}\right]\right]\dot{\mathbf{q}}.$$

(9)

The Lagrangian in the new generalized coordinates is

$$\mathcal{L}(\mathbf{q},\dot{\mathbf{q}})=\frac{1}{2}{\dot{\mathbf{q}}}^{\top }\mathcal{M}\left(\mathbf{q}\right)\dot{\mathbf{q}}-\mathcal{V}\left(\mathbf{q}\right).$$

(10)

Proof. 

The proof follows from the coordinate invariance property of the EL equations (or from straightforward calculation computing the derivative of the coordinate transformation and using the original dynamics).    □

Remark 1.

Notice that the matrix $T(.)$ can be used to shape the form of the mass matrix $\mathcal{M}(.)$ in the new generalized coordinates. However, we only consider invertible matrices $T(.)$ that satisfy the integrability Assumption 1. That is, given an invertible matrix $T(.)$, we assume there exists an invertible mapping $\mathsf{\Phi }:{\mathbb{R}}^n\to {\mathbb{R}}^n$ that satisfies

$$\dot{\mathsf{\Phi }}\left(q\right)=T^{-1}\left(q\right)\dot{q}.$$

Therefore, the generalized coordinated transformation (3) is well-defined.

We consider now mechanical systems (1) with an input matrix of the general form

$$\begin{array}{c}\hfill G\left(q\right)=\left[\begin{array}{c}{G}_u\left(q\right)\\ G_a\left(q\right)\end{array}\right],\end{array}$$

(11)

where rank $G\left(q\right)=m<n$, and $G_a\left(q\right)$ is an invertible $m\times m$ matrix. $G_u\left(q\right)$ and $G_a\left(q\right)$ are the underactuated and actuated components of $G\left(q\right)$, respectively. The EL dynamics (1) is coupled when $G_u\left(q\right)\not\equiv 0$. Furthermore, to simplify the notation, we partition the generalized coordinates and velocity as $q=\mathrm{col}(q_u,q_a)$, $\dot{q}=\mathrm{col}({\dot{q}}_u,{\dot{q}}_a)$ with $q_a,{\dot{q}}_a\in {\mathbb{R}}^m$ and $q_u,{\dot{q}}_u\in {\mathbb{R}}^s$, and partition the inertia and Coriolis matrices as

$$\begin{array}{c}\hfill M\left(q\right)=\left[\begin{array}{cc}{m}_{uu}\left(q\right)& m_{au}^{\top }\left(q\right)\\ m_{au}\left(q\right)& m_{aa}\left(q\right)\end{array}\right],\end{array}$$

$$\begin{array}{c}\hfill C(q,\dot{q})=\left[\begin{array}{cc}{c}_{uu}\left(q\right)& c_{ua}\left(q\right)\\ c_{au}\left(q\right)& c_{aa}\left(q\right)\end{array}\right],\end{array}$$

where $m_{aa}:{\mathbb{R}}^n\to {\mathbb{R}}^{m\times m}$, $m_{au}:{\mathbb{R}}^n\to {\mathbb{R}}^{s\times m}$, $m_{uu}:{\mathbb{R}}^n\to {\mathbb{R}}^{s\times s}$, $c_{aa}:{\mathbb{R}}^n\times {\mathbb{R}}^n\to {\mathbb{R}}^{m\times m}$, $c_{au}:{\mathbb{R}}^n\times {\mathbb{R}}^n\to {\mathbb{R}}^{s\times m}$, $c_{ua}:{\mathbb{R}}^n\times {\mathbb{R}}^n\to {\mathbb{R}}^{m\times s}$, $c_{uu}:{\mathbb{R}}^n\to {\mathbb{R}}^{s\times s}$. Next, we impose several assumptions to show particular forms of the EL dynamics (1) under generalized coordinate transformations.

Assumption 2.

There exists a function ${\mathsf{\Phi }}_a:{\mathbb{R}}^m\to {\mathbb{R}}^s$, such that

$${\dot{\mathsf{\Phi }}}_a\left(q_a\right)=m_{uu}^{-1}{m}_{au}^{\top }{\dot{q}}_a.$$

(12)

Assumption 3.

The inertia matrix depends only on the actuated variables $q_a$, i.e., $M\left(q\right)=M\left(q_a\right)$.

Assumption 4.

The sub-block matrix $m_{uu}$ of the inertia matrix is constant.

Assumption 5.

The potential energy can be written as

$$V\left(q\right)=V_a\left(q_a\right)+V_u\left(q_u\right).$$

Proposition 1.

The dynamics of the system (1), under Assumption 2 and using the generalised coordinates $\mathbf{q}=\mathrm{col}({\mathbf{q}}_1,{\mathbf{q}}_2)=\mathsf{\Phi }\left(q\right)$, can be written as follows

$$\begin{array}{l}{\mathbf{m}}_{uu}{\ddot{\mathbf{q}}}_1+\left[{\nabla }_{{\mathbf{q}}_1}\left({\mathbf{m}}_{uu}{\dot{\mathbf{q}}}_1\right)-\frac{1}{2}{\nabla }_{{\mathbf{q}}_1}^{\top }\left({\mathbf{m}}_{uu}^s{\dot{\mathbf{q}}}_1\right)\right]{\dot{\mathbf{q}}}_1+\\ \left[{\nabla }_{{\mathbf{q}}_2}\left({\mathbf{m}}_{uu}^s{\dot{\mathbf{q}}}_2\right)-\frac{1}{2}{\nabla }_{{\mathbf{q}}_1}^{\top }\left({\mathbf{m}}_{aa}{\dot{\mathbf{q}}}_2\right)\right]{\dot{\mathbf{q}}}_2+\\ {\nabla }_{{\mathbf{q}}_1}\mathcal{V}\left(\mathbf{q}\right)=G_u\left(q\right)u\\ {\mathbf{m}}_{aa}^s{\ddot{\mathbf{q}}}_2+\left[{\nabla }_{{\mathbf{q}}_1}\left({\mathbf{m}}_{aa}^s{\dot{\mathbf{q}}}_2\right)-\frac{1}{2}{\nabla }_{{\mathbf{q}}_2}^{\top }\left({\mathbf{m}}_{uu}{\dot{\mathbf{q}}}_1\right)\right]{\dot{\mathbf{q}}}_1\\ +\left[{\nabla }_{{\mathbf{q}}_2}\left({\mathbf{m}}_{aa}^s{\dot{\mathbf{q}}}_2\right)-\frac{1}{2}{\nabla }_{{\mathbf{q}}_2}^{\top }\left({\mathbf{m}}_{aa}^s{\dot{\mathbf{q}}}_2\right)\right]{\dot{\mathbf{q}}}_2+\end{array}$$

(13)

$$\begin{array}{ccc}& & {\nabla }_{{\mathbf{q}}_2}\mathcal{V}\left(\mathbf{q}\right)=\left[G_a\left(q\right)-G_u\left(q\right)m_{au}{m}_{uu}^{-1}\right]u,\hfill \end{array}$$

(14)

where

$$\begin{array}{ccc}\hfill \left[\begin{array}{c}{\mathbf{q}}_1\\ {\mathbf{q}}_2\end{array}\right]& =& \left[\begin{array}{c}{q}_u+{\mathsf{\Phi }}_a\left(q_a\right)\\ q_a\end{array}\right]\hfill \end{array}$$

(15)

$$\begin{array}{ccc}\hfill {\mathbf{m}}_{aa}^s\left(\mathbf{q}\right)& =& m_{aa}\left(q\right)-m_{au}\left(q\right)m_{uu}^{-1}\left(q\right)m_{au}^{\top }\left(q\right){|}_{q={\mathsf{\Phi }}^{-1}\left(\mathbf{q}\right)},\hfill \end{array}$$

(16)

$$\begin{array}{ccc}\hfill {\mathbf{m}}_{uu}\left(\mathbf{q}\right)& =& m_{uu}\left(q\right){|}_{q={\mathsf{\Phi }}^{-1}\left(\mathbf{q}\right)},\hfill \end{array}$$

(17)

$$\begin{array}{ccc}\hfill {\mathbf{m}}_{au}\left(\mathbf{q}\right)& =& m_{au}\left(q\right){|}_{q={\mathsf{\Phi }}^{-1}\left(\mathbf{q}\right)}.\hfill \end{array}$$

(18)

Proof. 

First notice that, under Assumption 2, the coordinate transformation (15) satisfies Assumption 1 with

$$T\left(q\right)=\left[\begin{array}{cc}{I}_s& -m_{uu}^{-1}{m}_{au}^{\top }\\ 0_{m\times s}& I_m\end{array}\right].$$

(19)

Then, from Lemma 1 we obtain that the dynamics can be written in the form (10) with

$$\begin{array}{c}\hfill \left[\begin{array}{c}{\dot{\mathbf{q}}}_1\\ {\dot{\mathbf{q}}}_2\end{array}\right]=\left[\begin{array}{cc}{I}_s& m_{uu}^{-1}{m}_{au}^{\top }\\ 0_{m\times s}& I_m\end{array}\right]\left[\begin{array}{c}{\dot{q}}_u\\ {\dot{q}}_a\end{array}\right]\end{array}$$

(20)

and Lagrangian

$$\mathcal{L}(\mathbf{q},\dot{\mathbf{q}})=\frac{1}{2}\left[\begin{array}{cc}{\dot{\mathbf{q}}}_1^{\top }& {\dot{\mathbf{q}}}_2^{\top }\end{array}\right]\left[\begin{array}{cc}{\mathbf{m}}_{uu}& 0_{s\times m}\\ 0_{m\times s}& {\mathbf{m}}_{aa}^s\end{array}\right]\left[\begin{array}{c}{\dot{\mathbf{q}}}_1\\ {\dot{\mathbf{q}}}_2\end{array}\right]-\mathcal{V}\left(\mathbf{q}\right).$$

(21)

The dynamics (13) and (14) follow, after some simple calculations, from the EL formula using the Lagrangian (21).    □

Corollary 1.

The system (1) satisfying Assumptions 1–3 can be written as in the EL form as follows

$$\begin{array}{ccc}& & m_{uu}\left(q_a\right){\ddot{\mathbf{q}}}_1+{\nabla }_{{\mathbf{q}}_1}\mathcal{V}({\mathbf{q}}_1,q_a)=G_u\left(q\right)u,\hfill \\ & & m_{aa}^s{\ddot{q}}_a+\left[{\nabla }_{q_a}\left[m_{aa}^s\left(q_a\right){\dot{q}}_a\right]-\frac{1}{2}{\nabla }_{q_a}^{\top }\left[m_{aa}^s\left(q_a\right){\dot{q}}_a\right]\right]{\dot{q}}_a+\hfill \end{array}$$

(22)

$$\begin{array}{ccc}& & {\nabla }_{q_a}\mathcal{V}({\mathbf{q}}_1,q_a)=\left[G_a\left(q\right)-G_u\left(q\right)m_{au}{m}_{uu}^{-1}\right]u,\hfill \end{array}$$

(23)

with $m_{aa}^s\left(q_a\right)=m_{aa}\left(q_a\right)-m_{au}^{\top }\left(q_a\right)m_{uu}^{-1}{m}_{au}\left(q_u\right)$. In addition, if Assumptions 3 and 4 also holds, then the EL dynamics can be written as follows

$$\begin{array}{ccc}& & m_{uu}{\ddot{\mathbf{q}}}_1+{\nabla }_{q_u}{V}_u{|}_{q_u={\mathbf{q}}_1-{\mathsf{\Phi }}_a\left(q_a\right)}=G_u\left(q\right)u,\hfill \\ & & m_{aa}^s{\ddot{q}}_a+\left[{\nabla }_{q_a}\left[m_{aa}^s{\dot{q}}_a\right]-\frac{1}{2}{\nabla }_{q_a}\left[m_{aa}^s{\dot{q}}_a\right]\right]{\dot{q}}_a\hfill \\ & & +{\nabla }_{q_a}{V}_a-m_{au}{m}_{uu}{\nabla }_{q_u}{V}_u{|}_{q_u={\mathbf{q}}_1-{\mathsf{\Phi }}_a\left(q_a\right)}=\hfill \end{array}$$

(24)

$$\begin{array}{ccc}& & \left[G_a\left(q\right)-G_u\left(q\right)m_{au}{m}_{uu}^{-1}\right]u.\hfill \end{array}$$

(25)

Proof. 

The proof follows from Proposition 1 and Assumptions 1–3 by setting in (13) and (14) the following conditions: ${\mathbf{q}}_1=q_u+{\mathsf{\Phi }}_a\left(q_a\right)$, ${\mathbf{q}}_2=q_a$, $m_{uu}$ is a constant matrix, and ${\mathbf{m}}_{aa}^s\left(\mathbf{q}\right)=m_{aa}^s\left(q_a\right)$. The second part follows from the fact that, under Assumption 4, the potential function is $\mathcal{V}\left(\mathbf{q}\right)={\mathcal{V}}_a\left(q_a\right)+{\mathcal{V}}_u({\mathbf{q}}_1-{\mathsf{\Phi }}_a\left(q_a\right))$.    □

Remark 2.

Notice that the system in the partial linear form (24) and (25) has been used to design a PID passivity-based controller in [18]. In that work, an outer partial feedback linearization (PFL) control is used to obtain the desired form, which compromises the robustness of the closed loop. However, this PFL control can be avoided by using a generalized change of coordinates as shown in Corollary 1.

The generalized coordinate transformation in Proposition 1 is also useful (as it will be shown in the next section) for the underactuated swarm of particles.

3. Underactuated Robot System

We consider an underactuated robot system with masses $m_1$, $m_2$, and $m_3$, as shown in Figure 1 that are rigidly fastened to the mass-less shaft and are free to move in the 2D plane. We now set up the equation of motion of the holonomic robot using convenient coordinates $q={[q_1,q_2,q_3]}^{\top }={[x_1,y_1,\theta ]}^{\top }$. An external force $f_1$ is applied to $m_1$ in the direction of $-x_1$ and $y_1$ respectively, and $f_3$ to $m_3$ in the direction of $-x_3$ and $y_3$ respectively. To simplify the notation, we assume that all representative particle masses are the same (e.g., $m_i=m$ for $i=1,\dots ,3$). Applying Lagrange’s equations, it immediately follows that

$$\begin{array}{c}\hfill \mathcal{L}={\displaystyle \frac{1}{2}}{\dot{q}}^{\top }\left[\begin{array}{ccc}3m& 0& -3Lmsin\left(\theta \right)\\ 0& 3m& 3Lmcos\left(\theta \right)\\ -3Lmsin\left(\theta \right)& 3Lmcos\left(\theta \right)& 5L^{2}m\end{array}\right]\dot{q}\end{array}$$

(26)

where $(x_1,y_1)$ is positioned at the center of the first mass particle, L is the distance between each mass, and $\theta$ is the inclination angle (see Figure 1). The equations of motion can be written in compact form as

$$\begin{array}{c}\hfill M\left(q\right)\ddot{q}+C(q,\dot{q})\dot{q}=G\left(q\right)u,\end{array}$$

(27)

where $M\left(q\right)$ is the generalized inertia matrix

$$\begin{array}{c}\hfill M\left(q\right)=\left[\begin{array}{ccc}3m& 0& -3Lmsin\left(\theta \right)\\ 0& 3m& 3Lmcos\left(\theta \right)\\ -3Lmsin\left(\theta \right)& 3Lmcos\left(\theta \right)& 5L^{2}m\end{array}\right].\end{array}$$

(28)

$C(q,\dot{q})$ is the Coriolis matrix

$$\begin{array}{c}\hfill C(q,\dot{q})=\\ \left[\begin{array}{ccc}0& 0& -3L\dot{\theta }mcos\left(\theta \right)\\ 0& 0& -3L\dot{\theta }msin\left(\theta \right)\\ {\displaystyle \frac{3L\dot{\theta }mcos\left(\theta \right)}{2}}& {\displaystyle \frac{3L\dot{\theta }msin\left(\theta \right)}{2}}& h_s\end{array}\right]\end{array}$$

(29)

with

$$\begin{array}{c}\hfill h_s=-{\displaystyle \frac{3Lm({\dot{x}}_{1}cos\left(\theta \right)+{\dot{y}}_{1}sin\left(\theta \right))}{2}}.\end{array}$$

(30)

Therefore, the elements of the inertia matrix for the holonomic robot are given by

$$\begin{array}{ccc}\hfill m_{uu}& =\hfill & 3m\hfill \\ \hfill m_{au}& =& m_{au}^{\top }=\left[\begin{array}{cc}0& -3Lmsin\left(\theta \right)\end{array}\right]\hfill \\ \hfill m_{aa}& =& \left[\begin{array}{cc}3m& 3Lmcos\left(\theta \right)\\ 3Lmcos\left(\theta \right)& 5L^{2}m\end{array}\right].\hfill \end{array}$$

(31)

The virtual work is given by

$$\begin{array}{c}\hfill \delta W=\left[\begin{array}{c}-(f_1+f_3)sin\left(\theta \right)\delta x_1\\ +(f_1+f_3)cos\left(\theta \right)\delta y_1\\ 2L{f}_3\delta \theta \end{array}\right].\end{array}$$

(32)

The derivation of (32) is given in Appendix A. Without loss of generality, $G\left(q\right)$ can be written as

$$G\left(q\right)=\left[\begin{array}{cc}-sin\left(\theta \right)& 0\\ cos\left(\theta \right)& 0\\ 0& 1\end{array}\right],$$

(33)

with $u={[u_1,u_2]}^{\top }={[f_1+f_3,2L{f}_3]}^{\top }$. Therefore, $G_u\left(q\right)=\left[\begin{array}{cc}-sin\left(\theta \right)& 0\end{array}\right]$ and $G_a\left(q\right)=\left[\begin{array}{cc}cos\left(\theta \right)& 0\\ 0& 1\end{array}\right]$. Note that $G_a\left(q\right)$ is an invertible $2\times 2$ matrix.

Mechanical Properties of the Underactuated Robot

The robot as defined by (27) has several fundamental properties, which can be used to facilitate the design of an automatic control mechanism.

P.1 

$M\left(q\right)$ is a positive definite matrix.

P.2 

The inertia matrix depends only on the actuated variables $q_a$, i.e., $M\left(q\right)=M\left(q_a\right)$.

P.3 

The sub-block matrix $m_{uu}$ of the inertia matrix is constant.

P.4 

From (28) and (29), and by using (30), we get

$$\begin{array}{cc}\dot{M}\left(q\right)-2C(q,\dot{q})=\hfill & \\ \left[\begin{array}{ccc}0& 0& {\displaystyle \frac{9L\dot{\theta }mcos\left(\theta \right)}{2}}\\ 0& 0& {\displaystyle \frac{9L\dot{\theta }msin\left(\theta \right)}{2}}\\ -{\displaystyle \frac{9L\dot{\theta }mcos\left(\theta \right)}{2}}& -{\displaystyle \frac{9L\dot{\theta }msin\left(\theta \right)}{2}}& 0\end{array}\right],\hfill \end{array}$$

(34)

which is a skew-symmetric matrix.

The system has three degrees of freedom and only two actuators, hence, we have an underactuated mechanical system. We have nonlinearities because the generalized inertia matrix is off-diagonal and the input matrix is highly coupled. Due to the lack of more actuators, this system cannot be fully linearized using exact feedback linearization. However, it is still possible to apply PFL to the system, such that the translational dynamics $q_1=x_1$ and $q_2=y_1$ become a double integrator. As already mentioned, PFL compromises the robustness of the closed loop. However, the PFL can be avoided by using the proposed transformation, as shown in Corollary 1. Given the properties P.1–P.4, we apply the generalized coordinate transformations based on Proposition 1 to decouple the system.

Proposition 2.

Considering the holonomic robot in (27), the dynamical system model can be rewritten as

$$\begin{array}{ccc}\hfill & & {\ddot{x}}_1=f_{x_1}(\theta ,\dot{\theta })-u_{1}Lsin\left(\theta \right),\hfill \end{array}$$

(35)

$$\begin{array}{ccc}\hfill & & {\ddot{y}}_1=f_{y_1}(\theta ,\dot{\theta })+{\displaystyle \frac{5cos\left(\theta \right)}{6m}}{u}_1-{\displaystyle \frac{cos\left(\theta \right)}{2m}}{u}_2,\hfill \end{array}$$

(36)

$$\begin{array}{ccc}\hfill & & \ddot{\theta }=-{\displaystyle \frac{1}{2m}}{u}_1+{\displaystyle \frac{1}{2m}}{u}_2.\hfill \end{array}$$

(37)

Proof. 

By applying Proposition 1 the result follows.    □

In the next section, we address questions related to the automatic control of a particle swarm that minimizes energy by applying the transformed underactuated model in (35)–(37). We prove that the mean of configuration variables is controllable and provide conditions under which the variance is also controllable.

4. Control Design for a Swarm

In this section, we present an automatic controller for a swarm of particles that minimizes energy. We show that it only relies on the first two moments of the swarm configuration variables, i.e., the position and the orientation angle distribution. The main objective of our automatic control approach is to act on forces optimally so that particles can reach the desired target position $q^{*}={[x^{*},y^{*},{\theta }^{*}]}^{\top }$ with the stable Euler angle ($\underset{t\to \infty }{lim}\theta =0$).

4.1. Swarm Dynamical System Model

By defining $x=(x_1,{\dot{x}}_1,y_1,{\dot{y}}_1,\theta ,\dot{\theta })$, the dynamics of (35)–(37) can be written as

$$\begin{array}{c}\hfill \dot{x}=A\left(x\right)x+B\left(x\right)u.\end{array}$$

(38)

The elements of $A\left(x\right)$ and $B\left(x\right)$ are

$$\begin{array}{c}\hfill \left[\begin{array}{c}{\dot{x}}_1\\ {\ddot{x}}_1\\ {\dot{y}}_1\\ {\ddot{y}}_1\\ \dot{\theta }\\ \ddot{\theta }\end{array}\right]=\left[\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 3Lmsin\left(\theta \right)\dot{\theta }(-\dot{\theta }+1)+5L& 3Lmcos\left(\theta \right)(2\dot{\theta }-1)\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& L{\dot{\theta }}^{2}cos\left(\theta \right)+3sin\left(\theta \right)& 2L\dot{\theta }sin\left(\theta \right)\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{x}_1\\ {\dot{x}}_1\\ y_1\\ {\dot{y}}_1\\ \theta \\ \dot{\theta }\end{array}\right]\\ \hfill +\left[\begin{array}{cc}0& 0\\ -Lsin\left(\theta \right)& 0\\ 0& 0\\ {\displaystyle \frac{5Lcos\left(\theta \right)}{6m}}& -{\displaystyle \frac{cos\left(\theta \right)}{2m}}\\ 0& 0\\ -{\displaystyle \frac{1}{2m}}& {\displaystyle \frac{1}{2m}}\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]\end{array}$$

(39)

The system is nonlinear, since matrices $A\left(x\right)$ and $B\left(x\right)$ both depend on the current state variables. Firstly, we analyze the number of controllable states as given by the following definition.

Definition 1.

The states in (38) are controllable if the pair $\left\{A\right(x),B(x\left)\right\}$ is point-wise controllable. This can be observed by the rank of the controllability matrix

$$\begin{array}{c}\hfill \mathfrak{C}=\left[\begin{array}{cccc}B\left(x\right)& A\left(x\right)B\left(x\right)& \dots & A^5\left(x\right)B\left(x\right)\end{array}\right].\end{array}$$

(40)

The consequence of Definition 1 is that the $\mathfrak{C}$ matrix for the system in (38) has the full rank (i.e., rank $\left(\mathfrak{C}\right)=6$). Therefore, all states are controllable. Previous work has shown that the mean and variance of many particles for simple fully actuated particles are controllable [5,7]. Next, we show how we can stabilize the nonlinear underactuated particles by a global state-feedback controller designed via state-dependent Riccati equation (SDRE) control [14]. Motivated by (38) and defining the mean states $\overline{x}$ that represent the mean states of N particles, we can write the dynamical system model of the swarm as

$$\begin{array}{c}\hfill \dot{\overline{x}}=A\left(\overline{x}\right)\overline{x}+B\left(\overline{x}\right)u.\end{array}$$

(41)

Interestingly, analyzing the controllability of the swarm dynamics results in the same form as in (38), hence, the mean states are controllable.

4.2. Control Law

Our objective is to find minimum energy inputs that steer the swarm to a given target state defined on $t\in [t_0,t_f]=[0,\infty ]$. To do so, consider now the following cost functional

$$\begin{array}{c}\hfill \mathcal{J}={\displaystyle \frac{1}{2}}{\int }_0^{\infty }\left({\overline{x}}^{\top }\mathcal{Q}\left(\overline{x}\right)\overline{x}+u^{\top }\mathcal{R}\left(\overline{x}\right)u\right)dt,\end{array}$$

(42)

with respect to the state $\overline{x}$ and control input u subject to the nonlinear dynamical system model constraint

$$\begin{array}{c}\hfill \dot{\overline{x}}=A\left(\overline{x}\right)\overline{x}+B\left(\overline{x}\right)u,\end{array}$$

where $\mathcal{Q}\left(\overline{x}\right)\ge 0$ penalizes the state, and $\mathcal{R}\left(\overline{x}\right)>0$ penalizes the control effort for all $\overline{x}$. We aim for a nonlinear state-feedback controller u that stabilizes solutions of problem (41) and (42).

Remark 3.

Cloutier [14] obtains the nonlinear feedback controller via SDRE. Our interest is to provide an alternative interpretation of solving the problem (41) and (42) via Pontryagin’s minimum principle [13].

From (41) and (42), the Hamiltonian can be written as

$$\begin{array}{cc}\hfill H=& {\displaystyle \frac{1}{2}}\left[{\overline{x}}^{\top }\mathcal{Q}\left(\overline{x}\right)\overline{x}+u^{\top }\mathcal{R}\left(\overline{x}\right)u\right]\hfill \\ & +p^{\top }\left(t\right)\left[A\left(\overline{x}\right)\overline{x}+B\left(\overline{x}\right)u\right],\hfill \end{array}$$

(43)

where $p\left(t\right)$ is the adjoint vector. The necessary condition is derived by differentiating (43) with respect to u which yields

$$\begin{array}{ccc}\hfill & & {\nabla }_{u}H=\mathcal{R}\left(\overline{x}\right)u+B^{\top }\left(\overline{x}\right)p=0.\hfill \end{array}$$

(44)

We obtain the nonlinear feedback controller

$$\begin{array}{c}\hfill u=-{\mathcal{R}}^{-1}\left(\overline{x}\right)B^{\top }\left(\overline{x}\right)p.\end{array}$$

(45)

Now, we define $p\triangleq P\left(x\right)x$, where the matrix $P\left(x\right)$ can be obtained by solving the algebraic Riccati equation

$$\begin{array}{c}{A}^{\top }\left(\overline{x}\right)P+PA\left(\overline{x}\right)-PB\left(\overline{x}\right){\mathcal{R}}^{-1}\left(\overline{x}\right)B^{\top }\left(\overline{x}\right)P\hfill \\ +\mathcal{Q}\left(\overline{x}\right)=0.\hfill \end{array}$$

(46)

By that we fulfill the second optimality condition

$$\begin{array}{c}\hfill \dot{p}=-{\nabla }_{\overline{x}}H(\overline{x},t,p).\end{array}$$

(47)

Therefore, as long as the two conditions in (44) and (47) hold, it is always possible to construct a nonlinear feedback controller that solves the problem (41) and (42). The closed-loop solution for this feedback controller is at least a local optimum and possibly the global optimum.

4.3. Stability Analysis

Theorem 1.

Consider the dynamical system model (41), with the feedback controller (45). Assume in addition that for a constant input weighting matrix $\mathcal{R}>0$, the state weighting matrix $\mathcal{Q}\left(x\right)>0$ can be chosen, such that $\dot{P}\left(x\right)<0$ for all x, where $P\left(\overline{x}\right)$ is the solution of (46). Then the zero equilibrium of the closed-loop system is semi-globally stable.

Proof. 

Consider the Lyapunov function candidate

$$\begin{array}{c}\hfill L\left(\overline{x}\right)={\overline{x}}^{\top }P\left(\overline{x}\right)\overline{x},\end{array}$$

(48)

the time derivative of which, along the trajectories of the closed-loop dynamical system, is such that

$$\begin{array}{c}\dot{L}\left(\overline{x}\right)={\dot{\overline{x}}}^{\top }P\left(\overline{x}\right)\overline{x}+{\overline{x}}^{\top }P\left(\overline{x}\right)\dot{\overline{x}}+{\overline{x}}^{\top }\dot{P}\left(\overline{x}\right)\overline{x}\hfill \\ ={\overline{x}}^{\top }[\dot{P}\left(\overline{x}\right)-\mathcal{Q}\left(\overline{x}\right)\hfill \\ -P\left(\overline{x}\right)B\left(\overline{x}\right){\mathcal{R}}^{-1}{B}^{\top }\left(\overline{x}\right)P\left(\overline{x}\right)]\overline{x},\hfill \end{array}$$

(49)

where $P\left(\overline{x}\right)B\left(\overline{x}\right){\mathcal{R}}^{-1}{B}^{\top }\left(\overline{x}\right)P\left(\overline{x}\right)>0$. In addition, based on the assumed selection of $Q\left(\overline{x}\right)$, yields $\dot{P}\left(\overline{x}\right)<0$ and $\dot{L}\left(x\right)<0$, hence our claim.    □

4.4. Controlling Mean and Variance

The variances ${\sigma }_{x_1}^2$ and ${\sigma }_{y_1}^2$ of N underactuated particle’s position is

$$\begin{array}{c}\hfill {\overline{x}}_1={\displaystyle \frac{1}{N}}\sum _{i=1}^N{x}_{1i},{\sigma }_{x_1}^2={\displaystyle \frac{1}{N}}\sum _{i=1}^N{(x_{1i}-{\overline{x}}_1)}^2,\end{array}$$

(50)

$$\begin{array}{c}\hfill {\overline{y}}_1={\displaystyle \frac{1}{N}}\sum _{i=1}^N{y}_{1i},{\sigma }_{y_1}^2={\displaystyle \frac{1}{N}}\sum _{i=1}^N{(y_{1i}-{\overline{y}}_1)}^2.\end{array}$$

(51)

The objective now is to control both, the mean and variance, effectively to ensure approaching a target position with minimum variance. Therefore, the selected strategy is the hysteresis-based approach following [5,19]. The idea is that the automatic controller regulates the mean states of N underactuated particles with radius r but switches to minimizing variance if the variance exceeds the threshold ${\sigma }_{max}=2.5r+{\sigma }_{optimal}^2(n,r)$ and until ${\sigma }_{min}=15r++{\sigma }_{optimal}^2(n,r)$ is reached [5]. The idea of using such values comes from Graham and Sloane [8]. They proved that the minimum variance to collect N 2D circles with radius r is $0.55N{r}^2$. Our proposed methodology in total consists of (1) applying the generalized coordinate transformation shown in Algorithm 1 and (2) proposing and analyzing the control mechanism to regulate the mean and variance of the swarm of underactuated particles shown in Algorithm 2.

Algorithm 1 Generalised Coordinate Transformation for Underactuated Particle (27).

begin procedure Step 1: Partition the generalized coordinates and velocity w Step 2: Construct the invertible mapping $$\dot{\mathsf{\Phi }}\left(q\right)=T^{-1}\left(q\right)\dot{q},$$ with $$T\left(q\right)=\left[\begin{array}{cc}{I}_s& -m_{uu}^{-1}{m}_{au}^{\top }\\ 0_{m\times s}& I_m\end{array}\right].$$ Step 3: Apply Proposition 1. end procedure

Algorithm 2 Hysteresis-based Mean and Variance Automatic Control

Require: Knowledge of underactuated particle swarm mean $\overline{x}$, variance ${\left[{\sigma }_{x_1}^2{\sigma }_{y_1}^2\right]}^{\top }$, the boundary of the search space $\left\{x_{min}{x}_{max}{y}_{min}{y}_{max}\right\}$, and the desired mean state $q^{*}={[x^{*},y^{*},{\theta }^{*}]}^{\top }$.     $x_{goal}\leftarrow x^{*}$, $y_{goal}\leftarrow y^{*}$     loop              if ${\sigma }_{x_1}^2>{\sigma }_{max}$ then ∣            $x_{goal}\leftarrow x_{min}$            else ∣            $x_{goal}\leftarrow x^{*}$            end              if ${\sigma }_{y_1}^2>{\sigma }_{max}$ then ∣            $y_{goal}\leftarrow y_{min}$            else ∣            $y_{goal}\leftarrow y^{*}$            end     Apply the automatic control law (45) to regulate the underactuated swarm to the desired state $q^{*}={[x^{*},y^{*},{\theta }^{*}]}^{\top }$     end loop

4.5. Fully-Actuated vs. Underactuated Particle Swarm

We now consider a small swarm of $N=4$ particles to showcase the performance of the proposed control law and highlight the advantage of the underactuated particle swarm over the fully-actuated swarm [5]. The sampling time is set to $0.01$ s and the physical parameters are given in

Table 1. Parameters of the simulated underactuated swarm.

Parameters	Symbol	Value	Unit
mass	m	$0.01$	kg
shaft	L	$0.02$	m

. The control gain matrices $\mathcal{Q}\left(\overline{x}\right)$ and $\mathcal{R}$ are based on the assumptions of Theorem 1 and we get

$$\begin{array}{ccc}\hfill & & \mathcal{Q}\left(\overline{x}\right)=\left(1+0.01{({\overline{x}}_5-0)}^2+0.01{({\overline{x}}_6-0)}^2\right)\hfill \\ \hfill & & 0.001(260,1,260,1,160,100),\mathcal{R}=\left[\begin{array}{cc}50& 0\\ 0& 50\end{array}\right].\hfill \end{array}$$

We compare it to the approach of Shahrokhi et al. [5]. Their control gains for the PD controller are $K_{p_x}=0.04$, $K_{p_y}=0.03$, $K_{d_x}=0.03$, and $K_{p_y}=0.04$. Figure 2 compares our approach and the PD controller [5] for the obtained trajectories of mean, variance, and the control inputs. Even though the settling times seem satisfactory for both approaches, the trajectory and the control inputs allow us to discriminate between the two approaches. The control inputs obtained through our approach are significantly smaller resulting in less energy consumption. Also, note that there are no sudden peaks in the control inputs. The fully-actuated approach consumes $1.0347$ of energy compared to the under-actuated one that consumes $0.4639$. This is an energy reduction of approximately $57\%$.

Both approaches minimize mean and variance. However, in the underactuated case, we stabilize the mean Euler angle $\overline{\theta }$ with only two global control inputs. Hence, we reasonably balance the tradeoff between control complexity and system performance.

5. Multi-Robot Simulations and Discussion

We also show the result for a swarm of $N=8$ robots to visualize our results in an accessible way. Time is discretized and the control signal is scaled by $\delta t=0.01$. The underactuated robots and arena boundaries are simulated as physical entities. Each underactuated robot has a random initial pose and the swarm’s mean position has a randomly generated target pose. The nonlinear controller described in Algorithm 2 steers the robot’s from a starting position to a target position (equilibrium point of the swarm) with a stable Euler angle. Figure 3 shows four screenshots during a representative simulation run. This result shows how the properties of the underactuated robot system (e.g., torque and inertia) are exploited to regulate the mean, to minimize the variance, and to steer the swarm to the target on the right pose.

Looking ahead there will be a need for the further abstraction of details like actuators and engines, making it a building-blocks tool using various components. There are several limitations of the automatic control mechanism at this time. At this stage of the method development, non-holonomic constraints do not consider. Furthermore, we only tackle to track only the boundary of a ‘cloud of robots’ and their center of gravity. Possibly also the particle density could be measured instead of each individual robot.

6. Conclusions

We have proposed a centralized automatic stochastic control of large-scale robot systems for underactuated robots based on a generalized change of coordinates. We transform underactuated robot systems to the partial form that can be used for control design. At the cost of centrally tracking all robots, we gain the benefit of optimal energy-efficient control in the task of minimizing positional variance and moving the robot system’s mean to a goal position. The requirement of having to track all robots is unlikely to scale arbitrarily. A future extension of our method could hence be to track only the boundary of a cloud of robots and their center of gravity. Possibly also the particle density could be measured instead of each individual robot. There is no immediate way of transferring our method to a decentralized approach, hence making it complementary to behavior-based approaches from swarm robotics that show increased robustness without a central element. However, centralized and decentralized approaches and their pros and cons are complementary to each other, which needs to be carefully considered by the designer for a given use case. In future work, we plan to test and study our approach on real robots with different physical characteristics, such as the Kilobot and other robots with bigger masses. Also, an extension of the method to a manipulation scenario [7] seems particularly relevant.