Navigation Performance Enhancement using Online Mosaicking

Transcription

1 Navigation Performane Enhanement using Online osaiking Vadim Indelman Supervisors: Prof. Pini Gurfil, AE, ehnion Prof. Ehud Rivlin, CS, ehnion Advisor: Dr. Hetor Rotstein, Rafael Nov.

2 Introdution Navigation: alulation of position, veloity and attitude over time Aerial, underwater, ground Spae Other planets ulti robot Consumer appliations Autonomous

3 Introdution Navigation Aiding Conept Inertial navigation solution is alulated based on measurements of inertial sensors (or other dead rekoning sensors) Imperfetness of these sensors leads to developing navigation errors Use external sensors to estimate navigation errors and orret navigation solution Estimate also parameterization of inertial measurement unit (IU) errors, and orret all subsequent IU measurements Pos V Ψ Pos V Ψ ost ommon: GPS

4 Introdution Researh Conept he GPS is unavailable or unreliable when operating Indoors, underwater, in urban environments, and over other planets his researh use images aptured during motion for navigation aiding Researh setup Platform is equipped only with an inertial navigation system (INS) and a single (gimbaled) amera No other sensors, no a-priori information (exept for initial onditions, amera alibration parameters) he images aptured during motion are stored in a repository, or/and used for onstruting mosai images Objetive: Utilize mosai onstrution and stored imagery for navigation aiding Computational effiieny 4

5 Introdution osai Construted based on amera-aptured images Represents the observed-so-far environment Enodes information about the platform s navigation history Soure images osai image 5

6 Introdution Navigation-Aiding Algorithms Navigation Aiding Based on Coupled Online osaiking and Camera Sanning Vision-Aided Navigation Based on hree-view Geometry Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry a Calulating Cross-Covariane for a General ulti-platform easurement odel 6

7 Introdution Inertial Navigation Errors odel Pos V Ψ Pos V Ψ State vetor (navigation errors, IU error parameterization) Proess model (Disrete) X P V Ψ d b ( t ) ( t ) X =Φ X + ω b t t a t t a b a b How an X be estimated only based on INS and inoming imagery? 7

8 Navigation Aiding Based on Coupled Online osaiking and Camera Sanning Navigation Aiding Based on Coupled Online osaiking and Camera Sanning, AIAA JGCD, vol., no. 6,, p Real-ime osai-aided Aerial Navigation: I. otion Estimation, AIAA GNC Conferene, 9 Real-ime osai-aided Aerial Navigation: II. Sensor Fusion, AIAA GNC Conferene, 9

9 . Navigation Aiding Based on Coupled Online osaiking and Camera Sanning otivation and Related Work Objetive Vision-based navigation aiding in hallenging senarios Narrow field-of-view amera Low texture senes Poor weather onditions Utilize online mosaiking and gimbaled amera sanning proedures Related Work otion estimation + online mosaiking: Improving Vision-based Planar otion Estimation for Unmanned Aerial Vehiles through online osaiking, Caballero F. et. al., 6 Epipolar onstraints + INS: Epipolar Constraints for Vision-Aided Inertial Navigation, Diel D. et. al., 5 SLA: 6DoF SLA aided GNSS/INS Navigation in GNSS Denied and Unknown Environments, Kim J. and Sukkarieh S., 5 9

10 . Navigation Aiding Based on Coupled Online osaiking and Camera Sanning ain Idea Platform equipped only with an INS and a gimbaled amera Couple between Camera sanning Online mosaiking Construt mosai based on amera-aptured imagery osai-based motion estimation Update INS Camera sanning image sequene (from Google Earth) t t t t 4 osai image onstrution... Camera optial axis otion Heading

11 . Navigation Aiding Based on Coupled Online osaiking and Camera Sanning ain Idea (ont.) otion estimation between urrent image and previous mosai image Based on the homography model Inreased overlapping region Additional mathing features Same quality? Improved motion estimation in hallenging senarios Narrow field-of-view amera Low-texture senes BU: ranslation is estimated up to sale Current image Previous mosai image Additional area Original area Allows reduing navigation errors normal to the motion heading What happens when the same sene is observed by several (>) views?

12 Vision-Aided Navigation Based on hree-view Geometry Real-ime Vision-Aided Loalization and Navigation Based on hree-view Geometry, IEEE AES, submitted osai Aided Navigation: ools, ethods and Results, IEEE\ION PLANS Conferene,

13 . Vision-Aided Navigation Based on hree-view Geometry otivation and Related Work Objetive Position and veloity update in all axes Setup: INS, amera, online-onstruted repository Real time navigation aiding Effiiently handle loop senarios Related work Smoothing + Range: Improved Real-ime Video osaiking of the Oean Floor, Fleisher D. et. al., 995 SLA: 6DoF SLA aided GNSS/INS Navigation in GNSS Denied and Unknown Environments, Kim J. and Sukkarieh S., 5 Pinhole projetion + ulti-view + INS: A ulti-state Constraint Kalman Filter for Vision-Aided Inertial Navigation, ourikis A. and Roumeliotis S., 7 ultiple-view + Bundle adjustment: A Dual-Layer Estimator Arhiteture for Longterm Loalization, ourikis A.I. and Roumeliotis S.I., 8 rifoal tensor + otion estimation: Reursive Camera-otion Estimation With the rifoal ensor, Yu Y.K., et. al., 6

14 . Vision-Aided Navigation Based on hree-view Geometry Conept Pos V Ψ d b Coordinate systems L - Loal Level Loal North (LLLN) C - Camera 4

15 . Vision-Aided Navigation Based on hree-view Geometry hree View Geometry t t q,λ t q,λ q,λ p q λ - stati landmark - line of sight (LOS) - sale parameter, s.t. is the range to landmark - translation from i to j ij λq p 5

16 . Vision-Aided Navigation Based on hree-view Geometry hree View Geometry (ont.) Position of a stati landmark relative to amera position at : λ q = + λ q λ q = + + λ q atrix formulation Note: Range parameters are unknown p t q q q q t q,λ 6 4 LOS and translation vetors are expressed in LLLN of A rank( A) < 4 t λ λ = λ 4 t t q,λ q,λ p 6 6

17 . Vision-Aided Navigation Based on hree-view Geometry hree View Geometry (ont.) heorem: rank(a)<4 if and only if all of the following onditions are satisfied ( ) ( ) q q = q q = ( q q ) ( q ) = ( q ) ( q q ) First two equations epipolar onstraints hird equation relates between the magnitudes of and Suffiient and neessary onditions Reformulating: g = f = u = w where (, ) (, ) (,, ) (,, ) g= g q q f = f q q u= u q q q w= w q q q 7

18 . Vision-Aided Navigation Based on hree-view Geometry hree View Geometry (ont.) ultiple features athing pairs between st and nd view athing pairs between nd and rd view athing triplets between the three views N C C { q, q } i i i= N C C { q, q } i i i= C C C { q, q, q } i i i N i= u i = w i=, K, N i f j = j=, K, N g k = k =, K, N U W F = N = N + N + N N G N 8

19 . Vision-Aided Navigation Based on hree-view Geometry Fusion with Navigation using Impliit Extended Kalman Filter (IEKF) Residual easurement Reall All original LOS vetors are expressed in amera system of the appropriate view are funtions of U W z F N G N, Pos( t), Pos( t), Pos( t) ( ( ) ( ) ( ) ( ) ( ) ( ) { C }) C C t, Ψ t, t, Ψ t, t, Ψ t,,, z= h Pos Pos Pos q q q i i i 9

20 . Vision-Aided Navigation Based on hree-view Geometry Fusion with Navigation using IEKF (ont.) Reall: X = P V Ψ d b Linearization ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) { C } C C t, t, t = t, Ψ t, t, Ψ t, t, Ψ t,,, z h Pos Pos Pos q q q t H X + H X + H X + Dv + H.O.. is the urrent time X needs to be estimated X, X are represented by the ovariane matries attahed to images X i X ( t ) P = E X % X %, P = E X % % X i i i i X, X X In a general ase, and may be orrelated

21 . Vision-Aided Navigation Based on hree-view Geometry Fusion with Navigation using IEKF (ont.) Sine only is estimated, the Kalman gain is given by: with ( t ) z( t, t, t ) Problem: how to alulate when are unknown a-priori? Inertial navigation is assumed between and : P are negleted K P P = X z z ( t ) ( t, t, t ) ( t, t, t ) P = P H + P H + P H X X P P P( ) = H [ ] [ ],, P t t t H + H H H H + DRD z P P, P t >> t, t P, P, P t, t t Valid for. e.g.: loop senarios Otherwise, alulate as shown in the sequel t P =Φ t t P

22 . Vision-Aided Navigation Based on hree-view Geometry Results - Experiment Experiment Setup An IU and a amera were mounted on top of a ground vehile IU\INS: Xsens i-g Camera: Axis 7W IU data and aptured images were stored and synhronized IU Hz Imagery 5Hz he method was applied in two senarios Sequential updates Loop updates

23 . Vision-Aided Navigation Based on hree-view Geometry Results Experiment (ont.) rue trajetory North [m] 4 rue trajetory rue rajetory East [m] Height [m] ime [s] Alt [m] - 4 North [m] 4 East [m] 6 Reorded imagery

24 . Vision-Aided Navigation Based on hree-view Geometry Results Experiment (ont.) Example Image athing riplets Implementation details (simplified) SIF features extration from eah image Feature mathing based on their desriptor vetors False mathes rejetion using RANSAC over the Fundamental matrix model he Fundamental Image matrix is not Image required elsewhere Image Image Image Image N N N { C } { } { } C C C C C C q, q, q, q, q, q, q i= i= i= i i i i i i i 4

25 . Vision-Aided Navigation Based on hree-view Geometry Results Experiment (ont.) Sequential upd Loop upd Nav. Error Sqrt. Cov. Inertial North [m] 5 5 Position errors East [m] - 5 Height [m] 5 ime [s] 5

26 . Vision-Aided Navigation Based on hree-view Geometry Conlusions Appliation of three-view onstraints for navigation aiding New formulation of three-view onstraints for a general stati sene he method allows Redution of position and veloity errors in all axes to the levels of errors present while the first two images were aptured Redution of attitude errors, partial estimation of bias Effiiently handle loop senarios Redued omputational requirements for vision-aided navigation phase Environment representation onstrution (e.g. mosai) may be exeuted in a bakground proess Various potential appliations Cooperative navigation - next 6

27 Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry Distributed Vision-Aided Cooperative Loalization and Navigation based on hree-view Geometry, IEEE Aerospae Conferene, submitted

28 . Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry otivation and Related Work Objetive Navigation update whenever the same sene is observed by different platforms Not neessarily at the same time he amera is not required to be aimed towards other platforms (in ontrast to relative pose measurements) Properly handle orrelation terms involved in the fusion proess Related work Use some robots as landmarks: Cooperative Positioning with ultiple Robots, Kurazume R. et. al., 994 Relative pose measurements between pairs of robots: Distributed ultirobot Loalization, Roumeliotis S.I. and Bekey G.A., Diret & indiret enounters between pairs of robots, nonlinear optimization: ultiple Relative Pose Graphs for Robust Cooperative apping, Kim B. et. al., Consistent information fusion: Consistent Cooperative Loalization, Bahr A. et. al., 9 8

29 . Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry Setup Consider a group of ooperative platforms Eah platform is equipped with its own INS Camera Perhaps, additional sensors or a-priori information All\some platforms maintain a repository of stored images assoiated with navigation information he platforms are able to exhange navigation and imagery data Inertial navigation error of the i-th platform i ( t ) ( t ) X =Φ X + ω i i b t t i a t t a b a b Eah platform maintains a loal graph, required for orrelation alulation 9

30 . Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry hree-view Geometry Several Platforms his time, eah image may be aptured by a different platform he images are not neessarily aptured at the same time Some images may be stored in repositories and retrieved upon demand I I I q,λ q,λ q,λ p

31 . Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry Overview Pos V Ψ d b

32 . Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry Fusion with Navigation Bak to the residual measurement model ( ) ( ) ( ) z H X t + H X t + H X t + Dv + H.O.. III II I ( t ), ( t ), ( t ) X X X III II I represent navigation errors of different platforms at different time instanes. None of these are known a-priori heoretially, all the partiipating platforms an be updated Example: Only platform III is updated three-view measurements

33 . Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry Fusion with Navigation (ont.) he measurement update step involves ross-ovariane terms E.g., if only platform III is updated: with where ( t ) z( t, t, t ) aintaining all the possible ross-ovariane terms impratial In ontrast to relative pose measurements K P P P = P H + P H + P H X = X III t z t t t z t t t ( ) (,, ) (,, ) P P P( ) = H [ ] [ ],, P t t t H + H H H H + DRD z P P ( ) X ( ) P E X% t % t ij i i j j herefore: either neglet, or alulate upon-demand

34 Distributed Vision-Aided Cooperative Navigation Based on hree- View Geometry a Calulating Cross-Covariane for a General ulti-platform easurement odel Graph-based Distributed Cooperative Navigation, IEEE ICRA, submitted 4

35 a. Calulating Cross-Covariane for a General ulti-platform easurement odel General ulti-platform easurement odel Assume a general ulti-platform (P) measurement model that involves information from r platforms Objetive: Calulate r i= ( ) ( ) ( ) z H X t + D t v t i i i i i i i ( t ) X ( t ) E % % X i i j j 5

36 a. Calulating Cross-Covariane for a General ulti-platform easurement odel Conept. Store ovariane and ross-ovariane terms from all the past ulti- Platform (P) measurement updates. Express and aording to the history of P measurement i( ti) %X j t j updates %X ( ) ( ) ( ). Calulate E based on expressions from step. i ti j t X% X% j Algorithm objetive: Automation of the above for general senarios using graph representation 6

37 a. Calulating Cross-Covariane for a General ulti-platform easurement odel General ulti-platform easurement odel Assume a general ulti-platform (P) measurement model that involves information from r platforms r i= ( ) ( ) ( ) z H X t + D t v t i i i i i i i ( ) ( ) Objetive: Calulate E i ti j t X% X% j Represent all P updates exeuted so far in a direted ayli graph (DAG) Ayli graph is assumed In a general senario, allows updating only platforms that ontribute their urrent (and not past) information For simpliity, we onsider updating only one suh platform Speifi senarios exist in whih all the involved platforms an be updated A-posteriori estimation error of the updated platform (denoted by q) + r r ( ) q = I KqH q q Kq H i i Kq Di i i=, i q i= X% X% X% v 7

38 a. Calulating Cross-Covariane for a General ulti-platform easurement odel Graph Representation Eah platform maintains its own DAG G=(V,E) A-priori and a-posteriori ovariane and ross-ovariane matries are stored in G after eah P update wo node types in G: Nodes representing (a-priori) information partiipating in an P measurement E.g., an image and navigation data obtained from some platform Update-event node, representing a-posteriori estimate of the updated platform I II III Eah P update is represented by r+ nodes a a a a + b b b b + 8

39 a. Calulating Cross-Covariane for a General ulti-platform easurement odel Graph Representation (ont.) Eah node an be onneted to another node by a ransition relation between node and Ar weight: he proess noise ovariane is also stored: Example: a X% =Φ X% + ω i i b t t a t t (, ) w a b Q a b a b Φ i t a t b b ( ω ) = E ω i i i t t t t t t a b a b a b a b a a a Φ I a b Φ II a b a + b b b b + Φ III a b 9

40 a. Calulating Cross-Covariane for a General ulti-platform easurement odel Graph Representation (ont.) P update relation i β α : a-priori information of the r partiipating platforms : a-posteriori estimation of the updated platform Ar weight ( ) X% = I K H X% K H X% K D v α βq βq βq βq βi βi βq βi βi i=, i= i q w ( i β, α) he measurement noise ovariane is also stored: r I Kβ Hβ i= q Kβ H i q q βi q q = ( ), r i Kβ D q β R i β K i β D R E q βi β = i vβ v i βi q β L q β α L hree-view a a a a + r β 4

41 a. Calulating Cross-Covariane for a General ulti-platform easurement odel Algorithm Conept Pd E X% X% d First: onstrut two inverse-trees ontaining all the routes in G to the nodes and d. Denote the trees as, Assume we need to alulate a i + a i i+ ai+ Notation: is the parent of the node. If several parents exist: a d,,... r L r d d d L d rd level d d nd level st level d Next: Express P using information stored in nodes in, d Start with st level and proeed upwards d 4

42 a. Calulating Cross-Covariane for a General ulti-platform easurement odel Algorithm Conept (ont.) Start with first-level nodes of, : and d % % d Aording to relation types represented by ar weights Sine E X X is unknown, proeed to next level in the trees Assume, e.g., transition relation in both ases E X % X% d may now be expressed as ( X ω ) E X% Φ % d d d + d d = E ( Φ ) X% + ω X% d = E ( Φ X% + ω )( Φ d d X% d + ω d d) d d d d Are any of the following expressions known (i.e. was stored in the graph)? Noise terms? E X% X%, E X% X%, E X% X% d d d 4

43 a. Calulating Cross-Covariane for a General ulti-platform easurement odel Algorithm Conept (ont.) If unknown, proeed to next level in trees the third level E.g., assume a transition relation in and an P update relation in q Let d, d be the nodes representing the a-priori and a-posteriori estimations of the updated platform X% =Φ X% + ω ( I K H ) X% = X% d q q q d d d d r K H X% K D q i i q i i d d d d d i=, i q i= r Now, E X % X% d may be expressed using nodes from the third level and lower levels. For example: v d d L d q r L d ( ) ( ) r r E Φ X% + ω Φ I K H X% K H K D + X% v ω d d q q q q i i q i i d d d d d d d d d d d i=, i q i= d d 4

44 a. Calulating Cross-Covariane for a General ulti-platform easurement odel Algorithm Conept (ont.) he algorithm proeeds to higher levels in and d until all the terms required for alulating % % are known E X X d Or, reahing top level in both trees Consider reahing the k-th level and analyzing some pair k, d with k k from and d from k d ( d ) ( ) Look for the pair, or j k k, d j, so that E % % X X or is known j d E X% k X% k d j (i.e. stored in G), with smallest j ( ) Assume E X% is known X% j dk d he two nodes ( j in and k in d ) partiipated in the same P update in the past Or, the two nodes are the same j d k j q r L L k d k q r d d L L d d d d 44

45 a. Calulating Cross-Covariane for a General ulti-platform easurement odel Algorithm Conept (ont.) Consider that some term E X% X% is known j dk No need to proeed to nodes from higher levels, whih are related to E X% X% j he ontribution of, to E % % X X d is alulated as j dk ( ) ( ) X% X% ( ) W j E j dk Wd dk + Q jdk L W is the overall weight of the route j in ( j) W d is the overall weight of the route in d ( ) k d k L d d d k d k j ( ) = (, ) W w j i i i= k ( ) = (, ) W d w d d d k i i i= j q r L L q r d d L L d d d 45

46 a. Calulating Cross-Covariane for a General ulti-platform easurement odel Algorithm Conept (ont.) he term Q d j k ( ) X% X% ( ) W E W d + Q j d d k d j k j k Represents the ontribution of proess and measurement noise that are involved when expressing E X % using and X% d %X %X j dk j Lemma: only if j in does not have any desendants that are anestors of d in, AND d k in does not have any desendants that are anestors of in Otherwise: k j d Q = d j d Should be alulated k Q d j k j O i q r L L Q d j k d k d O i N q r d L L d d d 46

47 Bak to the hree-view easurement odel Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry

48 . Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry Bak to the hree-view easurement odel easurement model: Assume only one platform is updated eah time e.g. update equations for platform III: Calulate gain: with ( ) ( ) ( ) z H X t + H X t + H X t + Dv + H.O.. III II I ( t ) z( t, t, t ) K P P = X III t z t t t z t t t ( ) (,, ) (,, ) P = P H + P H + P H X P P P( ) = H [ ] [ ],, P t t t H + H H H H + DRD z P P he ross-ovariane terms are alulated based on the developed approah Standard state and ovariane update IEKF equations 48

49 . Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry Simulation Results Leader-Follower Senario platforms: Leader, Follower Leader is equipped with a better IU Initial navigation errors and IU errors: Follower Leader rajetory: Straight and level, north heading flight Veloity: m/s Leader is m ahead ( seond delay) Height above ground level: ±m Follower is updated every seonds Leader is not updated (inertial navigation) Syntheti imagery 49

50 . Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry Simulation Results Leader-Follower Senario (ont.) onte Carlo results ( runs): Follower s navigation errors North [m] East [m] Alt [m] µ σ Sqrt ov. Inertial ime [se] Position errors V N [m/s] V E [m/s] V D [m/s] µ σ Sqrt ov. Inertial ime [se] Veloity errors 5

51 . Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry Simulation Results Leader-Follower Senario (ont.) onte Carlo results ( runs): Follower s navigation errors Φ [deg] Θ [deg] Ψ [deg] µ σ Sqrt ov. Inertial ime [se] Euler angle errors d x [deg/hr] d y [deg/hr] d z [deg/hr] 5 µ σ 5 Sqrt ov ime [se] b x [mg] b y [mg] b z [mg] ime [se] Drift and bias estimation errors 5

52 . Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry Simulation Results Leader-Follower Senario onte Carlo results ( runs): Follower s vs. Leader s navigation errors V N [m/s] V E [m/s] V D [m/s] µ σ Sqrt ov. σ Leader ime [se] b x [mg] b y [mg] b z [mg] 5 µ σ Sqrt ov. σ Leader ime [se] Veloity errors Bias estimation errors 5

53 . Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry Experiment Results Pattern Holding Senario he same experiment setup wo different trajetories IU and amera were turned off in between wo platforms with idential hardware (amera + IU) Height [m] I II Starting point of II - 4 North [m] Starting point of I 4 East [m] 6 5

54 . Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry Experiment Results Pattern Holding Senario (ont.) wo modes: ulti-platform update Self update (all images from the same platform) Self updates Images used in the first update: a a a 54

55 . Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry Experiment Results Pattern Holding Senario (ont.) ulti platform update Self update Position errors North [m] East [m] Height [m] 5 Nav. error Sqrt. Cov. Inertial ime [se] Platform I North [m] East [m] Height [m] Nav. error Sqrt. Cov. Inertial ime [se] Platform II 55

56 . Distributed Vision-Aided Cooperative Navigation Based on hree-view Geometry Conlusions Distributed ooperative navigation aiding Allows redution of navigation errors in some platforms based on other platforms in the group hree-view onstraints are formulated whenever the same sene is observed by several platforms he amera is no more required to be aimed towards other platforms (as in relative pose measurements) Range sensor is not required he views are not neessarily aptured at the same time Graph-based approah for on-demand alulation of ross-ovariane terms General multi-platform measurement model EKF framework 56

57 Summary Vision-aided navigation INS, amera Utilize inoming imagery, and onstruted mosais, for navigation aiding Algorithms Coupled mosaiking and amera sanning Improved navigation performane in hallenging senarios Redued omputational requirements hree-view geometry onstraints Redution of position and veloity errors in all axes Effiient handling of loop senarios Distributed ooperative navigation based on three-view onstraints Redution of navigation errors of some platforms based on navigation and imagery information obtained from other platforms Graph based approah for on-demand alulation of ross-ovariane terms for a general multi-platform measurement model 57

58 hank you 58