unicycle model and control

8/13/2019 Unicycle Model and Control

1/27


2/27

q(t) t [ts, te] q(s) s(t)

AT(q)q= 0

s(t)

q =dq

=dq

s = q s


3/27


4/27

k < n h(q, q) = 0

hi(q) = 0 i= 1,...,k

h() : C R G Rnk G qA G qB C\G h()

qB, i(qA) : C\G C i= 1,...,k

G C\G

ai(q,q) = 0 i= 1,...,k

a(q, q) = 0 AT(q)q= 0

A() ai() : C Rn

k = 1

aT(q) q= 0

h(q) =c

h(q) = qh(q) q = 0


5/27

C

n k

ki=1

aTi (q)q j = 1,...,p k

= spanh1(q)

q , ...,

hp(q)

q spanaT1(q),..., a

Tk(q)

det() = 0)

h1(q) =c1,...,hp(q) =cp

C Rn

G Rnp

p= k

det()= 0

AT(q)

N(AT(q)) = [g1(q),...,gnk(q)] =G(q)


6/27

sin() 0cos() 0

0 1

T

v

, q=

xy

cos() sin() 0sin() cos() 0

sin()/l 00 1

T

v1F v2

, q=

xy

cos() 0sin() 0

tan()/l 00 1

T

v1R v2

, q=

xy

vn=0


7/27

x

y

/2

y=v sin

x=v cos.

.

v

AT(q)q=

sin() cos() 0

q= 0

AT()

N(AT(q)) = g1 g2

=

cos() 0sin() 0

0 1

xy

=

cos()sin()

0

v+

001

[g1,g2] = g2

qg1

g1q

g2=

= [0]

cos()sin()

0

0 0 sin()0 0 cos()

0 0 0

001

=


8/27

l

x

y

xf

yf

x sin() y cos() = 0

xf sin(+) yf cos(+) = 0

xf =x+l cos()yf =y +l sin()

xf = x l sin()yf = y+l cos()

xf sin(+) yf cos(+) = 0

( l i ()) i ( + ) ( + l ()) ( + ) 0


9/27

AT(q)q=

sin() cos() 0 0sin(+) cos(+) l cos() 0

xy

= 0

AT nk= dim(q)rk(AT) = 2

N(AT) = g1 g2

=

cos()cos() 0

sin()cos() 0sin()/l 0

0 1

q = g1(q) u1+ g2(q) u2

xy

=

cos()cos()sin()cos()

sin()/l0

u1+

00

01

u2

u2 u1

u1= v

xy

=

cos()cos()sin()cos()

sin()/l0

v+

0001

= g1v+ g2

RA=3= g1 g2 [g1,g2] [g1, [g1,g2]]

[g1,g2] =g2

qg1

g1q

g2= cos()cos() 0 0 sin()cos() cos()sin() 0


10/27

0 0 sin()cos() cos()sin()

0 0 cos()cos() sin()cos()0 0 0 cos()/l0 0 0 0

cos()sin()

sin()sin() cos()/l0

=

=

(sin2() + cos2()) sin()/l(sin2() + cos2()) cos()/l

00

=

sin()/lcos()/l

00

RA=3=

cos()cos() 0 cos()sin() sin()/lsin()cos() 0 sin()sin() cos()/l

sin()/l 0 cos()/l 00 1 0 0

det(RA) = 1

l2 = 0

3

u1= v

cos()

xy

=

cos()sin()

tan()/l0

v+

0001

= g1v+ g2

[g1,g2] =g2

qg1

g1q

g2=

=

0 0 sin() 00 0 cos() 00 0 0 1/(l cos2())

0 0 0 0

000

1

[0]

cos()sin()

tan()/l

0

=

00

1/(l cos2())

0

[g1, [g1,g2]] = [g1,g2]

q g1

g1q

[g1,g2] =

0 0 0 00 0 0 00 0 0 2 tan()/ cos2()

cos()

sin()tan()/l

+


11/27

=

2 +k k N

3

z

q

e= yd(t) y(t)

C

v

r/2 r/2

r


12/27

zi = zi+1 i= 1, ..., r 1

zr = v

zj = q(z) j = r + 1, ..., n

y = z1

r z r

z = (x) =

h(x)Lfh(x)

Lr1f h(x)r+1(x)

n(x)

u = 1

LgLn1f h(x)

(Lnfh(x) +v)

h(x)

[g(x), adfg(x), ..., adn1f g(x)] n

span{g(x), adfg(x), ..., adn2f g(x)}

(2 input, n output)

r n

z= 1(z) v1+2(z) v2

1(z) = [1, 0, z2, z3, ..., zn]T

2(z) = [0, 1, 0, ..., 0]T

adk12(z) = [1, adk11

2(z)] =

0

(1)k

0

@ position k+ 2


13/27

dim(0) =n = 3 {1, 2} h1(q)

dh1

dh1 1, i = 0 i= {1, 2}

dh1 g1 = 1

z1 = h1

z2 = Lg1h2

z3 = h2

h2(q) h1(q)

h2 2= 0

v1 = u1

v2 = L2

g1h2u1+Lg2Lg1h2u2

xy

=

00

1

+

cos()sin()

0

v

{0, 1, 2}

adg1g2 = [g1, g2] = g2

qg1 g

1

qg2=

=

0 0 sin()0 0 cos()

0 0 0

001

[0]

cos()sin()

0

=

=

sin()cos()

0

0 = span

001

cos()sin()

0

sin()cos()

0

= span

cos()sin()

sin()cos()


14/27

dh1= 0 0 1

dh1 1, 1=

0 0 1

cos()sin()

0

= 0 True

dh1 1, 2=

0 0 1

sin()cos()

0

= 0 True

dh1 g1=

0 0 1

001

= 1 True

h1 h2(q)

h2(q) =x sin() y cos()

dh2=

sin() cos() x cos() +y sin()

dh2 2 =


cos()sin()

0

=

= sin()cos() sin()cos() = 0 True

Lg1h2 = h2

q g1=

=


001

=

= x cos() +y sin()

z= T(q) = x cos() +y sin()x sin() y cos()

L2g1h2 =

q(Lg1h2) g1 =

cos() sin() y cos() x sin()

00

= y cos()


15/27

q= T1(z) =

z2 cos(z1) +z3 sin(z1)z2 sin(z1) z3 cos(z1)z1

= v1

v = v2+z3v1

dz1dt = = v1

dz2dt

= x cos() x sin() + y sin() +y cos() =

= ( xv cos()

cos() + yv sin()

sin()) + (y cos() x sin()) =

=

v cos2() +v sin2()

+ (y cos() x sin())=

= v+ (y cos() x sin())== v+z3 =

= v2

dz3dt

= x sin() +x cos() y cos() +y sin() =

= (x sin() y cos())

=0(constraint)+ (x cos() +y sin()) =

= z2 == z2v1

z1 = v1

z2 = v2

z3 = z2v1

{h1, h2}

z1

z2

z3 z2


16/27

xy

=

cos() 0sin() 0

0 1

v

= [g1 g2]u

y1y2

=

x+b cos()

y+b sin()

=

h1h2

Lg1h1= h1

qg1=

1 0 bsin()

cos()sin()0

= cos()

Lg2h1= h1

qg2=

1 0 bsin()

001

= bsin()

Lg1h2= h2

qg1=

1 0 bcos()

cos()sin()0

= sin()

Lg2h2= h2

qg2=

1 0 bcos()

001

= b cos()

A(q) = cos() b sin()

sin() bsin()

det(A) =b >0

A1(q) =

cos() sin() sin()/b cos()/b

u = A1v

v = v1cos() +v2sin()1bsin() v1+

1bcos() v2

z1

z2

=

x+b cos()y+b sin()

xy

=

z1 bcos(z3)z2 b sin(z3)


17/27

e= yd y

v=

z1d k1(z1d z1)z2d k2(z2d z2)

k1, k2 k1> 0 k2> 0

xy

=

z1 b cos()z2 bsin()

z3

r

l

=

1/r d1/r d

cos(z3) b sin(z3)

sin(z3) b cos(z3)

v1v2

z1

6

8

10

12

14

16

y[m]

Unicycle model


18/27

0 2 4 6 8 101.5

1

0.5

0

0.5

1

time [s]

e

rror

Error

eZ1

eZ2

eZ3

12

10

8

6

4

2

0

2

4

6

Con

trolAction

Control Action

V1


19/27


20/27


21/27


22/27

e1vs e2 5 rand([1, ..., 1], = 0, 2 = 1)

0

1

2

3

]

Unicycle model

trajectory desired

trajectory followed


23/27

0 2 4 6 8 10

0.45

0.4

0.35

0.3

0.25

0.2

0.15

0.1

0.05

0

0.05

time [s]

error[m]

Error

ex[m]

ey[m]

40

50

60

70

80

ed[rad/s]

Differential drive control action

r

l


24/27

0.45 0.4 0.35 0.3 0.25 0.2 0.15 0.1 0.05 0

0.45

0.4

0.35

0.3

0.25

0.2

0.15

0.1

0.05

0

0.05

eZ1

eZ2

Error 1 vs 2

e1 vs e2

0 4

0.35

0.3

0.25

0.2

0.15

0.1

0.05

0

0.05

error[m]

Position errors for system without random errors on input

eZ1

[m]


25/27


26/27


27/27

unicycle model and control

Documents