arxiv: v1 [cs.ro] 30 Jul 2018

Transcription

1 Real Time Lidar and Radar High-Level Fusi for Obstacle Detecti and Tracking with evaluati a ground truth Hatem Hajri and Mohamed-Cherif Rahal arxiv:17.11v1 [cs.ro] 3 Jul 1 Abstract Both Lidars and Radars are sensors for obstacle detecti. While Lidars are very accurate obstacles positis and less accurate their velocities, Radars are more precise obstacles velocities and less precise their positis. Sensor fusi between Lidar and Radar aims at improving obstacle detecti using advantages of the two sensors. The present paper proposes a real-time Lidar/Radar data fusi algorithm for obstacle detecti and tracking based the global nearest neighbour standard filter (GNN). This algorithm is implemented and embedded in an automative vehicle as a compent generated by a real-time multisensor software. The benefits of data fusi comparing with the use of a single sensor are illustrated through several tracking scenarios ( a highway and a bend) and using real-time kinematic sensors mounted the ego and tracked vehicles as a ground truth. I. INTRODUCTION Data fusi [1] [3] has multiple benefits in the field of automous driving. In fact automous vehicles are often equipped with different sensors through which they communicate with the external world. A multisensor fusi takes advantages of each sensor and provides more robust and time-ctinuous informatis than sensors used separately. Several earlier works showed advantages of combining sensors such as Lidars, Radars and Cameras. For example, [] presents a Lidar-Radar fusi algorithm based Kalman filter and shows how fusi improves interpretati of road situatis and reduces false alarms. Subsequently [] uses Cramer-Rao lower bound to estimate performance of data fusi algorithms. The paper [] csiders Lidar-Radar fusi with applicatis to following cars highways. In order to test the performance of their fusi algorithm, authors of [] provide a study of mean square errors of relative distances and velocities in a highway tracking scenario using least squares polynomial approximati of sensors data as a ground truth. Another Lidar, Radar and Camera fusi approach based evidence theory apppears in [7] with applicatis to the classificati and tracking of moving objects. More recently, [] focuses fusi of multiple cameras and Lidars and presents tests real world highway data to verify the effectiveness of the proposed approach. The present paper is ccerned with data fusi between Lidar and Radar. In vue of the state of the art, we can distinguish two different general fusi methods which were applied for Lidar and Radar: Kalman filter and evidence theory. We believe that approaches which apply e of Institut VEDECOM, 77, rue des Chantiers 7 Versailles, France. firstname.name@vedecom.fr these methods agree the main steps. On the other hand, despite the previously mentined works, the litterature still clearly lacks a quantitative comparis between these sensors outputs such as relative coordinates, velocities, acceleratis etc and their fusi result in the presence of a ground truth at least e obstacle. Because of the lack of a ground truth, authors of these papers were led to work with simulated data or manually manage real data in order to create a ground truth and evaluate results. The present paper proposes a real-time high-level fusi algorithm between Lidar and Radar based the GNN filter which in turn is based Kalman filter. This algorithm is presented with several mathematical and implementati details which go alg with it. The performance of this algorithm is evaluated while focusing the main outputs of Lidar and Radar which are relative coordinates and velocities of obstacles. Evaluati is de using a ground truth methodology introduced in [9]. For this, two synchrised automous cars which are prototypes of the automous vehicle of VEDECOM equipped with real-time kinematic (RTK) sensors will be used in tracking scenarios. Figure 1 displays the sensor architecture of this prototype. It has five Lidars ibeo LUX with horiztal fields of view of 1 o mounted such that they provide a complete view around the car. These Lidars send measurements to the central computati unit (Fusi Box) which performs fusi of measured features, object detectis and tracks at the frequency of Hz. A lg-range Radar ARS 3 of frequency Hz is mounted at the frt of the car with a horiztal view of o, + o. The vehicle is moreover equipped with a RTK sensor with precisis. m positi and. m/s velocity and a CAN bus which delivers odometry informatis. Fig. 1: Sensor cfigurati: Five lidars, e radar and e RTK sensor The fusi algorithm uses informatis from Lidar/Radar and the CAN bus. RTK sensors will be used for performance

2 evaluati. Organisati of the paper. The ctent of this paper is as follows. Secti II develops the fusi method used in the paper. Secti III gives more details the implementati and integrati into vehicle of the proposed algorithm. Secti IV reviews the ground truth generati method introduced in [9] and presents the experiments (car followings highways/bends) carried out using the two vehicles to collect data. The ground truth is used to evaluate the mean square erros of Radars/Lidars and fusi measurements of relative positis and velocities of the obstacle vehicle. In additi several plots of temporal evolutis of these measurements are given showing interesting informatis about their smoothness and unavailability periods. Results show advantages of data fusi comparing with e sensor. II. KALMAN FILTER FOR DATA FUSION Csider a target (or state) which moves linearly in discrete time according to the dynamic x(k) = F (k)x(k 1)+v(k) and an observati z of it by a sensor S which takes the form z(k) = H(k)x(k)+w(k). Assume v(k), k = 1, and w(k), k = 1, are two independent centered Gaussian noises with covariances P S and N S respectively. Kalman filter is known to give an estimati of x(k) when ly z(1),, z(k) are observed. From a mathematical point of view, the problem amounts to calculating the cditial mean E[x(k) Z k ] where Z k = (z(1),, z(k)). Introduce the notatis ˆx(i j) = E[x(i) Z j ] which is the cditial mean of x(i) knowing Z j and its cditial covariance P (i j) = E[(x(i) ˆx(i j))(x(i) ˆx(i j)) T Z j ]. Kalman filter has an explicit soluti which is determined recursively. Assume x() is a Gaussian distributi with mean ˆx( ) and covariance P ( ). Knowing the estimati ˆx(k 1 k 1), ˆx(k k) is calculated following these two steps. (A) PREDICTION STEP. Compute ˆx(k k 1) and P (k k 1) by: ˆx(k k 1) = F (k)ˆx(k 1 k 1) P (k k 1) = F (k)p (k 1 k 1)F T (k) + P S This step requires the knowledge of z(1),, z(k 1). (B) UPDATE STEP. When z(k) becomes available the final soluti is obtained as follows ˆx(k k) = ˆx(k k 1) + W (k)(z(k) H(k)ˆx(k k 1)) P (k k) = P (k k 1) W (k)s(k)w T (k) with W (k) = P (k k 1)H T (k)s 1 (k) (Kalman gain) S(k) = N S + H(k)P (k k 1)H T (k) The matrix S(k) is the covariance of the innovati ν(k) = z(k) H(k)ˆx(k k 1) = z(k) ẑ(k k 1). The innovati measures the deviati between the estimates provided by the filter and the true observatis. Its practical interest lies in the fact that (under the Gaussian assumpti) the normalised innovati q(k) = ν T (k)s(k) 1 ν(k) is a χ distributi with dim(z(k)) degrees of freedom (see [3], []). As a csequence, with a high probability α, the observati z(k) associated with the object x(k) belgs to the area {z : d = (z ẑ(k k 1)) T S(k) 1 (z ẑ(k k 1)) γ} where α is such that P(χ < γ) = α. This area is known as the validati gate. When several measurements z are available for e specific object, the χ distributi makes it possible to identify those measurements which may correspd to the underlying object by applying a hypothesis test as follows: If there are observatis in the validati gate choose z which minimizes d as the ly valid observati. Otherwise no observati is associated. This soluti is known as the nearest neighbour filter. The soluti which csists in averaging all obsevatis which fall in the validati gate is known as the probabilistic data associati filter (see [3] for more background probabilistic filters). In practice, which is the case for Radar/Lidar, several tracked objects and new measurements can be available simultaneously. The task of associating new measurements with the underlying observed objects is known as the data associati problem. This problem is dealt with in Secti III by first solving a global optimizati problem and secd updating the list of objects using the nearest neighbour filter. The obtained filter is known as the GNN filter. In the present paper each obstacle (old state or new observati) is represented as a vector of its relative coordinates and velocities [x y v x v y ] T. It is assumed that obstacles move according to the cstant velocity moti model: if denotes the elapsed time between the k and k + 1 sensor data emissis, the matrices F and H describing the state and observati evolutis are given by 1 F (k) = 1 1, H(k) = I 1 The ctent of this paper can be adapted to other moti models such as the cstant accelerati moti model. We choose the cstant velocity model for two reass. First Lidars do not send informatis about acceleratis of obstacles. Secd the cstant velocity model requires less unkown parameters to estimate than the cstant accelerati model. III. REAL TIME IMPLEMENTATION This secti gives details of the implementati of the real-time fusi module between the two sensors Lidar ibeo LUX and Radar ARS 3. This module was first implemented in Matlab/Simulink and then embedded in the automative vehicle as a compent generated by the software RTMaps (Real Time, Multisensor applicatis) [11]. The RTMaps software is widely used for real-time applicatis in mobile robotics as it allows to synchrise, record and replay data from different sensors. The proposed fusi module is an iterative algorithm which is run as so as a sensor data arrives. Simulink delay functis offer a tool for this kind of

3 problems as they allow to store the last value of a module. The following Fig. shows the Simulink diagram used to generate the fusi module. Fig. : The Simulink diagram After a data by a sensor S k (Lidar or Radar) is received at time t k, this algorithm outputs a fused set of obstacles F O k. The main characteristics of each obstacle (other outputs such as identity, age and so will not be detailled) are 1) a vector [x, y, v x, v y ] T where x, y (resp. v x, v y ) are the relative (with respect to the ego-vehicle frame) coordinates (resp. velocities) of the object and ) an uncertainty matrix of the object. The input of the algorithm at time t k is the previous fused set of obstacles F O k 1, the linear and angular velocities of the ego-vehicle at time t k and the sensor data csisting in relative coordinates x, y and speeds v x, v y of the detected obstacles all at time t k. The angular and linear velocities are available the CAN bus. Since this sensor does not generate data at the same moments as Lidar or Radar, in practice an approximati value of these quantities at time t k are taken (for example the last known es before t k ). The fused list is initialized when the first sensor data arrives. For each object in this list, the vector [x, y, v x, v y ] T of relative coordinates and velocities given by the sensor is stored. The uncertainty of each object is initialized to the correspding sensor s uncertainty N S. Assume known the last fused list of objects F O k 1 at time t k 1 updated after recepti of data by the sensor S k 1 and assume a new sensor data arrives at time t k by the sensor S k. The update F O k of F O k 1 follows these steps. (a) Tracking. Call R k 1 and R k the ego-vehicle frames at times t k 1 and t k. Each object o k 1 = [x k 1, y k 1, vx k 1, vy k 1 ] T F O k 1 with uncertainty I k 1 is first tracked in the frame R k 1 under the cstant velocity hypothesis. Then it is mapped to the new frame R k by a rotati of its relative coordinates and relative velocities. Call ω and v the instantaneous angular and linear velocities of the ego-vehicle at time t k and define the following estimates of the cap angle and travelled distance θ = ω, d = v where = t k t k 1. Call o k 1,t = [x k 1,t, y k 1,t, vx k 1,t, vy k 1,t ] T the new object and I k 1,t its uncertainty in R k. More explicitly, the following identities hold: [x k 1,t, y k 1,t, vx k 1,t, vy k 1,t ] T ( ) x k 1 + vx k 1 d cos(θ) Rθ = y k 1 + vy k 1 d sin(θ) R θ vx k 1 vy k 1 and I k 1,t = ( ) ( ) T Rθ (F I k 1 F T Rθ + P R Sk ) θ R θ where R θ is the rotati matrix of angle θ and F is given in the previous secti. (b) Associati. In this setp, each new sensor measurement is associated with at most e tracked object that correspds best to it. For this, the classical Mahalanobis distance will be used as a similarity measure. This distance is defined for any tracked object o k 1,t with uncertainty I k 1,t in R k and new observati z i by d k,i = (o k 1,t z i ) T (I k 1,t ) 1 (o k 1,t z i ). The associati problem can be reformulated in the following optimizati form: find the set (c k,i ) which minimizes k,i c k,id k,i subject to the cstraints k c k,i = 1 for each i, i c k,i = 1 for each k and c k,i {, 1} for all k and i. A well known soluti to this problem is given by the Hungarian algorithm [1]. This algorithm was implemented in Matlab/Simulink and subsequently used in the fusi module. Notice if there are less tracked objects than new observatis, a tracked object is associated with exactly e observati and if not it may or may not be associated. (c) List update. The fused list of objects is updated as follows. First for a tracked object o k 1,t with uncertainty I k 1,t which is associated with an observati z i, the latter is accepted as an observati of o k 1,t if it falls in the validati gate arround o k 1,t that is if d k,i < γ with γ a quantile of order.9 of the χ distributi with degrees of freedom. In this case both o k 1,t and I k 1,t are updated as in Kalman correcti step based the observati z i. If the object is not associated with an observati, it is removed from the list. Another possibility is to ctinue tracking absent obstacles for a while. However we did not choose this opti since Lidars and Radars already track absent obstacles. Finally new observatis that are not associated with tracked objects are added to the list as for the first fused list. To summarize, the new list F O k is composed of tracked objects associated with new observatis and updated by Kalman filter and new observatis not associated with tracked objects. IV. EXPERIMENTS AND RESULTS The last secti is devoted to the generati of ground truth for the evaluati of the proposed data fusi algorithm (denoted Fusi in brief) and for compariss between Lidar, Radar and Fusi. The focus will be relative positis/velocities x, y, vx, vy of the target vehicle. Ground truth generati [9]. To generate the ground

4 truth, we used two synchrised automous vehicles. The ego vehicle is equipped with Lidar and Radar and both vehicles are equipped with RTK sensors. The idea behind this generati process is that given two moving points A and B represented by their positis and velocities in a global frame R and knowing the angular velocity of A, it is possible to deduce the positis and velocities of B in the moving frame of A (compositi of movements formula). In practice, these informatis are used: (1) global coordinates and velocities in the reference frame R of RTK sensors of the two vehicles during experiments all obtained from the RTK sensors. () heading of the ego vehicle in R during experiments obtained from the RTK sensor mounted this vehicle. These informatis in combinati with the compositi of movements formula provide ultra-precise estimatis of the relative positis/velocities of the target vehicle in the ego vehicle frame. In fact, at each time t, informatis (1) give the relative coordinates and speeds with the egovehicle identified to a point (that is without csiderati of its heading). A rotati of angle given by the heading at time t (obtained from ()) gives the desired estimatis. In order to perform comparis, e has to find the target car characteristics (x t, y t, vx t, vy t ) sent by Lidar/Radar/Fusi at a given time t. For this, the nearest obstacle to the ground truth positi at time t which is n static was csidered as the target car viewed by Lidar/Radar/Fusi. This method gives the desired obstacle most all the time. Since sensors have different frequencies, linear interpolati was used to get an estimate of any quantity (positi/velocity) which is not available at a given time. Evaluati of the fusi algorithm. In order to estimate the uncertainty matrices P S and N S for both Lidar and Radar involved in the algorithm, we used a ground truth collected during minutes. We set all off-diagal entries of these matrices to and tolerate more error than the obtained values. To evaluate the fusi algorithm, we generated new data in two challenging circuits: car followings a highway and a highly curved bend. In each case, the same carfollowing scenario is repeated seven times. For each case, figures displaying variatis of the relative coordinates and velocities x, y, vx, vy in e scenario are shown alg with the ground truth. We choose e scenario amg seven because of space limitatis. The two plots at the bottom (green and black refering to Radar and RTK) are the true es (without translati). For better visualizati, those at the midlle (red and black refering to Lidar and RTK) and the top (blue and black refering to Fusi and RTK) correspd to the true es + offset and true es + *offset. The offset is specified with each figure s title. Radar/Lidar/Fusi points are represented by small squares joined by lines with the same colors and RTK points are represented by ctinuous curves (linear interpolati of the values). In additi, two tables representing the mean square errors () of x, y, vx, vy for Radar/Lidar/Fusi are given for all the seven car following experiencies. These errors are calculated with respect to the RTK output csidered as a ground truth according to the formula q = 1 N N (qsensor t qrtk t ) t=1 In this formula, the notati qsensor with q {x, y, vx, vy} and sensor {Radar,Lidar,RTK, Fusi} refers to q of the target as seen by sensor, qsensor t is the value of qsensor at time t and N is the number of samples. Runtime performance. Multiple tests show that the fusi algorithm is able to treat obstacles in less than microsecdes. This period is negligible compared to periods of Lidar and Radar making the algorithm suitable for fusi in real time. A. Car-following a highway The first scenario is a tracking, shown in Figure 3, of the target vehicle a highway at high speed (between 9 and km/h). We repeated the same experience the same porti of the highway seven times and obtained a record of more than minutes. Fig. 3: Tracking a highway. The next figures display respectively the variatis of x, y (in metre) vx, vy (in metre/secd) as a functi of time (in secd) of the target vehicle by Radar/Lidar/Fusi and RTK in e experience amg the seven. 3 3 Fig. : variatis of x (in m) with Radar (green)/lidar (red)/fusi (blue) in comparis with RTK (black) as a functi of time (in s). Offset=.

5 1 B. Car-following a bend 1 The secd scenario is a tracking, shown in Figure, a successi of highly cruved bends. We repeated the same experience seven times getting a record of more than minutes Fig. : variatis of y with Radar/Lidar/Fusi. Offset=. 1 1 Fig. : Tracking a bend. The following Figure 9 shows the trajectory in the world frame of the ego vehicle obtained from the RTK sensor Fig. : variatis of vx with Radar/Lidar/Fusi. Offset= Fig. 9: Trajectory of the ego vehicle. The starting point is (, ). Variatis of the relative x, y (in metre) vx, vy (in metre/secd) as a functi of time (secd) in e experience amg the seven are displayed in the next figures Fig. 7: variatis of vy with Radar/Lidar/Fusi. Offset=. Table I shows the mean square erros () x, y, vx, vy by Radar,Lidar,Fusi correspding to the seven carfollowing experiences. Radar x y vx vy TABLE I: Table /Radar/Lidar/Fusi. of Lidar s of Fusi x, y, vx, vy by 3 Fig. : variatis of x (in m) with Radar (green)/lidar (red)/fusi (blue) in comparis with RTK (black) as a functi of time (in s). Offset=.

6 Fig. 11: variatis of y with Radar/Lidar/Fusi. Offset= Fig. 1: variatis of vx with Radar/Lidar/Fusi. Offset=. C. Comments (a) The plots of x and y are relatively smooth for Lidar/Radar/Fusi in both scenarios. In ctrast, the plots of vx and vy for Lidar present multiple brutal transitis and piecewise cstancies. These drawbacks are minimal for Radar and Fusi. (b) Multiple n detecti periods of the target are observed for Radar especially in the secd scenario. This can be explained by the fact that the target is not in the field of view of the sensor or also by a sensor malfuncti. (c) It is remarkable that Radar was more accurate x than Lidar in the first scenario. This fact is supported by the experiment presented in [] in which Radar was more accurate than Lidar the relative distance = x + y. (d) Experiments lead to the following cclusi. First, in terms of accuracy, Fusi provides a good compromise value between Radar and Lidar. Secd, Fusi is more robust against unavailability of Lidar and Radar. Unavailability of informati has dangerous impact in automous driving. This problem is very unlikely to occur for Fusi as the latter combines two sensors. D. Cclusi and future works This paper presented a real-time fusi algorithm between Lidar and Radar which was implemented and successfully integrated into the automous car. This algorithm is based the GNN filter and outputs multiple characteristics of the detected objects such as their relative positis/ velocities and uncertainties. Performance of Fusi in comparis with Radar and Lidar was evaluated through multiple tracking scenarios ( a highway and a bend) using two synchrised vehicles and relying data coming from ultra-precise RTK sensors as a ground truth. Benefits of Fusi were illustrated through two main central ideas: accuracy regarding the ground truth and robustness against sensors malfunctis. In future works, we plan to cduct experiencies in challenging cditis and use ground truth to compare our approach with the evidence theory approach [7] Fig. 13: variatis of vy with Radar/Lidar/Fusi. Offset=. Table II shows the mean square erros () x, y, vx, vy by Radar,Lidar,Fusi correspding to the seven experiences. Radar Lidar Fusi x..1. y...19 vx.1..9 vy TABLE II: Table of s of x, y, vx, vy by Radar/Lidar/Fusi. REFERENCES [1] D. L. Hall and S. A. H. McMullen, Mathematical Techniques in Multisensor Data Fusi. Artech House,. [] H. B. Mitchell, Multi-Sensor Data Fusi: An Introducti, 1st ed. Springer Publishing Company, Incorporated, 7. [3] Y. Bar-Shalom and T. E. Fortmann, Tracking and Data Associati, ser. Mathematics in Science and Engineering. Academic Press, 19, no [] C. Blanc, B. T. Le, Y. Le, and G. R. Moreira, Track to track fusi method applied to road obstacle detecti, IEEE Internatial Cference Informati Fusi,. [] C. Blanc, P. Checchin, S. Gidel, and L. Trassoudaine, Data fusi performance evaluati for range measurements combine with cartesian es for road obstacle tracking, 11th IEEE Internatial Cference Intelligent Transportati Systems, 7. [] D. Gohring, M. Wang, M. Schnurmacher, and T. Ganjineh, Radar/lidar sensor fusi for car-following highways, IEEE Int. Cf. Autom. Robot. Appl., pp. 7 1, 11. [7] R. O. Chavez-Garcia and O. Aycard, Multiple Sensor Fusi and Classificati for Moving Object Detecti and Tracking, IEEE Transactis Intelligent Transportati Systems, vol. 17, no., pp. 3,.

7 [] A. Rangesh and M. M. Trivedi, No blind spots: Full-surround multiobject tracking for automous vehicles using Cameras and Lidars, 1. [9] H. Hajri and al, Automatic generati of ground truth for the evaluati of obstacle detecti and tracking techniques, Available arxiv, 1. [] H. Durrant-Whyte, Introducti to estimati and the Kalman filter. Australian Centre for Fields Robotics. [11] F. Nashashibi, a framework for prototyping automotive multi-sensor applicatis, pp. 99 3,. [1] J. Munkres, Algorithms for the assignment and transportati problems, Journal of the Society of Industrial and Applied Mathematics, vol., no. 1, pp. 3 3, March 197.