Hybrid Control Design for a Wheeled Mobile RobotThomas Bak 1 docx

1 1 2
1
2
(X
F
, Y
F
)
(X, Y )
θ
(X
v
, Y
v
)
ξ = [X Y θ] ∈ R
2
× S
1
β
1
β
2
β
3
β
4
✶
▼
X

v
Y
v
X
Y
✲
✻
X
F
X
F

1
γ
1
θ
(X
F
, Y
F
)
(X
v
, Y
v
) θ 
1
γ
1
(X, Y )

i 1 ≤ i ≤ 4
γ
i

i
i X
v
β
i
β = [β
1
β
2
β
3
β
4
] ∈ S
4
β
β
c
∈ S
2
β
o
∈ S
2
β
1

= ±π/2 β
2
= ±π/2
β
c
= [β
3
β
4
]
±π/2
q
1
, q
2
q
3
q
1
β
c
= [β
1
β
2
] |β
1
| <
(
π

2
− e
β
) ∨ |β
2
| < (
π
2
− e
β
)
q
2
β
c
= [β
3
β
4
] |β
3
| <
(
π
2
− e
β
) ∨ |β
4
| < (

π
2
− e
β
)
q
3
β
1
= β
2
= β
3
= β
4
|β
1
| ≥
(
π
2
− e
β
) ∨ |β
2
| ≥ (
π
2
− e
β

)) ∧ (|β
3
| ≥ (
π
2
− e
β
) ∨ |β
4
| ≥ (
π
2
− e
β
))
e
β
β
c
= [β
1
β
2
]
β
c
= [β
3
β
4

]
η(t) ∈ R
Σ(β
c
) ∈ R
3
ζ(t) = [
˙
β
1
˙
β
2
] ∈ R
2
η(t)
˙χ =


˙
ξ
˙η
˙
β
c


=



0 R (θ)Σ(β
c
) 0
0 0 0
0 0 0


χ +


0 0
1 0
0 I



ν
ζ

ν
R (θ)
β
˙
β
x
1
= T(χ) =

ξ
ref

− ξ
˙
ξ
ref
−
˙
ξ

,
˙x
1
= A
1
x
1
+ B
1

δ(χ)

ν
ζ

− α(χ)

, A
1
=

0 I

0 0

, B
1
=

0
I

.
δ(χ) α(χ)
δ(χ) = R (θ)

Σ(β
c
) N(β
c
)η

α(χ) = sin(β
1
− β
2
)η
2


−
1
sin β

2
cos(β
1
− γ
1
) + 
2
sin β
1
cos(β
2
− γ
2
)

1
cos β
2
cos(β
1
− γ
1
) − 
2
cos β
1
cos(β
2
− γ
2

)
0


N(β
c
) = [N
1
N
2
]

ν
ζ

= δ(χ)
−1
(α(χ) − K
1
x
1
)
˙x
1
= (A
1
− B
1
K
1

)x
1
0
t → ∞ K
1
A
1
− B
1
K
1
β
c
= [β
3
β
4
]
˙x
2
= (A
2
− B
2
K
2
)x
2
˙
θ = 0

x
3
= Tχ =

ξ
ref
− ξ
˙
ξ
ref
−
˙
ξ

,
T
˙x
3
= A
3
x
3
+ B
3

ν
0

, A
3

=

0 I
A
31
A
32

, B
3
=

0
I

ν = −K
3
x
3
q
3
|θ
ref
− θ| a a
δ(χ) δ(χ)
η = 0
Σ(β
c
)
N

1
N
2
δ(χ) |η| ≥ n n
β
1
= β
2
= β
3
= β
4
q
3
ξ
ref
β
˙
ξ
ref
=

η
ref
ζ
ref

=

2n

0

q
0
q
4
q
1
q
2
β
ξ
ref
˙
ξ
ref
= 0
vehicle_v7/Automaton
Printed 17−Jan−2003 12:59:12
Driving
Rest
Q0
Q1
Q2
Q3
Q4
[(b[3]>=B | b[4]>=B) & b[1]<=B & b[2]<=B]
[eta<0.9*E]
[(b[1]>=B | b[2]>=B) & b[3]<=B & b[4]<=B]
[new_wp==1] [b[1]>B & b[2]>B & b[3]>B & b[4]>B]

[b[1]<=B & b[2]<=B | a>A]
[b[3]<=B & b[4]<=B | a>A]
[b[1]>B & b[2]>B & b[3]>B & b[4]>B]
[eta>=1.5*E]
β
i
π
2
− e
β
|θ
ref
− θ|
Q = {q
0
, q
1
, q
2
, q
3
, q
4
}
x
X ⊆ R
2
× S
1
× R

3
H = Q × X
f(q, x) =















(A
3
− B
3
K
3
)x
0
q = q
0
,
(A

1
− B
1
K
1
)x
1
q = q
1
,
(A
2
− B
2
K
2
)x
2
q = q
2
,
(A
3
− B
3
K
3
)x
3
q = q

3
,
(A
3
− B
3
K
3
)x
4
q = q
4
.
0 1 2 3 4 5 6 7 8 9 10
0
1
2
3
4
5
6
7
8
x [m]
y [m]
q
4
η
q
3

q
1
β
c
q
2
q
4
Σ(β
c
)
0 2 4 6 8 10 12
−5
0
5
β [rad]
q
1
q
2
q
4
q
4
q
0
0 2 4 6 8 10 12
0
0.5
1

pos. error [m]
0 2 4 6 8 10 12
−0.5
0
0.5
angle error [rad]
0 2 4 6 8 10 12
0
1
2
3
η
t [s]
β
i
η
q
0
β
1
= β
2
= β
3
= β
4
: V
0
(x
0

) = x
0
P
3
x
0
q
1
β
c
= [β
1
β
2
] : V
1
(x
1
) = x
1
P
1
x
1
q
2
β
c
= [β
3

β
4
] : V
2
(x
2
) = x
2
P
2
x
2
q
3
β
1
= β
2
= β
3
= β
4
: V
3
(x
3
) = x
3
P
3

x
3
q
4
β
1
= β
2
= β
3
= β
4
: V
4
(x
4
) = x
4
P
3
x
4
P
j
= P
j
> 0
P
j
(A

j
− B
j
K
j
) + (A
j
− B
j
K
j
) P
j
= −I
q
0
q
4
K
3
P
3
q
3
˙
V
j
= ˙x
j
P

j
x
j
+ x
j
P
j
˙x
j
= −x
j
x
j
.
q
j
, j = 0, . . . , 4
q

j,k
, k = 0, 1, 2 Λ
j
j
q

j,0
V
j
(x
j

)
j q

j,1
[ν ζ ] = −K
j
x
j
V
j
(x
j
)
0 q

j,2
V
j
(x
j
)
✲
✻
V
j
(0) + ∆V
j
V
j
(0)

T
Λ
j,0
(T ) V
j
(x
j
(t))
✠
Λ
j,1
(T )
T = 0 T = T
penalty
T = T
stable
✲
q

0
✲
q

1
✲
q

2
j
j t

j
∆V
j
Λ
j,0
(T ) = T + V
j
(t
j
), 0 ≤ T ≤ T
penalty
T
penalty
∆V
j
= T
penalty
T
j
0 q
j
T = T
penalty
˙
V
j
V
j
(x
j

(t)), t
j
≤
t ≤ t + T
stable
− T
penalty
Λ
j,1
(T ) =
−α
o
T + ∆V
j
+ V
j
(0), T
penalty
≤ T ≤ T
stable
, α
o
≥ 0
T
stable
Λ
j,1
(T ) = V
j
(0)

q

j,2
∆V
j
T
penalty
α
0
x
j
q
i
q
j
q
1
, q
2
, q
3
x
j
(P
j
−P
i
)x
j
P

q
4
q
0
x
4
P
3
x
4
x
4
ξ
ref
− ξ
˙
ξ
n
α
0
V
j
(0) ∆V
j
j
j
q

j,k
x

j
(t) = e
(A
j
−B
j
K
j
)t
x
j
(t
j
)
t ∈ [t
j
; t + (T
stable
− T
penalty
)]
V
j
(t) =

e
(A
j
−B
j

K
j
)t
x
j
(t
j
)

P
j
e
(A
j
−B
j
K
j
)t
x
j
(t
j
)
= P
1
2
e
(A
j

−B
j
K
j
)t
x
j
(t
j
)
 ·  (·) (A
j
, B
j
)
K
j
S
j
(A
j
− B
j
K
j
) =
S
j
D
j

S
−1
j
, D
(A
j
− B
j
K
j
)
V
j
(t)
1
2
= P
1
2
j
e
(A
j
−B
j
K
j
)t
x
j

(t
j
)
= P
1
2
j
S
j
e
D
j
t
S
−1
j
x
j
(t
j
)
≤ P
1
2
j
S
j
 S
−1
j

x
j
(t
j
) e
D
j
t

≤ P
1
2
j
S
j
 S
−1
j
x
j
(t
j
) (x
j
)e
λ
max
(D
j
)

P
1
2
P = P
1
2
P
1
2
t
j
(T
stable
− T
penalty
) α
0
x
j
(t
j
)
q
j
φ = [ φ
1
φ
2
φ

3
φ
4
] ∈ S
4
r = [r
1
r
2
r
3
r
4
] ∈ R
4
κ =

X Y θ β φ

=

ξ β φ

A(κ) ˙κ =

J
1
(β) R(θ) 0 J
2
C

1
(β)R(θ) 0 0

˙κ = 0
J
1
(β) =




cos β
1
sin β
1

1
sin(β
1
− γ
1
)
cos β
2
sin β
2

2
sin(β
2

− γ
2
)
cos β
3
sin β
3

3
sin(β
3
− γ
3
)
cos β
4
sin β
4

4
sin(β
4
− γ
4
)




, J

2
= rI
4×4
,
C
1
(β) =




− sin β
1
cos β
1

1
cos(β
1
− γ
1
)
− sin β
2
cos β
2

2
cos(β
2

− γ
2
)
− sin β
3
cos β
3

3
cos(β
3
− γ
3
)
− sin β
4
cos β
4

4
cos(β
4
− γ
4
)




, R(θ) =



cos θ sin θ 0
− sin θ cos θ 0
0 0 1


.
˙
ξ
β
1
β
2
˙
ξ ∈ { {R(θ) Σ(β
c
)}}
Σ(β
c
) ∈ R
3
C
1
C
1
(β) Σ(β
c
) ≡ 0 ∀β Σ C
1

(β)
Σ =



1
cos β
2
cos(β
1
− γ
1
) − 
2
cos β
1
cos(β
2
− γ
2
)

1
sin β
2
cos(β
1
− γ
1
) − 

2
sin β
1
cos(β
2
− γ
2
)
sin(β
1
− β
2
)


.
η(t) ∈ R Σ(β
c
)
R(θ)
˙
ξ(t) = Σ(β
c
)η(t) ∀t
ζ(t) = [
˙
β
1
˙
β

2
] ∈ R
2
η
d
dt

∂T
∂ ˙κ
k

−
∂T
∂κ
k
= c
k
(κ) λ + Q
k
T κ
k
k
c
k
(κ) k
A(κ) λ
Q
k
k
T =

1
2
˙κ


R(θ) MR(θ) R(θ) V 0
V R(θ) J
β
0
0 0 J
φ


˙κ
M J
β
J
φ
M =


m
f
+ 4m
w
0 −m
w

4
i=1


i
sin γ
i
0 m
f
+ 4m
w
m
w

4
i=1

i
cos γ
i
−m
w

4
i=1

i
sin γ
i
m
w

4

i=1

i
cos γ
i
I
f
+ m
w

4
i=1
γ
2
i


.
I
f
m
f
m
w
x
v
y
v
I
w

J
β
=
1
2
I
w
I
4×4
J
φ
= I
w
I
4×4
V =


0 0 0 0
0 0 0 0
I
w
I
w
I
w
I
w



.
h
1
(β) ˙η + Φ
1
(β)ζη = Σ Eτ
φ
E = J
1
J
−1
2
∈ R
3×4
τ
φ
∈ R
4
h
1
(β)
h
1
(β) = Σ (M + EJ
φ
E )Σ > 0
Φ
1
(β) ∈ R
Φ

1
(β) = Σ (M + EJ
φ
E )N (β
c
)
N(β
c
) = [N
1
N
2
]
N
1
=


−
1
cos β
2
sin(β
1
− γ
1
) + 
2
sin β
1

cos(β
2
− γ
2
)
−
1
sin β
2
sin(β
1
− γ
1
) − 
2
cos β
1
cos(β
2
− γ
2
)
cos(β
1
− β
2
)


N

2
=


−
1
sin β
2
cos(β
1
− γ
1
) + 
2
cos β
1
sin(β
2
− γ
2
)

1
cos β
2
cos(β
1
− γ
1
) + 

2
sin β
1
sin(β
2
− γ
2
)
− cos(β
1
− β
2
)


τ
φ
Σ Eτ
φ
= [a
1
a
2
a
3
a
4
][τ
1
τ

2
τ
3
τ
4
] = L
L τ
φ
= Hτ
0
, H ∈ R
4
H
i
= L (a
i
)/σ σ
Σ E
τ
0
=
1
Σ EH
(h
1
(β)ν + Φ
1
(β)ζη) ,
˙η = ν ν
t ≥ t

0
t
0
Σ
0
, Σ
1
, . . . , Σ
µ
x
0
(t), x
1
(t), . . . , x
µ
(t)
Σ
j
: ˙x
j
= f
j
(x
j
(t)), j = 0, 1, . . . , µ
t
i
, i =
0, 1, 2, . . . t
i

< t
i+1
∀i
Σ
j
t
i
< t ≤ t
i+1
Σ
k
t
i+1
< t ≤
t
i+2
Σ
j
V
j
(x
j
(t)) V
j
(0) = 0, V
j
(x
j
) ≥ 0
˙

V (x
j
) ≤ 0 x
j
= 0 V
j
Σ
j
V
j
(x
j
(t
q
)) ≥ V
j
(x
j
(t
r
))
0 ≤ j ≤ µ t
q
, t
r
∈ {t
i
} t
q
< t

r
Σ
j