Introduction to Flying Robots

Abstract

Many flying robots can be regarded as rigid bodies propelled by a thrust force whose direction is constant from the viewpoint of the robot, but whose magnitude can be controlled. Such robots are endowed with a mechanism to induce torques about three mutually orthogonal directions, allowing one to control the direction of the thrust force. Quadrocopters are examples of robots in this class. This chapter presents a mathematical model of such flying robots and it revisits the flocking problem for this model. In the special case when the robots fly in a horizontal plane, the solutions of the flocking problem for unicycles and flying robots turn out to be very similar. The chapter closes with a discussion about rendezvous of flying robots, an exciting open problem whose solution may lead to distributed coordination algorithms for formation control.

These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.

6.1 Introduction

In this chapter we discuss helicopter flying robots. Unlike airplanes, helicopters have the ability to hover at a fixed position in space. Their source of propulsion is a collection of one or more rotors that can be thought of as rotating wings—see Fig. 6.1. Similar to a wing, the relative velocity between the rotor blades and the surrounding air generates a force transversal to the rotor plane. This force can be reasonably assumed to be orthogonal to the rotor plane and to have a magnitude proportional to the squared speed of rotation.

A rotor alone is not sufficient to produce stable flight. Some mechanism is required to steer the helicopter, for otherwise one would have no way to affect the direction of flight. Additionally, by Newton’s third law, a torque on the rotor shaft will cause an opposite torque on the helicopter body, causing the helicopter to spin about the rotor axis. One must therefore devise a way to counteract this spin and to produce steering torques. There are many ways to achieve these desired properties; we will mention a few.

6.1.1 Common Flying Robots

A ducted fan aircraft, schematically depicted in Fig. 6.2, has only one rotor and two pairs of ailerons underneath it. Each aileron pair works like the ailerons on the wings of an airplane, producing torques about two orthogonal axes. Together, the four ailerons produce torques around three independent axes, and can be used to steer the aircraft while preventing it from spinning.

Fig. 6.1

The rotor of a helicopter, rotating with speed $\omega$ and producing a lift force with magnitude $k \omega ^2$

Fig. 6.2

Ducted fan aircraft

A conventional helicopter has two rotors. The main rotor generates a lift force while the tail rotor stabilizes the spin. The main rotor is mounted on a swash plate that varies the pitch of the rotor blades to produce appropriate roll and pitch torques.

The configuration of a conventional helicopter is not ideal for a miniature flying robot, since the energy expended by the tail rotor is not used to generate lift. A more energy efficient configuration is that of a co-axial helicopter, displayed in Fig. 6.3. This helicopter has two rotors mounted on a common axis and rotating in opposite directions. In this configuration, both rotors contribute to producing a lift force. Moreover, if $\tau _{r_1}$ and $\tau _{r_2}$ denote the torques applied by motors to the two rotors—see Fig. 6.3—then the helicopter body is subjected to a differential torque, $\tau _{r_1}-\tau _{r_2}$, about the rotor axis, which can be used to prevent the helicopter from spinning. Like conventional helicopters, coaxial helicopters often use swash plates for steering.

Fig. 6.3

Coaxial helicopter

Fig. 6.4

Quadrotor helicopter

Quadrotor helicopters (also called quadrocopters) are the most common flying robots. The structure of a quadrotor helicopter is shown in Fig. 6.4. It has four coplanar rotors. Viewed from the top, the rotor shafts are placed on the vertices of a square. Two rotors on a diagonal of the square rotate clockwise; the other two rotate counterclockwise. In Fig. 6.4, $\tau _{r_i}$ denotes the torque that the ith motor applies to rotor i, and $f_{r_i}$ denotes the magnitude of the force generated by rotor i ($\tau _{r_i}$ and $f_{r_i}$ are naturally related to one another; we will discuss this relationship later). As a result of this arrangement, three torques are applied about the body axes $\{b_1,b_2,b_3\}$ of Fig. 6.4. By Newton’s third law, the torque $\tau _{r_1} + \tau _{r_2} - \tau _{r_3}-\tau _{r_4}$ is applied about axis $b_3$; forces $f_{r_1}$ and $f_{r_2}$ produce a moment about axis $b_1$, resulting in the torque $l (f_{r_1}-f_{r_2})$; similarly, forces $f_{r_3}$ and $f_{r_4}$ produce the torque $l (f_{r_3}-f_{r_4})$ about axis $b_2$. Finally, the sum of the four forces $f_{r_1} + \cdots +f_{r_4}$ is the total vertical thrust on the helicopter.

6.1.2 Onboard Sensors

Every flying robot is equipped with an inertial measurement unit (IMU). IMUs typically contain three accelerometers, three rate-gyroscopes, and three magnetometers. The accelerometers measure the non-gravitational acceleration vector in a local coordinate frame (often the frame is printed on the chip), that is, the difference between the acceleration of the vehicle and the acceleration due to gravity. The rate-gyroscopes measure the roll, pitch, and yaw angle rates of the robot. Finally, the magnetometers measure the coordinates of the earth’s magnetic field vector in the local frame.

In addition to IMUs, flying robots may have one or more ultrasonic sensors to detect proximity to objects or to the ground, a barometric pressure sensor, one or more cameras (one pointing along the helicopter’s heading axis and sometimes one pointing downward), and a GPS sensor.

Now consider a collection of flying robots, each carrying a camera and a marker. If robot j’s marker is in the field of view of robot i’s camera, then from the dimension and position of the marker in the camera image, robot i can deduce its relative displacement to robot j in its own local frame. Moreover if the marker is suitably designed, from the camera image it is possible to deduce the relative rotation between the camera frame and the marker. In conclusion, we may assume that, in addition to the IMU data, vehicle i has access to the relative displacement and relative orientation of vehicle j with respect to vehicle i’s local frame.

6.2 Modelling

The flying robots discussed in the introduction have something in common. If we ignore the masses and moments of inertia of the rotors, each robot may be viewed as a rigid body propelled by a thrust vector that has constant direction in the body frame and is endowed with some steering mechanism that induces torques about three body axes. We will now model such a general setup, beginning with the simpler planar case.

6.2.1 2D Flying Robot

We begin with the simplified setup of Fig. 6.5, in which the robot flies in a horizontal plane, propelled by a thrust vector f parallel to its heading. The state of the robot is the position $x=(x_1,x_2)$ of its centre of mass in an inertial coordinate frame, the velocity $\dot{x}$, the heading angle $\theta$, and the angular speed $\dot{\theta }$. There are two control inputs: the magnitude, u, of the thrust vector and the torque, $\tau$, about the axis coming out of the page. The thrust vector f in inertial coordinates is given by

$$ f = u \begin{bmatrix} \cos (\theta ) \\ \sin (\theta ) \end{bmatrix}. $$

Let m denote the mass of the robot. Then Newton’s equation gives

$$ m \ddot{x} = f. $$

Fig. 6.5

2D flying robot

The torque $\tau$ causes the robot to rotate. Denoting by J the moment of inertia of the robot about the axis coming out of the page, computed with respect to the centre of mass, we have

$$ J \ddot{\theta }= \tau . $$

The complete model is therefore

$$\begin{aligned} \begin{aligned} m \ddot{x}&= u \begin{bmatrix} \cos (\theta ) \\ \sin (\theta ) \end{bmatrix} \\ J \ddot{\theta }&= \tau . \end{aligned} \end{aligned}$$

(6.1)

There is a better way to represent model (6.1), one that lends itself to a 3D generalization. As we did in Chap. 2 for the unicycle model, we attach an orthonormal frame $\mathcal{B}=\{r,s\}$ to the robot as in Fig. 6.6, and we define the rotation matrix of frame $\mathcal{B}$ with respect to the inertial frame as

$$ R := [r \ \ s] = \left[ \begin{array}{rr} \cos (\theta ) &{} -\sin (\theta ) \\ \sin (\theta ) &{} \cos (\theta ) \end{array}\right] . $$

Then, as in Chap. 2, we have

$$ \dot{R} = R \left[ \begin{array}{rr} 0 &{} \ \ -\omega \\ \omega &{} \ \ 0 \end{array}\right] =R S(\omega ). $$

The thrust vector f is parallel to the body frame axis r and has magnitude u,

$$ f = u R e_1, $$

where $e_1=[1 \ \ 0]^T$. In conclusion, we can rewrite model (6.1) as follows:

$$\begin{aligned} \begin{aligned} m\ddot{x}&= u R e_1 \\ \dot{R}&= R S(\omega ) \\ J \dot{\omega }&= \tau . \end{aligned} \end{aligned}$$

(6.2)

Fig. 6.6

Body frame

We will now see that this model has a straightforward generalization to the three-dimensional setting.

6.2.2 3D Flying Robot

Consider now the setup of Fig. 6.7, in which the robot flies in the three-dimensional Euclidean space. As before, we fix an inertial frame $\mathcal{I}$ and a body frame $\mathcal{B} = \{ b_1,b_2,b_3\}$, both orthonormal and right-handed. We denote by $x = (x_1,x_2,x_3)$ the coordinates of the robot’s centre of mass in frame $\mathcal{I}$. The body is propelled by a thrust vector f that is now assumed to point opposite to the body axis $b_3$. There are four control inputs: the magnitude u of the thrust vector and three torques $\tau _1,\tau _2,\tau _3$ about the three body axes. As in the planar case, we define the rotation matrix of frame $\mathcal{B}$ with respect to frame $\mathcal{I}$,

$$ R=[b_1 \ \ b_2 \ \ b_3]. $$

Fig. 6.7

3D flying robot

We pause for a moment to highlight a notable property of the rotation matrix R. Consider a vector with coordinates $(v_1,v_2,v_3)$ in frame $\mathcal{B}$—see Fig. 6.8. Now translate the tail of this vector to the origin of frame $\mathcal{I}$ and let $(w_1,w_2,w_3)$ be the coordinates in frame $\mathcal{I}$ of this translated vector. What is the relationship between the two coordinate representations $(v_1,v_2,v_3)$ and $(w_1,w_2,w_3)$? Bring in the unit vectors $b_i$ and note that

$$ \begin{bmatrix} w_1 \\ w_2 \\ w_3 \end{bmatrix} = v_1 b_1 + v_2 b_2 + v_3 b_3 = R \begin{bmatrix}v_1 \\ v_2 \\ v_3 \end{bmatrix}. $$

Fig. 6.8

Two coordinate representations in frames $\mathcal{I}$ and $\mathcal{B}$

In conclusion, the matrix R can be used to change the coordinate representation of vectors between frames $\mathcal{B}$ and $\mathcal{I}$.

We now return to modelling. The thrust vector f is proportional to $-b_3$ and has magnitude u. Therefore in the coordinates of frame $\mathcal{I}$ we have

$$ f= -u R e_3. $$

The gravity vector is parallel to the third axis of frame $\mathcal{I}$, and therefore its coordinate representation in frame $\mathcal{I}$ is $g e_3$. Newton’s equation gives

$$ m \ddot{x}= mg e_3 - u R e_3. $$

Next we need to model the rotation of the body. We begin with the observation that, since the columns of R form an orthonormal basis of ${\mathbb R}^3$, it holds that $R^T R = I_3$, where $I_3$ is the $3\times 3$ identity matrix. Now suppose that the body undergoes a rotation, so that R varies with time, $R=R(t)$. It still holds that $R(t)^T R(t) = I_3$ for all t. Differentiating both sides of this identity with respect to t, we obtain

$$ \dot{R}^T R + R^T \dot{R} = 0_{3\times 3}, $$

or

$$ ( R^T \dot{R} )^T = -( R^T \dot{R} ). $$

In other words, the matrix $R^T \dot{R}$ is skew-symmetric. Now, it is an easy exercise to show that an arbitrary $3 \times 3$ skew-symmetric matrix has the form

$$ \left[ \begin{array}{rrr} 0 &{} \ \ -\omega _3 &{} \ \ \omega _2 \\ \omega _3 &{} \ \ 0 &{} \ \ -\omega _1 \\ -\omega _2 &{} \ \ \omega _1 &{} \ \ 0 \end{array}\right] , $$

for suitable $\omega _1,\omega _2,\omega _3$. In fact, the set of $3 \times 3$ real skew-symmetric matrices

$$ \mathrm {se}(3)=\{ S \in {\mathbb R}^{3 \times 3}: S^T = -S\} $$

is a subspace of ${\mathbb R}^{3 \times 3}$, and the map ${\mathbb R}^3 \rightarrow \mathrm {se}(3)$,

$$ \omega \mapsto S(\omega ):= \left[ \begin{array}{rrr} 0 \ &{} \ \ -\omega _3 &{} \ \ \omega _2 \\ \omega _3 &{} \ \ 0 &{} \ \ -\omega _1 \\ -\omega _2 &{} \ \ \omega _1 &{} \ \ 0 \end{array}\right] $$

is an isomorphism of vector spaces. It is a fun exercise to prove the following fact: For any $\omega , v \in {\mathbb R}^3$, $S(\omega ) v$ is the vector (or cross) product of $\omega$ and v, $S(\omega )v = \omega \times v$.

So far we have established that corresponding to a rotation of the frame $\mathcal{B}$, there exists a unique time-dependent vector $\omega (t)$ such that $R^T \dot{R} = S(\omega )$. The vector $\omega$ is called the angular velocity of frame $\mathcal{B}$ with respect to frame $\mathcal{I}$ represented in frame $\mathcal{B}$. Using the coordinate transformation property of R described earlier, one gets that the angular velocity in frame $\mathcal{I}$ is $R \omega$, but this vector will not be needed in the following development. Since $R R^T =I$, multiplying both sides of the identity $R^T \dot{R} = S(\omega )$ on the left by R we get

$$ \dot{R} = R S(\omega ). $$

This is an ordinary differential equation whose state is the rotation matrix R. This equation models the kinematics of rotation of the robot in terms of its body frame angular velocity.

Now we need to model the evolution of $\omega$. Euler’s second law states that the rate of change of the angular momentum in frame $\mathcal{I}$ is equal to the total external torque applied to the robot. Let J denote the inertia matrix of the robot in the coordinates of frame $\mathcal{B}$, defined with respect to its centre of mass. We assume that the robot is a rigid body, so J is a constant matrix. The angular momentum of the robot in frame $\mathcal{B}$ is the vector $J \omega$. The representation of this vector in frame $\mathcal{I}$ is $R J \omega$. The robot is actuated by a torque vector $\tau =(\tau _1,\tau _2,\tau _3)$ in frame $\mathcal{B}$, or $R \tau$ in frame $\mathcal{I}$. Therefore, Euler’s second law gives

$$ \frac{d}{dt} (R J \omega ) = R \tau . $$

Multiplying both sides of the equation by $R^T$ and using the identity $R^T R = I$, we get

$$ R^T \frac{d}{dt} (R J \omega ) = \tau , $$

or

$$ R^T (\dot{R} J \omega + R J \dot{\omega }) = \tau . $$

Using the identity $\dot{R} = R S(\omega )$ and rearranging terms we obtain

$$ J \dot{\omega }+ S(\omega ) J \omega = \tau . $$

Since $S(\omega )v=\omega \times v$, the term $S(\omega )J\omega$ equals $\omega \times (J \omega )$.

In conclusion, the model¹ of the 2D flying robot is

$$\begin{aligned} \begin{aligned} m \ddot{x}&= mg e_3 - u R e_3 \\ \dot{R}&= R S(\omega ) \\ J \dot{\omega }&= - \omega \times ( J \omega ) + \tau . \end{aligned} \end{aligned}$$

(6.3)

The state is $(x,\dot{x},R,\omega )$. There are four control inputs: the magnitude u of the thrust vector and the three torques $\tau =(\tau _1,\tau _2,\tau _3)$.

6.2.3 Special Case: Quadrotor Helicopters

A quadrotor helicopter can be regarded as a rigid body propelled by a force vector with constant direction in its body frame. Moreover as we have seen in Sect. 6.1, with a judicious choice of the rotor speeds one may induce desired torques about the three body axes. Quadrotor helicopters, therefore, fit within the class of robots modelled by (6.3). How are the controls $(u,\tau )$ in (6.3) related to the physical control inputs of a quadrotor helicopter? In this section we answer this question.

Consider the quadrotor helicopter of Fig. 6.9. The four rotors are driven by electric motors applying torques $\tau _{r_i}$, $i=1,\ldots ,4$. These can be regarded as the physical control inputs. We seek a relationship between these inputs and the controls $(u,\tau )$ of the model (6.3), also depicted in Fig. 6.9. To this end, denote by $\omega _{r_i}$ the angular speed of rotor i. Then it can be shown that each rotor is modelled by

$$ J_{r_i} \dot{\omega }_{r_i} = -B \omega _{r_i}^2 + \tau _{r_i}, \ i=1,\ldots , 4, $$

where the term $-B \omega _{r_i}^2$ represents a torque due to the drag effects on the rotor blades. In practice the moment of inertia $J_{r_i}$ is negligible, so it is common to use a singular perturbation argument and set $J_{r_i}\dot{\omega }_{r_i} = 0$, which gives $\tau _{r_i} = B \omega _{r_i}^2$. The lift force produced by rotor i is

$$\begin{aligned} f_{r_i} = k \omega _{r_i}^2 =\frac{k}{B}\tau _{r_i}. \end{aligned}$$

(6.4)

Recall from Sect. 6.1 that

$$ \begin{aligned} u&=f_{r_1}+f_{r_2}+f_{r_3}+f_{r_4} \\ \tau _1&= l (f_{r_1}-f_{r_2}) \\ \tau _2&= l (f_{r_3} - f_{r_4}) \\ \tau _3&= \tau _{r_1}+\tau _{r_2} -\tau _{r_3} - \tau _{r_4}. \end{aligned} $$

Using (6.4) we obtain an invertible feedback transformation $(\tau _{r_1},\ldots , \tau _{r_4}) \mapsto (u,\tau )$:

$$\begin{aligned} \begin{bmatrix} u \\ \tau _1 \\ \tau _2 \\ \tau _3 \end{bmatrix} = \frac{k}{B} \left[ \begin{array}{rrrr} 1 &{} 1 &{} 1 &{} 1 \\ l &{} \ \ -l &{} 0 &{} 0 \\ 0 &{} 0 &{} l &{} -l \\ \frac{B}{k} &{} \frac{B}{k} &{} \ \ -\frac{B}{k} &{} \ \ -\frac{B}{k} \end{array}\right] \begin{bmatrix} \tau _{r_1} \\ \tau _{r_2} \\ \tau _{r_3} \\ \tau _{r_4} \end{bmatrix}. \end{aligned}$$

(6.5)

Fig. 6.9

The physical inputs $\tau _{r_i}$ of a quadrotor helicopter and the corresponding controls $u,\tau _1,\tau _2,\tau _3$ used in model (6.3)

In conclusion, the model of a quadrotor helicopter with control inputs $(\tau _{r_1},\ldots , \tau _{r_4})$ is given by (6.3) with $(u,\tau )$ given in (6.5).

6.3 Flocking of 2D Flying Robots

We now turn to the most basic coordination problem, flocking. We begin with the 2D case. Thus we have n robots modelled by

$$\begin{aligned} \begin{aligned} m_i\ddot{x}_i&= u_i R_i e_1\\ \dot{R}_i&= R_i S(\omega _i) \\ J_i \dot{\omega }_i&= \tau _i . \end{aligned} \quad i =1,\ldots , n. \end{aligned}$$

(6.6)

A clarification about notation. From now on, a subscript i on a quantity indicates that the quantity pertains to the ith robot.² In particular, $\mathcal{B}_i$ is the body frame of robot i. We let $\chi _i:=(x_i,\dot{x}_i,R_i,\omega _i)$ denote the state of the ith robot, and $\chi :=(\chi _1,\ldots ,\chi _n)$ denote the collective state.

As in Chap. 3, we indicate by $\mathcal{N}_i(\chi )$ the set of neighbours of robot i, and we rely on the visibility graph $\mathcal{G}(\chi )$ to keep track of who can see whom.

Before delving into control design, we need to clarify what is an admissible control for a flying robot. As discussed in Sect. 6.1, the onboard sensors of robot i can be used to deduce robot i’s relative orientation with respect to any neighbour $k \in \mathcal{N}_i(\chi )$ and the relative displacement measured in the local frame $\mathcal{B}_i$. These quantities are illustrated in Fig. 6.10. We may also assume the time derivatives of the above quantities to be available for feedback. Moreover, one of the rate gyroscopes in the IMU measures $\omega _i$. In conclusion, any control law $(u_i,\tau _i)$ will be called admissible for robot i if it is a locally Lipschitz function depending only on the quantities

$$ \begin{aligned}&\langle x_k - x_i,r_i \rangle , \ \ \langle x_k - x_i,s_i \rangle , \ \ \langle r_k, r_i\rangle , \ \ \langle r_k,s_i \rangle , \\&\langle \dot{x}_k - \dot{x}_i,r_i \rangle , \ \ \langle \dot{x}_k - \dot{x}_i,s_i\rangle , \ \ \omega _i, \end{aligned} $$

for each $k \in \mathcal{N}_i(\chi )$.

Fig. 6.10

Relative displacement and orientation between robots i and k as measured by robot i

An equivalent representation of the measured quantities above is obtained by leveraging the coordinate transformation property of rotation matrices. For instance, the relative displacement between robots i and k measured in robot i’s local frame is obtained by converting the inertial displacement $x_k - x_i$ to the coordinates of frame $\mathcal{B}_i$,

$$ (R_i)^{-1} (x_k-x_i) = R_i^T (x_k-x_i) . $$

The relative orientation between robot i and robot k is $R_i^T R_k$ (you can check that this matrix corresponds to a counterclockwise rotation by angle $\theta _k-\theta _i$). In essence, the measured quantities above can be represented as

$$ R_i^T (x_k-x_i), \ \ R_i^T (\dot{x}_k-\dot{x}_i), \ \ R_i^T R_k, \ \ \omega _i. $$

As in Chap. 3, the flocking problem for the robots in (6.6) is to find, if they exist, admissible controls $(u_i,\tau _i)$ so that there exists $\varepsilon >0$ such that for all initial conditions satisfying

• (i) $(\forall i,j=1,\ldots ,n) \ |\dot{x}_i(0) - \dot{x}_j(0) | < \varepsilon , | \theta _i(0) - \theta _j(0) | <\varepsilon , |\dot{\theta }_i(0)| < \varepsilon$,

• (ii) $\mathcal{G}(\chi (0))$ is connected,

then

$$ (\exists v_{ss}) \lim _{t \rightarrow \infty } \dot{x}_i(t) = v_{ss},\quad \text { for all } i=1,\ldots ,n. $$

In other words, the flying robots are required to move asymptotically along parallel straight lines whose slope depends on their initial conditions.

As for kinematic unicycles, there is currently no solution to the flocking problem. The main difficulty is the requirement of preserving the connectivity of the visibility graph, a problem of considerable difficulty even when the robots are modelled as kinematic integrators (see Chap. 5).

In the interest of a self-contained mathematical treatment, we make two assumptions. First, we assume that the visibility graph is constant (the set of neighbours of each robot does not change with time) and undirected (if robot i can see robot j, then robot j can see i). Second, we assume that each robot is affected by a drag force pointing opposite to its velocity vector:

$$ m_i \ddot{x}_i = u_i R_i e_1 - b \dot{x}_i, \quad b >0. $$

The above assumptions considerably simplify the flocking problem, for they allow us to discard the translational dynamics and focus on the subsystem

$$ \begin{aligned} \dot{R}_i&= R_i S(\omega _i) \\ J_i \dot{\omega }_i&= \tau _i, \quad i=1,\ldots , n, \end{aligned} $$

or, equivalently,

$$\begin{aligned} J_i \ddot{\theta }_i = \tau _i, \quad i = 1, \ldots , n. \end{aligned}$$

(6.7)

Indeed, set $u_1=\cdots =u_n=\bar{u}>0$ and suppose we were to design admissible controls $\tau _i$ making the angles $\theta _i$ converge to a common constant value $\theta _{ss}$. Then the translational motion of each robot would be described by

$$ m_i \frac{d}{dt} \dot{x}_i = - b \dot{x}_i + \bar{u} \begin{bmatrix} \cos (\theta _{ss}) \\ \sin (\theta _{ss}) \end{bmatrix} + \bar{u} \delta _i(t), $$

where $\delta _i(t)= (\cos (\theta _i(t)) - \cos (\theta _{ss}), \sin (\theta _i(t)) - \sin (\theta _{ss}))$ is a vanishing signal. The above differential equation is a stable linear time invariant system driven by an input signal converging to the constant vector. For this system we have $\dot{x}_i \rightarrow v_{ss}$, with $v_{ss} = \bar{u} /b (\cos (\theta _{ss}), \sin (\theta _{ss}))$, and the robots flock. The situation is illustrated in the block diagrams of Fig. 6.11.

Fig. 6.11

Block diagram of 2D flying robots with synchronization of the heading angles. In the figure, f is the vector of thrust forces $f_i = u_i R_i e_1$

To summarize, the flocking problem has been reduced to finding controls $\tau _i$ for system (6.7) making $(\theta _i,\dot{\theta }_i) \rightarrow (\theta _{ss},0)$, $i=1,\ldots , n$. An admissible control law for the reduced model (6.7) is one that relies only on $\theta _i - \theta _k$, $k \in \mathcal{N}_i$, and $\dot{\theta }_i$.

Our approach in the control design that follows will be to asymptotically stabilize the flocking manifold

$$\begin{aligned} \mathcal{F}=\{(\theta _1,\ldots ,\theta _n,\dot{\theta }_1,\ldots , \dot{\theta }_n): \theta _1 =\cdots = \theta _n, \dot{\theta }_1 =\cdots = \dot{\theta }_n=0\}, \end{aligned}$$

(6.8)

and then show that the $\theta _i$’s do not simply converge to each other, but they converge to a constant.

At this point, you may want to pause for a moment and compare the flocking problem under consideration to the one in Chap. 3. The model (6.7) is a rotational double-integrator, while the model (3.1) in Chap. 3 is a single rotational integrator. Since the control input here is an acceleration rather than a speed, we cannot directly apply the Kuramoto-inspired control law (3.5) to solve the flocking problem. Not surprisingly, though, the solution we are about to present is a straightforward adaptation of the Kuramoto-inspired control law.

Inspired by [11, 12], we begin our control design by a mechanical analogy. Consider the point particle in Fig. 6.12. The particle has mass $J_i$ and is constrained to move around a circular track of radius 1 metre. A force $\tau _i$ is applied to the particle in the direction tangent to the circle. The motion of such a particle is described by (6.7). Now imagine a collection of such masses moving on a common unit circle with the same visibility graph $\mathcal{G}$ as the flying robots. Solving the flocking problem for the flying robots is equivalent to determining admissible forces $\tau _i$ synchronizing the particles on the unit circle.

Fig. 6.12

Mechanical analogy for flocking control design

Fig. 6.13

Mechanical analogy for synchronization mechanism

Consider the three particles i, k, l in Fig. 6.13, with particles k, l neighbours of particle i. If we connect the particles by massless springs and add a damping force to each mass, it is reasonable to expect that the particles will synchronize provided their initial conditions are not too far apart and there are enough springs (this latter requirement will translate to requiring that the visibility graph be connected).

What is the control law $\tau _i$ corresponding to the setup of Fig. 6.13? The answer is in the next lemma.

Lemma 6.1

Consider n point particles constrained to move on the unit circle. Suppose the particles are connected by springs as described above, and each particle is subjected to a viscous friction force. The model of the particles is

$$\begin{aligned} J_i \ddot{\theta }_i = - b_i \dot{\theta }_i - \sum _{k\in \mathcal{N}_i} a_{ik} \sin (\theta _i - \theta _k), \quad i=1,\ldots ,n. \end{aligned}$$

(6.9)

Proof

In order to avoid the computation of the reaction forces arising from the fact that the particles move on a unit circle we take the Lagrangian approach.

The total kinetic energy of the collection of particles is

$$ K= \sum _{l=1}^n \frac{1}{2} J_l \, (\dot{\theta }_l)^2. $$

Letting $d_{lk}$ denote the length of the chord connecting robots l and k and $a_{lk}>0$ denote the associated spring constant (in what follows, we pick $a_{kl} = a_{lk}$), we find that the total potential energy is the sum of the potential energies of the springs:

$$ U =\sum _{l=1}^n \sum _{k \in \mathcal{N}_l, k<l} \frac{1}{2} a_{lk} \, d_{lk}^2. $$

The condition $k<l$ in the inner sum guarantees that the potential energy of the spring connecting robots l and k is counted only once. It can be shown that $d_{lk}^2 = 2 - 2 \cos (\theta _l-\theta _k)$. Substituting this expression into U, we obtain the Lagrangian

$$ \mathcal{L} = K - U = \sum _{l=1}^n \left[ \frac{1}{2} J_l (\dot{\theta }_l)^2 - \sum _{k \in \mathcal{N}_l, k<l} a_{lk} (1 - \cos (\theta _l - \theta _k) )\right] . $$

This is the Lagrangian of the system of particles subject to the spring forces.

Using the Euler–Lagrange equation

$$ \frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot{\theta }_i} - \frac{\partial \mathcal{L}}{\partial \theta _i} =0, \quad i=1,\ldots , n, $$

we get (the steps are omitted)

$$ J_i \ddot{\theta }_i + \sum _{k\in \mathcal{N}_i} a_{ik} \sin (\theta _i - \theta _k)=0, \quad i=1,\ldots ,n. $$

Adding viscous friction to each particle, we obtain (6.9).   $\square$

Comparing (6.7) and (6.9) we deduce the control law for robot i:

$$\begin{aligned} \tau _i = - b_i \dot{\theta }_i - \sum _{k\in \mathcal{N}_i} a_{ik} \sin (\theta _i - \theta _k). \end{aligned}$$

(6.10)

This control law is admissible. Does it solve the flocking problem? The answer is found in the next theorem.

Theorem 6.1

If the visibility graph is constant, undirected, and connected, then for any $a_{ik}=a_{ki}>0, \ b_i>0$, $i,k=1,\ldots ,n$, the control law (6.10) makes the flocking manifold $\mathcal{F}$ in (6.8) locally asymptotically stable for (6.7) and solves the flocking problem.

Proof

The total energy of the particles,

$$ E = K + U = \sum _{i=1}^n \left[ \frac{1}{2} J_i (\dot{\theta }_i)^2 + \sum _{k \in \mathcal{N}_i, k<i} a_{ik} (1 - \cos (\theta _i - \theta _k) )\right] , $$

is a nonnegative function. We claim that the level set $\{E=0\}$ coincides with the flocking manifold $\mathcal{F}$ in (6.8). Indeed, since the energy is a sum of nonnegative terms, $E=0$ if and only if

$$ \dot{\theta }_i=0 \text { and } 1 - \cos (\theta _i - \theta _k) =0, \quad i =1,\ldots , n, \quad k \in \mathcal{N}_i, \quad k<i. $$

Thus $\theta _i = \theta _k$ for all $i \in \{1,\ldots , n\}$ and all $k \in \mathcal{N}_i$, $k<i$. Since the visibility graph is connected, this latter condition is equivalent to $\theta _1 = \cdots = \theta _n$. Thus $\{E=0\} = \mathcal{F}$ and the claim is proved.

The time derivative of E along solutions is given by

$$ \dot{E} = - \sum _{i=1}^n b_i ( \dot{\theta }_i)^2 \le 0. $$

Thus the energy is nonincreasing along solutions, which implies that the flocking manifold $\mathcal{F}$ is stable.

Let , and for a given $\gamma \in (0,\pi /2)$, define the energy sublevel set

Since E is nonincreasing along solutions, $E_\gamma$ is positively invariant for any $\gamma$, that is, all solutions of (6.9) initialized in $E_\gamma$ remain there for all $t \ge 0$. Moreover, solutions in $E_\gamma$ satisfy $(1 - \cos (\theta _i(t) - \theta _k(t)))<(1 - \cos (\gamma ))$, or

$$\begin{aligned} (\forall t \ge 0) (\forall i\in \{1,\ldots ,n\}) (\forall k \in \mathcal{N}_i) \ | \theta _ i (t) - \theta _k(t) | < \gamma . \end{aligned}$$

(6.11)

To visualize $E_\gamma$ note that, in it, neighbouring particles lie on an arc of the unit circle whose central angle is $2 \gamma$ —see Fig. 6.14. Moreover, their speeds have a bound that depends on $\gamma$. We will see that if the arc in question and the speeds are sufficiently small (i.e., $\gamma$ is small), then the particles synchronize.

Fig. 6.14

Interpretation of set $E_\gamma$

By the LaSalle invariance principle,³ solutions of (6.9) initialized in $E_\gamma$ converge to the largest invariant subset of $E_\gamma \cap \{ \dot{\theta }_i=0, i=1,\ldots , n\}$. We will show that for sufficiently small $\gamma$, this set is contained in the flocking manifold $\mathcal{F}$. To this end, note that if $\dot{\theta }_i(t) \equiv 0$, then $\ddot{\theta }_i(t) \equiv 0$, implying that $\sum _{k \in \mathcal{N}_i} a_{ik} \sin (\theta _i - \theta _k) \equiv 0$. Define a Laplacian matrix L as follows:

$$ \begin{aligned} L_{ik}&:=-a_{ik},&k \in \mathcal{N}_i, k \ne i, \\ L_{ik}&:=0,&k \not \in \mathcal{N}_i, k \ne i, \\ L_{ii}&:= \sum _{k \in \mathcal{N}_i} a_{ik}. \end{aligned} $$

Then L is the Laplacian of the visibility graph $\mathcal{G}$ with positive weights $a_{ik}$ on the graph’s edges. Since⁴ $\mathcal{G}$ is connected, L has rank $n-1$ and $\ker L$ is spanned by $\mathbf {1}$.

Let $\theta = (\theta _1,\ldots , \theta _n)$ and define the vector function $r(\theta )$ as

$$ r_i(\theta ) :=\sum _{k \in \mathcal{N}_i} a_{ik} \left( \sin (\theta _i - \theta _k)- (\theta _i - \theta _k)\right) , \quad i=1,\ldots , n. $$

Then the condition $\sum _{k \in \mathcal{N}_i} a_{ik} \sin (\theta _i - \theta _k) \equiv 0$, $i=1,\ldots ,n$, may be expressed as

$$ L \theta + r(\theta )=0. $$

The above identity can hold only if $\Vert L \theta \Vert = \Vert r(\theta )\Vert$. It can be shown that

$$ \lim _{L \theta \rightarrow 0} \frac{r_i(\theta )}{\Vert L \theta \Vert } =0. $$

Therefore, for sufficiently small $\gamma$, the property (6.11) implies that

$$ \Vert r (\theta ) \Vert \le \Vert L \theta \Vert /2. $$

For such small $\gamma$, the unique solution of

$$ L \theta + r(\theta ) =0 $$

is $L\theta =0$, or $\theta _1 = \cdots = \theta _n$.

We have thus shown that the largest invariant subset of $E_\gamma \cap \{\dot{\theta }_i=0, i = 1,\ldots , n\}$ is contained in $\mathcal{F}$. By the LaSalle invariance principle, the flocking manifold $\mathcal{F}$ is locally asymptotically stable, and the set $E_\gamma$ is contained in its domain of attraction.

We are left to show that $\theta _i$ converges to a constant. This part requires a little more work, and it involves the centre manifold theorem. We omit the argument, but you may look at the proof of Theorem 4 in [20] to get the idea.   $\square$

6.4 Flocking of 3D Flying Robots

In this section we generalize the flocking control law to 3D flying robots. The generalization relies on the same mechanical analogy but now, instead of imagining point particles on the unit circle, we imagine rigid bodies on the unit sphere.

Consider a collection of n robots, each one modelled by (6.3). As before, the flocking problem⁵ is to find an admissible control law making the velocity vectors $\dot{x}_i$ converge to a common constant vector $v_{ss}$ dependent on the initial conditions. We assume, as before, that the visibility graph is constant and undirected, and that each robot is affected by a drag force pointing opposite to its velocity vector. We now also assume that the robots have identical masses m, so that their translational motions are modelled by

$$ m \ddot{x}_i = m g e_3 - u_i R_i e_3 - b \dot{x}_i. $$

As in the 2D case, if we set $u_1 = \cdots = u_n = \bar{u}>0$ and we make $R_i e_3$ converge to a common constant vector, then the velocities $\dot{x}_i$ converge to a common constant vector as well, and flocking is achieved. The problem has thus been reduced to the synchronization of the vectors $R_i e_3$, $i=1,\ldots , n$, which we will refer to as the thrust axes of the robots. For the design of flocking controllers we focus our attention on the rotational dynamics

$$\begin{aligned} \begin{aligned} \dot{R}_i&= R_i S(\omega _i) \\ J_i \dot{\omega }_i&+ \omega _i \times ( J_i \omega _i) = \tau _i, \quad i =1,\ldots , n. \end{aligned} \end{aligned}$$

(6.12)

Fig. 6.15

Coordinate representations of the thrust axes of robots i and j

Before continuing our development, we introduce some useful notation, illustrated in Fig. 6.15. We let $q_i :=R_i e_3$ denote the thrust axis of robot i represented in the common inertial frame $\mathcal{I}$, and by $R_j^i:= R_i^T R_j$ the relative orientation of robot j with respect to robot i. Operationally, $R_j^i$ transforms a vector in frame j to its representation in frame i. We let $q_j^i:=R_j^i e_3$ denote the representation of robot j’s thrust axis in the local frame of robot i. For flocking we would like to have $q_j^i=e_3$ for all i, j. Finally, we denote by $\varphi _j^i$ the angle between the third axis of frame $\mathcal{B}_i$ and $q_j^i$. Flocking corresponds to the condition $\varphi _j^i=0$ for all i, j.

With the notation just introduced, we have three equivalent definitions of the flocking manifold:

$$\begin{aligned} \mathcal{F} :&=\{(R_1,\ldots ,R_n,\omega _1,\ldots ,\omega _n): q_i = q_j, \ \omega _i =0, \ i,j = 1,\ldots , n\} \nonumber \\&=\{(R_1,\ldots ,R_n,\omega _1,\ldots ,\omega _n): q_j^i=e_3, \ \omega _i =0, \ i,j = 1,\ldots , n\} \nonumber \\&=\{(R_1,\ldots ,R_n,\omega _1,\ldots ,\omega _n): \varphi _j^i =0, \ \, \omega _i=0, \ i,j = 1,\ldots , n\}. \end{aligned}$$

(6.13)

Thus, on $\mathcal{F}$, the robots have zero angular velocity and identical thrust axes. This latter requirement is expressed in three equivalent ways in (6.13). The first identity expresses it in the common inertial frame $\mathcal{I}$, while the second and third identities express it in the robots’ body frames.

In the context of 3D flying robots, an admissible control law for the rotational dynamics of robot i is one that depends only on the quantities

$$ R_k^i, \ \ \omega _i, \ \ k \in \mathcal{N}_i $$

and is a locally Lipschitz function.

Since the vectors $q_i$ have unit length, the flocking problem can be regarded as one of synchronization on the unit sphere. In analogy with the 2D case, we imagine that the thrust axis $q_i$ is the position of the centre of mass of a rigid body on the unit sphere. We regard the control $\tau _i$ as a force applied at the centre of mass of the body in a direction tangent to the sphere. Once again, we aim to synchronize these imaginary rigid bodies by connecting them by springs and adding damping to each of them, with the convention that a spring with constant $a_{ik}>0$ interconnects bodies i and k if and only if there is an edge between nodes i and k of the visibility graph. It is an easy matter to check that the length of this spring is $\sqrt{ 2 (1 - \langle q_k^i, e_3\rangle )}$.

The total potential energy of the collection of springs is

$$ U = \sum _{i=1}^n \sum _{k \in \mathcal{N}_i, k<i} a_{ik} (1- \langle q_k^i, e_3\rangle ). $$

The total energy of the robots connected by springs is

$$ E = \sum _{i=1}^n \frac{1}{2} \omega _i^T J_i \omega _i + \sum _{i=1}^n \sum _{k \in \mathcal{N}_i, k<i} a_{ik} (1- \langle q_k^i, e_3\rangle ). $$

Inspired by the proof of Theorem 6.1, we define a control law for system (6.12) by requiring that

$$\begin{aligned} \dot{E} = - \sum _{i=1}^n \omega _i^T B_i \omega _i, \end{aligned}$$

(6.14)

where $B_i$ is a symmetric positive definite matrix. By imposing this identity, we get the control law

$$\begin{aligned} \tau _i = -B_i \omega _i - \sum _{k\in \mathcal{N}_i} a_{ik} (q_k^i \times e_3), \end{aligned}$$

(6.15)

which can also be expressed as

$$ \tau _i= -B_i \omega _i - R_i^T\sum _{k\in \mathcal{N}_i} a_{ik} (q_k \times q_i). $$

We postpone the verification that this control law gives identity (6.14) to the proof of Theorem 6.2. Note that this control law is admissible, as it relies only on $R_i^k, k \in \mathcal{N}_i$ and $\omega _i$.

At this point you may want to reflect on the similarity between (6.15) and its 2D counterpart (6.10). As an exercise, show that (6.10) is a special case of (6.13), in the following sense. Suppose the third body frame axis of each robot is parallel to the third axis of the inertial frame $\mathcal{I}$, i.e., $R_i e_3 = e_3$. Thus the rotation matrix of robot i has the form

$$ R_i = \left[ \begin{array}{rrr} \cos (\theta _i) &{} -\sin (\theta _i) &{} \ \ \ 0\\ \sin (\theta _i) &{} \cos (\theta _i) &{} 0\\ 0 \ \ \ \ &{} 0 \ \ \ \ &{} 1 \end{array} \right] . $$

Suppose further that the thrust axis is parallel to the first body axis rather than the third. Thus $q_j^i = R_j^i e_1$. Finally, let $B_i$ be a diagonal matrix with $(B_i)_{33} = b_i$. Then $\tau _i$ in (6.15) has the form $\tau _i = (0,0,(\tau _i)_3)$, where $(\tau _i)_3$ coincides with (6.10).

Theorem 6.2

If the visibility graph is constant, undirected, and connected, then for every $a_{ik}= a_{ki}>0$ and every symmetric positive definite matrix $B_i$, $i,k=1,\ldots ,n$, the control law (6.15) makes the flocking manifold $\mathcal{F}$ in (6.13) locally asymptotically stable for (6.12) and solves the flocking problem.

Proof

The energy E is nonnegative. It is zero if and only if $\omega _i=0$ and $\langle q_k^i,e_3 \rangle =1$ for all $i=1,\ldots , n$ and $j \in \mathcal{N}_i$. Since $q_k^i$ is a unit vector, the latter condition is equivalent to $q_k^i = e_3$. Since the visibility graph is connected, this identity is true for all $i,k =1,\ldots , n$, and therefore the level set $\{E = 0\}$ coincides with the flocking manifold $\mathcal{F}$ in (6.13).

Next we show that (6.14) holds. We have

$$ \dot{E} = \sum _{i=1}^n \omega _i^T J_i \dot{\omega }_i - \sum _{i=1}^n \sum _{k\in \mathcal{N}_i, k<i} a_{ik} \langle \dot{q}_k^i,e_3 \rangle . $$

Using the dynamics (6.12) with control (6.15), we get

$$ \dot{E} = - \sum _{i=1}^n \omega _i^T B_i \omega _i - \sum _{i=1}^n \sum _{k\in \mathcal{N}_i} a_{ik} \omega _i^T (q_k^i \times e_3) - \sum _{i=1}^n \sum _{k\in \mathcal{N}_i, k<i} a_{ik} \langle \dot{q}_k^i,e_3 \rangle . $$

Recalling the definition of $q_k^i$, $q_k^i = R_i^T R_k e_3$, and differentiating this identity with respect to time, we obtain

$$ \begin{aligned} \dot{q}_k^i&= \dot{R}_i^T R_k e_3 + R_i^T \dot{R}_k e_3 \\&=S(\omega _i)^T R_i^T R_k e_3 + R_i^T R_k S(\omega _k) e_3 \\&=-S(\omega _i) q_k^i + R_k^i S(\omega _k) e_3. \end{aligned} $$

In the second identity we used the derivative of a rotation matrix in (6.12). For any rotation matrix R and any vector $v \in {\mathbb R}^3$, we have

$$ R S(v) R^T = S(Rv). $$

Using this identity and the linearity of the operator S, we obtain

$$ \begin{aligned} \dot{q}_k^i&= S(-\omega _i + R_k^i \omega _k) q_k^i \\&=(-\omega _i + R_k^i \omega _k) \times q_k^i \\&=q_k^i \times (\omega _i - R_k^i \omega _k) \\&=S(q_k^i) (\omega _i - R_k^i \omega _k). \end{aligned} $$

We now take the inner product with $e_3$:

$$ \begin{aligned} \langle \dot{q}_k^i, e_3 \rangle&= (\omega _i - R_k^i \omega _k)^T S(q_k^i)^T e_3 \\&=- (\omega _i - R_k^i \omega _k)^T S(q_k^i) e_3 \\&=- \omega _i^T S(q_k^i) e_3 + \omega _k^T (R_k^i)^T S(q_k^i) e_3 \\&= - \omega _i^T (q_k^i \times e_3) + \omega _k^T S((R_k^i)^T q_k^i) (R_k^i)^T e_3 \\&= - \omega _i^T (q_k^i \times e_3) + \omega _k^T S(e_3) q_i^k \\&= - \omega _i^T (q_k^i \times e_3) - \omega _k^T ( q_i^k \times e_3). \end{aligned} $$

Using this result in the third term of the equation for $\dot{E}$, we get

$$ \begin{aligned} \dot{E}&= - \sum _{i=1}^n \omega _i^T B_i \omega _i - \sum _{i=1}^n \sum _{k\in \mathcal{N}_i} a_{ik} \omega _i^T (q_k^i \times e_3) \\&\quad + \sum _{i=1}^n \sum _{k\in \mathcal{N}_i, k<i} a_{ik} \omega _i^T (q_k^i \times e_3) \\&\quad + \sum _{i=1}^n \sum _{k\in \mathcal{N}_i, k<i} a_{ik} \omega _k^T ( q_i^k \times e_3). \end{aligned} $$

Since the graph is undirected and $a_{ik} = a_{ki}$, the last sum can be rewritten as

$$ \sum _{k=1}^n \sum _{i\in \mathcal{N}_k, i>k} a_{ki} \omega _k^T ( q_i^k \times e_3). $$

Now swap the indices i and k. Then the last three sums in the above expression for $\dot{E}$ evaluate to zero, and

$$ \dot{E} = - \sum _{i=1}^n \omega _i^T B_i \omega _i, $$

as required.

Let , and for a given $\gamma \in (0,\pi /2)$, define the energy sublevel set As in the proof of Theorem 6.1, $E_\gamma$ is positively invariant and all solutions initialized in it satisfy $\langle q_k^i(t),e_3 \rangle = \cos \varphi _k^i(t)> \cos \gamma$, or

$$ (\forall t \ge 0) (\forall i \in \{1,\ldots , n\}) (\forall k \in \mathcal{N}_i) \ |\varphi _k^i(t)| < \gamma . $$

Since the visibility graph $\mathcal{G}$ is connected, we may pick $\gamma$ small enough that

$$ \Big ( (\forall i \in \{1,\ldots , n\}) (\forall k \in \mathcal{N}_i) \ |\varphi _k^i| < \gamma \Big ) \implies \max _{i,k \in \{1,\ldots , n\}}\{|\varphi _k^i|\} < \frac{\pi }{2}. $$

Thus all solutions initialized in $E_\gamma$ have the property that their thrust axes $q_i(t)$ lie on a common half plane for all time. More precisely, for all initial conditions in $E_\gamma$, the solutions of (6.12) with control (6.15) satisfy

$$\begin{aligned} (\forall t \ge 0) (\exists v \in {\mathbb R}^n, v \ne 0) (\forall i \in \{1,\ldots , n\}) \ \langle q_i(t), v \rangle >0. \end{aligned}$$

(6.16)

By (6.14) the energy E is nonincreasing along solutions. Since its zero level set is the flocking manifold $\mathcal{F}$, $\mathcal{F}$ is a stable set. By the LaSalle invariance principle,⁶ all solutions of the closed-loop system initialized in $E_{\gamma }$ converge to the largest invariant set contained in $E_\gamma \cap \{\omega _i=0, i=1,\ldots ,n\}$. We will show that this set is contained in the flocking manifold.

If $\omega _i(t) \equiv 0$, then $\dot{\omega }_i(t) \equiv 0$ and so $J_i \dot{\omega }_i(t) \equiv 0$, implying that the control $\tau _i$ in (6.15) is identically zero. Thus, we have

$$ (\forall i \in \{1,\ldots ,n\}) \sum _{k \in \mathcal{N}_i} a_{ik} (q_k^i(t) \times e_3) \equiv 0. $$

Premultiplying this identity by $R_i(t)$ we get

$$ (\forall i \in \{1,\ldots ,n\}) \sum _{k \in \mathcal{N}_i} a_{ik} (q_k(t) \times q_i(t)) \equiv 0, $$

or

$$ (\forall i \in \{1,\ldots ,n\}) \ q_i(t) \times \left( \sum _{k \in \mathcal{N}_i} a_{ik} q_k(t) \right) \equiv 0. $$

Thus $q_i(t)$ and $\sum _{k \in \mathcal{N}_i} a_{ik} q_k(t)$ are parallel, i.e., there exists a scalar $\lambda _i(t)$ such that

$$\begin{aligned} q_i(t) = \lambda _i(t) \sum _{k \in \mathcal{N}_i} a_{ik} q_k(t). \end{aligned}$$

(6.17)

We claim that $\lambda _i(t) >0$. Indeed, taking the inner product of both sides of (6.17) with the vector v in (6.16) we obtain

$$ \langle q_i(t), v \rangle =\lambda _i(t) \sum _{k \in \mathcal{N}_i} a_{ik} \langle q_k(t),v \rangle . $$

By property (6.16), the left-hand side is positive. The sum in the right-hand side is also positive because $a_{ik} >0$ and each inner product is positive. Thus it must hold that $\lambda _i(t) >0$, as claimed. Since $q_i$ has unit norm we conclude that

$$\begin{aligned} \lambda _i(t) = \left( \Big \Vert \sum _{k \in \mathcal{N}_i} a_{ik} q_k(t) \Big \Vert \right) ^{-1}. \end{aligned}$$

(6.18)

From now on, we drop the time dependence on all variables. Consider the following weighted sum of the $q_i$’s, and use identity (6.17):

$$ \sum _{i=1}^n \left( \sum _{k \in \mathcal{N}_i} a_{ik} \right) q_i = \sum _{i=1}^n \lambda _i \left( \sum _{k \in \mathcal{N}_i} a_{ik}\right) \sum _{l \in \mathcal{N}_i} a_{il} q_l. $$

The right-hand side of this identity is a linear combination of $\{q_1,\ldots ,q_n\}$. A generic term $q_j$ appears in the right-hand side when $l=j$ and $j \in \mathcal{N}_i$, or since the visibility graph is undirected, $l=j$, $i \in \mathcal{N}_j$. Thus the above identity can be rewritten as

$$ \sum _{i=1}^n \left( \sum _{k \in \mathcal{N}_i} a_{ik} \right) q_i = \sum _{j=1}^n \mu _j q_j, $$

with

$$ \mu _j = \sum _{i \in \mathcal{N}_j} \lambda _i \left( \sum _{k \in \mathcal{N}_i} a_{ik} \right) a_{ij}. $$

We thus have

$$\begin{aligned} \sum _{i=1}^n \left[ \left( \sum _{k \in \mathcal{N}_i} a_{ik} \right) - \mu _i \right] q_i=0. \end{aligned}$$

(6.19)

Consider the definition of $\lambda _i$ in (6.18). Since the $q_i$’s have unit norm and $a_{ik}>0$, by the triangle inequality we have

$$ \lambda _i \ge \left( \sum _{k \in \mathcal{N}_i} a_{ik} \right) ^{-1}. $$

This lower bound on $\lambda _i$ gives a lower bound on $\mu _j$,

$$ \mu _j \ge \sum _{i \in \mathcal{N}_j} a_{ij}. $$

Thus the coefficient of $q_i$ in (6.19) is upper bounded as follows

$$ \left( \sum _{k \in \mathcal{N}_i} a_{ik}\right) - \mu _i \le \sum _{k \in \mathcal{N}_i} a_{ik} - \sum _{k \in \mathcal{N}_i} a_{ki} =0. $$

In conclusion, the left-hand side of (6.19) is a linear combination of all $q_i$’s with nonpositive coefficients. By property (6.16), identity (6.19) can hold only if

$$ \left( \sum _{k \in \mathcal{N}_i} a_{ik}\right) - \mu _i =0\quad \text { for all } i=1,\ldots , n, $$

which implies that $\lambda _i = \left( \sum _{k \in \mathcal{N}_i} a_{ik} \right) ^{-1}$, $i=1,\ldots , n$, or

$$ \Big \Vert \sum _{k \in \mathcal{N}_i} a_{ik} q_k \Big \Vert = \sum _{k \in \mathcal{N}_i} a_{ik} \quad \text { for all } i=1,\ldots , n. $$

This identity implies that, for each $i \in \{1,\ldots , n\}$, the vectors $\{q_k: k \in \mathcal{N}_i\}$ are parallel to each other. By (6.17), the vectors $\{q_i, q_k: k \in \mathcal{N}_i\}$ are also parallel. We have thus established that if robots i and k are neighbours in the visibility graph, then $q_i$ and $q_k$ are parallel. Since the visibility graph is connected, this property implies that $q_1, \ldots , q_n$ are parallel. Finally, the fact that the $q_i$’s have unit norm and lie on a common half-plane (property (6.16)) implies that $q_1 = \cdots = q_n$.

We have thus shown that the largest invariant subset of $E_\gamma \cap \{\omega _i=0,i=1,\ldots ,n\}$ is the flocking manifold $\mathcal{F}$. By the LaSalle invariance principle, all solutions originating in $E_\gamma$ converge to $\mathcal{F}$, and therefore $\mathcal{F}$ is asymptotically stable. The proof that the $q_i$’s converge to a constant is omitted.   $\square$

6.5 Rendezvous of 3D Flying Robots

The rendezvous problem is the gateway to more complex coordination problems such as the control of formations. The idea, as in Chap. 4, is to get n identical flying robots to convene using only feedback from onboard sensors. More precisely, consider n robots modelled by

$$\begin{aligned} \begin{aligned} m \ddot{x}_i&= mg e_3 - u_i R_i e_3 \\ \dot{R}_i&= R_i S(\omega _i) \\ J \dot{\omega }_i&= - \omega _i \times ( J \omega _i) + \tau _i, \end{aligned} \end{aligned}$$

(6.20)

with state $\chi :=(\chi _1,\ldots ,\chi _n)$, $\chi _i:=(x_i,\dot{x}_i,R_i,\omega _i)$. We define the rendezvous manifold

$$ \mathcal{R} = \{\chi : x_1=\cdots =x_n, \dot{x}_1 = \cdots = \dot{x}_n\}. $$

On this set, the centres of mass and velocities of all robots coincide, but the robots are not necessarily stationary. No constraint is placed on the robots’ orientations and angular velocities.

As before, an admissible control for robot i is a locally Lipschitz function of the quantities

$$ R_i^T (x_k-x_i), \ \ R_i^T (\dot{x}_k - \dot{x}_i), \ \ R_i^T R_k, \ \ \omega _i, $$

for each $k \in \mathcal{N}_i$.

We assume that the visibility graph is constant. The rendezvous problem for the robots in (6.20) is to find, if they exist, admissible controls $(u_i,\tau _i)$ so that there exists a subset of the rendezvous manifold that is locally asymptotically stable.

Allowing the stabilization of a subset of $\mathcal{R}$, rather than the entire $\mathcal{R}$, makes the problem less demanding and allows one to take different lines of attack for its solution. For instance, one may attempt to locally asymptotically stabilize the subset of $\mathcal{R}$ on which the thrust axes coincide,

$$ \{ \chi \in \mathcal{R}: R_1 e_3 = \cdots = R_n e_3\}, $$

in which case the requirement on the initial conditions would be that the robots’ initial positions, velocities, and thrust axes be close to each other. Or one may even attempt to fully synchronize the rotation matrices $R_i$. In another formulation, one may require the robots to stop moving, in which case the subset of $\mathcal{R}$ of interest would be

$$ \{ \chi \in \mathcal{R} : \dot{x}_i = \cdots = \dot{x}_n =0\}. $$

Combinations of the above formulations are of course possible, and they are all special instances of the general problem statement above. One may also formulate the global version of the rendezvous problem, or other variations along these lines (almost-global, semi-global, practical, and so on).

Even in our weak formulation, the rendezvous problem for flying robots is to date open. What makes the problem particularly hard is the requirement that robot i must be able to compute its own control $(u_i,\tau _i)$ without knowing its absolute position $x_i$ and orientation $R_i$ in the common inertial frame $\mathcal{I}$.