Motion estimation of unmanned marine vehicles Massimo Caccia

Transcription

1 Motion estimation of unmanned marine vehicles Massimo Caccia Consiglio Nazionale delle Ricerche Istituto di Studi sui Sistemi Intelligenti per l Automazione Via Amendola 122 D/O, 70126, Bari, Italy massimo.caccia@ge.issia.cnr.it

2 Unmanned Surface Vehicles Onboard sensors position Configuration 1 GPS Ashtech GG24C heading GPS, DGPS Charlie USV DGPS position & 1 Hz Compass compass KVH Azimuth Gyrotrack True North, pitch & 2 Hz speed GPS, DVL

3 USV motion equations USV longitudinal asimmetry effects sea current

4 USV motion estimation scheme

5 USV motion estimation: basic issues and tricks (1) model-based yaw motion estimation using extended Kalman filter model-based prediction of angular acceleration is used as a kind of virtual sensor the delay of the estimate is reduced improving the stability of closedloop heading system GPS signal could present local discontinuities practical approach modeling a piecewise constant measurement disturbance the Guidance & Control module needs to deal with a continuous estimate of the vehicle motion

6 USV motion estimation: basic issues and tricks (2 yaw motion estimation) angles are measured in radians or degrees in a 2p or 360 deg interval, typically (-p, p] rad or (-180, 180] deg REMEMBER this fact when executing operations with angles

7 USV motion estimation: basic issues and tricks (3 GPS discontinuity) piecewise constant GPS measurement disturbance S(t) : step function x, y : disturbance step amplitudes measurement equations

8 USV motion estimation: basic issues and tricks (4 GPS discontinuity)

9 USV motion estimation: results (yaw motion)

10 USV motion estimation: results (x-y motion)

11 ROV motion estimation Vertical motion estimation depth estimation altitude estimation Horizontal motion estimation dead-reckoning vision-based motion estimation, including SLAM (basics)

12 Optical vision ROV downward-looking camera

13 Camera sensor model f : focal length [m n] : image point in the image plane [ m, n ] : image motion field in the image plane [X Y Z] : coordinates of the generic point in the 3-D space (referred to the camera frame) Camera perspective model [] [ ] m=f X n Z Y

14 Motion field of a stationary scene point Speed of a generic 3-D point in the camera frame [ ] [ ] [ ][ ] 0 r q X X u 0 p Y Y = v r q p 0 Z w Z Motion field [] [] [ ] [ ] [ ] [] m = f u w m r n f q pn qm m Z v Z n f n m p n neglecting terms of higher order [] [] [ ] [ ] [ ] m = f u w m r n f q Z v Z n n m p

15 Motion from token displacements Motion field neglecting pitch and roll [] [] [ ] [ ] m = f u w m r n Z v Z n n m Normalising with respect to the scene depth Z, assumed constant u v w u = ; v = ; w= Z Z Z when N tokens are tracked [ f 0 f 0 0 f 0 f [] ] [ ] m1 n1 m 1 n1 m1 u n 1 v = w mn nn m N r n N n N m N and then... Least Squares

16 Open questions How to define and extract tokens from images of the seabed? How to track tokens? How to compute the image depth?

17 Image token definition Definition of an image token in an unstructured environment region with high local variance

18 Image token tracking Cross-correlation Measure of similarity between two waveforms as a function of a time-lag applied to one of them m= corr f, g [ n ] = m= f [ m ] g [ n m ] where f is the complex conjugate of f When the functions f and g match, the value of their correlation is maximized. The reason for this is that when lumps (positives areas) are aligned, they contribute to making the sum larger. Also, when the troughs (negative areas) align, they also make a positive contribution to the sum because the product of two negative numbers is positive. Normalised cross-correlation N: number of pixels in template t 1 x, y f x, y f t x, y t corr f x, y,t x, y = n 1 f t

19 Image depth estimation Stereo vision Acoustic echo-sounder time-of-flight sensor Laser spot tracking

20 Laser spot tracking instrument

21 Laser spot tracking instrument

22 Laser spot position estimation ll a lx Z ll Z ll X ll = X Laser ray equations (l=1..nl) Y ll =Y ll a Yl Z ll Z ll Laser ray parameters are estimated during the device calibration Set of measurements at different measured ranges and Least Squares Once detected the image laser spots, combining laser ray equations and camera perspective models, laser spot depths are estimated with Least Squares Z = L l L m ll n ll l a Yl Y ll a lx X f f 2 L m ll n a lx l a Yl f f 2,l =1..N L

23 Seabed model Z = tan cos X tan sin Y h : seabed slope : seabed maximum slope orientation Small tracked image: seabed is assumed locally flat Z=h

24 Vision-based motion estimation architecture

25 Experimental results: token detection & tracking

26 Experimental results: estimated surge & pitch rate (from inclinometers)

27 SLAM Simultaneous Localization And Mapping Problem formulation The ability for a robotic vehicle to move in an unknown environment building a map, using only relative observations of the environment, and simultaneously using this map to navigate Classical extended Kalman filter solution Dissanayake et al. (2001) A solution to the simultaneous localization and map building (SLAM) problem. IEEE Transactions on Robotics and Automation (2001)

28 SLAM state equations vehicle model [ [ ][ ] ] x k 1 = x k R k u k u k t y k 1 y k v k v k landmark model [ ][ ] X i k 1 X k = i Y i k 1 Y i k Classical SLAM kinematic model Vehicle state: position Control input: horizontal speed and heading The encapsulation of the uncertainty of the vehicle speed in the control input allows its projection on the vehicle motion direction avoiding a generic, overestimated, system noise on the vehicle Cartesian coordinates

29 SLAM observation model z i =hi x, y,, X i, Y i The estimation-theoretical formulation of SLAM relies on the assumption that the vector of control input and observations are independent [ ][ ] [ ] x k 1 = x k R k u k u k t y k 1 y k v k v k

30 SLAM estimation & data association Estimation Kalman filter according to the theoretic formulation of the SLAM problem (Dissanayake et al., 2001) Data association Associating an existing landmark or a new landmark or nothing to a measurement MAIN ISSUE in the case of error in data association the vehicle is lost

31 Vision-based SLAM architecture

32 Data association & estimation Estimation Kalman filter according to the theoretic formulation of the SLAM problem (Dissanayake et al., 2001) Data association Searching in the actual image any landmark template that could be observed by the camera Template search, based on correlation, is performed only if the landmark is expected to be in the camera field of view

33 Processing steps Laser spots extraction/tracking and image depth computing Inhibition of laser spot neighbourhoods for template extraction and tracking SLAM template tracking. Searching new landmark if no one is tracked and the vehicle is far from the existing ones Inhibition of SLAM template neighbourhood for speed template extraction and tracking Speed template tracking and speed computing SLAM prediction, observation and update SLAM state augmentation, if needed Speed template extraction

34 Observation model [ ] [ ][ ] mi = f RT X i x m Z ni Y i y n SLAM landmark position measurement [mi, ni] is provided by the SLAM Optical Template Detector & Tracker SLAM control input [ui, vi] is provided by the Speed Estimate Optical Template Detector & Tracker The estimation-theoretical formulation of SLAM relies on the assumption that the vector of control input and observations are independent No overlapping between landmark and speed estimate image templates

35 Experimental results Romeo ROV & SLAM

36 Experimental results predicted & SLAM estimated position

37 Experimental results estimated position, covariance & tracked templates

38 Experimental results tracked landmarks

39 Experimental results landmark 0 tracking

40 Ground truth ROV configuration does not include high precision DVL and Gyros or high frequency LBL Ground truth verification procedure Select a couple of images Tracking a set of 16 x 16 pixels templates in the two images Compute the corresponding displacement Compare the result with SLAM-computed displacement Results: more than 1 cm accuracy when a visual landmark is tracked

41 What can happen with acoustics... Exploration of the underwater part of the Campbell glacier ice tongue with the Romeo ROV (1998)

42 Romeo ROV configuration upward-looking sonar forward-looking sonar

43 Romeo ROV telemetry data

44 Acoustics is not random, it's physics (M. Cardew)

45 Acoustic multi-path approaching the ice tongue