Distributed Vision-Aided Cooperative Navigation Based on Three-View Geometry

Transcription

1 Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry Vadim Indelman, Pini Gurfil Distributed Space Systems Lab, Aerospace Engineering, echnion Ehud Rivlin Computer Science, echnion Hector Rotstein RAFAEL Advanced Defense Systems March 11 1

2 Contents Introduction hree-view Constraints Fusion with Navigation System Results Conclusions

3 Introduction A group of cooperative platforms is considered Required to autonomously perform different missions Navigation is an essential capability Dead reckoning \ inertial navigation errors have to be compensated External sensors (e.g.: GPS, camera, range sensor) Additional information (e.g.: DM) What happens if GPS is unavailable or unreliable? his work: Vision-based approach for cooperative navigation Each platform is equipped only with: INS, single camera No additional sensors or a priori information is required Except for initial navigation solution and camera calibration parameters

4 Previous Work Use some robots as landmarks: Cooperative Positioning with Multiple Robots, Kurazume R. et al., 1994 Relative pose measurements between pairs of robots: Distributed Multirobot Localization, Roumeliotis S.I. and Bekey G.A., Direct & indirect encounters between pairs of robots, nonlinear optimization: Multiple Relative Pose Graphs for Robust Cooperative Mapping, Kim B. et al., 1 Vision-aided navigation based on three-view geometry: Mosaic Aided Navigation: ools, Methods and Results, Indelman V. et al., 1 Consistent information fusion: Consistent Cooperative Localization, Bahr A. et al., 9 4

5 Concept Navigation update whenever the same scene is observed by three views Possibly captured by different platforms Not necessarily at the same time he camera is not required to be aimed towards other platforms (in contrast to relative pose measurements) Setup Each platform is equipped with its own INS, Camera Perhaps, additional sensors or a-priori information All\some platforms maintain a repository of stored images associated with navigation information he platforms are able to exchange navigation and imagery data Each platform maintains a local graph, required for correlation calculation

6 Overview Pos V Ψ d b 6

7 hree-view Constraints Each image may be captured by a different platform he images are not necessarily captured at the same time Images are stored in repositories and retrieved upon demand p q λ - static landmark - line of sight (LOS) I 1 1 q1,λ1 - scale parameter, s.t. is the range to landmark - translation from i to j ij λq I 1 q,λ p q,λ I 7

8 hree-view Constraints (cont.) ( ) ( ) q1 1 q = q q = ( q q ) ( q ) = ( q ) ( q q ) First two equations epipolar constraints hird equation relates between the magnitudes of and Reformulating: 1 g 1 1 = f 1 = u 1 = w 1 1 where ( 1, ) (, ) ( 1,, ) (,, ) g= g q q f = f q q u= u q q q w= w q q q 1 8

9 hree-view Constraints (cont.) Multiple features Matching pairs between 1st and nd view Matching pairs between nd and rd view Matching triplets between the three views N C 1 1 C { q1, q } i i i= 1 N C C { q, q } i i i= 1 C1 C C { q1, q, q } i i i N 1 i=1= 1 u i = w i= 1,, N 1 i f j = j= 1,, N 1 1 g k = k = 1,, N 1 1 U W F = N = N + N + N 1 N G N 1 1 9

10 Fusion with Navigation using Implicit Extended Kalman Filter (IEKF) Residual Measurement Recall U W z F 1 N G N All original LOS vectors are expressed in camera system of the appropriate view, are functions of 1 Pos, Pos, Pos 1 Pos Pos ( t ) i i i ( { C }) 1 C C, Ψ,, Ψ, 1, Ψ 1, 1,, z= h Pos Pos Pos q q q i i i 1

11 Fusion with Navigation using IEKF (cont.) State vector definition: X = P V Ψ d b i Inertial navigation error of the i-th platform: ( t ) ( t ) X =Φ X + ω i i b t t i a t t a b a b Linearization of z ( { C }) 1 C C, Ψ,, Ψ, 1, Ψ 1, 1,, z= h Pos Pos Pos q q q ( ) ( ) ( ) III II 1 I 1 i i i H X t + H X t + H X t + Dv + H.O.. ( t ), ( t ), ( t ) X X X III II I 1 represent navigation errors of different platforms at different time instances. None of these are known a-priori Can be correlated heoretically, all the participating platforms can be updated 11

12 Fusion with Navigation (cont.) he measurement update step involves cross-covariance terms E.g., if only platform III is updated: with where ( t ) z( t, t, t ) K P P P = P H + P H + P H X = X III t z t t t z t t t ( ) (,, ) (,, ) P = H 1 P,, H + H t t t H1 H H1 DRD P1 P + z 1 ( ) [ ] [ ] Maintaining all the possible cross-covariance terms impractical In contrast to relative pose measurements herefore: either neglect, or calculate upon-demand P ( ) X ( ) P E Xɶ t ɶ t ij i i j j P 1

13 Explicit Calculation of Cross-covariance terms - Concept Algorithm: More details: Graph-based Distributed Cooperative Navigation, Indelman V., et al., ICRA 11 Allows updating only one platform Concept: 1. Store covariance and cross-covariance terms from all the past three-view measurement updates ɶX ( ) ( ). Express and according to the history of MP measurement updates i ti ɶX j t j. Calculate E ( ) ( ) based on expressions from step. i ti j t Xɶ Xɶ j Automation of the above for general scenarios using graph representation 1

14 Simulation Results Leader-Follower Scenario platforms: Leader, Follower Leader is equipped with a better IMU Initial navigation errors and IMU errors: rajectory: Straight and level, north heading flight Velocity: 1 m/s Leader is m ahead ( second delay) Height above ground level: ±m Follower is updated every 1 seconds Leader is not updated (inertial navigation) Synthetic imagery Follower Leader 14

15 Simulation Results Leader-Follower Scenario (cont.) Monte Carlo results (1 runs): Follower s navigation errors North [m] East [m] Alt [m] µ σ Sqrt cov. Inertial ime [sec] Position errors V N [m/s] V E [m/s] V D [m/s] 1 µ σ Sqrt cov. Inertial ime [sec] Velocity errors 1

16 Simulation Results Leader-Follower Scenario (cont.) Monte Carlo results (1 runs): Follower s navigation errors Φ [deg] Θ [deg] Ψ [deg] µ σ Sqrt cov. Inertial ime [sec] Euler angle errors d x [deg/hr] d y [deg/hr] d z [deg/hr] 1 µ σ 1 Sqrt cov ime [sec] b x [mg] b y [mg] b z [mg] ime [sec] Drift and bias estimation errors 16

17 Simulation Results Leader-Follower Scenario Monte Carlo results (1 runs): Follower s vs. Leader s navigation errors V N [m/s] V E [m/s] V D [m/s] 4 µ σ Sqrt cov. σ Leader ime [sec] b x [mg] b y [mg] b z [mg] 1 µ σ Sqrt cov. σ Leader ime [sec] Velocity errors Bias estimation errors 17

18 Experiment Results Pattern Holding Scenario Experiment Setup An IMU and a camera were mounted on top of a ground vehicle IMU\INS: Xsens Mi-G Camera: Axis 7MW IMU data and captured images were stored and synchronized IMU 1Hz Imagery 1Hz he method was applied in two modes: Multi-platform update Self update (all images from the same platform) 18

19 Experiment Results Pattern Holding Scenario (cont.) wo different trajectories IMU and camera were turned off in between 1. 1 I II Starting point of II wo platforms with identical hardware (camera + IMU) Heig ght [m]. -. Recorded imagery -1 4 North [m] Starting point of I 4 East [m] 6 19

20 Experiment Results Pattern Holding Scenario (cont.) Example Image 1 Matching riplets Implementation details (simplified) SIF features extraction from each image Feature matching based on their descriptor vectors False matches rejection using RANSAC over the Fundamental matrix model he Fundamental Image 1 matrix is not Image required elsewhere Image Image Image Image N N N { q C } { } { } 1 1 1, q C, q C, q C C 1, q C C, q, q 1 1 i= 1 i= 1 i= 1 i i i i i i i

21 Experiment Results Pattern Holding Scenario (cont.) Multi platform update Self update Position errors North [m] East [m] Height [m] 1 Nav. error Sqrt. Cov. Inertial ime [sec] Platform I North [m] East [m] Height [m] 1 Nav. error Sqrt. Cov. Inertial ime [sec] Platform II 1

22 Conclusions Distributed cooperative navigation aiding hree-view constraints are formulated whenever the same scene is observed by several platforms he camera is no more required to be aimed towards other platforms (as in relative pose measurements) Range sensor is not required he views are not necessarily captured at the same time Allows reduction of navigation errors in some platforms based on other platforms in the group Including position and velocity errors in all axes