Local controllability of trident snake robot based on sub-Riemannian extremals

Note Mat. 37 (2017) suppl. 1, 93–102. doi:10.1285/i15900932v37suppl1p93

Local controllability of trident snake robot based on sub-Riemannian extremals

Jaroslav Hrdina

ⁱ

Institute of Mathematics, Brno University of Technology hrdina@fme.vutbr.cz

Received: 29-07-2016; accepted: 04-11-2016.

Abstract.

To solve trident snake robot local controllability by differential geometry tools, we construct a privileged system of coordinates with respect to the distribution given by Pffaf system based on local nonholonomic conditions and, furthermore, we construct a nilpotent approximation of the transformed distribution with respect to the given filtration. We com- pute normal extremals of sub-Riemanian structure, where the Hamiltonian point of view was used. We demonstrated that the extremals of sub-Riemannian structure based on this distri- bution play the similar role as classical periodic imputs in control theory with respect of our mechanism.

Keywords:

local controllability, nonholonomic mechanics, planar mechanisms, sub–Rieman- nian geometry, differential geometry.

MSC 2010 classification:

primary 53A17, secondary 70H05, 70Q05

1 Introduction

Originally, the general trident snake robot has been introduced in [5]. It is a planar robot with a body in the shape of a triangle and with three legs consisting of ` links. Then, its simplest non–trivial version (see figure 1), corresponding to ` = 1, has been mainly discussed, see e.g. [6],[4]. Local controllability of such robot is given by the appropriate Pfaff system of ODEs. The solution with respect to

˙

q = ( ˙ x, ˙ y, ˙ θ, ˙ Φ

₁

, ˙ Φ

₂

, ˙ Φ

₃

)

gives a control system ˙ q = Gu. In the case length one links the control matrix G is a 6 × 3 matrix spanned by vector fields g

1

, g

2

, g

3

, where

g

₁

= cos θ∂

_x

− sin θ∂

_y

+ sin Φ

₁

∂

_Φ1

+ sin(Φ

₂

+

^2π₃

)∂

_Φ2

+ sin(Φ

₃

+

^4π₃

)∂

_Φ3

, g

₂

= sin θ∂

_x

+ cos θ∂

_y

− cos Φ

₁

∂

_Φ1

− cos(Φ

₂

+

^2π₃

)∂

_Φ2

− cos(Φ

₃

+

^4π₃

)∂

_Φ3

, g

3

= ∂

_θ

− (1 + cos Φ

₁

)∂

_Φ1

− (1 + cos Φ

₂

)∂

_Φ2

− (1 + cos Φ

₃

)∂

_Φ3

iThis work was supported by a grant of the Czech Science Foundation (GA ˇCR) no. 17–

21360S.

http://siba-ese.unisalento.it/ c 2017 Universit` a del Salento

φ

₁

φ

2

φ

₃

θ

x y

S[x, y]

Figure 1. 1–link trident snake robot model

and u : R → R

³

is the control of our system. It is easy to check that in regu- lar points these vector fields define a (bracket generating) distribution H with growth vector (3, 6). It means that in each regular point the vectors g

1

, g

2

, g

3

together with their Lie brackets span the whole tangent space. Consequently, the system is controllable by Chow–Rashevsky theorem.

The sub-Riemannian structure on manifold M is a generalization of Rie- mannian manifold. Given a distribution H ⊂ T M equipped with metrics based on control u we can define so called Carnot-Carath´ eodory metric on M and sub-Remannian Hamiltonian.

2 Nilpotent aproximation

In order to simplify the trident snake robot control we construct a nilpotent approximation of the transformed distribution with respect to the given filtra- tion. Note that all constructions are local in the neighborhood of 0. Following [3], we group together the monomial vector fields of the same weighted degree and thus we express g

i

, i = 1, 2, 3 as a series

g

_i

= g

⁽⁻¹⁾_i

+ g

⁽⁰⁾_i

+ g

_i⁽¹⁾

+ · · · ,

where g

_i^(s)

is a homogeneous vector field of order s. Then the following proposi- tion holds, [7]. Set X

_i

= g

_i⁽⁻¹⁾

, i = 1, 2, 3. The family of vector fields (X

₁

, X

₂

, X

₃

) is a first order approximation of (g

1

, g

2

, g

3

) at 0 and generates a nilpotent Lie algebra of step r = 2, i.e. all brackets of length greater than 2 are zero. In our case [3], we obtain the following vector fields:

X

1

= ∂

x

+



− y 2



∂

a

+



− y 2 − d 

∂

b

+



− x 2



∂

c

, X

2

= ∂

y

+

 x 2



∂

a

−  x 2



∂

_b

+

 y 2 − d 

∂

c

, X

₃

= ∂

_d

.

with respect to a new coordinates (x, y, d, a, b, c). In particular, the family of vector fields (X

1

, X

2

, X

3

) is the nilpotent approximation of (g

1

, g

2

, g

3

) at 0 as- sociated with the coordinates (x, y, d, a, b, c). The remaining three vector fields are generated by Lie brackets of (X

1

, X

2

, X

3

). Note that due to linearity of the three latter coordinates of (X

1

, X

2

, X

3

), the coordinates of (X

4

, X

5

, X

6

) must be constant. We get

X

4

= ∂

a

, X

5

= ∂

b

, X

₆

= ∂

_c

.

Note that the vector fields (X

1

, X

2

, X

3

, X

4

, X

5

, X

6

) in (x, y, θ, Φ

1

, Φ

2

, Φ

3

) coordinates are of the form

X

₁

= ∂

_x

+ θ∂

_Φ1

− 

−

√ 3 2

+

^θ₂



∂

_Φ2

− 

−

√ 3 2

+

₂^θ



∂

_Φ3

, X

2

= ∂

y

− ∂

_Φ1

+



−

¹₂

+

√3θ 2



∂

_Φ2

− 

−

¹₂

+

√3θ 2



∂

_Φ3

, X

3

= ∂

θ

− 2∂

_Φ1

− 2∂

_Φ2

− 2∂

_Φ3

X

4

= ∂

Φ1

+ ∂

Φ2

+ ∂

Φ3

, X

₅

= −

√ 3 2

∂

_Φ2

+

√ 3 2

∂

_Φ3

, X

₆

= −∂

_θ

+

¹₂

∂

_Φ2

+

¹₂

∂

_Φ3

.

To show how nilpotent approximation affects on integral curves of the distri- butions and the resulting control we computed the Lie brackets of relevant vector fields. In Fig. 2, there is a comparison of the Lie bracket g

5

motions realized in (x, y, θ, Φ

₁

, Φ

₂

, Φ

₃

) coordinates (dotted line) and in nilpotent approximation.

Fig. 3 show the comparison of g

₆

motions. The following figures show the trajec-

tories of the root center point, vertices and wheels when a particular Lie bracket

motion is realized.

-1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 -1.5

-1 -0.5 0 0.5 1 1.5

Figure 2. g

₅

motion

-2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5

-1.5 -1 -0.5 0 0.5 1 1.5 2

Figure 3. g

₆

motion

3 sub-Riemannian Hamiltonian

Note that the functions in C

^∞

(M ) are in one–to–one correspondence with functions in C

^∞

(T

^∗

M ) that are constant on fibers:

C

^∞

(M ) ∼ = C

_const^∞

(T

^∗

M ) = {π

^∗

α|α ∈ C

^∞

(M )} ⊂ C

^∞

(T

^∗

M ),

where π : T

^∗

M → M denotes the canonical projection. In what follows, with no abuse of notation, we often identify the function π

^∗

α ∈ C

^∞

(T

^∗

M ) with the function α ∈ C

^∞

(M ). In a similar way, smooth vector fields on M are in a one–

to–one correspondence with functions in C

^∞

(T

^∗

M ) that are linear on fibers via the map Y 7→ a

_Y

, where a

_Y

(λ) := hλ, Y (q)i and q = π(λ), i.e.

V ec(M ) ∼ = C

_lin^∞

(T

^∗

M ) = {a

_Y

|Y ∈ V ec(M )} ⊂ C

^∞

(T

^∗

M ).

Our mechanism can be understood as a model of sub-Riemannian structure of constant rank, i.e. the triple (M, D, h·, ·i), where D is a vector subbundle of T M locally generated by family of vector fields {f

₁

, . . . , f

_m

} (in our case gen- erated by X

₁

, X

₂

and X

₃

) and h·, ·i

_q

is a family of scalar products on D

_q

. Then the sub-Riemannian Hamiltonian is the function on T

^∗

M defined as follows [1]

h : T

^∗

M → R h(λ) = max

u∈Uq



hλ, f

_u

(q)i − 1 2 |u|

²



, q = π(λ), where f

_u

= P

i

u

_i

f

_i

. For every generating family {f

₁

, . . . , f

_m

} of the sub- Riemannian structure, the sub-Riemannian Hamiltonian H can be rewritten [1] as follows

h(λ) = 1 2

X

i

hλ, f

_i

(q)i

²

, λ ∈ T

_q^∗

M, q = π(λ).

To simplify our equations below we can define the Poisson bracket as an binary operation {, } on functions in C

^∞

(T

^∗

M ). In fact, we can define

{a

_X

, a

_Y

} := a

_{[X,Y ]}

(1)

and there exists a unique bilinear and skew–symmetric map {·, ·} : C

^∞

(T

^∗

M ) × C

^∞

(T

^∗

M ) → C

^∞

(T

^∗

M )

that extends Poisson bracket (1) on C

^∞

(T

^∗

M ), and that is a derivative (i.e.

satisfies the Lipschitz rule) in each argument. Let (x, p) denote coordinates on T

^∗

M , the formula for Poisson bracket of two functions a, b ∈ C

^∞

(T

^∗

M ) reads

{a, b} =

n

X

i=1

∂a

∂p

i

∂b

∂x

i

− ∂a

∂x

i

∂b

∂p

i

and the Hamiltonian vector field associated with the smooth function a ∈ C

^∞

(T

^∗

M ) is defines as the linear operation

~a : C

^∞

(T

^∗

M ) → C

^∞

(T

^∗

M ), ~a(v) = {a, b}.

We can easily write the coordinate expression of ~a for an arbitrary function a ∈ C

^∞

(T

^∗

M ) as

~a = {a, ·} =

n

X

i=1

∂a

∂p

i

∂

∂x

i

− ∂a

∂x

i

∂

∂p

i

.

Let γ : [0, T ] → M be an admissible curve with respect to controlling dis- tribution which is lenght-minimizer, parametrized by constant speed. Let ¯ u be the corresponding minimal control. Then there exists a Lipschitz curve λ(t) ∈ T

_γ(t)^∗

M such that

˙λ =

m

X

i=1

¯

u

_i

(t)~h

_i

(λ(t)) = ~h(t)(λ(t)) = {h, λ},

where ~h = P

m

i=1

u ¯

_i

(t)~h

_i

. On the controlling vector fields X

₁

, X

₂

and X

₃

and the general 1–form λ ∈ T

^∗

M

λ = λ

_x

dx + λ

_y

dy + λ

_d

dd + λ

_a

da + λ

_b

db + λ

_c

dc we define functions h

i

= hλ, X

i

i

h

₁

(λ) := λ

_x

− y

2 λ

_a

−  y 2 + d 

λ

_b

− x 2 λ

_c

, h

₂

(λ) := λ

_y

+ x

2 λ

_a

− x

2 λ

_b

+  y 2 − d 

λ

_c

,

h

3

(λ) := λ

_d

.

In our case the sub-Riemannian Hamiltonian with respect to corresponding minimal control is then of the form

h(λ) = u

₁

h

₁

(λ) + u

₂

h

₂

(λ) + u

₃

h

₃

(λ)

= u

₁

 λ

_x

− y

2 λ

_a

−  y 2 + d 

λ

_b

− x 2 λ

_c

 + u

₂



λ

_y

+ x 2 λ

_a

− x

2 λ

_b

+  y 2 − d 

λ

_c



+ u

₃

λ

_d

.

In canonical coordinates (p, x), the Hamiltonian vector filed associated with h is expressed as follows

~h = {h, ·} =

n

X

i=1

∂h

∂p

i

∂

∂x

i

− ∂h

∂x

i

∂

∂p

i

and the Hamiltonian system ˙λ = ~h(λ) is rewritten as

˙ x

_i

= ∂h

∂p

i

, ˙ p

_i

= − ∂h

∂x

i

i.e. in our case

˙ x = ∂h

∂λ

_x

= ∂(u

₁

h

₁

+ u

₂

h

₂

+ u

₃

h

₃

)

∂λ

_x

= u

₁

˙ y = ∂h

∂λ

_y

= ∂(u

₁

h

₁

+ u

₂

h

₂

+ u

₃

h

₃

)

∂λ

_y

= u

₂

d = ˙ ∂h

∂λ

d

= ∂(u

₁

h

₁

+ u

₂

h

₂

+ u

₃

h

₃

)

∂λ

d

= u

₃

˙λ

_x

= − ∂h

∂x = − ∂(u

₁

h

₁

+ u

₂

h

₂

+ u

₃

h

₃

)

∂x = 1

2 (−λ

_c

u

₁

+ (λ

_a

− λ

_b

)u

₂

)

˙λ

_y

= − ∂h

∂y = − ∂(u

1

h

1

+ u

2

h

2

+ u

3

h

3

)

∂y = 1

2 (λ

a

− λ

_b

)u

1

+ (λ

c

)u

2

)

˙λ

_d

= − ∂h

∂d = − ∂(u

₁

h

₁

+ u

₂

h

₂

+ u

₃

h

₃

)

∂d = 1

2 (−λ

_b

u

₁

− (λ

_c

)u

₂

)

˙λ

_a

= 0 ˙a = − y

2 u

₁

+ x 2 u

₂

˙λ

_b

= 0 ˙b = −( y

2 + d)u

₁

− x 2 u

₂

˙λ

_c

= 0 ˙c = − x

2 u

1

− ( y

2 − d)u

₂

A function a ∈ C

^∞

(T

^∗

M ) is a constant of the motion of the Hamiltonian system associated with h ∈ C

^∞

(T

^∗

M ) if and only if {h, a} = 0. If γ : [0, T ] → M is a length minimizer on sub-Riemannian manifold, associated with a control u(·), then due the Pontryagin maximum principle there exists λ

₀

∈ T

_γ(0)^∗

M such that defining

λ(t) = (P

_0,t⁻¹

)

^∗

λ

0

, λ(t) ∈ T

_γ(t)^∗

M, where P

0,t

is the flow of the nonautonomous vector field

X

_u(t)

=

m

X

i=1

u

i

(t)X

i

(γ(t))

and one of the following condition is satisfied

(N ) u

i

(t) = hλ(t), X

i

(γ(t))i, ∀i = 1, . . . , m, (A) 0 = hλ(t), X

i

(γ(t))i, ∀i = 1, . . . , m.

If λ(t) satisfies (N) then it is called normal extremal (and γ(t) a normal extremal trajectory). If λ(t) satisfies (A) then it is called abnormal extremal (and γ(t) a abnormal extremal trajectory).

4 Normal extremals

Following [1], every normal extremal is a solution of the Hamiltonian system

˙λ = ~h(λ(t)), i.e. in our case of the system

˙λ = ~h(λ(t)) = 1

2 (h

²₁

(λ) + h

²₂

(λ) + h

²₃

(λ)), h

_i

(λ) = hλ, X

_i

(q)i, and let us introduce

h

₄

(λ) := hλ, X

₄

(q)i, h

₅

(λ) := hλ, X

₅

(q)i, h

₆

(λ) := hλ, X

₆

(q)i.

Since X

1

, . . . , X

6

are linearly independent then {h

1

, . . . , h

6

} defines a system

coordinates on T

^∗

M and thus we can consider λ to be parametrized by h

_i

consequently, we are looking for a solution of the system ˙h

_i

= {h, h

_i

}, i.e.

˙h

₁

= {h, h

₁

(t)} = {h

²₁

+ h

²₂

+ h

²₃

, h

₁

} = {h

₂

, h

₁

}h

₂

+ {h

₃

, h

₁

}h

₃

= −h

₄

h

₂

− h

₅

h

₃

˙h

₂

= {h, h

₂

(t)} = {h

²₁

+ h

²₂

+ h

²₃

, h

₂

} = {h

₁

, h

₂

}h

₁

+ {h

₃

, h

₂

}h

₃

= h

₄

h

₁

− h

₆

h

₃

˙h

₃

= {h, h

3

(t)} = {h

²₁

+ h

²₂

+ h

²₃

, h

3

} = {h

₁

, h

3

}h

₁

+ {h

2

, h

3

}h

₂

= h

5

h

1

+ h

6

h

2

˙h

4

= ~h(h

4

(t)) = {h, h

4

(t)} = {h

²₁

+ h

²₂

+ h

²₃

, h

4

} = 0

˙h

5

= ~h(h

5

(t)) = {h, h

5

(t)} = {h

²₁

+ h

²₂

+ h

²₃

, h

5

} = 0

˙h

₆

= ~h(h

₆

(t)) = {h, h

₄

(t)} = {h

²₁

+ h

²₂

+ h

²₃

, h

₆

} = 0

To analyze the system of ordinary differential equations above we have the following assignments:

˙h

₄

= 0 ; h

4

is a constant function ; h

4

= k

₁

,

˙h

₅

= 0 ; h

5

is a constant function ; h

5

= k

₂

,

˙h

₆

= 0 ; h

6

is a constant function ; h

6

= k

3

and the system of linear ordinary differential equations, which can be written in the matrix form as





˙h

₁

˙h

2

˙h

₃



 =





0 −k

₁

k

₂

k

₁

0 −k

₃

−k

₂

k

3

0







 h

₁

h

₂

h

3



 .

The main tool to analyze the systems of linear ODEs, is eigenvalues and eigen- vectors computations. In our case, the straightforward computation

     

−λ −k

₁

k

2

k

₁

−λ −k

₃

−k

₂

k

₃

−λ      

= −λ

³

− λ(k

₁²

+ k

₂²

+ k

²₃

) = −λ(λ

²

+ (k

²₁

+ k

²₂

+ k

²₃

)) = 0 leads to one real and two complex conjugated eigenvalues:

λ

1

= 0, λ

±i

= ±i q

k

²₁

+ k

²₂

+ k

²₃

, where the eigenvectors are

v

0

:=



 k

₃

k

2

k

₁



 , v

i

:=





k

²₁

+ k

²₂

−ipk

²₁

+ k

²₂

+ k

²₃

k

1

+ k

2

k

3

ipk

₁²

+ k

₂²

+ k

²₃

k

₂

− k

₁

k

₃



 ,

v

−i

:=





k

²₁

+ k

₂²

ipk

₁²

+ k

²₂

+ k

²₃

k

₁

− k

₂

k

₃

−ipk

²₁

+ k

²₂

+ k

₃²

k

2

− k

₁

k

3



 ,

respectively. From the theory of linear systems of ODEs in matrix form, the solution can be described as a linear combination of real and imaginary part of u = v

o

exp(0t) + v

i

exp(it) + v

−i

exp(−it). The straightforward computation leads to



 u

₁

u

2

u

₃



 =



 v

₀^x

v

^y₀

v

^d₀



 +





v

_i^x

exp(it) v

^y_i

exp(it) v

^d_i

exp(it)



 +





v

^x_−i

exp(−it) v

^y_−i

exp(−it) v

^d_−i

exp(−it)





=



 k

₃

k

2

k

₁



 +





(k

²₁

+ k

²₂

)(cos(t) + i sin(t)) (−ikk

1

+ k

2

k

3

)(cos(t) + i sin(t))

(ikk

₂

− k

₁

k

₃

)(cos(t) + i sin(t))





+





(k

²₁

+ k

²₂

)(cos(t) − i sin(t)) (ikk

1

− k

₂

k

3

)(cos(t) − i sin(t)) (−ikk

₂

− k

₁

k

₃

)(cos(t) − i sin(t))





=



 k

₃

k

2

k

1



 +





2(k

₁²

+ k

²₂

) cos(t) 2kk

1

sin(t)

−2k

₁

k

3

cos(t) − 2kk

2

sin(t)



 + i



 0 2k

2

k

3

sin(t)

0



 , where k = pk

²₁

+ k

²₂

+ k

²₃

.

5 Local controllability

To perform the Lie bracket motions we apply a periodic input, i.e. for the vector fields X

4

= [X

1

, X

2

], X

5

= [X

1

, X

3

], X

6

= [X

2

, X

3

], respectively, the input

v

₁

(t) = (−Aω sin ωt, Aω cos ωt, 0), v

2

(t) = (0, −Aω sin ωt, Aω cos ωt), v

3

(t) = (−Aω sin ωt, 0, Aω cos ωt)

is applied, because, according to [6], the Lie bracket of a pair of vector fields corresponds to the direction of a displacement in the state space as a result of a periodic input with sufficiently small amplitude A, i.e. the bracket motions are generated by periodic combination of the vector controlling fields. The the- oretic approach above leads to four new control sequences with respect to the parameters k

₁

, k

₂

and k

₃

. These control sequences are the following:

e

₁

(t) = (2k

₁²

cos(t), 2kk

₁

sin(t), −2k

₁

k

₃

cos(t)), e

2

(t) = ((k

₁²

+ k

²₂

) cos(t), 2k

1

k

3

sin(t), −2kk

2

sin(t)), e

_3A

(t) = (2k

₂²

cos(t), 0, −2kk

₂

sin(t)),

e

_3B

(t) = (0, 2k

1

k

3

sin(t), 0).

It is easy to see that by appropriate choice of coordinates k

₁

, k

₂

and k

₃

we can get classical periodic inputs as a normal extremal of the underlying sub–

Riemannian structure. For example, the choice k

1

7→ − √

Aω, k

2

= k

3

= 0 and t 7→ ωt leads to identification e

₁

(t) ∼ = −v

1

(t).

References

[1] A. Agrachev, D. Barilari, U. Boscain, Introduction to Riemannian and sub- Riemannian geometry, Preprint SISSA, 2016

[2] A A Ardentov, Yu L Sachkov, Extremal trajectories in a nilpotent sub- Riemannian problem on the Engel group, Russian Academy of Sciences Sbornik Mathematics 202(11):1593,2012.

[3] Hrdina, J, N´ avrat, A, Vaˇ s´ık, P: Nilpotent approximation of a trident snake robot controlling distribution, arXiv:1607.08500 [math.DG], 2016.

[4] Hrdina, J., N´ avrat, A., Vaˇ s´ık, P., Matouˇ sek, R.: CGA-based robotic snake control, Adv.Appl. Clifford Algebr. (in print), 1–12 online (2016).

[5] Ishikawa, M.: Trident snake robot: locomotion analysis and control. NOL- COS, IFAC Symposium on Nonlinear Control Systems 6, 1169–1174 (2004).

[6] Ishikawa, M., Minami, Y., Sugie, T.: Development and control experiment of the trident snake robot. IEEE/ASME Trans. Mechatronics 15(1), 9–15 (2010).

[7] Jean, F.: Control of Nonholonomic Systems: From Sub–Riemannian Geom- etry to Motion Planning, SpringerBriefs in Mathematics, Springer, 2014.

[8] Murray,R. M., Zexiang,L., Sastry, S. S.: A Mathematical Introduction to Robotic Manipulation, CRC Press, 1994.

[9] Selig, J.M.: Geometric Fundamentals of Robotics, Springer, Monographs in

Computer Science, 2004.