A Reliable Way To Compute Robot Trajectories
A Reliable Way To Compute
Robot Trajectories
Simon Rohou1 , Luc Jaulin1 , Lyudmila Mihaylova2 ,
Fabrice Le Bars1 , Sandor M. Veres2
2
1 ENSTA Bretagne, Lab-STICC, UMR CNRS 6285, France
University of Sheffield, Western Bank Sheffield S10 2TN, UK
[email protected]
TOMS Seminar
February 2017
TOMS 2017
1 / 35
A Reliable Way To Compute Robot Trajectories
Section 1
Introduction
[email protected]
TOMS 2017
2 / 35
A Reliable Way To Compute Robot Trajectories
Introduction
Mobile Robotics: formalization
Robot localization = state estimation problem.
Classically, we have:
ẋ(t) = f (x(t), u(t))
(navigation)
y(t) = g(x(t))
(measurements)
Where:
I
x ∈ Rn is the state vector (position, bearing, . . . )
I
u ∈ Rm is the input vector (command)
I
y ∈ R is some exteroceptive measurement (e.g. bathymetry)
I
f : Rn × Rm → Rn is the evolution function
I
g : Rn → R is the observation function
[email protected]
TOMS 2017
3 / 35
A Reliable Way To Compute Robot Trajectories
Introduction
Constraint Network based on trajectories
A problem involving constraints is classically presented
with a Constraint Network:
[Mackworth1977], [Chabert2009]

Variables:






 Constraints:







Domains:
[email protected]
TOMS 2017
4 / 35
A Reliable Way To Compute Robot Trajectories
Introduction
Constraint Network based on trajectories
A problem involving constraints is classically presented
with a Constraint Network:
[Mackworth1977], [Chabert2009]

Variables:






 Constraints:







Domains:
Our approach consists in using:
[email protected]
TOMS 2017
4 / 35
A Reliable Way To Compute Robot Trajectories
Introduction
Constraint Network based on trajectories
A problem involving constraints is classically presented
with a Constraint Network:
[Mackworth1977], [Chabert2009]

x(·),
u(·),
y
Variables:






 Constraints:







Domains:
Our approach consists in using:
I
trajectories as variables: x(·), u(·)
[email protected]
TOMS 2017
4 / 35
A Reliable Way To Compute Robot Trajectories
Introduction
Constraint Network based on trajectories
A problem involving constraints is classically presented
with a Constraint Network:
[Mackworth1977], [Chabert2009]

x(·),
u(·),
y
Variables:






 Constraints:







Domains: [x](·), [u](·), [y]
Our approach consists in using:
I
trajectories as variables: x(·), u(·)
I
tubes as domains: x(·) ∈ [x](·), u(·) ∈ [u](·)
[email protected]
TOMS 2017
4 / 35
A Reliable Way To Compute Robot Trajectories
Introduction
Constraint Network based on trajectories
A problem involving constraints is classically presented
with a Constraint Network:
[Mackworth1977], [Chabert2009]

x(·),
u(·),
y
Variables:






 Constraints:
– ẋ (t) = f (x(t), u(t))
(evolution equation)


–
y(t)
=
g
(x
(t))
(observation
equation)





Domains: [x](·), [u](·), [y]
Our approach consists in using:
I
trajectories as variables: x(·), u(·)
I
tubes as domains: x(·) ∈ [x](·), u(·) ∈ [u](·)
Now we can consider constraints defined with differential equations or
any other non-linear operation on trajectories.
[email protected]
TOMS 2017
4 / 35
A Reliable Way To Compute Robot Trajectories
Section 2
Tubes: Envelopes of Trajectories
[email protected]
TOMS 2017
5 / 35
A Reliable Way To Compute Robot Trajectories
Tubes: Envelopes of Trajectories
Definition
Tube [x](·): interval of functions [x− , x+ ] such that ∀t ∈ R, x− (t) 6 x+ (t)
[x]
x∗ (t)
tf
t0
t
Figure: tube [x](·) enclosing an uncertain trajectory x∗ (·)
[email protected]
TOMS 2017
6 / 35
A Reliable Way To Compute Robot Trajectories
Tubes: Envelopes of Trajectories
Tubes arithmetic
Example:
Tube arithmetic makes it possible to compute the following tubes:
[a](·) = [x](·) + [y](·)
[b](·) = sin [x](·)
Z
[c](·) =
[x](τ )dτ
0
Definition:
If f is an
elementary function such as sin, cos, . . . ,
f [x](·)
is the smallest tube containing all feasible values for
f x(·) , x(·) ∈ [x](·).
[email protected]
TOMS 2017
7 / 35
A Reliable Way To Compute Robot Trajectories
Tubes: Envelopes of Trajectories
Tubes arithmetic: example
[x]
[y]
t
[email protected]
t
TOMS 2017
8 / 35
A Reliable Way To Compute Robot Trajectories
Tubes: Envelopes of Trajectories
Tubes arithmetic: example
[x]
[y]
t
t
[a]
t
a(·) = x(·) + y(·)
[email protected]
TOMS 2017
8 / 35
A Reliable Way To Compute Robot Trajectories
Tubes: Envelopes of Trajectories
Tubes arithmetic: example
[x]
[y]
t
[a]
t
[b]
t
a(·) = x(·) + y(·)
[email protected]
t
b(·) = sin x(·)
TOMS 2017
8 / 35
A Reliable Way To Compute Robot Trajectories
Tubes: Envelopes of Trajectories
Tubes arithmetic: example
[x]
[y]
t
[a]
t
[b]
[c]
t
a(·) = x(·) + y(·)
t
b(·) = sin x(·)
t
Z
c(·) =
x(τ )dτ
0
[email protected]
TOMS 2017
8 / 35
A Reliable Way To Compute Robot Trajectories
Section 3
Tube contractors
[email protected]
TOMS 2017
9 / 35
A Reliable Way To Compute Robot Trajectories
Tube contractors
Tubes: arithmetic and contractors
Example of constraints on trajectories:
a(·) = x(·) + y(·)
b(·) = sin x(·)
Z
c(·) =
x(τ )dτ
0
Tubes [a](·), [b](·), [c](·) respectively contain solutions a(·), b(·), c(·).
[email protected]
TOMS 2017
10 / 35
A Reliable Way To Compute Robot Trajectories
Tube contractors
Tubes: arithmetic and contractors
Example of constraints on trajectories:
a(·) = x(·) + y(·)
b(·) = sin x(·)
Z
c(·) =
x(τ )dτ
0
Tubes [a](·), [b](·), [c](·) respectively contain solutions a(·), b(·), c(·).
Contractors for tubes:
For each primitive constraint, tubes from both sides of the equation
are contracted without removing any feasible solution.
[email protected]
TOMS 2017
10 / 35
A Reliable Way To Compute Robot Trajectories
Tube contractors
Tubes: minimal and non-minimal contractors
Example:
Minimal contractor C+ associated with the constraint
a(·) = x(·) + y(·):




[a] (·)
[a] (·) ∩ ([x] (·) + [y] (·))
 [x] (·)  7→  [x] (·) ∩ ([a] (·) − [y] (·)) 
[y] (·)
[y] (·) ∩ ([a] (·) − [x] (·))
[email protected]
TOMS 2017
11 / 35
A Reliable Way To Compute Robot Trajectories
Tube contractors
Tubes: minimal and non-minimal contractors
Example:
Minimal contractor C+ associated with the constraint
a(·) = x(·) + y(·):




[a] (·)
[a] (·) ∩ ([x] (·) + [y] (·))
 [x] (·)  7→  [x] (·) ∩ ([a] (·) − [y] (·)) 
[y] (·)
[y] (·) ∩ ([a] (·) − [x] (·))
Example:
Non-minimal contractor CR associated with the constraint
0
R
c(·) = 0 x(τ )dτ :
[x] (·)
[c] (·)
[email protected]
7→
[x]
R (·)
[c] (·) ∩ 0 [x] (τ ) dτ
TOMS 2017
11 / 35
A Reliable Way To Compute Robot Trajectories
Tube contractors
Contractors for state estimation
State equations:
ẋ(t) = f (x(t), u(t))
(evolution)
y(t) = g(x(t))
(observation)
Need for a differential contractor C d and a probe contractor Cobs .
dt
[email protected]
TOMS 2017
12 / 35
A Reliable Way To Compute Robot Trajectories
Tube contractors
Contractors for state estimation
State equations:
ẋ(t) = f (x(t), u(t))
(evolution)
y(t) = g(x(t))
(observation)
Need for a differential contractor C d and a probe contractor Cobs .
dt
I
C d : related to the constraint ẋ = v
dt
x(·) ∈ [x](·), v(·) ∈ [v](·)
[email protected]
TOMS 2017
12 / 35
A Reliable Way To Compute Robot Trajectories
Tube contractors
Contractors for state estimation
State equations:
ẋ(t) = f (x(t), u(t))
(evolution)
y(t) = g(x(t))
(observation)
Need for a differential contractor C d and a probe contractor Cobs .
dt
I
C d : related to the constraint ẋ = v
dt
I
x(·) ∈ [x](·), v(·) ∈ [v](·)
Cobs : related to the constraint x(t) = z
t ∈ [t], z ∈ [z], x(·) ∈ [x](·)
[email protected]
TOMS 2017
12 / 35
A Reliable Way To Compute Robot Trajectories
Tube contractors
Contractors for state estimation
Cobs : contraction based on the observation [z1 ] made at time [t1 ].C d
dt
[x1 ]
[z1 ]
[t1 ]
t
Figure: tube [x1 ](·) before contraction
[email protected]
TOMS 2017
13 / 35
A Reliable Way To Compute Robot Trajectories
Tube contractors
Contractors for state estimation
Cobs : contraction based on the observation [z1 ] made at time [t1 ].C d
dt
[x1 ]
[z1 ]
[t1 ]
t
Figure: contraction of tube [x1 ](·) and both [z1 ] and [t1 ]
[email protected]
TOMS 2017
13 / 35
A Reliable Way To Compute Robot Trajectories
Tube contractors
Contractors for state estimation
Cobs : contraction based on the observation [z1 ] made at time [t1 ].C d
dt
[x1 ]
[z1 ]
[t1 ]
t
Figure: contraction of tube [x1 ](·) and both [z1 ] and [t1 ]
[email protected]
TOMS 2017
13 / 35
A Reliable Way To Compute Robot Trajectories
Tube contractors
Contractors for state estimation
C d : contraction based on the differential constraint:
dt
[x1 ]
[z1 ]
[t1 ]
t
Figure: contraction of tube [x1 ](·) and both [z1 ] and [t1 ]
[email protected]
TOMS 2017
14 / 35
A Reliable Way To Compute Robot Trajectories
Tube contractors
Contractors for state estimation
C d : contraction based on the differential constraint:
dt
[x1 ]
[z1 ]
[t1 ]
t
Figure: tube contraction in forward
[email protected]
TOMS 2017
14 / 35
A Reliable Way To Compute Robot Trajectories
Tube contractors
Contractors for state estimation
C d : contraction based on the differential constraint:
dt
[x1 ]
[z1 ]
[t1 ]
t
Figure: tube contraction in forward/backward
[email protected]
TOMS 2017
14 / 35
A Reliable Way To Compute Robot Trajectories
Section 4
Simulation of Mobile Robots
[email protected]
TOMS 2017
15 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Dead-reckoning
Considering a car R which state

 ẋ(t) =
ẏ(t) =
R

θ̇(t) =
[email protected]
is x = (x, y, θ)| .
v · cos(θ)
v · sin(θ)
u(t)
TOMS 2017
16 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Dead-reckoning
Considering a car R which state

 ẋ(t) =
ẏ(t) =
R

θ̇(t) =
is x = (x, y, θ)| .
v · cos(θ)
v · sin(θ)
u(t)
I
car’s speed: v = 10m.s−1
I
−6π
x0 ∈ [−1, 1] × [−1, 1] × [ −6π
5 − 0.002, 5 + 0.002]
t + 33
+ [−0.02, 0.02]
u(t) ∈ − cos
5
I
[email protected]
TOMS 2017
16 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Dead-reckoning
Considering a car R which state

 ẋ(t) =
ẏ(t) =
R

θ̇(t) =
is x = (x, y, θ)| .
v · cos(θ)
v · sin(θ)
u(t)
I
car’s speed: v = 10m.s−1
I
−6π
x0 ∈ [−1, 1] × [−1, 1] × [ −6π
5 − 0.002, 5 + 0.002]
t + 33
+ [−0.02, 0.02]
u(t) ∈ − cos
5
I
Initial tube [u](·) is obtained with the analytical expression.
Other tubes [θ](·), [ẋ](·), [x](·), . . . , are initialized to [−∞, +∞].
[email protected]
TOMS 2017
16 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Dead-reckoning
Dead-reckoning: state estimation without observation.
−6π
x(0) ∈ [−1, 1] × [−1, 1] × [ −6π
5 − 0.002, 5 + 0.002]
x(14) ∈ [53.9, 55.9] × [6.9, 8.9] × [−2.36, −2.32]
Figure: Interval simulation of the robot following car equations. A strong
drift can be observed, depicting cumulative uncertainties.
[email protected]
TOMS 2017
17 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Observations
Same simulation considering constraints on both initial and final states:
−6π
x(0) ∈ [−1, 1] × [−1, 1] × [ −6π
5 − 0.002, 5 + 0.002]
x(14) ∈ [53.9, 55.9] × [6.9, 8.9] × [−2.36, −2.32]
Figure: With a forward/backward contraction, uncertainties are maximal
in the middle of the mission.
[email protected]
TOMS 2017
18 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Observations
Same simulation adding measurements from beacons.
−6π
x(0) ∈ [−1, 1] × [−1, 1] × [ −6π
5 − 0.002, 5 + 0.002]
x(14) ∈ [53.9, 55.9] × [6.9, 8.9] × [−2.36, −2.32]
Figure: With a forward/backward contraction, uncertainties are maximal
in the middle of the mission.
[email protected]
TOMS 2017
19 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Observations
Same simulation without any knowledge on initial or final states:
−6π
x(0) ∈ [−1, 1] × [−1, 1] × [ −6π
5 − 0.002, 5 + 0.002]
x(14) ∈ [53.9, 55.9] × [6.9, 8.9] × [−2.36, −2.32]
Figure: With a forward/backward contraction, uncertainties are maximal
between precise constraints.
[email protected]
TOMS 2017
20 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Lissajous example: complex differential equations
Example: a robot described by its state x| = {x, y} and following
a Lissajous trajectory:
2 cos(t)
x(t) = 5 ·
sin(2t)
[email protected]
TOMS 2017
21 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Lissajous example: complex differential equations
Example: a robot described by its state x| = {x, y} and following
a Lissajous trajectory:
2 cos(t)
x(t) = 5 ·
sin(2t)
x2
5
0
−5
−10
0
Initial conditions: x(0) ∈
[email protected]
10
x1
9.8, 10.2
0
, ẋ(0) =
−0.2, 0.2
10
TOMS 2017
21 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Lissajous example: defining constraints
Defining a set of constraints based on the trajectory equation:
10 cos(t)
x(t) =
5 sin(2t)
[email protected]
TOMS 2017
22 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Lissajous example: defining constraints
Defining a set of constraints based on the trajectory equation:
10 cos(t)
x(t) =
5 sin(2t)
1. Calculating derivatives:
I
I
−10 sin(t)
10 cos(2t) −10 cos(t)
ẍ(t) =
−20 sin(2t)
ẋ(t) =
[email protected]
TOMS 2017
22 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Lissajous example: defining constraints
Defining a set of constraints based on the trajectory equation:
10 cos(t)
x(t) =
5 sin(2t)
1. Calculating derivatives:
I
I
−10 sin(t)
10 cos(2t) −10 cos(t)
ẍ(t) =
−20 sin(2t)
ẋ(t) =
2. Inter-variable relations:
I
ẍ2 (t) = −20 sin(t)
[email protected]
TOMS 2017
22 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Lissajous example: defining constraints
Defining a set of constraints based on the trajectory equation:
10 cos(t)
x(t) =
5 sin(2t)
1. Calculating derivatives:
I
I
−10 sin(t)
10 cos(2t) −10 cos(t)
ẍ(t) =
−20 sin(2t)
ẋ(t) =
2. Inter-variable relations:
I
I
I
I
ẍ2 (t) = −20 sin(t)
ẍ2 (t) = −20 · 2 · sin(t) · cos(t)
1 (t)
1 (t)
ẍ2 (t) = −40 · ẋ−10
· ẍ−10
ẍ2 (t) = −0.4 · ẋ1 (t) · ẍ1 (t)
[email protected]
TOMS 2017
22 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Lissajous example: defining constraints
Defining a set of constraints based on the trajectory equation:
10 cos(t)
x(t) =
5 sin(2t)
1. Calculating derivatives:
I
I
−10 sin(t)
10 cos(2t) −10 cos(t)
ẍ(t) =
−20 sin(2t)
ẋ(t) =
2. Inter-variable relations:
I
I
I
I
Selected constraint:
ẍ(t) =
−10 cos(t)
−0.4 · ẋ1 · ẍ1
ẍ2 (t) = −20 sin(t)
ẍ2 (t) = −20 · 2 · sin(t) · cos(t)
1 (t)
1 (t)
ẍ2 (t) = −40 · ẋ−10
· ẍ−10
ẍ2 (t) = −0.4 · ẋ1 (t) · ẍ1 (t)
[email protected]
TOMS 2017
22 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Lissajous example: CSP
The following CSP will be used for state estimation:

Variables: x(·), ẋ(·), ẍ(·)






Constraints:




– ẍ1 (t) = −10 cos(t)


– ẍ2 (t) = −0.4 · ẋ1 (t) · ẍ1 (t)



0
9.8, 10.2


– x(0) ∈
, ẋ(0) =


−0.2, 0.2
10





Domains: [x](·), [ẋ](·), [ẍ](·)
Domains (tubes) are initialized to [−∞, ∞] ∀t.
[email protected]
TOMS 2017
23 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Lissajous example: CSP
The following CSP will be used for state estimation:

Variables: x(·), ẋ(·), ẍ(·)






Constraints:




– ẍ1 (t) ∈ −10 cos(t) + [−0.001, 0.001]


– ẍ2 (t) = −0.4 · ẋ1 (t) · ẍ1 (t)



0
9.8, 10.2


– x(0) ∈
, ẋ(0) =


−0.2, 0.2
10





Domains: [x](·), [ẋ](·), [ẍ](·)
Domains (tubes) are initialized to [−∞, ∞] ∀t.
[email protected]
TOMS 2017
23 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Lissajous example: computations
Other kind of constraints are easily added,
such as periodic constraints: x2 (t) = x2 (t + π)

Variables: x(·), ẋ(·), ẍ(·)






Constraints:




– ẍ1 (t) ∈ −10 cos(t) + [−0.001, 0.001]


– ẍ2 (t) = −0.4 · ẋ1 (t) · ẍ1 (t)



9.8, 10.2
0


– x(0) ∈
, ẋ(0) =


−0.2, 0.2
10





Domains: [x](·), [ẋ](·), [ẍ](·)
x2
5
0
−5
−10
0
10
x1
Figure: Set-membership state estimation of a Lissajous robot
[email protected]
TOMS 2017
24 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Lissajous example: computations
Other kind of constraints are easily added,
such as periodic constraints: x2 (t) = x2 (t + π)

Variables: x(·), ẋ(·), ẍ(·)






Constraints:




– ẍ1 (t) ∈ −10 cos(t) + [−0.001, 0.001]





– ẍ2 (t) = −0.4 · ẋ1 (t) · ẍ1 (t)















– x2 (t) = x2 (t + π)
9.8, 10.2
0
– x(0) ∈
, ẋ(0) =
−0.2, 0.2
10
Domains: [x](·), [ẋ](·), [ẍ](·)
x2
5
0
−5
−10
0
10
x1
Figure: Set-membership state estimation of a Lissajous robot
[email protected]
TOMS 2017
24 / 35
A Reliable Way To Compute Robot Trajectories
Simulation of Mobile Robots
Lissajous example: computations
Other kind of constraints are easily added,
such as periodic constraints: x2 (t) = x2 (t + π)

Variables: x(·), ẋ(·), ẍ(·)






Constraints:




– ẍ1 (t) ∈ −10 cos(t) + [−0.001, 0.001]





– ẍ2 (t) = −0.4 · ẋ1 (t) · ẍ1 (t)















– x2 (t) = x2 (t + π)
9.8, 10.2
0
– x(0) ∈
, ẋ(0) =
−0.2, 0.2
10
Domains: [x](·), [ẋ](·), [ẍ](·)
x2
5
0
−5
−10
0
10
x1
Figure: Set-membership state estimation of a Lissajous robot
[email protected]
TOMS 2017
24 / 35
A Reliable Way To Compute Robot Trajectories
Section 5
Application on Real Data-Sets
[email protected]
TOMS 2017
25 / 35
A Reliable Way To Compute Robot Trajectories
Application on Real Data-Sets
State estimation of a real underwater robot
Figure: Daurade AUV managed by DGA Techniques Navales Brest and
the Service Hydrographique et Océanographique de la Marine, during an
experiment in the Rade de Brest, October 2015.
[email protected]
TOMS 2017
26 / 35
A Reliable Way To Compute Robot Trajectories
Application on Real Data-Sets
State estimation of a real underwater robot
Figure: Daurade AUV managed by DGA Techniques Navales Brest and
the Service Hydrographique et Océanographique de la Marine, during an
experiment in the Rade de Brest, October 2015.
[email protected]
TOMS 2017
26 / 35
A Reliable Way To Compute Robot Trajectories
Application on Real Data-Sets
State estimation of a real underwater robot
Classical kinematic model of an underwater robot:
ṗ = R(ψ, θ, ϕ) · vr
v̇r = ar − ω r ∧ vr
Where:
I
R0 is the absolute inertial coordinate system
I
R1 is robot’s own coordinate system
I
(ψ, θ, ϕ) and R(ψ, θ, ϕ) are Euler angles and matrix
I
p = (px , py , pz ) gives the center of the robot in R0
I
vr is the speed vector in R1
I
ar is the acceleration vector in R1
I
ω r = (ωx , ωy , ωz ) is the rotation vector of the robot relative to
R0 expressed in R1
[email protected]
TOMS 2017
27 / 35
A Reliable Way To Compute Robot Trajectories
Application on Real Data-Sets
State estimation of a real underwater robot
Classical kinematic model of an underwater robot:
ṗ = R(ψ, θ, ϕ) · vr
v̇r = ar − ω r ∧ vr
Sensed values:
Speed vector vr , acceleration vector ar , Euler angles (ψ, θ, ϕ)
Corresponding tubes:
I
feeded with sensor data:
[vr ](·), [ar ](·), [ψ](·), [θ](·), [ϕ](·)
I
to be computed:
[p](·)
[email protected]
TOMS 2017
27 / 35
A Reliable Way To Compute Robot Trajectories
Application on Real Data-Sets
State estimation of a real underwater robot
Daurade’s tubes:
[p1 ]
[ṗ1 ]
t
t
I
[ṗ1 ](·) – velocity along x in R0
tube’s thickness coming from measurements remains constant
I
[p1 ](·) – position along x in R0
tube becomes thicker depicting cumulative errors due to the
dead-reckoning method and [ṗ1 ](·) non-zero thickness
[email protected]
TOMS 2017
28 / 35
A Reliable Way To Compute Robot Trajectories
Application on Real Data-Sets
1. Dead-reckoning
p2
500
400
300
200
100
0
-600
-500
-400
-300
-200
-100
0
100
p1
Figure: Mission map: Daurade exploring a 25 hectares seabed area.
[email protected]
TOMS 2017
29 / 35
A Reliable Way To Compute Robot Trajectories
Application on Real Data-Sets
2. Observations (USBL measurements)
p2
500
400
300
200
100
0
-600
-500
-400
-300
-200
-100
0
100
p1
Figure: Mission map: Daurade exploring a 25 hectares seabed area.
[email protected]
TOMS 2017
30 / 35
A Reliable Way To Compute Robot Trajectories
Application on Real Data-Sets
2. Observations (USBL measurements)
-280
-260
-240
20
250
230
240
220
230
210
220
200
210
190
200
180
190
170
180
160
170
150
160
140
40
60
Figure: Zoom on the two state observations: these are bounded
positioning measurements obtained at t1 = 14 min and t2 = 32 min.
[email protected]
TOMS 2017
31 / 35
A Reliable Way To Compute Robot Trajectories
Conclusion
Tubes and related contractors provide a simple and efficient
constraint-based method for the computation of uncertain,
non-linear and dynamical systems.
[email protected]
TOMS 2017
32 / 35
A Reliable Way To Compute Robot Trajectories
Conclusion
Tubes and related contractors provide a simple and efficient
constraint-based method for the computation of uncertain,
non-linear and dynamical systems.
Advantages:
I
I
I
I
simple problem description by constraints definition
competitive method for robotics applications
easily applicable on data-sets
reliable results
[email protected]
TOMS 2017
32 / 35
A Reliable Way To Compute Robot Trajectories
Conclusion
Tubes and related contractors provide a simple and efficient
constraint-based method for the computation of uncertain,
non-linear and dynamical systems.
Advantages:
I
I
I
I
simple problem description by constraints definition
competitive method for robotics applications
easily applicable on data-sets
reliable results
Drawbacks:
I
I
I
envelope of solutions only
pessimism added by implementation
computation time?
[email protected]
TOMS 2017
32 / 35
A Reliable Way To Compute Robot Trajectories
Conclusion
An open-source C++/python tube library will be soon available.
[email protected]
[x]
x∗ (t)
δ
tf
t0
[email protected]
t
TOMS 2017
33 / 35
A Reliable Way To Compute Robot Trajectories
Support:
DGA
Direction Générale de l’Armement
Tools:
IBEX library
used for interval arithmetic, contractor programming
VIBES
used for rendering
[email protected]
TOMS 2017
34 / 35
A Reliable Way To Compute Robot Trajectories
References:
[Mackworth1977] A. K. Mackworth,
Consistency in networks of relations,
Artificial Intelligence, 8(1):99–118, 1977.
[Chabert2009] G. Chabert, L. Jaulin,
Contractor Programming,
Artificial Intelligence, 173:1079–1100, 2009.
[LeBars2012] F. Le Bars, J. Sliwka, O. Reynet, L. Jaulin,
State estimation with fleeting data,
Automatica, 48(2):381–387, 2012.
[Bethencourt2014] A. Bethencourt, L. Jaulin,
Solving non-linear constraint satisfaction problems involving
time-dependant functions,
Mathematics in Computer Science, 8(3), 2014.
[Aubry2013] C. Aubry, R. Desmare, L. Jaulin,
Loop detection of mobile robots using interval analysis,
Automatica, 2013.
[Rohou2017] S. Rohou, L. Jaulin, L. Mihaylova, F. Le Bars, S. M. Veres,
Guaranteed Computation of Robot Trajectories,
Robotics and Autonomous Systems, to be published.
[email protected]
TOMS 2017
35 / 35
A Reliable Way To Compute Robot Trajectories
Section 8
Appendix
[email protected]
TOMS 2017
1/5
A Reliable Way To Compute Robot Trajectories
Appendix
Tubes: implementation
The computer representation of a tube encloses [x− (·), x+ (·)] inside an
interval of step functions [x− (·), x+ (·)] such that:
[x]
∀t ∈ R, x− (t) 6 x− (t) 6 x+ (t) 6 x+ (t)
x∗ (t)
δ
tf
t0
t
Figure: tube implementation with a set of boxes – this outer representation
adds pessimism but enables guaranteed and simple computations
[email protected]
TOMS 2017
2/5
A Reliable Way To Compute Robot Trajectories
Appendix
Integral of tubes
Definition: the integral of a tube [x](·) = [x− , x+ ] is an interval:
Z
b
Z
[x](τ )dτ =
a
b
Z b
Z b
x(τ )dτ | x ∈ [x] =
x− (τ )dτ ,
x+ (τ )dτ
a
a
a
[Aubry2013]
[x]
x− (t)
Z
b
x− (τ )dτ
a
b
a
t
Figure: blue area: lower bound of the tube’s integral
[email protected]
TOMS 2017
3/5
A Reliable Way To Compute Robot Trajectories
Appendix
Integral of tubes
Definition: the integral of a tube [x](·) = [x− , x+ ] is an interval:
Z
b
b
Z
[x](τ )dτ =
a
Z b
Z b
x(τ )dτ | x ∈ [x] =
x− (τ )dτ ,
x+ (τ )dτ
a
a
a
[Aubry2013]
[x]
Z
b
x+ (τ )dτ
x+ (t)
a
a
b
t
Figure: orange area: upper bound of the tube’s integral
[email protected]
TOMS 2017
3/5
A Reliable Way To Compute Robot Trajectories
Appendix
Definition
A contractor applied on a tube [a](·) aims at removing unfeasible
trajectories according to a given constraint L:
[Bethencourt2014]
CL
[a](·) −→ [b](·)
[email protected]
TOMS 2017
4/5
A Reliable Way To Compute Robot Trajectories
Appendix
Definition
A contractor applied on a tube [a](·) aims at removing unfeasible
trajectories according to a given constraint L:
[Bethencourt2014]
CL
[a](·) −→ [b](·)
The output of the contractor CL is the tube [b](·) such that:
∀t, [b](t) ⊆ [a](t)
L a(·)
=⇒ a(·) ∈ [b](·)
a(·) ∈ [a](·)
[email protected]
(contraction)
(completeness)
TOMS 2017
4/5
A Reliable Way To Compute Robot Trajectories
Appendix
Storyboard of a forward/backward tube contraction
ẋ
4
x
k=0
k=1
k=2
k=3
k=4
4
δ
3
3
2
2
1
1
0
1
2
3
4
5
t
Figure: Derivative tube [ẋ](·) used to
contract [x](·) initialized to
[−∞, ∞]∀t
[email protected]
0
1
2
3
4
5
t
Figure: First step of forward
integration contractor: first slice k0 is
contracted from [−∞, ∞] to [0, 2]
TOMS 2017
5/5
A Reliable Way To Compute Robot Trajectories
Appendix
Storyboard of a forward/backward tube contraction
ẋ
4
x
k=0
k=1
k=2
k=3
k=4
4
δ
3
3
2
2
1
1
0
1
2
3
4
5
t
Figure: Derivative tube [ẋ](·) used to
contract [x](·) initialized to
[−∞, ∞]∀t
[email protected]
0
1
2
3
4
5
t
Figure: End of forward contractions.
The minimal envelope of solutions
compliant with [ẋ](·) is pictured in
blue. Slices representation – outer
approximation – is depicted in gray
TOMS 2017
5/5
A Reliable Way To Compute Robot Trajectories
Appendix
Storyboard of a forward/backward tube contraction
ẋ
4
x
k=0
k=1
k=2
k=3
k=4
4
δ
3
3
2
2
1
1
0
1
2
3
4
5
t
Figure: Derivative tube [ẋ](·) used to
contract [x](·) initialized to
[−∞, ∞]∀t
[email protected]
0
1
2
3
4
5
t
Figure: First step of backward
integration contractor, with the last
condition x(5) = 4. A reduced
envelope, pictured in dark blue, is
computed from t = 5 to t = 0
TOMS 2017
5/5
A Reliable Way To Compute Robot Trajectories
Appendix
Storyboard of a forward/backward tube contraction
ẋ
4
x
k=0
k=1
k=2
k=3
k=4
4
δ
3
3
2
2
1
1
0
1
2
3
4
5
t
Figure: Derivative tube [ẋ](·) used to
contract [x](·) initialized to
[−∞, ∞]∀t
[email protected]
0
1
2
3
4
5
t
Figure: End of backward processing
TOMS 2017
5/5
A Reliable Way To Compute Robot Trajectories
Appendix
Storyboard of a forward/backward tube contraction
ẋ
4
x
k=0
k=1
k=2
k=3
k=4
4
δ
3
3
2
2
1
1
0
1
2
3
4
5
t
Figure: Derivative tube [ẋ](·) used to
contract [x](·) initialized to
[−∞, ∞]∀t
[email protected]
0
1
2
3
4
5
t
Figure: Tube [x](·) is contracted to
optimally enclose the thin envelope
pictured in dark blue
TOMS 2017
5/5