F. Arrichiello and G. Antonelli and A.P. Aguiar and A. Pascoal, Observability metrics for the relative localization of AUVs based on range and depth measurements: theory and experiments, 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, San Franscisco, CA, pp. 3166--3171, 2011.
17
Observability metric for the relative localization of AUVs based on range and depth measurements: theory and experiments Filippo Arrichiello 1 , Gianluca Antonelli 1 Antonio Pedro Aguiar 2 , Antonio Pascoal 2 1.University of Cassino, Italy Robotics Research Group of the DAEIMI http://webuser.unicas.it/lai/robotica 2.Technical University of Lisbon, Portugal Lab. of Robotics and Systems in Engineering and Science http://welcome.isr.ist.utl.pt F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Transcript
Observability metric for the relativelocalization of AUVs based on range and
depth measurements: theory andexperiments
Filippo Arrichiello1, Gianluca Antonelli1
Antonio Pedro Aguiar2, Antonio Pascoal2
1.University of Cassino, ItalyRobotics Research Group of the DAEIMIhttp://webuser.unicas.it/lai/robotica
2.Technical University of Lisbon, PortugalLab. of Robotics and Systems in Engineering and Sciencehttp://welcome.isr.ist.utl.pt
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Underwater localization
Localization of an Autonomous Underwater Vehicle (AUV)
◮ GPS not working under the water
◮ Dead-reckoning (IMU, DVL)
◮ drift
◮ External array of acoustic baseline (LBL, SBL, USBL)
◮ expensive◮ limited coverage area
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Single beacon localization
Analysis of the relative localization using a single beacon(transducer/transponder couple) and on board sensors
GPS
d η2,z
◮ Acoustic communication: range measurement and sensor dataexchange
◮ On-board sensors information: depth and velocity
◮ We study the observability of the system and define a metrics
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Modeling and objective
xv,1xv,2
x
ΣI
Σv,1
Σv,2
◮ Model:
x = v
y =
[
12x
Tx
x3
]
◮ Objective: We want to estimate the relative positioning x of vehicle2 with respect to vehicle 1 from the output y ∈ IR2, i.e., fromdistance and depth difference
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Observability analysis
Starting from the generic non-linear model
{
x = f(x, u)
y = h(x)
we discuss the local weak observability of system
Reference:R. Hermann and A. Krener. Nonlinear controllability and observability.IEEE Transactions on Automatic Control, 22(5):728–740, 1977
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Local weak observability
By defining the Lie derivatives of the scalar output hj as
L0f hj = hj
L1f hj = ▽hj · f =
∂hj∂x · f =
∑3i=1
∂hj∂xi
· fi
L2f hj = ∂
∂x[
L1f hj
]
· f
· · ·
Lnf hj = ∂
∂x[
Ln−1f hj
]
· f
O =
▽L0f h1
▽L0f h2
▽L1f h1
▽L1f h2...
▽Lnf h1
▽Lnf h2
=
x1 x2 x30 0 1v1 v2 v30 0 00 0 0
...
rank(O) < 3 ⇔ x1v2 − x2v1 = 0
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Observability and metric for 2D model
Neglecting the vertical components that is given by the depthmeasurements, the 2D model is:
{
x = v
y = 12x
Tx
We want to study how the 2D relative motions effects the observability
x
vθ
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Observability and metric for 2D model
It can be easily observed that the terms ▽Lkf h1 of O different from zero
are only for k = {1, 2}. Thus O =
[
xT
vT
]
. Defining x = ‖x‖, v = ‖v‖,
γ = ‖x‖‖v‖ , and θ = φ− α, we can reformulate as:
O =
[
x cosα x sinαv cosφ v sinφ
]
= v
[
γ cosα γ sinαcosφ sinφ
]
We define as metric the condition number C ≥ 1 of O :
C =max{σ1,2}
min{σ1,2}=
γ2 + 1 +√
γ4 + 2γ2 cos(2θ) + 1
‖2γ sin(θ)‖.
Note: C is function of γ and θ
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Observability metrics for 2D model
The condition number C−1 of O as function of γ and θ is plotted:
02
46
−5
0
50
0.5
1
0 1 2 3 4−3
−2
−1
0
1
2
3
2.5
5
30
210
60
240
90
270
120
300
150
330
180 0
−5
0
5
−5
0
50
0.5
1
θ
θ γγ
C−1
C−1
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Observability metrics for 2D model
C−1 has a maximum for{
γ = 1 (i.e., x = v)θ = ±π
2 (tangential motion)
C−1 is null for θ = 0 (radial motion), γ = 0, and γ → ∞
Assuming a constant relative speed, the observability conditions andtherefore the expected performance of any position observer degradeswhen the distance between two AUVs increases.
X
X
V
V
If γ = 1 then C−1 = 1 C−1 = 0
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Simulation results
AUV localization with respect to a fixed buoy using EKF and range-onlymeasurement
Set of orthogonal segments
20
40
80
0
0
-20
-20-40
-40-60-80
60
[m]
[m]
Estimation error
Distances from transponders
Eigenvalue ekf covarinace
1/Observability index50
50
50
50
100
100
100
100
100
150
150
150
150
200
200
200
200
200
10
00
00
00
00
5
24
0.10.2
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Simulation results
AUV localization with respect to a fixed buoy using EKF and range-onlymeasurement
Same circular paths (θ = π/2) at different velocities (changing γ)
20
40
60
80
100
0
0-20
[m]
[m]50-50
Estimation error
Distances from transponders
Eigenvalue ekf covarinace
1/Observability index50
50
50
50
100
100
100
100
100
150
150
150
150
200
200
200
200
200
250
250
250
250
1020
00
00
00
00
12
0.05
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Simulation results
AUV localization with respect to a fixed buoy using EKF and range-onlymeasurement
Different circular paths with constant γ
50
100
100
150
200
200-100-2000
0
[m]
[m]
250
300
Estimation error
Distances from transponders
Eigenvalue ekf covarinace
1/Observability index
0.010.005
50
50
50
50
100
100
100
100
100
150
150
150
150
200
200
200
200
200
250
250
250
250
1020
00
00
00
00
12
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Relative localization experiments
Inverse localization problem: a surface vehicle has to estimate theposition of an underwater transponder (whose depth is known) usingrange measurement from acoustic model.
◮ surface vehicle with GPS
◮ a transponder at 3m depth (as a second vehicle)
◮ acoustic modem exchanging few bytes every 2-3 seconds
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Relative localization experiments
The ASV was commanded to perform different paths: a set ofparallel/orthogonal segments or circular paths
The data were post-processed to test an extended Luenberger observerestimating the relative positioning between the ASV and the transponder
utmx [m]
utm
y [
m]
T:0
T:83T:165
T:248T:330
T:413T:496
−100 −80 −60 −40 −20 0 20 40 60−150
−100
−50
0
utmx [m]
utm
y [
m]
T:0T:83
T:166
T:248
T:331
T:414
T:497
−80 −60 −40 −20 0 20 40 60−150
−140
−130
−120
−110
−100
−90
−80
−70
utmx [m]
utm
y [
m]
T:0
T:49
T:98
T:147
T:196
T:245T:294
−100 −80 −60 −40 −20 0 20 40 60−150
−100
−50
0
Experiments in Lisbon, Nov. 2010.
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
Conclusions and future works
◮ Observability conditions for cooperative underwater localization
◮ Find the relative motions that do not ensure observability
◮ We defined a metric for the observability
◮ We tested different observers for relative positioning estimation
◮ Find elementary behaviors/maneuvers to move the robots ensuringobservability
◮ Extend the observability issues to more than two vehicles
F. Arrichiello, G. Antonelli, A.P. Aguiar, A. Pascoal IEEE/RSJ IROS, San Francisco, 28 September 2011
CO3AUVs Project
framework : COoperative COgnitive COntrol for AutonomousUnderwater Vehicles (CO3AUVs) Project
