Vehicle Formations Based on Formations Based on Ranges and Ranges and Bearings Bearings

Transcription

1 Collaborative Collaborative Localization Localization of Vehicle of Vehicle Formations Based on Formations Based on Ranges and Ranges and Bearings Bearings Beatriz Q. Ferreira, João Gomes, Cláudia Soares, João P. Costeira Institute for Systems and Robotics - LARSyS Instituto Superior Técnico, University of Lisbon jpg@isr.ist.utl.pt Italy - August 31, 2016

2 Scenario Goal Methods Hybrid Collaborative Localization: CLORIS Localization of Moving Vehicle Formations: CLORIS with time domain Strategy Results Conclusions and Future Work J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

3 Sea bottom monitorization WiMUST project: conduct geoacoustic surveys where a team of marine robots tow acoustic sources (a) Team of autonomous vehicles tow multiple streamer arrays (a) 2D surface configuration (b) 3D submerged configuration J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st (b)

4 Shared data include: inter-vehicle distances and/or bearing measurements Set of reference points (anchors), at fixed positions We assume measurements are available as needed for vehicles to compute their positions d 1k d 1k d 1k u 1k d 2k d 2k Vehicle 1 u 21 d 12 u 1k Range anchors Visual anchors u 12 u 2k Vehicle 2 J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

5 In the absence of GPS, the localization of underwater vehicles is key to georeference acquired data and control the coordinated surveying operation Goal: Collaborative localization Jointly determine the positions of multiple underwater vehicles based on pairwise range and bearing measurements. J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

6 CLORIS: hybrid method based on convex relaxation that integrates range and bearing measurements (B. Ferreira et al., 2015) 1 Higher accuracy than previous formulations - single global minimum found in a robust way Parallelizable and fully distributed (when vehicles can determine absolute bearing) CLORIS operates on measurement snapshots 1 B. Ferreira, J. Gomes, and J. P. Costeira, A unified approach for hybrid source localization based on ranges and video, in Proceedings of the IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP 15), April J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

7 Now, we incorporate time into the estimation process of moving vehicle formations We adopt a strategy by (S. Schlupkothen et al., 2015) 2, that adds a regularizing term to the CLORIS cost function penalizing deviations between estimated and predicted vehicle positions based on previous snapshots 2 S. Schlupkothen, G. Dartmann, and G. Ascheid, A novel low complexity numerical localization method for dynamic wireless sensor networks, IEEE Transactions on Signal Processing, vol. 63, no. 15, pp , Aug J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

8 Measurement Model Vehicle formation, at each instant, represented as an undirected graph G = (V,ℇ), whose nodes are the vehicles and the arcs pairwise measurements x i R n, i V is the position of node i at a given instant a k R n is the position of anchor k d ij or d ik is the range between node i and node j or between node i and anchor k u ij or u ik is the bearing of node i w.r.t. node j or anchor k J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

9 Measurement Model d ij = x i x j, d ik = x i a k u ij = x i x j x i x j, u ik = x i a k x i a k d 1k d 1k d 1k u 1k d 2k d 2k Range anchors Visual anchors x 1 u 21 d 12 u 12 u 1k u 2k Actual measurements are corrupted by noise J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st x 2

10 The collaborative hybrid localization problem consists in estimating, for each time instant, the node positions x i, from the available measurements d ij, d ik and u ij, u ik We adopt a Least-Squares formulation d 1k d 1k d 1k u 1k d 2k d 2k x 1 u 21 d 12 u 12 u 1k u 2k x 2 J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

11 Cost function terms We replace the non-convex LS range-based terms ( x d ) 2 by the convex underestimator D Bd 2 x = ( x d ) 2, if x d 0, otherwise. B d d 0 D Bd x x Disk Relaxation J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

12 Cost function terms For bearing we have D Lu 2 x = (I uu T ) x 2 x L u i a i J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

13 Hybrid cost function for static cooperative localization f x t = D Bij 2 x i x j + D Bik 2 x i a k i R ~ j i k R i + D Lij 2 x i x j + D Lik 2 x i a k i T ~ j i k T i Existence of range measurements involving nodes i and j Existence of angle measurements involving nodes i and j J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

14 Anchors have known positions over time Vehicles move freely according to the model x t = x t 1 + v t T, t Z Vector of concated nodes coordinates Vector of concated velocities for all nodes Time difference between snapshots Dynamic localization problem adds the knowledge of previous estimates to the static case formulation J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

15 The new optimization problem is 2 min f(x (t)) + λ x t x(t) x t Remember: f x t = D Bij 2 x i x j + D Bik 2 x i a k i T ~ j D Lij 2 x i x j + D Lik 2 x i a k k Ti i i R i k Ri + ~ j prior on expected positions Prediction of x(t) uses the model x(t) = x t 1 + v t T, with v t estimated from coordinate differences in previous snapshots J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

16 v t is estimated using the polar representation Magnitudes and phases are computed through Weighted Moving Averages v i t = v i t = t 1 j=1 t 1 j=1 w v,j x i t j x i (t j 1) T t 1 j=1 w v,j w v,j (x i t j x i (t j 1)) T t 1 j=1 w v,j w v,j and w v,j are weights that emphasize recent contributions J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

17 The solution of the optimization problem 2 min f(x (t)) + λ x t x(t) x t retains the good properties of CLORIS Can be efficiently performed in parallel (separable), for each node Truly distributed collaborative localization solution involving physical vehicles J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

18 Simulation General purpose convex solver (CVX) Benchmark against CLORIS Performance Evaluation Accuracy estimation for the total trajectory followed by the vehicle formation RMSE = 1 MC 1 N 1 T MC k=1 N i=1 T t=1 x i t x i (t) k 2 Monte Carlo runs Number of vehicles Time instants captured True position Estimated position J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

19 Add white Gaussian noise ω~n(0, η 2 δ 0 2 I) η: noise factor δ 0 : nominal difference between position vectors (δ 0 = x i x j or δ 0 = x i a k ) δ = δ 0 + ω Noisy Range measurements d = Bearing measurements u= δ δ δ J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

20 100 Monte Carlo trials Noise factor η (0.001, 0.005, 0.01, 0.05,0.1,0.2) 2D networks 1 unit square N = 10 vehicles Linear motion T = 10 instants J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

21 All vehicles with same linear velocity v = (0.1,0.1) Approx. 10% improvement J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

22 All vehicles with different but constant linear velocities v 0.05,0.15 J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

23 All vehicles with different and varying linear velocities v 0.05,0.15 J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

24 Anchor positions are not known. All vehicles with same linear velocity v = (0.1,0.1)? Anchor geometric configuration is recovered by factorization of Euclidean Distance Matrix (EDM) generated by anchor-anchor ranges 3 Improvement higher than 10% 3 J. Gomes, E. Zamanizadeh, J. Bioucas-Dias, J. Alves, and T. Furfaro, Building location awareness into acoustic communication links and networks through channel delay estimation, in Proceedings of the 7 th ACM International Conference on Underwater Networks and Systems (WUWNet 12), Los Angeles, CA, USA, November J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

25 CLORIS: convex relaxation strategy allows to retain node positions as optimization variables Inclusion of hybrid terms (bearing) in the cost function Inclusion of regularization terms that induce temporal filtering of node positions Reduced RMSE by 10%, relative to instantaneous localization, over several scenarios with linear velocity J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st

26 Develop better velocity prediction models, to address richer trajectories Derive efficient parallel minimization algorithms for the regularized hybrid cost function Solve for multiple time instants in a single optimization for even better accuracy J. Gomes (ISR/IST) - Collaborative Localization of Vehicle Formations Based on Ranges and Bearings Ucomms 16, August st st