Geometric Control of Robotic Systems

Abstract

In this chapter, we’ll present geometrical methods to control robotic systems, we’ll start by presenting the classical approach and its benefits, as well as its limits, and we will explain how the geometric approach allow us to get rid of these limits.

1. Introduction

Robotic systems are powerful tools that allow us to gain time, money and enery, they had revolutionized several fields as industry, medicine, agriculture and many other fields.

The main difficulty caused by robotic systems is the nonlinearity of the dynamics. The classical approach modelise the configuration space by an Euclidean space, and applies the principle of least action to derive the equation of motion, and applies Lyapunov and Lasalle’s techniques to control the robot [1, 2].

However, this elegent approach do not fit well with regulation under constraints problem or optimal regulation, the goal here is to illustrate these limits, and to show how the geometric method allow us to get ride of these limits.

2. The classical approach and its limits

The configuration space of the robot is ${\mathbb{R}}^n$ where n is the number of degrees of liberty (DOF) of the robot, the principle of least action gives the dynamics

$$G\left(q\right){q^{\prime }}^{\prime }+\frac{1}{2}G{\left(q\right)}^{\prime }+S\left(q,q^{\prime }\right)q^{\prime }+g\left(q\right)=\tau .$$E1

This allow us to ensure locally the regulation to a configuration $q^{\ast }\in {\mathbb{R}}^n$ with the control

$$\tau \left(q,q^{\prime }\right)=g\left(q\right)-K_p\left(q-q^{\prime }\right)-K_v{q}^{\prime }$$E2

which is a PD controller.

The classical approach with all its benefits [1, 2], do not fit well with the problem of regulation under constraints or optimal regulation.

For the optimal control problem, the general formula which gives the angular velocity in function of the configuration of a Rigid body that minimizes the cost [3]

$$J\left(\mathrm{\Omega }\right)={\int }_0^{\infty }\left[\mathit{tr}\left(I-R_d^{T}R\right)+\frac{1}{2}{‖\mathrm{\Omega },‖}^2\right]\mathit{dt}$$E3

for the kinematic $R^{\prime }=R\mathrm{\Omega }$ and ensure regulation $R\to R_d$

$${\mathrm{\Omega }}^{\ast }\left(R\right)=-\frac{R_d^{T}R-R^T{R}_d}{\sqrt{1+\mathit{tr}\left(R_d^{T}R\right)}}$$E4

which has not the form of a proportional controller, so this prevents to get an optimal PD regulator for robotic systems.

For the regulation under constaints problem (tool of the robot must stay in a security surface S), the set of admissibles configurations is $N=x^{-1}\left(S\right)$ where x is the function which gives the position of the tool in function of the configuration. The set N has no reason to be a linear subspace of R since x is a nonlinear function, the set N will be a curved space in ${\mathbb{R}}^n$ and we arrive to a formalism naturally adapted to the resolution of the problem of regulation under constraints.

Sensitivity with respect to initial conditions remains one of the main problems in conceptions of efficients observers.

For example, for a rigid body which dynamics are^1,2

$$R^{\prime }=R\mathrm{\Omega }$$E5

$${\underset{-}{\omega }}^{\prime }=I^{-1}\left(I\mathrm{\omega }\times \mathrm{\omega }\right)-2I^{-1}{\left[\mathit{\text{Skew}}\left(R\right)\right]}^{\times }$$E6

where $I=\left[\mathrm{2,2,8}\right]$ and $R\left(0\right)=exp\left({\left[\mathrm{0.82,0.21,0.73}\right]}^{\times }\right)$$\mathrm{\omega }\left(0\right)=\left[0.8,\mathrm{0.5,0.6}\right]$, the Luenberger observer [4] is

$${\widehat{R}}^{\text{'}}=\widehat{R}\left(\widehat{\mathrm{\Omega }}-2j\left(I^{-1}{\left[\mathit{\text{Skew}}\left(R^T\widehat{R}\right)\right]}^{\times }\right)\right)$$E7

$${\widehat{\mathrm{\omega }}}^{\text{'}}=I^{-1}\left(I\widehat{\mathrm{\omega }}\times \widehat{\mathrm{\omega }}\right)-2I^{-1}{\left[\mathit{\text{Skew}}\left(R\right)\right]}^{\times }-2I^{-1}{\left[\mathit{\text{Skew}}\left(R^T\widehat{R}\right)\right]}^{\times }$$E8

where $\widehat{R}\left(0\right)=exp\left({\left[0.8,\mathrm{0.5,0.6}\right]}^{\times }\right)$ and $\widehat{\mathrm{\omega }}\left(0\right)=\left[\mathrm{0,0,0}\right]$, the simulations shows that the Luennberger observer is not efficient when real and observed configuration and velocity are lightly different (Figure 1).

Figure 1.

Insufficiency of Luenberger observer.

3. The geometric formalism

The idea of the geometric formalism is to pass the complexity of the equation to the configuration space, thus in the geometric formalism, the configuration space is not an Euclidean space, but a curved space that we call manifold.

Manifolds [5] are objects that looks like Euclidean space locally, we can always reduce the local study of manifolds to computations in ${\mathbb{R}}^n$, it is exactly what we do when we parametrize the configuration of a rigid body with Euler angles.

The configuration space of a rigid body [6] which has a fixed point is the space of Euclidean rotations $\mathit{SO}\left(3\right)$, when all its points moves freely the configuration space becomes $\mathit{SO}\left(3\right)\times {\mathbb{R}}^3$, the possible translations of a point cover ${\mathbb{R}}^3$.

A robot is an assembly of s rigid bodies, which are related from there extremeties by articulations, those links are called holonomic constraints.

The configuration space of the robot is M a submanifold of dimension n of ${\left(\mathit{SO}\left(3\right)\times {\mathbb{R}}^3\right)}^s$, the holonomic constraints are related to the local coordinates by the following result.

Proposition 1Let M be a subset of${\left(\mathit{SO}\left(3\right)\times {\mathbb{R}}^3\right)}^s$, there is equivalence between

• Holonomic constraints: for each$q\in M$, there exists$V_q$a neighborhood of$q$in${\left(\mathit{SO}\left(3\right)\times {\mathbb{R}}^3\right)}^s$and a submersion³$f:V_q\to {\mathbb{R}}^{6s-r}$such that$M\cap V_q=f^{-1}\left(0\right)$.

• Local coordinates: for each$q$in$M$, there exists a neighborhood$V_q$of$q$and an immersion⁴$\theta :0\in U\to V_q\cap M$is a homeomorphism.⁵

$M$is said to be a manifold of dimension$n$, the number$n$is the degree of freedom of the robot, and the number$r$is the number of constraints, we have the well known formula$n=6s-r$.

Once we know that we are dealing with curved objects, it may seems more complicated to do computations. Thanks to the tangent space $T_{q}M$, which is the set of virtual velocities $\dot{\gamma }\left(0\right)$ of curves $\gamma \left(t\right)$ that pass throught $\gamma \left(0\right)=q\in M$ and stay in $\gamma \left(t\right)\in M$, this space is linear and of dimension n (Figure 2).

Figure 2.

Tangent space.

When we do parametrization with holonomic constraints, the tangent space is $T_{q}M=\mathit{Ker}\left({\mathit{df}}_q\right)$, if we parametrize our manifold with local coordinates, then $T_{q}M=\mathit{\text{Range}}\left(d{\theta }_0\right)$, the basis adapted to the coordinates is ${\partial }_i=\mathit{d\theta }\left(e_i\right)$ where $e_i$ is the canonical basis of ${\mathbb{R}}^n$.

The tangent linear map between two manifolds $\varphi :M\to N$ is the linear map

$$T_q\varphi :T_{q}M\to T_{\varphi \left(q\right)}N$$E9

defined by

$$T_q\varphi \left(v\right)={\left(\varphi \circ \gamma \right)}^{\prime }\left(0\right)$$E10

where $\gamma \left(0\right)=q$, $\gamma \left(t\right)\in M$ and ${\gamma }^{\prime }\left(0\right)=v$ (Figure 3).

Figure 3.

Linear tangent map.

A vector field is the data of a tangent vector in $T_{q}M$ for each $q\in M$ which a smooth way, the set of vector fields is $\mathrm{\Gamma }\left(\mathit{TM}\right)$.

To each vector field corresponds a motion of diffeomorphisms⁶ by solving the following ODE (Figure 4)

Figure 4.

Vector field and its flow.

$${\partial }_t{\varphi }_t^X\left(q\right)=X\left({\varphi }_t^X\left(q\right)\right)$$E11

$${\varphi }_0^X\left(q\right)=q.$$E12

When we have two vector fields $X,Y$, a natural question is to ask when we have

$${\varphi }_t^X\circ {\varphi }_s^Y={\varphi }_s^Y\circ {\varphi }_t^X$$E13

the obstruction of the commutation between the flows is measured by the Lie bracket $\left[X,Y\right]$ which is defined by

$${\left.\left[X,Y\right]=\frac{d}{\mathit{dt}}\right|}_{t=0}T{\varphi }_{-t}^X\left(Y\circ {\varphi }_t^X\right).$$E14

The Lie derivative is related to the derivation of functions by

$$\left[X,Y\right].f=X.\left(Y.f\right)-Y.\left(X.f\right)$$E15

where $X.f=\mathit{Tf}\left(X\right)$.

Now that we have introduced all necessary tools, we will talk about Riemannian geometry. The idea behind Riemannian geometry is to consider a scalar product that is dependent of the position, this will simplify the formalism, provided that we know how to manipulate the covariant differential calculus, which we will present right now.

The kinetic energy of a ball that is constrained to stay in a surface S is independant of the position. In the general case, when we compute the kinetic energy of a rigid body or two link robot, this energy will depend of the configuration, this gives a Riemannian metric that is inherited from the inertia of the robot.

The innovative viewpoint of Arnold [6] is to see the motion of a rigid body fixed on one point as a curve in $\mathit{SO}\left(3\right)$, the kinetic energy depends only on the left angular velocity⁷ and thus we say that the Riemannian metric is left invariant (Figure 5).

Figure 5.

The rigid body.

Return to our ball in a surface, in the absence of forces and potential, the principle of least action postulates that the real trajectory is a critical point of the kinetic energy, this means that the acceleration of the ball must be orthogonal to the surface, such trajectories are called geodesic, remark that if the ball was not constrained in S, this condition would give that the acceleration is $0$, and thus the motion is a straightline, let us do the calculation

the kinetic energy for a ball with mass m is

$$E\left(\gamma \right)={\int }_0^1\frac{1}{2}m{\left|\dot{\gamma }\left(t\right)\right|}^2\mathit{dt}$$E16

since $\gamma \left(t\right)\in S$, the authorized perturbations are ${\gamma }_s\left(t\right)\in S$ such that ${\gamma }_0\left(t\right)=\gamma \left(t\right)$, ${\gamma }_s\left(0\right)=\gamma \left(0\right)$ and ${\gamma }_s\left(1\right)=\gamma \left(1\right)$.

The postulate is that

$${\left.\frac{d}{\mathit{ds}}\right|}_{s=0}E\left({\gamma }_s\right)=0$$E17

this gives

$${\int }_0^{1}m\dot{V}\left(t\right).\dot{\gamma }\left(t\right)\mathit{dt}=0$$E18

where ${\left.V\left(t\right)=\frac{\partial }{\partial s}\right|}_{s=0}{\gamma }_s\left(t\right)\in T_{\gamma \left(t\right)S}$ after integrating by parts we get

$${\int }_0^{1}V\left(t\right).{{\gamma }^{\prime }}^{\prime }\left(t\right)\mathit{dt}=0$$E19

since $V\left(t\right)\in T_{\gamma \left(t\right)}S$, we get that

$${{\gamma }^{\prime }}^{\prime }\left(t\right)\perp T_{\gamma \left(t\right)}S.$$E20

The most important step in these calculations is the following one

$$\frac{d}{\mathit{dt}}X\left(t\right).Y\left(t\right)=\frac{\mathit{dX}}{\mathit{dt}}.Y\left(t\right)+X\left(t\right).\frac{\mathit{dY}}{\mathit{dt}}$$E21

where $X,Y$ are tangent vector field to $\gamma \left(t\right)\in S$, that is $X\left(t\right),Y\left(t\right)\in T_{\gamma \left(t\right)}S$. In this case the kinetic energy which is the Euclidean scalar product does not depend on the position, for our applications, the kinetic energy depends on the position so we need to construct a “good derivative” that allow us to derive scalar products by computing the product of derivative only without computing the derivative of the metric.

The parallel transport [7] of a vector $v\in T_{q}M$ along a curve $\gamma \left(t\right)\in M$ is defined by the following way: the point at the origin of the vector follows the curve and the vector evolves continuously by preserving its length and the angle with $\dot{\gamma }\left(t\right)$.

This process defines a mapping $P\left(\gamma \left(1\right),\gamma \left(0\right)\right):T_{\gamma \left(0\right)}M\to T_{\gamma \left(1\right)}M$ which is a linear isometry (preserving angle and length) (Figure 6).

Figure 6.

Parallel transport.

The convariant derivative of two vector fields is

$${\left.{\nabla }_{X}Y\left(q\right)=\frac{d}{\mathit{dt}}\right|}_{t=0}P\left(q,{\varphi }_t^X\left(q\right)\right)Y\left({\varphi }_t^X\left(q\right)\right)$$E22

this covariant derivative satisfies

$$\nabla g=0$$E23

which means that

$$X.g\left(Y,Z\right)=g\left({\nabla }_{X}Y,Z\right)+g\left(Y,{\nabla }_{X}Z\right),$$E24

we say that this connexion kills the metric, in addition of that, this connexion is without torsion, that is we can close locally the parallelogram by doing parallel transport along geodesics by two different paths, the mathematical formula is

$$T\left(X,Y\right)={\nabla }_{X}Y-{\nabla }_{Y}X-\left[X,Y\right]=0$$E25

for the computations in coordinates, we defines

$${\nabla }_{{\partial }_i}{\partial }_j={\mathrm{\Gamma }}_{\mathit{ij}}^k{\partial }_k$$E26

the previous identities are⁸

$${\mathrm{\Gamma }}_{\mathit{ij}}^k={\mathrm{\Gamma }}_{\mathit{ji}}^k,T=0$$E27

$${\mathrm{\Gamma }}_{\mathit{ij}}^k=\frac{1}{2}{g}^{\mathit{km}}\left(g_{\mathit{mj},i}+g_{\mathit{mi},j}-g_{\mathit{ij},m}\right),\nabla g=0$$E28

we construct a covariant derivative $\frac{D}{\mathit{Dt}}:\mathrm{\Gamma }\left(\gamma \right)\to \mathrm{\Gamma }\left(\gamma \right)$ along curves $\gamma \left(t\right)\in M$ that satisfies

• $\frac{\mathit{DX}+Y}{\mathit{Dt}}=\frac{\mathit{DX}}{\mathit{Dt}}+\frac{\mathit{DY}}{\mathit{Dt}}$

• $\frac{\mathit{DfX}}{\mathit{Dt}}=f^{\prime }X+f\frac{\mathit{DX}}{\mathit{Dt}}$ for scalar function f

• ${\nabla }_{{\gamma }^{\prime }\left(t\right)}V\left(\gamma \left(t\right)\right)=\frac{\mathit{DV}\circ \gamma \left(t\right)}{\mathit{Dt}}$ for $X\in \mathrm{\Gamma }\left(\mathit{TM}\right)$.

In coordinates we get by setting $X\left(t\right)=X^i\left(t\right){\partial }_{i\mid \gamma \left(t\right)}$

$$\frac{\mathit{DX}}{\mathit{Dt}}=\left(\frac{d}{\mathit{dt}}\left(X^i\right)+{\mathrm{\Gamma }}_{\mathit{kl}}^i\frac{d}{\mathit{dt}}\left(q^k\right)X^l\right){\partial }_{i\mid \gamma \left(t\right)}.$$E29

This covariant derivative satisfies

$$\frac{d}{\mathit{dt}}g\left(V,W\right)=g\left(\frac{\mathit{DV}}{\mathit{Dt}},W\right)+g\left(V,\frac{\mathit{DW}}{\mathit{Dt}}\right).$$E30

Now we suppose that our robot must evolves from $q_1$ to $q_2$ in M by minimizing the kinetic energy, we consider the real trajectory $q\left(t\right)$ and its admissible variations $q_s\left(t\right)$, by deriving we obtain

$$\frac{d}{\mathit{ds}}{\left|{}_{s=0}E\left(q_s\right)=\frac{1}{2}{\int }_0^1{g}_{q_s\left(t\right)}({\dot{q}}_s\left(t\right),{\dot{q}}_s\left(t\right))\mathit{dt}={\int }_0^1{g}_{q_s\left(t\right)}(\frac{D}{\mathit{Ds}}\right|}_{s=0}{\dot{q}}_s\left(t\right),\dot{q}\left(t\right))\mathit{dt}$$E31

using the Schwarz lemma

$$\frac{D}{\mathit{Ds}}\frac{\partial }{\partial t}=\frac{D}{\mathit{Dt}}\frac{\partial }{\partial s}$$E32

we get

$${\int }_0^1{g}_{q\left(t\right)}\left(\frac{D}{\mathit{Dt}}V\left(t\right),\dot{q}\left(t\right)\right)\mathit{dt}=0$$E33

which gives by integration by parts that

$$\frac{D}{\mathit{Dt}}\dot{q}=0$$E34

which is the geodesic equation which are curves that are parallel along themselves.

Now we will talk about curvature, curvature is the opposite of flatness, in an Euclidean space, when we look at the parallel transport along closed curves, the obtained linear map is always identity, in a sphere, the set of obtained linear mappings cover all $\mathit{SO}\left(2\right)$, because of the curvature of the sphere.

So the curvature is the change of a vector by parallel transport along the parallelogram obtained by geodesics and parallel transport, the mathematical formula is

$$R\left(X,Y\right)Z={\nabla }_X{\nabla }_{Y}Z-{\nabla }_Y{\nabla }_{X}Z-{\nabla }_{\left[X,Y\right]}Z$$E35

the curvature is a tensor, that is we can compute $R\left(u,v\right)w$ for $u,v,w\in T_{q}M$ without needing any extention.

The sectional curvature is

$$K\left(u,v\right)=\frac{g\left(R\left(u,v\right)v,u\right)}{{\left|u\right|}^2{\left|v\right|}^2-g\left(u,v\right)},$$E36

and it is the Gaussian curvature of the geodesic surface generated by $\left(u,v\right)$.

Curvature is also a factor in the sensitivity with respect to initial conditions, the geodesic deviation is related to the sign of curvature by the following formula

$$\frac{D^{2}J}{{\mathit{Dt}}^2}+R\left(J,\dot{q}\right)\dot{q}=0$$E37

where J is a virtual motion that represents a local variation of geodesic (Figure 7).

Figure 7.

Geodesic variation.

When curvature is 0, the sensitivity is linear, which allow the Luenberger observer to be performant, when the curvature is negative, the geodesic instabillity is exponential [7], which explains that the Luenberger observer diverges (Figure 8).

Figure 8.

Negative curvature and sensitivity of the geodesic flow.

The exponential map corresponds to a tangent vector the range by the geodesic after a unitary time, we denote it ${exp}_q:V_q\subset T_{q}M\to M$, the exponential is a local diffeomorphism, so around a configuration, each configuration can be joined by a unique geodesic, the parallel transport along this geodesic is denoted by P (Figure 9).

Figure 9.

Exponential map.

when the exponential map is defined on $T_{q}M$, we say the $\left(M,g\right)$ is complete, it is the fact when M is compact.

The geodesic distance is a natural distance induced by the Riemannian metric

$$d_g\left(q_1,q_2\right)=\underset{\text{curves\hspace{0.17em}in}M\text{joining}{q}_1\text{to}{q}_2.}{inf}{\int }_0^1\sqrt{g_{q\left(t\right)}(\dot{q}\left(t\right),\dot{q}\left(t\right)}\mathit{dt}.$$E38

Thanks to Cauchy-Schwarz inequality, we can show that

$$\frac{1}{2}{d}_g{\left(q_1,q_2\right)}^2=\frac{1}{2}\underset{\text{curves\hspace{0.17em}in}M\text{joining}{q}_1\text{to}{q}_2.}{inf}{\int }_0^1{g}_{q\left(t\right)}(\dot{q}\left(t\right),\dot{q}\left(t\right)\mathit{dt}.$$E39

The gradient of a function f is the unique vector field $\nabla f$ that satisfies

$$\mathit{df}\left(X\right)=g\left(\nabla f,X\right)$$E40

The gradient of the function $U\left(q,.\right)=\frac{1}{2}{d}_g^2\left(q,.\right)$ is

$$\nabla U\left(p\right)=-{exp}_p^{-1}\left(q\right).$$E41

The configuration space of the robot is a n dimensional compact manifold equipped with a Riemannian metric g that is the kinetic energy. In the presence of potential, the claculus of variations must applies to the Lagrangian $L\left(q,v\right)=\frac{1}{2}g\left(v,v\right)-W\left(q\right)$ and this gives the equation

$$\frac{D\dot{q}}{\mathit{Dt}}=-\nabla W\left(q\right)$$E42

in robotics, we have a control law that allow us to enslave the robot, so the equation becomes [8, 9]

$$\frac{D\dot{q}}{\mathit{Dt}}=-\nabla W\left(q\right)+u$$E43

We recover a Newton-like equation, the left term quantifies an obstruction of parallelism, and the right hand term an exterior force, as well as the Newton fundamental law “The trajectory of an object remains parallel along itself as long as there are no forces applying to it” (Figure 10).

Figure 10.

Inverse of the exponential.

The kinetic energy theorem gives the variation of the mechanical energy in function of the applied forces, this will be useful to compute feedback law to ensure regulation (Figure 11)

Figure 11.

Two link manipulator and its configuration space.

$$\frac{d}{\mathit{dt}}\left(\frac{1}{2}{\left|\dot{q}\left(t\right)\right|}_{q\left(t\right)}^2+W\left(q\left(t\right)\right)\right)=g_{q\left(t\right)}\left(\dot{q}\left(t\right),u\left(t\right)\right).$$E44

In some applications, especially those concerning the motion of a rigid body, the configuration space is a Lie group G, which is a group furnished with a manifold structure such that multiplication and inverse are smooth, the consideration of a Riemannian metric that is left invariant allow us simplificate the geodesic equation [9], the curvature tensor and the parallel transport, and thus simplificate the practical implementation of the optimal regulator [10, 11] and the Riemannian observer [12].

4. Regulation under constraints

In this section, we will propose a method to ensure regulation under constraints, before going further, lets see the case of regulation without constraints.

Let fix some reference configuration $q^{\ast }\in M$, and consider $W=0$, by taking

$$u\left(t\right)={exp}_{q\left(t\right)}^{-1}\left(q^{\ast }\right)-k\dot{q}\left(t\right)$$E45

we get

$$\frac{d}{\mathit{dt}}\left(\frac{1}{2}{\left|\dot{q}\left(t\right)\right|}_{q\left(t\right)}^2+\frac{1}{2}{d}_g^2\left(q\left(t\right),q^{\ast }\right)\right)=-k{\left|\dot{q}\left(t\right)\right|}_{q\left(t\right)}^2$$E46

which ensure regulation $q\left(t\right)\to q^{\ast }$ by Lasalle’s invariance principle, this result is very classical, but what we will prove in Section 6, is that this Riemannian PD-regulator is optimal for a natural cost.

Now we are interested by the regulation of the tool of a robot [13], the tool is the terminal organ of the robot, the control of the tool of a robot is frequent tackled problem in industry (Figure 12).

Figure 12.

Regulation of the tool.

The position of the tool is modelised by a function $x:M\to {\mathbb{R}}^3$, we call the workspace $W_s=x\left(M\right)$ the range of x, that is the set of all possible positions that can be tooken by the tool.

A singular configuration is a configuration where the tool can not moves locally in all directions, that is a configuration $q\in M$ where $T_{q}x$ is not onto.

We have the following result that relates absence of singularities to the possibility of regulation of the tool [10].

Proposition 2 Suppose that the tool function is without singularities, take $x_d\in W_s$ and

$$u=-\nabla V-\mathit{kv}$$E47

where$V\left(q\right)=\frac{1}{2}{‖x\left(q\right)-x_d‖}^2$, this controle stabilize the tool to$x_d$whith 0 velocity.

Proof E: quilibrium configurations of this dynamics are critical points of V, which are $q\in M$ such that

$$T_{q}V\left(v\right)=\left(x\left(q\right)-x_d\right).{\mathit{dx}}_q\left(v\right)=0$$E48

since q is not singular, we must have $q\in x^{-1}\left(x_d\right)$.

Since the function $\frac{1}{2}{\left|v\right|}_q^2+V\left(q\right)$ satisfies Lasalle’s invariance principle hypethesis ans

$$\frac{d}{\mathit{dt}}\left(V\left(q\left(t\right)\right)+\frac{1}{2}{\left|\dot{q}\left(t\right)\right|}_{q\left(t\right)}^2\right)=-k{\left|\dot{q}\left(t\right)\right|}_{q\left(t\right)}^2\le 0$$E49

The large subset invariant subset $M\times 0$ is $\mathrm{\Omega }=x^{-1}\left(x_d\right)\times 0$. W

Now we turn to the regulation under constraints [13], if one wants to ensure regulation of the robot that leaves the tool in a safety area $S={\mathrm{\Phi }}^{-1}\left(0\right)$ where $\mathrm{\Phi }:{\mathbb{R}}^3\to \mathbb{R}$ is a submersion on $x\left(M\right)$ (Figure 13).

Figure 13.

Regulation under constraints.

We reduce the problem $x\left(q\left(t\right)\right)\in S$ to $q\left(t\right)\in N$ where $N=x^{-1}\left(S\right)$ is a hypersurface of M, the same calculus of variations done in the previous section allow us to see that

$$\frac{D\dot{q}}{\mathit{Dt}}+\nabla U\left(q\right)=C\in T_q{N}^{\perp }$$E50

C can be seen as a contact force which force the configuration to stay in N (Figure 14)

Figure 14.

Contact force.

Thanks to an inverse calculation [10] one can show that the expression of the unique orthogonal component that allow the tool to remain in s is given by

$$u\left(q,v\right)=\frac{-<D_v\nabla \mathrm{\Psi }\left(q\right),v>}{{\left|\nabla \mathrm{\Psi }\left(q\right)\right|}^2}\nabla \mathrm{\Psi }\left(q\right),$$E51

where $\mathrm{\Psi }=x\circ \mathrm{\Phi }$.

In some papers, the problem of tracking is tackled, and the most useful result is this one [14].

Proposition 3For the dynamic$\frac{D{q}^{\prime }}{\mathit{Dt}}=u$, let a smooth trajectory reference on M, the regulator$u=u_{\mathit{FF}}+u_{\mathit{PD}}$decreases the function

$$t\in \mathbb{R}\to \frac{1}{2}{d}_g^2\left(q_{\mathit{ref}}\left(t\right),q\left(t\right)\right)+\frac{1}{2}{\left|v\left(t\right)-P(q\left(t\right),q_{\mathit{ref}}\left(t\right))v_{\mathit{ref}}\left(t\right)\right|}^2$$E52

where

$$u_{\mathit{FF}}\left(q,v\right)=\frac{d}{\mathit{dt}}P\left(q,q_{\mathit{ref}}\left(t\right)\right)v_{\mathit{ref}}\left(t\right)+D_{v}P\left(q,q_{\mathit{ref}}\right)v_{\mathit{ref}},$$E53

$$u_{\mathit{PD}}={exp}_q^{-1}\left(q_{\mathit{ref}}\left(t\right)\right)-k.\left(v-P\left(q,q_{\mathit{ref}}\right)v_{\mathit{ref}}\right).$$E54

5. The Riemannian observer

Another application of this formalism is the conception of observers of velocity. We want to get an estimation $t\to \widehat{v}\left(t\right)$ of the velocity with the only data of the configuration $q\left(t\right)$.

We have explained in the second section that negative curvature is closely related to the sensitivity to the initial conditions of the geodesic flow. So for free systems, if we do not take into account the curvature, the observer will diverge if the observed initial velocity is far from the initial real velocity, even if the two configurations are very close.

The idea [15] is to copy the dynamic of the system and add a correction that cancel the curvature term that appears naturally in the derivation of the linearized equation around the real trajectory (Jacobi equation when absence of external forces). The observer of the dynamic

$$\frac{D{q}^{\prime }}{\mathit{Dt}}=S\left(q\right),$$

is

$$\begin{array}{c}\frac{d\widehat{q}}{\mathit{dt}}=\widehat{v}+k_1{exp}_{\widehat{q}}^{-1}\left(q\left(t\right)\right),\\ \frac{D\widehat{v}}{\mathit{Dt}}=P\left(\widehat{q},q\left(t\right)\right)\left(S\left(q\left(t\right)\right)\right)+R\left(\widehat{v},{exp}_{\widehat{q}}^{-1}\left(q\left(t\right)\right)\right)\widehat{v}+k_2{exp}_{\widehat{q}}^{-1}\left(q\left(t\right)\right).\end{array}$$

We can compute the form of this observer in the case of a Lie group furnished with a left invariant metric [12], this will give for a rigid body dynamic⁹

$$\dot{\widehat{R}}=\widehat{R}.\left(\widehat{\mathrm{\Omega }}-2j\left(\zeta \right)\right),$$E55

$$\begin{array}{l}\dot{\widehat{\mathrm{\omega }}}=I^{-1}.\left[\left(I.\widehat{\mathrm{\omega }}\right)\times \widehat{\mathrm{\omega }}-\left(I\widehat{\mathrm{\omega }}\times \zeta +\mathit{I\zeta }\times \widehat{\mathrm{\omega }}\right)\right]+\zeta \times \widehat{\mathrm{\omega }}-2\zeta -2.{\widehat{R}}^T.R.\\ I^{-1}{\left[\mathit{\text{skew}}\left(R\right)\right]}^{\times }+R\left(\widehat{\mathrm{\omega }},\zeta \right)\widehat{\mathrm{\omega }},\end{array}$$E56

with $\zeta =I^{-1}.{\left[\mathit{log}\left(R^T.\widehat{R}\right)\right]}^{\times }$.

With the same conditions as the Luenberger observer of the first section, we get for the Riemannian observer the following simulations (Figure 15).

Figure 15.

Convergence of the Riemannian observer.

6. Optimal regulation and tracking

In this section we are concerned by the optimal control of robots, we consider the dynamics

$$\frac{D\dot{q}}{\mathit{Dt}}=u$$E57

and we consider the cost

$$J\left(u\right)={\int }_0^{\infty }{\left[\frac{1}{2}{\left|v_u\right|}^2\right]}_g+\frac{1}{2}{d}_g{\left(q_u\left(t\right),q^{\ast }\right)}^2+{\left|u\right|}_g^2]e^{-\mathit{\gamma t}}\mathit{dt}$$E58

where $\left(q_u,v_u\right)$ is the unique solution to the dynamics (after fixing an initial position and velocity).

The HJB theory gives that the feedback control [10]

$$u_{\mathit{PD}}=k{exp}_{q\left(t\right)}^{-1}\left(q^{\ast }\right)-k^{\prime }\dot{q}\left(t\right)$$E59

where $k,k^{\prime }$ solves an appropriate Riccati equation, is the unique that solves the previous problem.

For the tracking problem, we have the following result [10].

Proposition 4Let a smooth reference trajectory$q_{\mathit{ref}}:\left[0,T\right]\to M$, the tracking regulator that minimizes the cost

$$J\left(u\right)={\int }_0^T\left[\frac{1}{2}{d}_g^2\left(q_{\mathit{ref}}\left(t\right),q_u\left(t\right)\right)+\frac{1}{2}{\left|v_u\left(t\right)-P(q_u\left(t\right),q_{\mathit{ref}}\left(t\right))v_{\mathit{ref}}\left(t\right)\right|}^2+\frac{1}{2}{\left|u-u_{\mathit{FF}}(t,q,v)\right|}^2\right]\mathit{dt},$$E60

is a PD + Feed forward regulator $u=u_{\mathit{FF}}+u_{\mathit{PD}}$ where

$$u_{\mathit{FF}}\left(t,q,v\right)=\frac{d}{\mathit{dt}}P\left(q,q_{\mathit{ref}}\left(t\right)\right)v_{\mathit{ref}}\left(t\right)+D_{v}P\left(q,q_{\mathit{ref}}\left(t\right)\right)v_{\mathit{ref}})\left(t\right),$$E61

$$u_{\mathit{PD}}\left(t,q,v\right)=k_1\left(t\right){exp}_q^{-1}\left(q_{\mathit{ref}}\left(t\right)\right)-k_2\left(t\right)\left[v-P\left(q,q_{\mathit{ref}}\right)v_{\mathit{ref}}\left(t\right)\right],$$E62

avec$\left(k_1,k_2\right)$solves an appropriate Riccati equation.

For the applications to a rigid body motion [11, 16]

$$R^{\prime }=R\mathrm{\Omega },$$E63

$${\underset{-}{\omega }}^{\prime }=I^{-1}.\left(I\mathrm{\omega }\times \mathrm{\omega }\right)+\tau .$$E64

We have the following PD that ensures optimal regulation to $R_d$

$${\tau }_{\mathit{PD}}=-k_P{\left[log\left(R_d^T.R\right)\right]}^{\times }-k_D\mathrm{\omega }.$$E65

For the optimal tracking problem, the PD + FF regulator is

$${\tau }_{\mathit{PD}}=-k_P{\left[log\left(R_{\mathit{ref}}^{T}R\right)\right]}^{\times }-k_D.\left(\mathrm{\omega }-R^T{R}_{\mathit{ref}}.{\mathrm{\omega }}_{\mathit{ref}}\right),$$E66

$$\begin{array}{c}{\tau }_{\mathit{FF}}=\frac{1}{2}\left(\mathrm{\omega }\times \left(R^T{R}_{\mathit{ref}}.{\mathrm{\omega }}_{\mathit{ref}}\right)-I^{-1}.\left(\left(I,.,R^T,R_{\mathit{ref}},{\mathrm{\omega }}_{\mathit{ref}}\right)\times \mathrm{\omega }+\left(I,.,\mathrm{\omega }\right)\times \left(R^T,R_{\mathit{ref}},{\mathrm{\omega }}_{\mathit{ref}}\right)\right)\right)\end{array}$$E67

7. Conclusion

Classical robots allow us to ensure local regulation of the configuration and optimal control of kinematics of rigid body. However, some observations shows that it do not fit well with regulation under constraints and optimal regulation of the dynamics. We cannot explain the origin of the sensitivity with respect to initial conditions, which is present in most robotic systems and prevent us to design performant observers.

We have introduced the geometric formalism with a comprehensive way, and we explained the origin of the sensitivity with respect to initial conditions by the notion of curvature, that we avoid by the Riemannian observer.

We have exposed how to ensure regulation under constraints under a hypothesis of absence of singularities, and we showed how to ensure optimal regulation and tracking by algorithmic methods.

However, this method requires fastidious computations and sometimes difficult, approximations are quiete often used, and by doing that we see that we recover regulators that are proposed by the Euclidean approach.

We can see the classical robotics as an approximation of the geometric robotics, a bit like newtonian mechanics is the approximation of general relativity.

Geometric robotics is a field active in research, and most of recent results are presented in this chapter. Open problems remains in soft robotics, where we control a quasilinear hyperbolic PDE written in the Lie algebra of the group of Euclidean isometries, for this purpose one can see the work of [17] and the reference within.