PROBABILISTIC ROBOTICS:

ROBOT MOTION

Pierre-Paul TACHER

1
Evolution of the state will be approximated to the second and first order, for x and ẋ respectively:
1
xt+1 = xt + ∆t × ẋt + (∆t)2 × ẍˆt + o(∆t2 )
2
1
⇒ xt+1 ≈ xt + ∆t × ẋt + (∆t)2 × ẍˆt
2

ẋt+1 = ẋt + ∆t × ẍˆt + o(∆t)

⇒ ẋt+1 ≈ ẋt + ∆t × ẍˆt

where ẍˆt is the actual acceleration. If we note ẍt the commanded acceleration,

ẍˆt = ẍt + t

where t is gaussian t ,→ N (0, σ 2 ) We can modelize evolution of the state with matrix equality:
      1 2

xt+1 1 ∆t xt (∆t)
= + 2 ẍˆt
ẋt+1 0 1 ẋt ∆t
| {z }
   1 2
 G 1
(∆t)2
   
xt+1 1 ∆t xt (∆t)
= + 2 ẍt + 2 t
ẋt+1 0 1 ẋt ∆t ∆t
| {z }
0
t

We have
1 4 1 3

G×G = T 4 (∆t) 2 (∆t)
1 3 2
2 (∆t) (∆t)

0
As a linear function of gaussian t , the random variable t is known to be multivariate Gaussian
0 x
N( , σ 2 GGT ). The conditional law of random variable t+1 given xt , ẋt , ẍt is then known to
0 | {z } ẋt+1
R   1
(∆t)2

x
be also Gaussian N (A t + 2 ẍt , R). The corresponding density is
ẋt ∆t
| {z }
µ
" # " #
xt+1 xt+1
− 12 ( −µ)T R−1 ( −µ)
1 ẋt+1 ẋt+1
p(xt+1 , ẋt+1 | xt , ẋt , ẍt ) = p ×e
2π |R|
1
2 PROBABILISTIC ROBOTICS: ROBOT MOTION

The fact that the covariance matrix R is not diagonal shows that xt+1 and ẋt+1 are correlated. The
coefficient of correlation is
1 3
2 (∆t)
ρ= q
1 4
4 (∆t) × ∆t)2
ρ=1
showing position and speed are fully correlated.

2
 
xt+1
Conditioned on xt , ẋt , ẍt , the r.v. is gaussian; we can then show (cf. A.1) that the r.v.
ẋt+1
ẋt+1 conditioned on xt+1 , xt , ẋt , ẍt is also gaussian ẋt+1 ,→ N (ν, Λ), where
1 3
2 (∆t)
ν = E(ẋt+1 ) + 1 4
(xt+1 − E(xt+1 ))
4 (∆t)
2
(xt+1 − E(xt+1 ))
= E(ẋt+1 ) +
∆t
( 1 (∆t)3 )2
Λ = (∆t)2 − 21 4
4 (∆t)
Λ=0
showing again the speed is deterministic function of the position.

3
We have the model
   1
(∆t)2
   
xn+1 1 ∆t xn 2
= + t
ẋn+1 0 1 ẋn ∆t
| {z }
0
t
   
0 0 2 T xn 0
with t ,→ N ( ,σ GG ). If we assume the random variables and n to be independant and
0 | {z } ẋn
  R  
xn xn+1
,→ N (µn , Σn ), then is Gaussian N (Aµn , AΣn AT + R). We will study the sequence of
ẋn ẋn+1 |{z} | {z }
µn+1 Σn+1
covariance matrices defined by the recurrence relation
∀n ∈ N, Σn+1 =AΣn AT + R
It is convenient to write it in vectorized form
 2  2  1 4
1 ∆t ∆t (∆t)2
  
σX,n+1 σX,n 4 (∆t)
σXY,n+1  0 1 1 3
0 ∆t  σXY,n  + σ 2  21 (∆t) 
 
σXY,n+1  = 0 0
  
1 ∆t σXY,n 3
2 (∆t)
   
2
σY,n+1 0 0 0 1 2
σY,n 2
(∆t)
 2  1
(∆t)4
 
σX,n 4
 a11 A a12 A   1 3
 σXY,n  + σ 2  21 (∆t) 

=  σXY,n   (∆t)3 
a21 A a22 A 2
2
σY,n (∆t)2
⇔ vec(Σn+1 ) = A ⊗ A × vec(Σn ) + vec(R)
 PROBABILISTIC ROBOTICS: ROBOT MOTION 3

where ⊗ is the Kronecker product. A little observation shows that

1 n∆t n∆t n2 (∆t)2

 
0 1 0 n∆t 
∀n ∈ N, (A ⊗ A)n =  0

0 1 n∆t 
0 0 0 1

and
n−1
X
n
∀n ∈ N, vec(Σn ) = (A ⊗ A) vec(Σ0 ) + ( (A ⊗ A)i ) vec(R)
i=0

Then, using the formulas of a sum of integers and sum of squared integers,

n n(n−1) n(n−1) n(n−1)(2n−1)

 2

2 ∆t 2 ∆t 6 (∆t)
n−1
 
0 n(n−1)
X n 0 ∆t 
(A ⊗ A)i =  2
 

0 n(n−1)
i=0  0 n 2 ∆t 

0 0 0 n

from which we can build a closed formula for Σn . What is of interest for us here is the asymptotic
behavior,

2 n(n − 1)(2n − 1)
σX,n ∼ (∆t)2 × (∆t)2 σ 2
n→∞ 6
n3
∼ (∆t)4 σ 2
n→∞ 3
n(n − 1)
σXY,n ∼ ∆t × (∆t)2 σ 2
n→∞ 2
n2
∼ (∆t)3 σ 2
n→∞ 2
2
σY,n ∼ n(∆t)2 σ 2
n→∞

leading to
n2 3 2
2 (∆t) σ
ρXY,n ∼ q
n→∞ n3 4 2
3 (∆t) σ × n(∆t)2 σ 2
√
3
= <1
2
Asymptotically position and speed do not get fully correlated.

4
The conventions used for this exercice and the following 2 are represented in figure 1. Let’s study
−
→ − → 0 −
→ − →
the movement of the center of front wheel B in frame R = (O, i , j ). We let R = (A, i , j ) the
frame with origin the center of back wheel, translating with regards to R. We can write
−
v→ −→ −
→
B /R = vB /R0 + v R0 /R
=−v→ −
→
B /R0 + vA /R À
4 PROBABILISTIC ROBOTICS: ROBOT MOTION

+
α
−
v→
B

−
→
v
B

−
→
u
−
→ A θ −
→
j u r
−
→ θ
i

Figure 1. Ideal bicycle model

0
In frame R , the length AB being constant, B is in rotation

−−→ 2
∀t > 0, AB(t) = l

−−−→
dAB −−→
⇒ ∀t > 0, (t) · AB(t) = 0
dt
−−→
⇔ ∀t > 0, −v→B /R0 (t) · AB(t) = 0

so

−
v→ −
→
B /R0 = lθ̇ uθ

A is restrained to move in the direction of the bicycle axis:

−
v→
A /R = vur

À⇒−
v→ −
→
B /R = lθ̇ uθ + vur

B is restrained to move in the direction of front wheel, so if we project the previous relation we get

(
vB sin α = lθ̇
vB cos α = v
⇒ v tan α = lθ̇ Á
 PROBABILISTIC ROBOTICS: ROBOT MOTION 5

−
→ − →
Projecting now −
v→
B on ( i , j ), we obtain
(
vB cos(α + θ) = ẋ
vB sin(α + θ) = ẏ
 v
 cos(α + θ) = ẋ
cos α

⇔ v

 sin(α + θ) = ẏ
cos α

 v(cos θ − tan α sin θ) = ẋ
 Â
⇔
 v(tan α cos θ + sin θ) = ẏ
 Ã

We discretize time in Á, and à to obtain approximate motion equations:


 xt+1 = xt + v̂(cos θt − tan α̂ sin θt )∆t

yt+1 = yt + v̂(tan α̂ cos θt + sin θt )∆t

 v̂
θt+1 = θt + tan α̂ × ∆t
l

with
α̂ = α + σα2
v̂ = v + σv2
where σ2 ,→ N (0, σ 2 ).

5
cf. file bicyclespl.m
   for the matlab code. In figure 2, I plotted one step of a simulation starting
x 0
from state y  =  0 .
θ π/4

6
This exercise consists in inversing formulas Á, and à for α, v and θt+1 .

 v(cos θ − tan α sin θ) = ẋ

⇔
 v(tan α cos θ + sin θ) = ẏ

 ẋ
 cos θ − tan α sin θ =
v

⇔ ẏ

 tan α cos θ + sin θ =
v
 ẋ
 ẏ(cos θ − tan α sin θ) = ẏ

v
⇔
 ẋ(tan α cos θ + sin θ) = ẋ ẏ

v
⇒ tan α(ẋ cos θ + ẏ sin θ) = ẏ cos θ − ẋ sin θ
ẏ cos θ − ẋ sin θ
⇔ tan α =
ẋ cos θ + ẏ sin θ
6 PROBABILISTIC ROBOTICS: ROBOT MOTION

1.5

1
y

0.5

−0.5

0 0.5 1 1.5 2
x

Figure 2. 5000 samples from motion model, α = 0, v = 90 × 10−2 , σα2 = 15◦2 ,

σv2 = 0.04v 2 .

In order to compute the most likely value of steering angle α, it suffices to insert in previous relation
−xt −yt
ẋ = xt+1
∆t , ẏ = yt+1
∆t and θ = θt . From there we can compute the speed
v2
= ẋ2 + ẏ 2
cos2 α
⇔ v 2 = cos2 α(ẋ2 + ẏ 2 )

and finally the most likely final orientation will be given by

v
θt+1 =θt + tan α × ∆t
l

7
The model is represented in figure 3. The forward and sideward speeds are designed by v1 and
v2 respectively.
ẋ = v1 cos θ − v2 sin θ
ẏ = v1 sin θ + v2 cos θ
θ̇ = ω

We discretize to get
xt+1 = xt + (v1 cos θt − v2 sin θt )∆t
yt+1 = yt + (v1 sin θt + v2 cos θt )∆t
θt+1 = θt + ω∆t
 PROBABILISTIC ROBOTICS: ROBOT MOTION 7

v2 −
→
u θ

v1 −
→
u r

−
→
u
−
→ θ −
→
j u r
−
→ θ
i

Figure 3. holonomic robot model

To compute the transition probability density, one needs to invert the previous system for the
controls v1 , v2 and ω;for that it suffices to notice that:
 −1  
cos θ − sin θ cos θ sin θ
=
sin θ cos θ − sin θ cos θ

so that
1
v1 = ((xt+1 − xt ) cos θt + (yt+1 − yt ) sin θt )
∆t
1
v2 = (−(xt+1 − xt ) sin θt + (yt+1 − yt ) cos θt )
∆t
θt+1 − θt
ω=
∆t

Var(b2 ) = E[X 2 ] − (E[X])2

√
6b
|x|
Z Z
1
X 2 dP = √ x
2
( √ − 2 ) dx
Ω − 6b 6b 6b
Z √6b
1 |x|
=2 x2 ( √ − 2 ) dx
0 6b 6b
Z √6b Z √6b
1 2 1
= 2( √ x dx − 2 x3 dx)
6b 0 √ 6b 0
 3  6b  √6b
1 x 1 x4
= 2( √ − 2 )
6b 3 0 6b 4 0
√ 3
1 6 6b 1 36b4
= 2( √ − 2 )
6b 3 6b 4
= b2
8 PROBABILISTIC ROBOTICS: ROBOT MOTION

so,
Var(b2 ) = b2

√
6
Let now X and Y 2 independant uniform r.v. ,→ U ([−b, b]), and let Z = 2 [X + Y ]. We have
3
Var(Z) = [Var(X) + Var(Y )]
2

and
Var(X) = Var(Y ) = E[X 2 ] − (E[X])2
= E[X 2 ]
Z b
1
= x2 dx
−b 2b
Z b
1
=2 x2 dx
0 2b
 b
1 x3
=2
2b 3 0
1 b3
=
b 3
b2
=
3

so that
Var(Z) = b2
 PROBABILISTIC ROBOTICS: ROBOT MOTION 9

Appendix A
A.1.
lemma. Let Z = (X, Y ) be a gaussian r.v. which takes its values in Rn × Rp , Z ,→ N (µ, Λ). We
note  
T
 ΛX ΛXY 
Λ= 


ΛXY ΛY

Then, the law of r.v. Y conditionned on (X = x) is gaussian ,→ N (m, Σ), where

m = EY + ΛXY Λ−1
X (x − EX)
Σ = ΛY − ΛXY Λ−1 T
X ΛXY

Proof. We already know that X is gaussian, as linear transfomation (namely projection) of gaussian
Z. Let Y 0 = Y − CX. The r.v. (X, Y 0 ) is also gaussian and we can verify that
ΛXY 0 = ΛXY − CX

We can choose C = ΛXY Λ−1 0

X so that X and Y are decorrelated, but this is the same as independance
since (X, Y ) is gaussian. The law of Y = Y 0 + CX conditionned on (X = x) is then simply the
0

law of Y 0 + Cx. This shows it is gaussian and

m = EY 0 + Cx
= (EY − ΛXY Λ−1 −1
X EX) + ΛXY ΛX x
= EY + ΛXY Λ−1
X (x − EX)
Σ = ΛY − CΛX C T
= ΛY − ΛXY Λ−1 −1 T
X ΛX ΛX ΛXY
= ΛY − ΛXY Λ−1 T
X ΛXY

where we have used the fact that ΛX is symetric and Y 0 and CX are independant.