Diapositiva 1 - CONFERENCE CENTER
Download
Report
Transcript Diapositiva 1 - CONFERENCE CENTER
G. Casalino, E. Zereik, E. Simetti, A. Turetta, S. Torelli and A. Sperindè
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Agenda
• Introduction
• Visual Odometry
• Additional Measurements
• State Estimators
• Sequence Estimators
• Multi-Sensor Integration
• Discussion and Conclusion
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Introduction
• Planetary Robot
• accurate localization and motion estimation
• Different techniques
• WO
• IMU
• VO
• Improve VO
• additional image processing
• Extended/Iterated Kalman Filters
• Sequence Estimators
• Integration scheme
• Final multi-sensor scheme
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Visual Odometry
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Visual Odometry
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Visual Odometry
• At each step i , the rover position i 1 ri and orientation
computed with respect to the previous step i 1
i are
i 1
• Sequence of positions-orientations truly attained by the vehicle
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Estimations
i 1
ri rˆi i ;
i 1
i 1
i 1
ˆ
i i i ;
i 1
i 1
i 1
independent white noise sequences affecting
position and orientation measurements
i 1
i ,
i 1
i
i ,
i 1
i
i 1
i N i ,0, i
i 1
measured rover position and orientation
i 1
i 1
i 1
ˆi
rˆi ,
i 1
i N i ,0, i
i 1
position and orientation covariance matrices
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
i 1
Estimations
• At each step, VO provides the rover relative position and orientation
i 1
i
T
• Absolute rover position and orientation:
0 ˆ 1ˆ
i 1 ˆ
ˆ
Ti T1 T2 Ti
0
where
i 1 ˆ
Ri
i 1 ˆ
Ti
0
rˆi
;
1
i 1
Rˆi R
i 1
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
ˆ
i 1
i
Estimations
• Linear open chain of frames, with independent positioning
• Error progressively increasing with the number of stages
• No further constraints
• Improvements:
• Additional measurements
i 2
rˆi , i 2ˆi of the occurred
• At each step provide measurements
motion between frames i 2 and i
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Additional Measurements
• Assumption:
stereo camera can recognize in the current frame a sufficient number
of features belonging to images i 1 and i 2
i 2
ri i 2ri 1 R
i 1 i 1ri
i 2
i i 2i1 i 1i
i 2
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
State Space Model
i 1 i 0
i 2
i 1 1
i 2ˆi 1 0
i 2 ˆ
i 1
0 i 2i 1 1 i 1 ˆ 1 i 1
i 3 i i
0 i 2 0
0
1 i 2 i 1 i 2i 1
orientation
i 1 i 2
1 i i
i 1ri 0 0 i 2 ri 1 I
i 2
i 3
ri 1 I 0 ri 2 0
1
i 2 rˆi 1 0
i 2
i 2
1
R
i 1
ˆ
ri
I
rˆi
0
i 1
i 2 ri 1 i 2 i 1
i 1 i 2
ri i
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
i
i 1
position
State Space Model
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
State Estimators
• System evolution estimated via standard Kalman filters
• Extended (EKF)
• Iterated (IKF)
• Gaussianity hypothesis for the filter
• Gaussianity is not suitable due to system non-linearity
• such an approximation leads to suboptimality
• These are recursive filters
• linearly increasing errors have still to be expected for increasing
number of stages
• Incoming acquisitions to better all the past state estimations
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Sequence Estimators
1,i k 1 k ; k 1,2,i
r
1,i
z1,i
w
1,i
k 1
rk ; k 1,2,i
k 1
rˆk ,
k 2
Z z ; w
1,i
rˆk ; k 1,2, i
1,i 1,i
1,i
X ; r arg max p X 1,i / Z 1,i
X 1,i
1,i
measurement
sequence
• The problem becomes:
state sequence
ˆk , k 2ˆk ; k 1,2,i
k 1
X 1,i 1,i ; r1,i
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
1,i
Sequence Estimators
1,i
• Renew, at each stage, the entire sequence X , without relying on the
previous one X 1,i 1
• Error generally characterizing IKF and EKF should result strongly reduced
BUT
• Increasing dimensionality with increasing number of stages
• linear and quite acceptable with a reasonably high maximum number of
stages
• after this freeze and restart the procedure
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Sequence Estimators
• Solve the following cascade of sub-problems:
1,i
arg max p / z
1,i
1,i
1,i
r
1,i
1,i
arg max p r / w ,
r1,i
1,i
1,i
• Maintain a manageable implementative form that otherwise cannot be
guaranteed, considering the general problem
• Minimization of the Gaussian p.d.f. exponents
• Bayes formula
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Sequence Estimators
• At each stage a linear parametrization is obtained
• It is the constraint for the previous stage
• Back Substitution scheme
• Dynamic Programming strategy
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Comments
• Wait until stage
i
• Renew the interpolated sequence in correspondence of each new stage
• Backward Phase: computational effort increasing with the number of stages
• Restart the procedure from the last stage considered as the new initial one
• Smaller drifting errors
• Accepted suboptimality vs. joint estimation of both sequences 1,i ; r1,i
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Integration Scheme
• Exploit different sensors
• Augmented sensor with better performances
• Different integration schemes:
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Integration Scheme
• First sequential scheme
• SQE fed with smaller variance measurements
data from STE
• Feedback loop used to re-initialize the STE module
• Totally useless scheme
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Integration Scheme
• Without further data, things can be bettered with a parallel integration scheme
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Multi-Sensor Integration
• IMU: measurements about the angular velocity vector
acceleration vector v
• WO integration
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
and the linear
Discussion and Conclusion
• Previously developed VO module
• 5001000 ms via software
• CUDA implementation:
• 10 m s SURF extraction and descriptors via CUDA
• 60 m s matching and tracking via software with SAD
• 50 m s pose estimation via software
• Sequence Estimator
• Sensor data integration
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
120 m s
Discussion and Conclusion
• Open issues:
• still under consideration
• many simulations and experimental tests are to be carried out
• not only planar motion
• mange the non-holonomic constraints for the rover
• “Visual Odometry Centric” scheme
• different state space model starting from a different sensor
• integration scheme with possibly different characteristics
• worth a comparison
EUCASS 2011 – 4-8 July, St. Petersburg, Russia
Questions?
Thank you!
EUCASS 2011 – 4-8 July, St. Petersburg, Russia