Click here to load reader

A Minimum Energy Solution to Monocular Simultaneous Localization And Mapping

  • View
    68

  • Download
    1

Embed Size (px)

DESCRIPTION

Paper abstract: This paper presents a Model Predictive Control (MPC) scheme for nonlinear continuous time systems where an extra performance index, which is not a measure of the distance to the set point, is introduced to influence the transient behavior of the controlled system. The scheme is based on the following fact, proven in the paper: Given a stabilizing MPC controller, adding a function, integrable in the interval [t;+\infty), to the stage cost does not change the asymptotic convergence property of the closed loop state trajectory. As a numerical example, this result is applied to solve a simple visual servo control problem where an MPC controller drives the state to the origin while penalizing weakly observable trajectories.

Text of A Minimum Energy Solution to Monocular Simultaneous Localization And Mapping

  • 1. A Minimum Energy Solution to Monocular Simultaneous Localization And Mapping Andrea Alessandretti 1 (speaker) Antnio Pedro Aguiar 2 Joo Pedro Hespanha 3 Paolo Valigi 4 1 IST-EPFLjoint doctoral program, Portugal/Switzerland 2 Instituto Superior Tcnico (IST), Portugal 3 University of California Santa Barbara (UCSB), USA 4 University of Perugia, ItalyConference on Decision and Control 2011, Orlando, USA

2. Motivation Autonomous robotics Navigation without a priori knowledge of the map Localization in environment without global positioning ( e.g. underwater vehicle ) 3. Motivation Autonomous robotics Navigation without a priori knowledge of the map Localization in environment without global positioning ( e.g. underwater vehicle )u(t), t[0, T ]y (t), t[0, T ]SLAM modulep (T ) (T ), i = 1, . . . , N liSLAM problem : Estimate the robot position and environment map using only information of the control input and relative observation of the environment. 4. Motivation Autonomous robotics Navigation without a priori knowledge of the map Localization in environment without global positioning ( e.g. underwater vehicle )u(t), t[0, T ]y (t), t[0, T ]SLAM modulep (T ) (T ), i = 1, . . . , N liSLAM problem : Estimate the robot position and environment map using only information of the control input and relative observation of the environment.Monocular SLAM problem : Perform SLAM using a single camera sensor for the relative observation. 5. Outline From SLAM to System with perspective outputs Minimum Energy Observer SLAM algorithm Simulation results 6. Kinematic model Model description Camera conguration : (p W , RW C ){C}SE(3)T liC = RW C liWT RW C p WpW{W }liW = p W + RW C liC 7. Kinematic model Model description{C}Camera conguration : (p W , RW C ) Camera twist : (v C ,SE(3)C)se(3)Control input : u = vCC TT liC = RW C liWT RW C p WpW{W }liW = p W + RW C liC0 C=33 220 110 8. Kinematic model Model description{C}Camera conguration : (p W , RW C ) Camera twist : (v C ,SE(3)C)se(3)Control input : u = vCC Tpli T liC = RW C liWT RW C p WpW{W }liW = p W + RW C liC0 C=33 220 110 9. Kinematic model Model description{C}Camera conguration : (p W , RW C ) Camera twist : (v C ,SE(3)C)se(3)Control input : u = vCC Tpli T liC = RW C liWT RW C p WpW{W }liW = p W + RW C liC State equation p l 1 lNC=0 C0 0...00p l10 ...ClN+vC 0 00 C=33 220 110 10. Perspective outputs Observation yi =yi = liC = li ipi= ||liC ||Normalized Retina (i.e. f=1)liC = (u, v ) Image pointUnknown scalar C = li,3 Normalized retina : i Omnidirectional camera :C C li,1 /li,3 C C li,2 /li,3 1{C}yi(uc , vc ) Principal PointOptic center Camera Image Plane{W }C li,1 C li,2 C li,3 11. Perspective outputs Observation yi =yi = liC = li ipiNormalized Retina (i.e. f=1)liC = (u, v ) Image pointUnknown scalar C = li,3 Normalized retina : i Omnidirectional camera :C C li,1 /li,3 C C li,2 /li,3 1{C}= ||liC ||yi(uc , vc ) Principal PointOptic center Camera Image Plane System with perspective outputs: Cp l 1=p N i yi... C0 0= li000p l 10 ...p , i = 1, . . . , NCp N+vC 0 0{W }C li,1 C li,2 C li,3 12. Minimum Energy Observer System with perspective outputs :x = A(u)x + b(u) + G(u)d i yi= Ci (u)x + di (u) + ni , i = 1, . . . , N (nonlinear dynamic) Minimum Energy observer : x (t) := arg min J(z, t) n z RJ(z ; t) :=min d:[0,t),j (ti ), n i=0,1,...,kji{(x(0) t+x0 )T P0 1 (x(0) kd( ) 02x0 ) nj (ti )d + i=0 j J2: x(t) = z } Deterministic unknown noise Solution (lter equation) : impulsive dynamic lter We present the fully discrete version Aguiar, A. P., & Pedro, J. (2006). Minimum-Energy State Estimation for Systems with Perspective Outputs. IEEE Trans. on Automat. Contr., 51, 226--241. 13. Minimum Energy Observer System with perspective outputs :x = A(u)x + b(u) + G(u)d i yi= Ci (u)x + di (u) + ni , i = 1, . . . , N (nonlinear dynamic) Minimum Energy observer : x (t) := arg min J(z, t) n z RJ(z ; t) :=min d:[0,t),j (ti ), n i=0,1,...,kji{(x(0) t+x0 )T P0 1 (x(0) kd( ) 02x0 ) nj (ti )d + i=0 j J Deterministic unknown noise2: x(t) = z }Example : Solution (lter equation) : impulsive dynamic lter We present the fully discrete version Aguiar, A. P., & Pedro, J. (2006). Minimum-Energy State Estimation for Systems with Perspective Outputs. IEEE Trans. on Automat. Contr., 51, 226--241. 14. Minimum Energy Observer System with perspective outputs :x = A(u)x + b(u) + G(u)d i yi= Ci (u)x + di (u) + ni , i = 1, . . . , N (nonlinear dynamic) Minimum Energy observer : x (t) := arg min J(z, t) n z RJ(z ; t) :=min d:[0,t),j (ti ), n i=0,1,...,kji{(x(0) t+x0 )T P0 1 (x(0) kd( ) 02x0 ) nj (ti )d + i=0 j J Deterministic unknown noise2: x(t) = z }Example : Solution (lter equation) : impulsive dynamic lter We present the fully discrete version Aguiar, A. P., & Pedro, J. (2006). Minimum-Energy State Estimation for Systems with Perspective Outputs. IEEE Trans. on Automat. Contr., 51, 226--241. 15. Minimum Energy Observer System with perspective outputs :x = A(u)x + b(u) + G(u)d i yi= Ci (u)x + di (u) + ni , i = 1, . . . , N (nonlinear dynamic) Minimum Energy observer : x (t) := arg min J(z, t) n z RJ(z ; t) :=min d:[0,t),j (ti ), n i=0,1,...,kji{(x(0) t+x0 )T P0 1 (x(0) kd( ) 02x0 ) nj (ti )d + i=0 j J Deterministic unknown noise2: x(t) = z }Example : Solution (lter equation) : impulsive dynamic lter We present the fully discrete version Aguiar, A. P., & Pedro, J. (2006). Minimum-Energy State Estimation for Systems with Perspective Outputs. IEEE Trans. on Automat. Contr., 51, 226--241. 16. Minimum Energy Observer System with perspective outputs :x = A(u)x + b(u) + G(u)d i yi= Ci (u)x + di (u) + ni , i = 1, . . . , N (nonlinear dynamic) Minimum Energy observer : x (t) := arg min J(z, t) n z RJ(z ; t) :=min d:[0,t),j (ti ), n i=0,1,...,kji{(x(0) t+x0 )T P0 1 (x(0) kd( ) 02x0 ) nj (ti )d + i=0 j J Deterministic unknown noise2: x(t) = z }Example : Solution (lter equation) : impulsive dynamic lter We present the fully discrete version Aguiar, A. P., & Pedro, J. (2006). Minimum-Energy State Estimation for Systems with Perspective Outputs. IEEE Trans. on Automat. Contr., 51, 226--241. 17. Minimum Energy Observer System with perspective outputs :x = A(u)x + b(u) + G(u)d i yi= Ci (u)x + di (u) + ni , i = 1, . . . , N (nonlinear dynamic) Minimum Energy observer : x (t) := arg min J(z, t) n z RJ(z ; t) :=min d:[0,t),j (ti ), n i=0,1,...,kji{(x(0) t+x0 )T P0 1 (x(0) kd( ) 02x0 ) nj (ti )d + i=0 j J Deterministic unknown noise2: x(t) = z }Example : Solution (lter equation) : impulsive dynamic lter We present the fully discrete version Aguiar, A. P., & Pedro, J. (2006). Minimum-Energy State Estimation for Systems with Perspective Outputs. IEEE Trans. on Automat. Contr., 51, 226--241. 18. Minimum Energy Observer System with perspective outputs :x = A(u)x + b(u) + G(u)d i yi= Ci (u)x + di (u) + ni , i = 1, . . . , N (nonlinear dynamic) Minimum Energy observer : x (t) := arg min J(z, t) n z RJ(z ; t) :=min d:[0,t),j (ti ), n i=0,1,...,kji{(x(0) t+x0 )T P0 1 (x(0) kd( ) 02x0 ) nj (ti )d + i=0 j J Deterministic unknown noise2: x(t) = z }Example : Solution (lter equation) : impulsive dynamic lter We present the fully discrete version Aguiar, A. P., & Pedro, J. (2006). Minimum-Energy State Estimation for Systems with Perspective Outputs. IEEE Trans. on Automat. Contr., 51, 226--241. 19. Outline From SLAM to System with perspective outputs Minimum Energy Observer SLAM algorithm Simulation results 20. SLAM algorithm Image acquisition Feature extraction Feature matchingli Filter Prediction stepp Initialization new feature Filter update step 21. SLAM algorithm Image acquisition Feature extraction Feature matchingli Filter Prediction stepp Initialization new feature Filter update step 22. SLAM algorithm Image acquisition Feature extraction Feature matchingli Filter Prediction stepp Initialization new feature Filter update step 23. Feature Initialization Initialization function : yn = y1y2TC lny1= g(y1 , y2 , ) =Ty2R50px0px2 0 0 2T15 20px0px120px150px0pxT1 00 1102T1 0 0 10pxT10 250 0 0 5081 24. Feature Initialization Initialization function : yn = y1y2TC lnPln =g50px0px2 0 0 2T15 20px0pxTy2 R 0y2RLANDMARK ESTIMATEC = y1 lny1= g(y1 , y2 , ) =T0 2T g120px150px0pxT1 00 1102T1 0 0 10pxT10 250 0 0 5081 25. Feature Initialization Initialization function : yn = y1y2TC lnPln =g50px0 2C + x = x (x,ln ) p PT0 Pln + Pp(x, P )15 20px0px120pxSTATE AUGMENTATIONP =2 0 0 2TT g150pxP 00pxTy2 R 0y2RLANDMARK ESTIMATEC = y1 lny1= g(y1 , y2 , ) =T(x, P )0pxT1 00 1102T1 0 0 10pxT10 250 0 0 5081 26. Observer equation PREDICTIONUPDATEPi+1 = Ai Pi AT + Gi GiT iPi+1 = ((Pi+1 )xi+1 = Ai xi + bi xi+1 = xi+1 withCjT (u) IW (ti ) := j Jk Jyj (ti )yj (ti )T yj (ti )21+ Wi+1 )1Pi+1 Wi+1 xi+1 Cj (u) Recursive solution Exact solution ( No linearization procedures ! vs EKF SLAM ) 27. Convergence analysis ISS with respect to the disturbances N||(t)|| xcex (t) := x (t) t||(0)|| + xdsup ||d( )|| +j(0,t)x(t)j=1sup ||nj ( )|| (0,t)persistence of excitation-like condition ( e.g. parallax of observation ) The landmark uncertainly is lower bounded by the camera pose uncertainty at the time of the rst observationAt time ti :Ppm (ti ) Pm (ti )Pi+1 = ((Pi+1 ) xi+1 = xi+1 Analysis assumption : single landmark, static camera Pp (ti ) Pmp (ti )UPDATE= with M(ti ) =Pp (0) Pp (0)+ Wi+1 )1Pi+1 Wi+1 xi+1 Pp (0) Pp (0) + M 1 (ti ) CjT (u)M(y (ti ))Cj (u)M(y (tk )) Wi := k i,k Io1j Jk J 28. Simulation results Camera 2015Observed Landmarks1050True Landmark Position Uncertainty Ellipses5 15105051015 29. Simulation results 2D TOP VIEWSettings [s] [m/s]t = 1/30 v=1 Simulated noise Q = (0.02)2 I3 R = (0.06)2 I33 3[(m/s)2 ] [m2 ] Initialization parameters [m] ini = 0 [m] ini = 2000 30. Simulation results State estimation error (m) 10 8 6 4 2 0 -2 -4 -6 -8 -100.1 0.08 0.06 0.04 0.02 0 -0.02 -0.04 -0.06 -0.08 690102030Time (s)40506069.269.4 69.6 Time (s)69.87070 Trace of the covariance matrix of the landmarks (m) 1200 10000.14 0.12 0.1 0.08 0.06 0.04 0.02 0 -0.02 -0.04 -0.06800 600 400 200 0690102030Time (s)4050607069.1 69.2 69.3 69.4 69.5 69.6 69.7 69.8 69.9 70 Time (s) 31. Conclusion Optimal solution to Monocular SLAM No linearization error introduced ( vs EKF SLAM ) Convergency analysis 32. Thanks for your attention Any questions ?