Date post: | 31-Dec-2015 |
Category: |
Documents |
Upload: | rama-wheeler |
View: | 11 times |
Download: | 2 times |
G. Casalino, E. Zereik, E. Simetti, A. Turetta, S. Torelli and A. Sperindè
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
• Introduction• Visual Odometry• Additional Measurements• State Estimators• Sequence Estimators• Multi-Sensor Integration• Discussion and Conclusion
AgendaAgenda
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
• 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
IntroductionIntroduction
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
Visual OdometryVisual Odometry
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
Visual OdometryVisual Odometry
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
Visual OdometryVisual Odometry
• At each step , the rover positionposition and orientationorientation are computed with respect to the previous step
• Sequence of positions-orientations truly attained by the vehicle
i 1i
ii r1 i
i 1
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
iii
ii
ii
ii
ii
i Nrr 111111 ,0,;ˆ
iii
ii
ii
ii
ii
i N 111111 ,0,;ˆ
ii
ii r ̂,ˆ 11 measured rover position and
orientation
ii
ii 11 , independent white noise sequences
affecting position and orientation measurements
ii
ii 11 , position and orientation covariance
matrices
EstimationsEstimations
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
• At each step, VO provides the rover relativerelative position and orientation
• Absolute Absolute rover position and orientation:
where
Ti i1
ii
i TTTT ˆˆˆˆ 12
11
00
iii
iii
ii
ii RR
rRT ̂ˆ;
10
ˆˆˆ 11
111
EstimationsEstimations
• Linear open chain open chain of frames, with independentindependent positioning
• ErrorError progressively increasingincreasing with the number of stages
• No further constraints
• Improvements:
• Additional measurements• At each step provide measurements of the occurred motion between frames and
ii
ii r ̂,ˆ 22
i 2i
EstimationsEstimations
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
ii
ii
ii
ii rRrr 1
12
122
ii
ii
ii 1
122
• Assumption:
stereo camera can recognize in the current frame a sufficient number of features belonging to images and
1i 2i
Additional MeasurementsAdditional Measurements
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
ii
ii
ii
ii
ii
ii
11
23
12
12
1
0
1ˆ0
1
01
00
ii
ii
ii
ii
ii
ii
21
2
11
2
21
2
11
10ˆ
ˆ
ii
ii
ii
ii
ii
ii I
rI
r
r
Ir
r11
23
12
12
1
0ˆ
00
00
ii
ii
iii
i
ii
ii
ii
r
r
Rr
r
21
2
11
2
122
12
1
10
ˆ
ˆ
orientationorientation
positionposition
State Space ModelState Space Model
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
State Space ModelState Space Model
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
• System evolution estimated via standard Kalman filters• Extended Extended (EKF)• Iterated Iterated (IKF)
• GaussianityGaussianity hypothesis for the filter
• Gaussianity is not suitable due to system non-linearity• such an approximation leads to suboptimalitysuboptimality
• These are recursiverecursive filters• linearly increasing errors have still to be expected for increasing number of stages
• Incoming acquisitions to better all the past state estimations
State EstimatorsState Estimators
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
ikrr
ik
kki
kki
,2,1;
,2,1;1,1
1,1
iii rX ,1,1,1 ;
ikrrw
ikz
kk
kki
kk
kki
,2,1;ˆ,ˆ
,2,1;ˆ,ˆ
21,1
21,1
iii wzZ ,1,1,1 ;
ii
X
iii ZXprXi
,1,1,1,1,1 /maxarg;,1
• The problem becomes:
measurememeasurementnt sequencesequence
state state sequencesequence
Sequence EstimatorsSequence Estimators
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
• Renew, at each stage, the entire sequence , without relying on the previous one
• Error generally characterizing IKF and EKF should result strongly reducedstrongly reduced
BUTBUT
• Increasing dimensionalitydimensionality with increasing number of stages
• linearlinear and quite acceptable with a reasonably high maximum number of stages
• after this freezefreeze and restartrestart the procedure
iX ,1
1,1 iX
Sequence EstimatorsSequence Estimators
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
iii
r
iiii wrprzpii
,1,1,1,1,1,1,1 ,/maxarg/maxarg,1,1
• Solve the following cascade of sub-problemssub-problems:
• Maintain a manageablemanageable implementative form that otherwise cannot be guaranteed, considering the general problem
• Minimization of the Gaussian p.d.f. exponents
• BayesBayes formula
Sequence EstimatorsSequence Estimators
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
• At each stage a linear parametrizationlinear parametrization is obtained
• It is the constraint for the previous stage
• Back Substitution Back Substitution scheme
• Dynamic ProgrammingDynamic Programming strategy
Sequence EstimatorsSequence Estimators
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
ii r ,1,1 ;
• Wait until stage
• RenewRenew the interpolated sequence in correspondence of eacheach new stage
• Backward PhaseBackward Phase: computational effort increasing with the number of stages
• RestartRestart the procedure from the last stage considered as the new initial one
• SmallerSmaller drifting errors
• Accepted suboptimalitysuboptimality vs. joint estimation of both sequences
i
CommentsComments
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
• Exploit different sensors
• Augmented sensorAugmented sensor with better performances
• Different integration schemes:
Integration SchemeIntegration Scheme
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
• First sequentialsequential scheme
• SQE fed with smaller variance smaller variance measurements data from STE
• Feedback loop used to re-initialize the STE module
• Totally useless Totally useless scheme
Integration SchemeIntegration Scheme
• Without further data, things can be bettered with a parallelparallel integration scheme
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
Integration SchemeIntegration Scheme
• IMUIMU: measurements about the angular velocity vector and the linear acceleration vector
• WOWO integration
v
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
Multi-Sensor IntegrationMulti-Sensor Integration
• Previously developed VO module
• via softwarevia software
• CUDA implementation:
• SURF extraction and descriptors via CUDAvia CUDA• matching and tracking via software with SAD• pose estimation via software
• Sequence Estimator
• Sensor data integration
ms10ms60ms50
ms120
ms1000500
Discussion and ConclusionDiscussion and Conclusion
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia
• Open issues:
• still under considerationunder 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
Discussion and ConclusionDiscussion and Conclusion
EUCASS 2011 – 4-8 July, EUCASS 2011 – 4-8 July, St. Petersburg, RussiaSt. Petersburg, Russia