1.1. Rotation matrices

For three-dimensional modeling, it is essential to have a good understanding of the concepts related to rotation matrices, which are recalled in this section. It is by using this tool that we will perform our coordinate system transformations and position our objects in space.

1.1.1. Definition

Let us recall that the j^th column of the matrix of a linear application of ℝⁿ → ℝⁿ represents the image of the j^th vector e_j of the standard basis (see Figure 1.1). Thus, the expression of a rotation matrix of angle θ in the plane ℝ² is given by:

Concerning rotations in the space ℝ³ (see Figure 1.2), it is important to specify the axis of rotation. We distinguish three main rotations: the rotation around the Ox axis, around the Oy axis and around the Oz axis.

The associated matrices are respectively given by:

Let us recall the formal definition of a rotation. A rotation is a linear application which is an isometry (i.e. it preserves the scalar product) and is direct (it does not change the orientation in space).

THEOREM.– A matrix R is a rotation matrix if and only if:

PROOF.– The scalar product is preserved by R if, for any u and v in ℝⁿ, we have:

Therefore R^TR = I. The symmetries relative to a plane, as well as all the other improper isometries (isometries that change the orientation of space, such as a mirror), also verify the property R^T·R = I. The condition det R = 1 allows us to be limited to the isometries which are direct. ■

1.1.2. Lie group

The set of rotation matrices of ℝⁿ forms a group with respect to the multiplication. It is referred to as a special orthogonal group (special because det R =1, orthogonal because R^T·R = I) and denoted by SO(n). It is trivial to check that (SO(n), ·) is a group where I is the neutral element. Moreover, the multiplication and the inversion are both smooth. This makes SO(n) a Lie group which is a manifold of the set of matrices ℝ^n×n.

The set ℝ^n×n of n × n-matrices is of dimension n². Since the matrix R^T · R is always symmetric, the matrix equation R^T · R = I can be decomposed into independent scalar equations. For instance, for n = 2, we have scalar equations:

As a consequence, the set SO(n) forms a manifold of dimension .

• – for n = 1, we get d = 0. The set SO (1) is a singleton which contains a single rotation matrix: R =1;
• – for n = 2, we get d =1. We need a unique parameter (or angle) to represent the rotations of SO(2);
• – for n = 3, we get d = 3. We need three parameters (or angles) to represent SO (3).

1.1.3. Lie algebra

An algebra is an algebraic structure over a body , if (i) is a vector space over ; (ii) the multiplication rule × of is left- and right-distributive with respect to +; and (iii) for all α, , and for all x, , α·x × β ·y and (αβ) · (x × y). Notice that in general, an algebra is non-commutative (x × y ≠ y × x) and non-associative ((x × y) × z ≠ x × (y × z)). A Lie algebra is a non-commutative and non-associative algebra in which multiplication, denoted by a so-called Lie bracket, verifies that (i) [·, ·] is bilinear, i.e. linear with respect to each variable; (ii) [x, y] = − [y, x] (antisymmetry) and (iii) [x, [y, z]] + [y, [z, x]] + [z, [x, y]] = 0 (Jacobi relation).

For Lie groups, we can define the associated Lie algebras. Lie algebras allow us to consider infinitesimal motions around a given element (i.e. a rotation matrix) in order to use derivatives or differential methods.

Consider the rotation matrix I of SO(n) corresponding to the identity. If we move I by adding a small matrix, say A · dt of ℝ^n×n, we generally do not obtain a rotation matrix. We are interested in matrices A such that I + A · dt ∈ SO(n). We have

i.e. A · dt + A^T · dt = 0 + o (dt). Therefore, A should be skew-symmetric. This means that we are able to move in SO(n) around I by adding infinitesimal skew-symmetric matrices A · dt that are not elements of SO(n). This corresponds to a new operation in SO(n) which is not the multiplication that we already had. Formally, we define the Lie algebra associated with SO(n) as follows:

and it corresponds to the skew-symmetric matrices of ℝ^n×n.

If we now want to move around any matrix R of SO(n), we generate a rotation matrix around I and we transport it to R. We get R (I + A · dt) where A is skew-symmetric. This means that we add to R the matrix R · A · dt.

In robotics, Lie groups are often used to describe transformations (such as translations or rotations). The Lie algebra corresponds to velocities or equivalently to infinitesimal transformations. Lie group theory is useful, but requires some non-trivial mathematical backgrounds that are beyond the scope of this book. We will try to focus on SO (3) or use more classical tools such as Euler angles and rotation vectors, which are probably less general but are sufficient for control purposes.

1.1.4. Rotation vector

If R is a rotation matrix depending on time t, by differentiating the relation RR^T = I, we get

Thus, the matrix is a skew-symmetric matrix (i.e. it satisfies A^T = −A and therefore its diagonal contains only zeroes, and for each element of A, we have (a_ij = −a_ji). We may therefore write, in the case where R is of dimension 3 × 3:

[1.1]

The vector ω = (ω_x, ω_y, ω_z) is called the rotation vector associated with the pair . It must be noted that is not a matrix with good properties (such as for instance the fact of being skew-symmetric). On the other hand, the matrix has the structure of equation [1.1] since it allows positioning within the coordinate system in which the rotation is performed, and this is due to the change of basis performed by R^T. We will define the cross product between two vectors ω and x ∈ ℝ³ as follows:

1.1.5. Adjoint

For each vector ω = (ω_x, ω_y, ω_z), we may adjoin the skew-symmetric matrix:

which can be interpreted as the matrix associated with a cross product by the vector ω. The matrix Ad (ω) is also written ω∧.

PROPOSITION.– If R(t) is a rotation matrix that depends on time, its rotation vector is given by:

[1.2]

PROOF.– This relation is a direct consequence of equation [1.1]. ■

PROPOSITION.– If R is a rotation matrix in ℝ³ and if a is a vector of ℝ³, we have:

[1.3]

which can also be written as

PROOF.– Let x be a vector of ℝ³. We have:

PROPOSITION.– (Duality) We have:

[1.4]

This relation expresses the fact that the matrix is associated with the rotation vector ω, associated with R (t) but expressed in the coordinate system associated with R, whereas is associated with the same vector, but this time expressed in the coordinate system of the standard basis.

PROOF.– We have:

1.1.6. Rodrigues rotation formulas

Matrix exponential. Given a square matrix M of dimension n, its exponential can be defined as:

where I_n is the identity matrix of dimension n. It is clear that e^M is of the same dimension as M. Here are some of the important properties concerning the exponentials of matrices. If 0_n is the zero matrix of n × n and if M and N are two matrices n × n, then:

Matrix logarithm. Given a matrix M, the matrix L is said to be a matrix logarithm of M if e^L = M. As for complex numbers, the exponential function is not a one-to-one function and matrices may have more than one logarithm. Using power series, we define the logarithm of a square matrix as

The sum is convergent if M is close to identity.

Rodrigues formulas. A rotation matrix R has an axis represented by a unit vector n and an angle α with respect to this axis. From n and α we can also generate the matrix R. The link R ↔ (n, α) is made by the following Rodrigues formulas

[1.5]

The first equation (i) will be shown in exercise 1.4 and (ii) is the reciprocal of (i). In these formulas, αn∧ is a notation to represent the matrix Ad (αn). As will be shown in exercise 1.4, R = e^αn∧ is a rotation matrix and n is an eigenvector associated with the eigenvalue 1 of R.

Axis and angle of a rotation matrix. Given a rotation matrix R, the axis can be represented by normalized eigenvector n associated with the eigenvalue λ = 1 (two of them can be found, but we need one). To compute α and n, we may use the relation αn = log R, which works well if R is close to identity. In the general case, it is more suitable to use the following proposition.

PROPOSITION.– Given a rotation matrix R, we have R = e^αn∧ where

[1.6]

PROOF.– Take a rotation matrix R with eigenvalues 1, λ₁, λ₂ and eigenvectors n, v₁, v₂. Consider the generalized polynomial f (x) = x − x⁻¹, where x is the indeterminate. From the correspondence theorem of eigenvalues/vectors, the eigenvalues of f (R) = R − R⁻¹ = R − R^T are f (1) = 0, f (λ₂), f (λ₃) and the eigenvectors are still n, v₁, v₂. Since f (R) is skew symmetric and f (R) · n = 0, we have f (R) ∝ Adn. Thus, the vector Ad⁻¹ (R − R^T) is an eigenvector of R associated with the eigenvalue 1. It provides us the axis of R. To find the angle α, we use the property that the trace of a matrix is similarity-invariant, which means that for any invertible matrix P, we have tr(R) = tr(P⁻¹ · R · P). Take P as the rotation matrix with transforms R into a rotation along the first axis. We get

which provides us with the angle of the rotation. Exercise 1.5 provides a geometrical proof of this proposition. ■

1.1.7. Coordinate system change

Let and be two coordinate systems and let u be a vector of ℝ³ (refer to Figure 1.3). We have the following relation:

where (x₀, y₀, z₀) and (x₁, y₁, z₁) are the coordinates of u in and , respectively.

Thus, for any vector v we have:

By taking respectively v = i₀, j₀, k₀, we obtain the following three relations:

However, since the basis (i₀, j₀, k₀) of is orthonormal, 〈i₀, i₀〉 = (j₀, j₀) = 〈k₀, k₀〉 = 1 and 〈i₀, j₀〉 = 〈j₀, k₀〉 = 〈i₀, k₀〉 = 0. Thus, these three relations become:

or in matrix form:

[1.7]

We can see a rotation matrix whose columns are the coordinates of i₁, j₁, k₁ expressed in the absolute system . Thus,

This matrix depends on time and links the frame to . The matrix is often referred to as a direction cosine matrix since its components involve the direction cosines of the basis vectors of the two coordinate systems. Likewise, if we had several systems (see Figure 1.4), we would have:

Dead Reckoning. Let us consider the situation of a robot moving in a three-dimensional environment. Let us call its reference frame (for example, the frame of the robot at an initial time). The position of the robot is represented by the vector p(t) expressed in and its attitude (i.e. its orientation) by the rotation matrix R(t) which represents the coordinates of the vectors i₁, j₁, k₁ of the coordinate system of the robot expressed in the coordinate system , at time t. It follows that:

This matrix can be returned by a precise attitude unit positioned on the robot. If the robot is also equipped with a Doppler Velocity Log (or DVL) which provides it with its speed vector v_r relative to the ground or the seabed, expressed in the coordinate system of the robot, then the speed vector v of the robot satisfies:

or equivalently:

[1.8]

Dead reckoning consists of integrating this state equation from the knowledge of R(t) and v_r (t).

1.2. Euler angles

1.2.1. Definition

In related literature, the angles proposed by Euler in 1770 to represent the orientation of solid bodies in space are not uniquely defined. We mainly distinguish between the roll-yaw-roll, roll-pitch-roll and roll-pitch-yaw formulations. It is the latter that we will choose since it is imposed in the mobile robotics language. Within this roll-pitch-yaw formulation, the Euler angles are sometimes referred to as Cardan angles. Any rotation matrix of ℝ³ can be expressed in the form of the product of three matrices as follows:

or, in developed form:

[1.9]

The angles φ, θ, ψ are the Euler angles and are respectively called the bank, the elevation and the heading. The terms roll, pitch, yaw are often employed, although they correspond, respectively, to variations of bank, elevation and heading.

Gimbal lock. When (the same effect happens as soon as cos θ = 0), we have

and thus,

This corresponds to a singularity which tells us that when , we cannot move on the manifold of rotation matrices SO(3) in all directions using the Euler angles. Equivalently, this means that some trajectories R(t) cannot be followed by Euler angles.

Rotation matrix to Euler angles. Given a rotation matrix R, we can easily find the three Euler angles by solving, following equation [1.9], the equations:

By imposing , φ ∈ [−π, π] and ψ ∈ [−π, π], we find:

Here atan2 is the two-argument arctangent function defined by

[1.10]

1.2.2. Rotation vector of a moving Euler matrix

Let us consider a solid body moving in a coordinate system and a coordinate system attached to this body (refer to Figure 1.5). The conventions chosen here are those of the SNAME (Society of Naval and Marine Engineers). The two coordinate systems are assumed to be orthonormal. Let R(t) = R(ψ(t), θ(t), φ(t)) be the rotation matrix that links the two systems. We need to find the instantaneous rotation vector ω of the solid body relative to in function of φ, θ, ψ, . We have:

Thus, after having calculated the quantities k, R_ψ j and R_ψ R_θ i in the coordinate system , we have:

From this we get the result:

[1.11]

Note that this matrix is singular if and only if cos θ = 0. We will therefore take care to never have an elevation θ equal to , to avoid the gimbal lock.

1.3. Inertial unit

Assume that we are enclosed inside a box near to the Earth without any ability to see the environment outside. Inside the box we may perform some inertial experiments such as observing bodies translating or rotating. These experiments allow us to indirectly measure the tangential accelerations and the angular speed of the box in its own frame. The principle of an inertial unit is to estimate the pose (position and orientation) of the box from these inertial measurements only (called proprioceptive measurements), assuming that the initial pose is known.

For this purpose, we will describe the motion of our box with a kinematic model, the inputs of which are the tangential accelerations and the angular speeds, which are directly measurable in the coordinate system of the box. The state vector is composed of the vector p = (p_x, p_y, p_z) that gives the coordinates of the center of the box expressed in the absolute inertial coordinate system , the three Euler angles (φ, θ, ψ) and the speed vector v_r of the box expressed in its own coordinate system . The inputs of the system are first the acceleration of the center of the box also given in and second, the vector corresponding to the rotation vector of the box relative to expressed in . We have to express a, ω in the coordinate system of the box since these quantities are generally measured via the sensors attached on it. The first state equation is:

Let us differentiate this equation. We obtain:

with R = R (φ, θ, ψ). From this equation, we isolate to get

which constitutes the second state equation. Figure 1.6 shows a situation where a robot has a constant speed and follows a circle. The speed vector v_r is a constant whereas a_r is different from zero.

We also need to express in function of the state variables. From equation [1.11], the relation

becomes

By isolating the vector in this expression, we obtain the third state equation. By bringing together the three state equations, we obtain the navigation mechanization equations [FAR 08]:

[1.12]

On a horizontal plane. If we move on a horizontal plane, we have φ = θ = 0. The last equation gives us and . In such a case, there is a perfect correspondence between the components of ω_r and the differentials of the Euler angles. There are singular cases, for instance when (this is the case when the box points upwards), in which the differentials of the Euler angles cannot be defined. Using the rotation vector is often preferred since it does not have such singularities.

Dead reckoning. For dead reckoning (i.e. without external sensors) there are generally extremely precise laser gyrometers (around 0.001 deg/s.). These make use of the Sagnac effect (in a circular optical fiber turning around itself, the time taken by light to travel an entire round-trip depends on the path direction). Using three fibers, these gyrometers generate the vector ω_r = (ω_x, ω_y, ω_z). There are also accelerometers capable of measuring the acceleration a_r with a very high degree of precision. In pure inertial mode, we determine our position by differentiating equations [1.12] only using the acceleration a_r and the rotation speed ω_r, both expressed in the coordinate system of the box. In the case where we are measuring the quantity v_r (also expressed in the frame of the inertial unit attached to the robot) with a Doppler velocity log, we only need to integrate the first and the last of these three equations. Finally, when the robot is a correctly ballasted submarine or a terrestrial robot moving on a relatively plane ground, we know a priori that on average the bank and the elevation are equal to zero. We may therefore incorporate this information through a Kalman filter in order to limit the drift in positioning. An efficient inertial unit integrates an amalgamation of all the available information.

Inertial unit. A pure inertial unit (without hybridization and without taking into account Earth’s gravity) represents the robot by the kinematic model of Figure 1.7, which itself uses the state equations given in [1.12]. This system is written in the form ẋ = f (x, u) where u = (a_r, ω_r) is the vector of the measured inertial inputs (accelerations and rotation speeds viewed by an observer on the ground, but expressed in the frame of the robot) and x = (p, v_r, φ, θ, ψ) is the state vector. We may use a numerical integration method such as the Euler method. This leads to replacing the differential equation ẋ = f (x, u) with the recurrence:

With gravity. In the case where there exists a gravity g (p) which depends on p, the actual acceleration a_r corresponds to the gravity plus the measured acceleration . In this case, the inertial unit can be described by the following state equations:

[1.13]

In this case, the inputs of the system are and ω_r which are obtained by the accelerometers and the gyroscope. To integrate this state equation for localization purposes, we need to know the initial state and a gravity map g(p).

Using rotation matrix instead of Euler angles. From equation [1.4], we have . Thus, the state equations [1.13] can be written without any Euler angles as

[1.14]

The main advantage of this representation is that we do not have the singularity that exists in [1.13] for cos θ = 0. But instead, we have redundancies, since R of dimension 9 replaces the three Euler angles. For numerical reasons, when we integrate for a long time, the matrix R(t) may loose the property R · R^T = I. To avoid this, at each iteration, a renormalization is needed, i.e. the current matrix R should be projected onto SO(3). One possibility is to use a QR factorization. A QR-decomposition of a square matrix M provides two matrices Q,R such that M = Q · R where Q is a rotation matrix and R is triangular. When M is almost a rotation matrix, R is almost diagonal and also almost a rotation, i.e. on the diagonal of R the entries are approximately ±1 and outside the entries are almost zeros. The following PYTHON code performs the projection of M on SO(3) and generates the rotation matrix M₂.

Q,R = numpy.linalg.qr(M)
v=diag(sign(R))
M2=Q@diag(v)

1.4. Dynamic modeling

1.4.1. Principle

A robot (airplane, submarine, boat) can often be considered as a solid body whose inputs are the (tangential and angular) accelerations. These quantities are analytic functions of the forces that are at the origin of the robot’s movement. For the dynamic modeling of a submarine the reference work is Fossen’s book [FOS 02], but the related notions can also be used for other types of solid robots such as planes, boats or quadrotors. In order to obtain a dynamic model, it is sufficient to take the kinematic equations and to consider the angular and tangential accelerations caused by forces and dynamic performance. These quantities become the new inputs of our system. The link between the accelerations and the forces is done by Newton’s second law (or the fundamental principle of dynamics). Thus, for instance if f is the net force resulting from the external forces expressed in the inertial frame and m is the mass of the robot, we have

Since the speed and accelerations are generally measured by sensors embedded in the system, we generally prefer to express the speed and accelerations in the frame of the robot:

where f_r is the applied forces vector expressed in the robot frame. The same type of relation, known as the Euler’s rotation equation, exists for rotations. It is given by

[1.15]

where τ_r is the applied torque and ω_r is the rotation vector, both expressed in the robot frame. The inertia matrix I is attached to the robot (i.e. computed in the robot frame) and we generally choose the robot frame to make I diagonal. This relation is a consequence of Euler’s second law which states that in an inertial frame, the time derivative of the angular momentum is equal to the applied torque. In the inertial frame, this can be expressed as

1.4.2. Modeling a quadrotor

As an illustration, we consider a quadrotor (see Figure 1.8) for which we want a dynamic model. The robot has four propellers that can be tuned independently. This will allow us to control the attitude and position of the robot by changing the speeds of the motors.

We distinguish the front/rear propellers (blue and black), rotating clockwise and right/left propellers (red and green), rotating counterclockwise. The value of the force generated by the ith propeller is proportional to the squared speeds of the rotors, i.e. is equal to β · ω_i ·|ω_i|, where β is the thrust factor. Denote by δ the drag factor and ℓ the distance between any rotor and the center of the robot. The forces and torques generated on the robot are

where τ₀ is the total thrust generated by the rotors and τ₁, τ₂, τ₃ are the torques generated by differences in the rotor speeds.

We will now show that the state equations for the quadrotor are given by

where p is the position and (φ, θ, ψ), are the Euler angles of the robot. The first three equations (i), (ii), (iii) correspond to the kinematic equations and have already been derived (see equation [1.12]). Note that in equation (iii), the acceleration a_r (expressed in the robot frame) comes from Newton’s second law where the forces are the total force τ₀ and the weight mg:

Since the gravity vector is expressed naturally in the inertial frame (contrary to the global force τ₀), we have to multiply the gravity vector by the Euler rotation matrix R^T (φ, θ, ψ). Equation (iv) comes from Euler’s rotation equation [1.15],

where τ_r = (τ₁, τ₂, τ₃) is the torque vector.

1.5. Exercises

EXERCISE 1.1.– Properties of the adjoint matrix

Let us consider the vector ω = (ω_x, ω_y, ω_z) and its adjoint matrix Ad (ω).

1. 1) Show that the eigenvalues of Ad (ω) are {0, ||ω|| · i,−||ω|| · i}. Give an eigenvector associated with 0. Discuss.
2. 2) Show that the vector Ad (ω) x = ω ∧ x is a vector perpendicular ω to and x, such that the trihedron (ω, x, to ∧ x) is direct.
3. 3) Show that the norm of ω ∧ x is the surface of the parallelogram mediated by ω and x.

EXERCISE 1.2.– Jacobi identity

The Jacobi identity is written as:

1. 1) Show that this identity is equivalent to:
   

   where Ad (ω) is the adjoint matrix of the vector to ω ∈ ℝ³.

2. 2) In the space of skew-symmetric matrices, the Lie bracket is defined as follows:
   

   show that:

   
3. 3) In the exercise 1.4, we will show that an infinitesimal rotation following the skew symmetric matrix A is exp . Consider two skew-symmetric matrices A, B and define the two infinitesimal rotation matrices
   

   Compute a Taylor expansion of second order associated with the rotation

   

   From this result, give an interpretation of the Lie bracket [A, B] = AB − BA.

4. 4) Verify that the set (ℝ³, + , ∧, ·) forms a Lie algebra.

EXERCISE 1.3.– Varignon’s formula

Let us consider a solid body whose center of gravity remains at the origin of a Galilean coordinate system and is rotating around an axis Δ with a rotation vector of ω. Give the equation of the trajectory of a point x of the body.

EXERCISE 1.4.– Rodrigues’ formula

Let us consider a solid body whose center of gravity remains at the origin of a Galilean coordinate system and is rotating around an axis Δ. The position of a point x of the body satisfies the state equation (Varignon’s formula):

where ω is parallel to the axis of rotation Δ and ||ω|| is the rotation speed of the body (in rad.s⁻¹).

1) show that this state equation can be written in the form:

Explain why the matrix A is often denoted by ω ∧.

2) Give the expression of the solution of the state equation.

3) Deduce from this that the expression of the rotation matrix R with angle ||ω|| around ω is given by the following formula, referred to as Rodrigues’ formula:

4) Calculate the eigenvalues of A and show that ω is the eigenvector associated with the zero eigenvalue. Discuss.

5) What are the eigenvalues of R?

6) Using the previous questions, give the expression of a rotation around the vector ω = (1, 0, 0) of angle α.

7) Write an expression of the Euler matrix that uses Rodrigues’ formula.

EXERCISE 1.5.– Geometric approach to Rodrigues’ formula

Let us consider the rotation of angle φ around the unit vector n. Let u be a vector that we will subject to this rotation. The vector u can be decomposed as follows:

where u_∥ is collinear to n and u_⊥ is in the plane P_⊥ orthogonal to n (see Figure 1.9).

1) Prove Rodrigues’ formula given by:

2) Using the double cross product formula a∧ (b ∧ c) = (a^Tc) · b − (a^Tb) · c, on the element n∧ (n ∧ u), show that Rodrigues’ formula can also been written as:

Deduce from this that the matrix associated with the linear operator is written as:

3) Conversely, we are given a rotation matrix R_n,φ for which we wish to find the axis of rotation n and the angle of rotation φ. Compute the trace tr (R_n,φ) and and use it to obtain n and φ in function of R_n,φ. For a geometric illustration, see Figure 1.10.

4) Deduce a method to find a interpolation trajectory matrix R(t) between two rotation matrices R_a, R_b such that R(0) = R_a and R(1) = R_b.

5) Using a Maclaurin series development of sinφ and cosφ, show that:

which sometimes written as:

EXERCISE 1.6.– Quaternions

Quaternions, discovered by W.R. Hamilton, are also used to represent a rotation in a three-dimensional space (see, e.g. [COR 11]). A quaternion is an extension of the complex number. It corresponds to a scalar s plus a vector v of ℝ³. We use the following equivalent notations:

where

1) From these relations, fill the following multiplication table:

2) A unit-quaternion is a quaternion with a unit magnitude:

Consider the rotation obtained by a rotation around the unit vector v with an angle θ. From the Rodrigues formula, we know that the corresponding rotation matrix is exp(θ · v∧). To this rotation, we associate the quaternion:

Check that this quaternion has a unit magnitude.

3) Show that the quaternion and its opposite both correspond to the same rotation in ℝ³.

4) Assuming that the composition of rotations corresponds to a multiplication of quaternions, show that

5) We consider the Euler rotation matrix with . It corresponds to a rotation around the unit vector v with an angle α. Give the values of v and α using three methods: the geometrical method (using hands only), the matrices and the quaternions.

EXERCISE 1.7.– Schuler pendulum

One of the fundamental components of an inertial unit is the inclinometer. This sensor gives the vertical direction. Traditionally, we use a pendulum (or a plumb line) for this. However, when we are moving, due to the accelerations the pendulum starts to oscillate and it can no longer be used to measure the vertical direction. Here, we are interested in designing a pendulum for which any horizontal acceleration does not lead to oscillations. Let us consider a pendulum with two masses m at each end, situated at a distance ℓ₁ and ℓ₂ from the axis of rotation of the rod (see Figure 1.11). The axis moves over the surface of the Earth. We assume that ℓ₁ and ℓ₂ are small in comparison to Earth’s radius r.

1. 1) Find the state equations of the system.
2. 2) Let us assume that . For which values of ℓ₁ and ℓ₂ does the pendulum remain vertical, for any horizontal movement of the pendulum? What values does ℓ₂ have to take if we let ℓ₄ = 1 m and we take r = 6 400 km for the Earth’s radius?
3. 3) Let us assume that, as a result of disturbances, the pendulum starts to oscillate. The period of these oscillations is called the Schuler period. Calculate this period.
4. 4) Simulate the system graphically by taking a Gaussian white noise as the acceleration input. Since the system is conservative, an Euler integration method will not perform well (the pendulum would gain energy). A higher-order integration scheme, such as that of Runge Kutta, should be used. This is given by:
   

For an easier graphical representation, we will take r = 10 m, ℓ₁ = 1 m and g =10 ms⁻². Discuss the results.

EXERCISE 1.8.– Brake detector

We will now look at a problem that involves basis changes and rotation matrices. A car is preceded by another car m (which we will assume to be a point). We attach to this car the coordinate system as represented in Figure 1.12. The coordinate system is a ground frame assumed to be fixed.

This car is equipped with the following sensors:

• – several odometers placed on the rear wheels, allowing us to measure the speed v of the center of the rear axle;
• – a gyro giving the angular speed of the car , as well as the angular acceleration ;
• – an accelerometer placed at o₁, allowing us to measure the acceleration vector (α, β) of o₁ expressed in the coordinate system ;
• – using two radars placed at the front, our car is capable of (indirectly) measuring the coordinates (α₁,b₁) of the point m in the coordinate system as well as the first two derivatives and .

On the other hand, the car is not equipped with a positioning system (such as a GPS, for example) that would allow it to gain knowledge of . It does not have a compass for measuring the angle θ. The quantities in play are the following:

When a quantity is measured, we assume that its differentials are also measured, but not their primitive. For example, are considered to be measured since a₁, b₁ are measured. However, is measured but θ isn’t. We do not know the state equations of our car. The goal of this problem is finding a condition on the measured variables (and their derivatives) that will allow us to tell whether the point m is braking or not. We understand that such a condition would allow us to build a warning system informing us that the preceding vehicle is braking, even when its rear brake lights are not visible (fog, trailer without brake lights, etc.) or defective.

1. 1) By expressing the Chasle relation (o₀ m = o₀o₁ + o₁ m) in the coordinate system , show the basis change formula:
   

   where R_θ is a rotation matrix.

2. 2) Show that is a skew-symmetric matrix and find its expression.
3. 3) Let u be the speed vector of the point m viewed by a fixed observer. Find an expression of the speed vector u expressed in the coordinate system . Express in function of the measured variables .
4. 4) We will now use the accelerometer of our vehicle, which gives us the acceleration vector (α, β) of o₁, expressed in . By differentiating two times, give the expression of the acceleration of m in the coordinate system . Deduce from this its expression in the coordinate system . Give the expression of only in function of the measured variables.
5. 5) Find a condition on the measurements which allows us to detect whether the vehicle in front is braking.

EXERCISE 1.9.– Modeling an underwater robot

The robot we will be modeling is the Redermor (greyhound of the sea in the Breton language). A photograph is shown in Figure 1.13. It is an entirely autonomous underwater robot. This robot, developed by GESMA (Groupe d’Etude Sous-Marine de l’Atlantique – Atlantic underwater research group), has a length of 6 m, a diameter of 1 m and a weight of 3,800 kg. It has an efficient propulsion and control system with the aim of finding mines on the seabed.

Let us build a local coordinate system over the area traveled by the robot. The point o₀ is placed on the surface of the ocean. The vector i₀ indicates north, j₀ indicates east and k₀ is oriented towards the center of the Earth. Let p = (p_x, p_y, p_z) be the coordinates of the center of the robot expressed in the coordinate system . The state variables of the underwater robot are its position p in the coordinate system , its tangential v and its three Euler angles φ, θ, ψ. Its inputs are the tangential acceleration as well as three control surfaces which act respectively on ω_x, ω_y, ω_z. More formally, we have:

where the factor v preceding u₂, u₃ indicates that the robot is only able to turn left/right (through u₃) or up/down (through u₂) when it is advancing. Give the kinematic state model for this system.

EXERCISE 1.10.– 3D robot graphics

Drawing two- or three-dimensional robots or objects on the screen is widely used for simulation in robotics. The classic method (used by OPENGL) relies on modeling the posture of objects using a series of affine transformations (rotations, translations, homotheties) of the form:

with n = 2 or 3. However, the manipulation of compositions of affine functions is less simple than that of linear applications. The idea of the transformation in homogeneous coordinates is to transform a system of affine equations into a system of linear equations. Notice first of all that an affine equation of the type y = Ax + b can be written as:

We will thus define the homogeneous transformation of a vector as follows:

Thus, an equation of the type

involving the composition of three affine transformations, can be written as:

A pattern is a matrix with two or three rows (depending on whether the object is in the plane or in space) and n columns representing the n vertices of a rigid polygon embodying the object. It is important that the union of all the segments formed by two consecutive points of the pattern forms all the vertices of the polygon that we wish to represent.

1) Let us consider the underwater robot (or AUV, short for Autonomous Underwater Vehicle) whose pattern matrix in homogeneous coordinates is:

Draw this pattern in perspective view on a piece of paper.

2) The state equations of the robot are the following:

where (φ, θ, ψ) are the three Euler angles. The inputs of the system are the tangential acceleration u₁, the pitch u₂ and the yaw u₃. The state vector is therefore equal to x = (p_x, p_y, p_z, v, φ, θ, ψ). Give a function capable of drawing the robot in 3D together with its shadow, in the plane x-y. Verify that the drawing is correct by moving the six degrees of freedom of the robot one by one. Obtain a three-dimensional representation such as the one illustrated in Figure 1.14.

3) By using the relation

draw the instantaneous rotation vector of the robot.

4) Simulate the robot in various conditions using a Euler method.

EXERCISE 1.11.– Manipulator robot

A manipulator robot, such as Staubli (represented on Figure 1.15), is composed of several rigid arms. We retrieve the coordinates of the end effector, at the extremity of the robot, using a series of geometric transformations. We can show that a parametrization with four degrees of freedom allows us to represent these transformations. There are several possible parametrizations, each with its own advantages and disadvantages. The most widely used one is probably the Denavit–Hartenberg parametrization. In the case where the articulations are rotational joints (as is the case of the Staubli robot where the joins can turn), the parametrization represented by the figure might prove to be practical since it makes it easier to draw the robot. This transformation is the composition of four elementary transformations: (i) a translation of length r following z; (ii) a translation of length d following x; (iii) a rotation of α around y and (iv) a rotation of θ (the variable activated around z). Using the figure for drawing the arms and the photo for the robot, perform a realistic simulation of the robot’s movement.

EXERCISE 1.12.– Floating wheel

Consider a wheel floating and spinning in the space as shown on Figure 1.16.

We assume that the wheel is solid, similar to a homogeneous disk of mass m = 1 and radius ρ =1. Its inertia matrix is given by:

1. 1) Give the state equations of the wheel. The state variables we choose are (i) the coordinates p = (x, y, z) of the center of the wheel, (ii) the orientation (φ, θ, ψ) of the wheel, (iii) the speed v_r of the center of the wheel in the wheel frame, and (iv) the rotation vector ω_r expressed in the wheel frame. We thus have a system of order 12.
2. 2) The torque-free precession occurs when the angular velocity vector changes its orientation with time. Provide a simulation illustrating the precession.
3. 3) Show that the angular momentum ℒ = R · I · R^T · ω and the kinetic energy are both constant. Verify this with a simulation.
4. 4) Using symbolic computation, compute the Jacobian matrix of the evolution function f at point

Give an interpretation.

EXERCISE 1.13.– Schuler oscillations in an inertial unit

Consider the Earth as static, without rotation, with a gravity field directed toward the center and with a magnitude g = 9.81ms⁻². An inertial unit is fixed in a robot at the surface of Earth of radius r. Its position is with p = (r, 0, 0) and its Euler angles rate is (φ, θ, ψ) = (0, 0, 0) as in Figure 1.17. The robot will always be static in the exercise.

1. 1) Give the values for and ω_r collected by the inertial unit of .
2. 2) We now want to lure the inertial unit by initializing the robot at some other place, say , and finding a trajectory so that the unit believes that it is fixed in . More precisely, we want to find a motion p(t) = (x(t), y(t), z(t)) for so that the inertial unit senses exactly the inputs , ω_r of . For this, we assume that at time t = 0, (φ, θ, ψ) = (0, 0, 0) for . For simplicity, we also assume that remains in the plane y = 0. Simplify the state equations for in our specific case.
3. 3) Provide a simulation of the trajectory of , initialized at p(0) = (r, 0, 1)^T. Interpret.
4. 4) Linearize the state equation for at the state vector corresponding to . Compute the eigenvalues and give an interpretation.
5. 5) Assuming that the inertial unit is not perfectly initialized, what can you conclude for the corresponding error propagation?

EXERCISE 1.14.– Lie bracket for control

Consider two vector fields f and g of ℝⁿ corresponding to two state equations and . We define the Lie bracket between these two vector fields as

We can check that the set of vector fields equipped with the Lie bracket is a Lie algebra. The proof is not difficult but it is tedious and will not be demanded in this exercise.

1) Consider the linear vector fields f(x) = A · x, g(x) = B · x. Compute [f, g](x).

2) Consider the system

We propose to apply the following cyclic sequence:

where δ is an infinitesimal time period δ. This periodic sequence will be denoted by

Show that

3) Which periodic sequence should we take in order to follow the field ν · [f, g]? We will first consider the case ν ≥ 0 and then the case ν ≤ 0.

4) Propose a controller such that the system becomes

where a = (a₁, a₂, a₃) is the new input vector.

5) Consider a Dubins car described by the following state equations:

where u₁ is the speed of the car, θ its orientation and (x, y) the coordinates of its center. Using Lie brackets, add a new input to the system, i.e. a new direction of control.

6) Propose a simulation to check the good behavior of your controller. To do this, take δ small enough to be consistent with the second order Taylor approximation but large with respect to the sampling time dt. For instance, we may take . The initial state vector is taken as x(0) = (0, 0, 1).

7) Propose a controller so that the car goes up. Consider the same questions, where the car goes down, right and left, respectively.

EXERCISE 1.15.– Follow the equator

We consider a mobile body moving around a planet, Earth, with a radius ρ_E rotating along the vertical axis with a rotation rate .

1) Draw Earth rotating with a static body at position p with the following frames (see Figure 1.18):

1. i) is the Earth-centered inertial frame, fixed with respect to the stars.
2. ii) is the Earth frame and turns with the Earth with a rotational speed .
3. iii) is the navigation frame, ego-centered and pointing towards the sky.
4. iv) is the ego-centered frame of the robot.

For the simulation, take , ρ_E = 10m.

2) Provide the kinematic state equations for the body. The state variables are taken as (p, R₃, v₃), where p is the position of the body in , R₃ is the orientation of the body, and v₃ is the speed of the body seen from and expressed in . The inputs are chosen as ω₃, the rotation vector of the body and a₃, the measured acceleration, both expressed in . Propose a simulation with Earth rotating and the body moving in space with a gravity given by .

3) The planet is now assumed to be made of water and the body is an underwater robot equipped with actuators (such as propellers) to move inside the water. The gravity is taken as . The water of the rotating planet drags the robot through friction forces (tangential and rotational). Moreover, the robot, which has the same density as the surrounding fluid, receives an Archimedes upward buoyant force. Propose a dynamical model describing the motion of the robot. The inputs will be the rotational acceleration u_ω3 and the tangential acceleration u_a3 expressed in R₃. These accelerations result from the action of the actuators creating forces and torques on the robot. Propose a simulation illustrating all these effects.

4) The robot behaves like a 3D Dubins-like car model. More precisely, it is equipped with a single propeller which pushes the robot forward without any possibility of controlling the speed. We take u_a3 = (1, 0, 0)^T. We assume that the robot can chose its rotation vector using rudders, jet pumps or inertial wheels. Find a controller such that the robot goes east along the equator. Validate this with a simulation.

1.6. Corrections

CORRECTION FOR EXERCISE 1.1.– (Properties of the adjoint matrix)

1) The characteristic polynomial of the matrix Ad (ω) is given by:

From this we obtain the eigenvalues {0, ∥ω∥i, −∥ω∥i}. Finally, we have:

and therefore the eigenvector associated with 0 is ω. The matrix Ad (ω) is associated with a speed vector field of a turning coordinate system with ω. Since the axis ω does not move, we will have Ad (ω) · ω = 0.

2) (i) We now show that x ⊥ (ω ∧ x). For this, it is sufficient to prove that x^T Ad (ω) x = 0. We have:

(ii) Since

we get that ω ⊥ (ω ∧ x).

(iii) it is easily shown that:

For this, we need to develop the two expressions above and verify their equality. The positivity of the determinant implies that the trihedron (ω, x, ω ∧ x) is direct.

3) The parallelepiped carried by ω, x and ω ∧ x has a volume of:

However, since ω ∧ x is orthogonal to x and to ω, the volume of the parallelepiped is equal to the surface of its basis multiplied by its height h = ∥ω ∧ x∥, i.e.:

By equating the two expressions for v we obtain .

CORRECTION FOR EXERCISE 1.2.– (Jacobi identity)

1) We have

Therefore, for all c

Or equivalently

2) We simply have:

3) We have

Thus, corresponds to an infinitesimal rotation following the skew symmetric matrix [A, B] = AB − BA.

As a consequence, if in a space probe, we are able to generate only two rotation motions following A and B using inertial disks, we can also generate a rotation following [A, B] alternating the sequence of infinitesimal rotations with respect to B, A, −B, −A, B, A, −B, −A,…

4) The verification is trivial and we will not give it here. Let us note that this result enables us to deduce that the set of skew-symmetric matrices equipped with addition, bracket and the standard outer product is also a Lie algebra.

CORRECTION FOR EXERCISE 1.3.– (Varignon’s formula)

The position of a point x of the solid body satisfies the state equation:

where ω is parallel to the axis of rotation Δ and ∥ω∥ is the rotation speed of the solid (in rad.s⁻¹). After integrating this linear state equation, we find:

We could also have obtained this formula by applying Rodrigues’ formula studied in exercise 1.4. This property can be interpreted by the fact that Ad (ω) represents a rotation movement, whereas its exponential represents the result of this movement.

CORRECTION FOR EXERCISE 1.4.– (Rodrigues’ formula)

1) It is sufficient to verify that:

2) The solution of the state equation is:

3) At time t the solid body has turned by an angle of ∥ω∥ · t and therefore for t =1, it has turned by ∥ω∥. Therefore, the rotation R of angle ∥ω∥ around ω is given by:

4) The characteristic polynomial of A is . The eigenvalues of A are 0, i∥ω∥, −i∥ω∥. The eigenvectors associated with the eigenvalue 0 are collinear to ω. This is logical since the points of the axis of rotation have zero speed.

5) The eigenvalues of R are obtained using the eigenvalue correspondence theorem and are therefore equal to e⁰, e^i∥ω∥,e^−i∥ω∥.

6) The expression of a rotation around the vector ω = (1, 0, 0) of angle α is:

7) Rodrigues’ formula tells us that the orientation matrix around the vector ω of angle φ = ∥ω∥ is given by:

CORRECTION FOR EXERCISE 1.5.– (Geometric approach to Rodrigues’ formula)

1) We have:

Whence Rodrigues’ formula:

2) We have:

Therefore:

Rodrigues’ formula can therefore also be written as:

The operator can be represented by the following rotation matrix:

or, in a developed form:

3) We have

Moreover

Thus

4) The trajectory has the form R(t) = exp (tA) · R_a and we have to find A which is skew-symmetric (so that exp (tA) is a rotation matrix). For t = 0, we have R(0) = R_a and for t = 1, we want R(1) = R_b. We thus have to solve R_b = exp (A) · R_a or equivalently , with A skew-symmetric. We may write but the logarithm of a matrix is not unique. In this exercise, it is assumed that all matrices are of dimension 3 × 3. To find an interpolation trajectory matrix R(t) between R_a and R_b, we take the results of the previous question and we perform the following

Then, we get R(t) = exp (tA) · R_a. We clearly see here that finding A such that is not a unique solution. For instance, we could have taken A = (φ + 2kπ) n∧, with k ≠ 0, but several turns are now needed to go from R_a to R_b.

5) Let us recall that the Maclaurin series development of the sine and cosine functions is written as

Let H = Ad (n). Since n is an eigenvector of H associated with the zero eigenvalue, we have H (n · n^T) = 0. Moreover

Therefore

Thus, Rodrigues’ formula is written as

Or equivalently

CORRECTION FOR EXERCISE 1.6.– (Quaternions)

1) We have:

Note that the multiplication is not commutative.

2) We have

3) Both quaternions , and correspond to the same rotation. Now,

4) We have:

5) (i). Since the rotations are simple, the first method is to get the result directly by hand moving a simple object for instance. We obtain a rotation with respect to (0, 1, 0) with an angle .

(ii) We build the rotation Euler matrix R associated to . We get

Then, we take one normalized eigenvector v of R associated with the eigenvalue λ =1: v = (0, 1, 0)^T. The rotation R can be obtained as a rotation of an angle α around v. To compute α, we apply the formula [1.9]

where the sign chosen to have e^α·v∧ = R. We get .

(iii) We now use the quaternion method. We get

With all approaches, we obtain a rotation with an angle around the vector v = (0, 1, 0).

CORRECTION FOR EXERCISE 1.7.– (Schuler pendulum)

1) The state vector is . In order to have a horizontal movement, we need a horizontal force f. Following the fundamental principle of dynamics, we have:

with and f = 2ma. Since , the state equations of the system are written as:

2) The pendulum remains vertical if, for α = 0, we have . Thus

or equivalently:

We must therefore satisfy the equation . Solving it gives:

For ℓ₁ = 1, we obtain:

3) The equation describing the oscillations is:

For a = 0, we have:

which is the equation of a pendulum of length ℓ = r. By linearizing this equation, we obtain the characteristic polynomial and therefore the pulse . The Schuler period is therefore equal to:

4) The program is the following:

Note that for an initialization , the pendulum always points towards the center of the Earth. Otherwise, it oscillates and conserves this oscillation at the Schuler frequency. This oscillation can be observed in modern inertial units and there are methods for compensating for it using information gathered by the other inertial sensors.

CORRECTION FOR EXERCISE 1.8.– (Brake detector)

1) We have:

Let us express this relation in the coordinate system :

which leads to:

2) We have:

3) in the coordinate system this vector u is expressed by:

Therefore:

4) We have:

Therefore:

However:

We get

5) The vehicle in front of us is braking if

i.e. if

CORRECTION FOR EXERCISE 1.9.– (Modeling an underwater robot)

The derivative of the position vector is obtained by noticing that:

since i₁ corresponds to the first column of matrix [1.9]. Finally, by taking into account equations [1.12], we can write the state equations for our submarine:

Here we have a kinematic model (i.e. it does not involve forces or torques). There are no parameters and the model can therefore be considered correct if the underwater robot is solid (that is, it cannot be contorted), and if its trajectory is tangent to the axis of the robot. Such a model allows us to use nonlinear control methods such as feedback linearization which will be discussed in Chapter 2. These methods are indeed less robust with respect to a model error, but are formidably efficient if an accurate model for our system is known.

CORRECTION FOR EXERCISE 1.10.– (3D robot graphics)

2) To draw the robot at state x = (p_x, p_y, p_z, v, φ, θ, ψ), we build the pattern matrix

and we compute the transformed pattern matrix (to be painted)

3) The simulation could be done by using Euler’s integration method as shown by Figure 1.19 with the initial vector

and the control u = (0, 0, 0.2)^T. This simulation will be picked up in exercise 2.4 for performing the control of the trajectory of the robot.

CORRECTION FOR EXERCISE 1.11.– (Manipulator robot)

One must draw the arms one after the other. For this, we have to build the translation by a vector v and the rotation around ω with an angle ∥ω∥. They are represented by the two matrices

In this exercise, we need a translation of length r following the z-axis, a translation of length d following x, a rotation of α around y and a rotation of θ. They are respectively given by the 4 × 4 matrices

The seven arms of the robot in a configuration q (whose components are the coordinates of the articulations) can be drawn as follows:

Each arm is drawn using the pair of two homogeneous matrices , for j ∈ {1,…, 7}. Figure 1.20 corresponds to a simulation for a robot with the following parameter vectors:

CORRECTION FOR EXERCISE 1.12.– (Floating wheel)

1) Considering the Euler’s rotation equation

where the torque τ_r = 0, and the fact that we have no acceleration of our wheel, from [1.12] we get

[1.16]

2) For the simulation, we take

and we get the result depicted in Figure 1.12. The wheel translates with respect to p_x. We see from the shadow (black) that the rotation axis oscillates. This corresponds to the precession.

3) We have ℒ = RIR^Tω = RIω_r. Therefore

Moreover

4) We write the following PYTHON code using the SYMPY library:

from sympy import *
x = MatrixSymbol(’x’, 12, 1)
wx, wy, wz, rho, m, s=symbols(“wx wy wz rho m s”)
phi, theta, psi=x[3:6]
vr=Matrix(x[6:9])
wrx, wry, wrz=x[9:12]
wr=Matrix(x[9:12])
ci, si, cj, sj, ck, sk=cos(phi), sin(phi), cos(theta), sin(theta),
                             cos(psi),sin(psi)
Ri = Matrix([[1, 0, 0], [0, ci, -si], [0, si, ci]])
Rj = Matrix([[cj, 0, sj], [0, 1, 0], [-sj, 0, cj]])
Rk = Matrix([[ck, -sk, 0], [sk, ck, 0], [0, 0, 1]])
E= Rk*Rj*Ri
dp=E*vr
da= Matrix([[1, si*sj/cj, ci*sj/cj], [0, ci, -si],
          [0, si/cj, ci/cj]])*wr
A=Matrix([[0, -wrz, wry], [wrz, 0, -wrx], [-wry, wrx, 0]])
dvr=-A*vr
I=(m/2)*rho**2*Matrix([[1, 0, 0], [0, 1/2, 0], [0, 0, 1/2]])
dwr=-(I**-1)*(A*I*wr)
f=Matrix((dp, da, dvr, dwr))
J=f.jacobian(x)
x0=Matrix([[0], [0], [0], [0], [0], [0], [0], [0], [0], [wx], [wy], [wz]])
J=J.subs(x, x0)

The matrix J at point x₀ is written in Figure 1.23.

The yellow blocks with zeros can be understood by the graph of Figure 1.23. An arc represents a differential delay. For instance, the arc between the node v_r and p means that depends algebraically on v_r.

The matrix J is block triangular and we can easily compute the characteristic polynomial. It is given by

The term is consistent with the fact when no precession exists, the wheel spins around ω_r at a pulsation ∥ω_r∥. The terms and correspond to the precession.

If the wheel is not perfectly solid, internal frictions will tend to dampen the precession, and the rotation axis will align itself with one eigenvector of I. This vector can either be one vector of the wheel plane, or the axis of the wheel (orthogonal to the wheel plane).

CORRECTION FOR EXERCISE 1.13.– (Schuler oscillations in an inertial unit)

1) Since the Earth is static, we have

2) All Euler angles of are constant (as for ) and equal to zero. They do not appear as state variables anymore. The Euler matrix R (φ, θ, ψ) is equal to the identity. The state equations of become

They can be rewritten as follows:

3) We get the trajectory represented in Figure 1.24. We observe some oscillations known as the Schuler oscillations.

4) Around z = 0, x = r, we get

The eigenvalues are . Some oscillations exist and this system is unstable due to the fact that we have two roots in 0.

5) In practice, the inertial unit is not perfectly initialized, and thus it finds a trajectory similar to that of whereas it is fixed for . Since the error is small, the linear approximation is realistic. The integration method inside the inertial unit returns some unwanted oscillations corresponding to a solution which is not the actual one, as illustrated by Figure 1.25. These oscillations correspond to a period , named the Schuler period. For many applications (such as when we are in an aircraft), we know that such oscillations are dummies and we may improve the integration method in order to damp these Schuler oscillations.

CORRECTION FOR EXERCISE 1.14.– (Lie bracket for control)

1) We have

2) Without loss of generality, the proof will be given for t = 0. We shall use the following notations

For a given t, and a small δ we have

[1.17]

with

Thus

We get

[1.18]

The sum yields

Now,

(i)

(ii) B_δ = B₀ + o(1)

Therefore

[1.19]

The same reasoning yields

This result could be obtained directly from [1.19] by rewriting: δ → −δ, f → −g, g → −f, A₀ → −B₀, B₀ → −A₀. Thus

The consequence is that, using the periodic sequence, we are now able to move with respect to the direction [f, g].

3) We have shown that within a time period of 4δ we moved in the direction [f, g] by [f, g]δ². This means that we follow the field which is infinitesimal. Multiplying the cyclic sequence by the scalar α ∈ ℝ amounts to multiplying both f, g by α. The field thus becomes . If we want the field ν · [f, g], we have to take multiply the sequence by . If ν is negative, we have to change the orientation of the sequence. The cyclic sequence is thus

where ε = sign(ν) changes the orientation of the sequence (clockwise for ε = 1, counterclockwise for ε = −1).

4) If we want to follow the field a₁f + a₂g + a₃[f, g], we have to take the sequence

with and ε = sign(v).

5) If we set x = (x, y, θ) we have

We have

We can now move the car laterally.

6) if we apply the cyclic sequence as a controller, we get

We made 4 simulations for a = (0.1, 0, 0), a = (0, 0, 0.1), a = (−0.1, 0, 0), a = (0, 0, −0.1). We took for initial vector x(0) = (0, 0, 1), t ∈ [0, 10] and dt = 0.01. We obtained the results depicted by Figure 1.26. We observe that after each simulation, the distance to the origin is 0.1 × 10 = 1 approximately which is consistent with the fact that f (x) and [f, g] have a norm equal to 1. We did not show the simulation for a = (0, ±0.1, 0) since there is no displacement: the car turns on itself.

7) We have

We take a = , where .

For sequentially desired directions: . We get the results shown in Figure 1.27.

CORRECTION FOR EXERCISE 1.15.– (Follow the equator)

1) The rotation matrices to go from one frame to another are . We have

2) The kinematic equations, taken from [1.13], are:

On the simulation (see Figure 1.28), we observe that the trajectory corresponds to an ellipse which is consistent with the behavior of a satellite. The rotation of the body is due to the initial conditions.

3) The dynamic model is composed of the kinematic model, to which we add the following state equation to generate the inputs a₃, ω₃ (see Figure 1.29):

This dynamic (left) block has ω₃ among the state variables.

Let us explain the first equation:

The friction term tells us that if we have no actuators, the robot will stop its rotation with respect to the water and thus converge on a rotation equation which is that of the Earth. To this friction, we should add the rotation coming from rudders or propellers.

The second equation is composed of three terms

1. (i) An acceleration exists due to the friction. As a first approximation, we may assume that the acceleration is proportional to the speed difference between the robot and the fluid. Since the speed of the fluid is v_f = ω_E ∧ p, we get an acceleration due to the friction that may be approximated in the frame by .
2. (ii) The acceleration of the fluid at p is
   

   If the robot is static with respect to the fluid and has the same density as the fluid, it will have the acceleration resulting from the Archimedes force. Now, due to the gravity, it will measure the acceleration in the frame.

3. (iii) The acceleration u_a3 coming from the propellers are expressed in the robot frame .

4) To control the direction of the robot, we consider a pose field approach, i.e. we associate with each point p a pose (represented by a rotation matrix R₄) that the robot tries to satisfy. For instance if we want to follow the equator from west to east, we choose as a pose field

Then, we choose the rotation vector for the control to make R₃ like R₄ (p). We get (see equation 1.6):

where and . The corresponding simulation is depicted in Figure 1.30.