Upload
matteoragni
View
231
Download
0
Embed Size (px)
Citation preview
8/13/2019 Unicycle Model and Control
1/27
8/13/2019 Unicycle Model and Control
2/27
q(t) t [ts, te] q(s) s(t)
AT(q)q= 0
s(t)
q =dq
=dq
s = q s
8/13/2019 Unicycle Model and Control
3/27
8/13/2019 Unicycle Model and Control
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
8/13/2019 Unicycle Model and Control
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)
8/13/2019 Unicycle Model and Control
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
8/13/2019 Unicycle Model and Control
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/13/2019 Unicycle Model and Control
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
8/13/2019 Unicycle Model and Control
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
8/13/2019 Unicycle Model and Control
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
+
8/13/2019 Unicycle Model and Control
11/27
=
2 +k k N
3
z
q
e= yd(t) y(t)
C
v
r/2 r/2
r
8/13/2019 Unicycle Model and Control
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
8/13/2019 Unicycle Model and Control
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()
8/13/2019 Unicycle Model and Control
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 =
sin() cos() x cos() +y sin()
cos()sin()
0
=
= sin()cos() sin()cos() = 0 True
Lg1h2 = h2
q g1=
=
sin() cos() x cos() +y sin()
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()
8/13/2019 Unicycle Model and Control
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
8/13/2019 Unicycle Model and Control
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)
8/13/2019 Unicycle Model and Control
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
8/13/2019 Unicycle Model and Control
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
8/13/2019 Unicycle Model and Control
19/27
8/13/2019 Unicycle Model and Control
20/27
8/13/2019 Unicycle Model and Control
21/27
8/13/2019 Unicycle Model and Control
22/27
e1vs e2 5 rand([1, ..., 1], = 0, 2 = 1)
0
1
2
3
]
Unicycle model
trajectory desired
trajectory followed
8/13/2019 Unicycle Model and Control
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
8/13/2019 Unicycle Model and Control
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]
8/13/2019 Unicycle Model and Control
25/27
8/13/2019 Unicycle Model and Control
26/27
8/13/2019 Unicycle Model and Control
27/27