Botsch, M.

Kammenhuber, A.

Nadarajan, P.

Notomista, G.

Selvaggio, M.

English

Archivio della ricerca - Università degli studi di Napoli Federico II

Relative Motion Estimation Based on Sensor Eigenfusion Usinga Stereoscopic Vision System and Adaptive Statistical FilteringGennaro Notomista, Università degli Studi di Napoli, gennaro.notomista@iit.it, ItalyAlexander Kammenhuber, Audi AG, alexander1.kammenhuber@audi.de, GermanyParthasarathy Nadarajan, Technische Hochschule Ingolstadt, Parthasarathy.Nadarajan@thi.de, GermanyMichael Botsch, Technische Hochschule Ingolstadt, Michael.Botsch@thi.de, GermanyMario Selvaggio, Università degli Studi di Napoli, mario.selvaggio@iit.it, ItalyAbstractThis paper presents a method to estimate the relative motion between two vehicles with high accuracy. The estimatedquantities are intended to be used as a reference system for automotive sensing techniques and online embeddedmotion-estimation algorithms. We propose the sensor eigenfusion which makes use of a stereoscopic vision systemmounted on-board of a host vehicle. Highly reliable markers, i. e., QR-codes, mounted on a remote vehicle are used forrobust features detection and tracking. In the case of the mentioned camera system, the proposed method uses the 3Dreconstruction capabilities of stereoscopic vision and optical flow techniques usually used in monocular vision systems.The measurements are then shaped, smoothed and fused using a Kalman filter. To achieve the required high accuracythe characteristic statistical parameters of the filter are adapted online according to confidence measures which dependboth on the 3D reconstruction and on the optical flow analysis.1 IntroductionThe ability of estimating the motion of mobile robotswith respect to the environment in which they are movingis of a great importance. For autonomous mobile robots,which aim to navigate themselves in an unknown envi-ronment, this is a non-optional feature. Even more impor-tant is the case in which the robot requires the motion ofother objects that are present in the environment. In manycases, for instance, the objects need to be recognized andcategorized as potential obstacles. Another case is repre-sented by the situations in which a moving object is con-sidered as a reference for motion planning functions (e. g.target chasing and dynamic manipulation). A confirma-tion of what has been mentioned is the great success ofcompetitions such as the DARPA Grand and Urban Chal-lenges [1, 2, 3].A field in which both the above-mentioned cases are rel-evant is autonomous driving. In order to ensure safe ap-plications in future road traffic, one key challenge is theplanning of collision-free paths over time. Since the envi-ronment of an autonomous vehicle or robot is not static,the motion of other objects needs to be considered. Othervehicles, pedestrians or animals can, in fact, trigger au-tonomous intervention functions and, in this way, directlyinfluence the trajectory of the vehicle. In order to opti-mally plan this trajectory, quantitative information aboutthe motion of the remote objects is required. This pa-per, therefore, is aimed at presenting a method for esti-mating the relative position and velocity of the objectspresent in the environment with respect to a host vehicle.This way the situation can be evaluated and an eventualautonomous intervention can be planned enough time inadvance. Commonly used ways of interpreting a situationare summarized in [4].Different techniques are already used for environmentdetection and description as described in [5]. They al-ready make use of several sensor technologies involvingradar, laser, infrared, ultrasound and video [6]. Mod-ern cameras, in particular, are very powerful sensors be-cause of the huge quantity of information they can deliverand that can be interpreted in different ways [7]. Meth-ods to exploit this information have been developed instereoscopic vision systems [8] and in monocular sys-tems that make use of optical flow techniques [9]. Thislatter, inspired by biological creatures such as bees [10]and humans [11], is able to provide information aboutthe relative motion between the observer and the objectsas shown in [12] and [13]. Different techniques havebeen devised in order to combine the depth measurements(i. e., measures of position in the 3D world) with the op-tical flow vectors (namely measures of velocity in the 2-dimensional image space)[14, 15]. Moreover, also theflow/depth constraint can be exploited to detect movingobjects in a more robust way [16]. Finally, as reportedin [17], optical flow techniques are applied to camera im-ages stabilized using a stereo architecture and knowledgeon robot kinematics.In this paper a stereoscopic system has been designed forthe real-time computation of the relative dynamics be-tween the host and the remote systems. The combinationof the depth information and the sparse optical flow isperformed by means of a statistical filter, whose param-eters take into account the relationship between the twomeasurements techniques. Additionally, the optical flowanalysis is performed on the images of both cameras andits output is used as part of the measurement vector in thefiltering process. Finally, the statistical parameters of the604ISR 2016 (June 21 – 22, 2016, Munich, Germany)ISBN 978-3-8007-4231-8 © VDE VERLAG GMBH  Berlin  Offenbach605ISR 2016 (June 21 – 22, 2016, Munich, Germany)ISBN 978-3-8007-4231-8 © VDE VERLAG GMBH  Berlin  Offenbachthe following equation, relating it to the velocity in theimage plane, can be derived:vIi = JIiv. (4)The matrix JIi is the image Jacobian defined as [12]JIi =[− 1zCi0 XizCiXiYi −(1 +X2i ) Yi0 − 1zCiYizCi(1 + Y 2i ) −XiYi −Xi],(5)where zCi, the component of the detected feature alongthe zC axis, is computed in the camera reference systemof Fig. 1.Combining all the vectors vIi, relative to the different de-tected features, in the vector vIc, next equation holdsvIc =⎡⎢⎢⎢⎣vI1vI2...vIn⎤⎥⎥⎥⎦ =⎡⎢⎢⎢⎣JI1JI2...JIn⎤⎥⎥⎥⎦v = JIcv. (6)A least square technique can be employed to solve Eq. (6)for the vector v. This solution is given by Eq. (7) whereJ†Ic is the image Jacobian pseudo-inversev = J †IcvIc. (7)The vectors v are evaluated for the right and the left im-age and expressed in the same reference system xCyCzCof Fig. 1. The weighted average v̄ of these two vectorsCvL andCvR is defined byv̄ =1εLCvL +1εRCvR1εL+ 1εR. (8)The expression of the errors εL and εR is computed usingthe following definition where also the values of vIc, JIcand v are relative to the left and the right image, respec-tively:ε = ‖vIc −JIcv‖ . (9)3 Statistical Filtering of Measure-mentsThe measurements provided by the 3D reconstruction andby the optical flow are merged, filtered and fused by theaid of a Kalman filter (KF). This techniques has been ap-plied in [26, 27, 28, 29]. In order to obtain a high accu-racy of the estimates, the KF is designed in such a waythat it takes account for the nonstationarity of the mea-surement noise. The noise covariance matrix is chosenadaptively based on confidence measures connected tothe 3D reconstruction and the optical flow analysis, asexplained in Subsection 3.2.3.1 System ModelConsidering only the planar motion, in Fig. 3 the hostand the remote vehicle are depicted in the world refer-ence system OXY .xr[n]ψr[n]XYOXhostYhostyr[n]Host vehicleRemote vehicleFigure 3: Estimated quantitiesThe host reference system, whose origin coincides withthe mid point of the rear axle of the host vehicle, is alsorepresented. In this reference system, the quantities xr,yr and ψr, together with their derivatives, are to be esti-mated. The well-known KF-equations [30] are used forthis task. The state and observation equations that de-scribe the dynamic system and which are required to ap-ply the KF, are described in the following.The state vector x[n] and measurement vector y[n] for theKF are introduced asx[n] =[xr[n], yr[n], ψr[n], ẋr[n], ẏr[n], ψ̇r[n]]T, (10)y[n] =[xmr [n], ymr [n], ψmr [n], ẋmr [n], ẏmr [n], ψ̇mr [n]]T,(11)where the superscript “m” indicates that the quantities aremeasured, i. e., they contain measurement noise. Sincethe relative dynamics of two objects is to be estimated,a decoupling of xr[n], yr[n], and ψr[n] is assumed. Thisway the state equations for xr and ẋr arexr[n+ 1] = xr[n] + T ẋr[n] +T 22ηx,s (12)ẋr[n+ 1] = ẋr[n] + Tηx,s, (13)where T is the time step, and the second derivative of xris modelled as process noise ηx,s. Then, the noise vari-ance σ2ηx,s , that is required in the process noise covariancematrix in the KF, is determined using the approximationσ2ηx,s ≈19(ηmaxx,s)2, (14)where ηmaxx,s is the maximum value of the second deriva-tive of xr. This approximation is based on the 3σ-rulefor Gaussian distributions. The state equations for yr andẏr as well as for ψr and ψ̇r are equivalent to Eqs. (12)and (13). Also the process noise variances for these quan-tities are approximated equivalently to Eq. (14).All quantities of the state vector can be measured with theproposed system and so the observation matrix is a simpleidentity matrix. However, the measurement noise vectorηm ∈ R6 in the observation equation cannot be assumedto be a stationary random process, for example becauseof the pixel-quantization in the cameras which leads toa larger noise power for objects that are far away. Thatis why the measurement noise covariance matrix Cηm ischosen adaptively in this work.606ISR 2016 (June 21 – 22, 2016, Munich, Germany)ISBN 978-3-8007-4231-8 © VDE VERLAG GMBH  Berlin  Offenbach607ISR 2016 (June 21 – 22, 2016, Munich, Germany)ISBN 978-3-8007-4231-8 © VDE VERLAG GMBH  Berlin  Offenbach608ISR 2016 (June 21 – 22, 2016, Munich, Germany)ISBN 978-3-8007-4231-8 © VDE VERLAG GMBH  Berlin  Offenbach[7] A. Chatterjee, A. Rakshit, and N. N. Singh, VisionBased Autonomous Robot Navigation. Algorithmsand Implementations, J. Kacprzyk, Ed. Springer-Verlag Berlin Heidelberg, 2013.[8] U. Franke and A. Joos, “Real-time stereo vision forurban traffic scene understanding,” in IEEE Intel-ligent Vehicles Symposium, Dearborn (MI), USA,October 2000, pp. 273 – 278.[9] H. Chao, Y. Gu, and M. Napolitano, “A survey ofoptical flow techniques for robotics navigation ap-plications,” Journal of Intelligent & Robotic Sys-tems, vol. 73, no. 1-4, pp. 361–372, January 2014.[10] N. Franceschini, “Visual guidance based on opticflow: a biorobotic approach,” Journal of Physiol-ogy, vol. 98, pp. 281 – 292, 2004.[11] P. A. Warren and S. K. Rushton, “Optic flow pro-cessing for the assessment of object movementduring ego movement,” Current Biology, vol. 19,no. 18, pp. 1555 – 1560, 2009.[12] P. Corke, Robotics, Vision and Control. Funda-mental Algorithms in MATLAB, B. Siciliano andO. Khatib, Eds. Berlin Heidelberg: Springer-Verlag, 2011.[13] R. Szeliski, Computer Vision. Algorithms and Ap-plications. Springer London, 2011.[14] H. Badino, Complex Motion. Springer Berlin Hei-delberg, 2007, ch. A Robust Approach for Ego-Motion Estimation Using a Mobile Stereo Platform,pp. 198 – 208.[15] U. Franke, C. Rabe, H. Badino, and S. Gehrig, Pat-tern Recognition. 27th DAGM Symposium, Vienna,Austria, August 31 - September 2, 2005. Proceed-ings. Springer Berlin Heidelberg, 2005, ch. 6D-Vision: Fusion of Stereo and Motion for Robust En-vironment Perception, pp. 216 – 223.[16] S. Heinrich, Pattern Recognition - 24th DAGM Sym-posium Zurich, Switzerland, September 16 - 18,2002 Proceedings. Springer Berlin Heidelberg,2002, ch. Real Time Fusion of Motion and StereoUsing Flow/Depth Constraint for Fast Obstacle De-tection, pp. 75 – 82.[17] J. Schmudderich, V. Willert, J. Eggert, S. Rebhan,C. Goerick, G. Sagerer, and E. Korner, “Estimatingobject proper motion using optical flow, kinematics,and depth information,” IEEE Transactions on Sys-tems, Man, and Cybernetics, Part B: Cybernetics,vol. 38, no. 4, pp. 1139 – 1151, August 2008.[18] A. Mitiche and J. K. Aggarwal, Computer VisionAnalysis of Image Motion by Variational Meth-ods. Switzerland: Springer International Publish-ing, 2014, ch. Optical Flow Estimation, pp. 41 – 93.[19] A. Zisserman and R. Hartley, Multiple View Geom-etry in Computer Vision, 2nd ed. Cambridge Uni-versity Press, 2004.[20] B. K. P. Horn and B. G. Schunck, “Determining op-tical flow,” Artificial Intelligence, vol. 17, no. 1-3,pp. 185–203, August 1981.[21] D. Patel and S. Upadhyay, “Optical flow measure-ment using Lucas-Kanade method,” InternationalJournal of Computer Applications, vol. 61, no. 10,pp. 6 – 10, January 2013.[22] D. Willersinn and W. Enkelmann, “Robust obsta-cle detection and tracking by motion analysis,” inIEEE Conference on Intelligent Transportation Sys-tem (ITSC), Boston, MA, 1997, pp. 717 – 722.[23] W. Kruger, W. Enkelmann, and S. Rossle, “Real-time estimation and tracking of optical flow vectorsfor obstacle detection,” in Proceedings of the Intel-ligent Vehicles Symposium. Detroit, MI: IEEE,September 1995, pp. 304 – 309.[24] J. T. Philip, B. Samuvel, K. Pradeesh, and N. K.Nimmi, “A comparative study of block matchingand optical flow motion estimation algorithms,” inInternational Conference on Magnetics, Machines& Drives (AICERA-2014 iCMMD), 2014.[25] V. Borsu and P. Payeur, “Pose and motion estima-tion of a moving rigid body with few features,” inIEEE International Workshop on Robotic and Sen-sors Environments (ROSE), November 2009, pp.116 – 121.[26] L. Tong, System Simulation and Scientific Comput-ing. Berlin Heidelberg: Springer Berlin Heidel-berg, 2012, ch. An Approach for Vehicle State Esti-mation Using Extended Kalman Filter, pp. 56 – 63.[27] T. Dang, C. Hoffmann, and C. Stiller, “Fusing op-tical flow and stereo disparity for object tracking,”in IEEE 5th International Conference on IntelligentTransportation Systems, Singapore, 2002, pp. 112 –117.[28] S. Gehrig, C. Rabe, and L. Krueger, “6D visiongoes fisheye for intersection assistance,” in Cana-dian Conference on Computer and Robot Vision(CRV). Windsor, Ont.: IEEE, May 2008, pp. 34– 41.[29] J. Eggert, S. Rebhan, J. Schmudderich, andV. Willert, “Estimating objects proper motion us-ing optical flow, kinematics and depth informa-tion,” U.S. Patent US 2011/0 243 390 A1, October6, 2011.[30] D. Simon, Optimal State Estimation. WILEY,2006.[31] S. Garrido-Jurado, R. Muñoz-Salinas, F. J. Madrid-Cuevas, and M. J. Marín-Jiménez, “Automaticgeneration and detection of highly reliable fidu-cial markers under occlusion,” Pattern Recognition,vol. 47, no. 6, pp. 2280–2292, June 2014.609ISR 2016 (June 21 – 22, 2016, Munich, Germany)ISBN 978-3-8007-4231-8 © VDE VERLAG GMBH  Berlin  Offenbach

Relative motion estimation based on sensor eigenfusion using a stereoscopic vision system and adaptive statistical filtering

https://www.iris.unina.it/bitstream/11588/944948/1/isr2016.pdf

Relative motion estimation based on sensor eigenfusion using a stereoscopic vision system and adaptive statistical filtering

Abstract

Similar works

Full text

Available Versions

Archivio della ricerca - Università degli studi di Napoli Federico II