G. Casalino, E. Zereik , E. Simetti, A. Turetta, S. Torelli and A. Sperindè

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


• 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


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



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


• 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


• 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



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


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


State Space ModelState Space Model


• 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


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


• 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



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



• 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



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


• Exploit different sensors

• Augmented sensorAugmented sensor with better performances

• Different integration schemes:

Integration SchemeIntegration Scheme



• 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


• Without further data, things can be bettered with a parallelparallel integration scheme



• IMUIMU: measurements about the angular velocity vector and the linear acceleration vector

• WOWO integration

v


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


• 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


Questions?Questions?

Thank you!Thank you!


G. Casalino, E. Zereik , E. Simetti, A. Turetta, S. Torelli and A. Sperindè

Documents

different sensors

different characteristics

filter gaussianity

previous stage

linear parametrization

angular velocity vector

planar motion mange

holonomic constraints