Surrounding Objects Detection and Tracking for Autonomous Driving Using LiDAR and Radar Fusion

Radar and LiDAR are two environmental sensors commonly used in autonomous vehicles, Lidars are accurate in determining objects’ positions but significantly less accurate as Radars on measuring their velocities. However, Radars relative to Lidars are more accurate on measuring objects velocities but less accurate on determining their positions as they have a lower spatial resolution. In order to compensate for the low detection accuracy, incomplete target attributes and poor environmental adaptability of single sensors such as Radar and LiDAR, in this paper, an effective method for high-precision detection and tracking of surrounding targets of autonomous vehicles. By employing the Unscented Kalman Filter, Radar and LiDAR information is effectively fused to achieve high-precision detection of the position and speed information of targets around the autonomous vehicle. Finally, the real vehicle test under various driving environment scenarios is carried out. The experimental results show that the proposed sensor fusion method can effectively detect and track the vehicle peripheral targets with high accuracy. Compared with a single sensor, it has obvious advantages and can improve the intelligence level of autonomous cars.


Introduction
Autonomous vehicle is a kind of intelligent car, which mainly relies on the computer system and sensor system inside the car to realize autonomous. Autonomous cars are integrated with automatic control, architecture, artificial intelligence, visual computing and many other technologies [1]. It is a highly developed product of computer science, pattern recognition and intelligent control technology, as well as an important symbol to measure a country's scientific research strength and industrial level [2]. It has a broad application prospect in the field of national defense and national economy. In recent years, auto intelligent technology, such as lane departure warning, ACC adaptive cruise, higher driving Assistance Systems such as automatic parking (Advanced Driver Assistance Systems, ADAS) [3,4] has been applied in car design, and to a certain extent, realize the self-driving car, bring many convenience to people's life, and to reduce accidents, to provide a safe and convenient driving experience plays an indispensable role [5,6]. With the continuous development and progress of science and technology, intelligent driving technology makes selfdriving vehicles no longer out of reach on the road [7][8][9].
An autonomous vehicle is inseparable from environmental awareness, decision planning and motion control. As one of the keys of autonomous technology, environmental perception technology adds eyes to autonomous cars through a variety of on-board sensors to accurately perceive the surrounding environment to ensure the safety and reliability of driving [10]. At present, the most commonly used on-board sensors are LiDAR, radar and

Open Access
Chinese Journal of Mechanical Engineering vision camera, etc., but all kinds of sensors have their advantages and disadvantages. LiDAR has good directivity, high measuring precision and is not affected by road clutter. According to the structure and type, LiDAR can be divided into two types: single-line (two-dimensional) and multi-line (threedimensional). Multi-line LiDAR has a certain pitch Angle and can realize surface scanning, but its price is relatively expensive. LiDAR is difficult to detect in close range and can be affected by the surrounding environment and weather. In addition, LiDAR is easy to be crosstalk, LiDAR can not judge whether the pulse light emitted by itself causes the shape of the object can not be judged. Radar speed ranging is the use of Doppler frequency shift, outward radiation wavelength for millimeter electromagnetic wave to complete the task of detecting the target. The electromagnetic wave is received by the receiver after being reflected by the target, and the information in the echo is analyzed to obtain the distance and relative velocity of the target. Radar has the advantages of strong penetrating ability of bad weather and good temperature stability.
However, the accuracy of radar in the detection range is directly restricted by the frequency band loss, and it is also unable to perceive the surrounding target categories, and accurate modeling of all surrounding obstacles cannot be carried out. Both LiDAR and RADAR can sense nearby targets. LiDAR are accurate in determining objects' positions but significantly less accurate on measuring their velocities. However, Radars are more accurate on measuring objects velocities but less accurate on determining their positions as they have a lower spatial resolution. Different types of targets perceived by different sensors will collide.
Our method are two comprehensive LiDAR and radar sensor data, through the way of data fusion to perceive the autonomous vehicle targets around there are both accurate location information and accurate speed information, improve the environmental awareness accuracy, and control to provide effective data for the decision of autonomous cars, and thus reduce the autonomous car accidents.
In recent years, the fusion of LiDAR and Radar has become a hot topic for researchers at home and abroad. In 2017, Kwon et al. [11] created a Detection scheme for a partially occluded pedestrian based on occluded depth in LiDAR-radar sensor fusion. It is verified that LiDAR and Radar fusion data can effectively solve the problem of partially occluded targets. In 2019, Lee et al. [12] worked out the A Geometric Model based 2D LiDAR/Radar sensor fusion for tracking surrounding vehicles. The proposed fusion system improved the estimation performance by reflecting the characteristics of each sensor is confirmed. In 2020, Kim et al. designed Extended Kalman Filter (EKF) for vehicle position tracking using reliability function of Radar and LiDAR, the study confirmed that the accuracy of distance measurements was improved as a result of the LiDAR and radar sensor fusion, and the method that reflected distance errors was more accurate in the extended Kalman filter's composition. In the same year, Farag, Wael put forward Road-objects tracking for autonomous driving using LiDAR and radar fusion. In this paper, a real-time road-object detection and tracking method for autonomous cars is proposed, implemented and described in detail [13].
Information fusion is the study of efficient methods for automatically or semi-automatically transforming information from different sources and different points in time into a representation that provides effective support for human or automated decision making. In this paper, radar and LiDAR, which are relatively mature in engineering applications, are used as environmental sensing sensors to solve the detection and tracking problems of surrounding targets of autonomous vehicles through sensor information fusion, so as to improve the stability, reliability and environmental adaptability of the environmental sensing system.
The rest of the work is arranged as follows. In Section 2, we provide an overview of related work. In Section 3, we derive our algorithms. First, we discuss the fusion of Radar and LiDAR. Subsequently, we present our modifications of the UKF (Unscented Kalman Filter, UKF) architecture to leverage the fused data. In Section 4, we discuss our results and finally give a summary and an outlook on future work.

Related Work
In terms of the specific model selection of the sensor, we used Delphi ESR millimeter wave radar produced by Delphi Corporation of the United States and Shenzhen Robosense RS-LiDAR-16 lines radar. Radar is a sensor that USES electromagnetic wave reflection to measure the distance, azimuth and velocity of the target. It is accurate and reliable in the measurement of distance and velocity information, but has poor identification of azimuth. Delphi 77G ESR, the radar frequency band is 77 GHz, calculates the distance from the target object by calculating the time needed to transmit electromagnetic wave to the environment and receive reflected wave, and calculates the frequency shift of the reflected wave received to get the motion velocity of the target object to be measured. With this radar, 64 targets can be acquired at the same time. Each target contains parameters such as the longitudinal distance of the target, the transverse Angle of the target and the longitudinal speed of the target. Radar can directly output the target sequence, but in the detection process, radar sometimes encounters the phenomenon of false detection. There is no target in the detection range that radar pays attention to, but the radar detects the presence of the target. Delphi 77G ESR is shown in Figure 1.
The master-slave control is widely employed in the robot manipulation. In most cases, the joystick or the keyboard is the routine input device for the robot masterslave control system. The system presented in this paper is shown in Figure 1.
In addition, the system characteristics, scanning range, accuracy and other performance parameters of Delphi 77G ESR are shown in Table 1.
The LiDAR system works simply by emitting a laser beam to complete a 360° scan in a horizontal direction counterclockwise. When the laser spot hits the target, the reflected laser beam passes through the optical receiving system and is detected by the optical detector, and then mixed with the original laser beam is converted into an electrical signal, which passes through the filter and amplifier and is input to the digital signal processor. After processing, the output to the computer shows the target point cloud. As the distance of the obstacle increases, the distance between LiDAR's two adjacent scan lines increases. We use Shenzhen Robosense RS-LiDAR-16 radar. RS-LiDAR-16, launched by RoboSense, is the first of its kind in China, world leading 16-beam miniature LiDAR product. RS-LiDAR-16, as a solid-state hybrid LiDAR, integrates 16 laser/detector pairs mounted in a compact housing. The compact housing of RS-LiDAR-16 mounted with 16 laser/detector pairs rapidly spins and sends out high-frequency laser beams to continuously scan the surrounding environment. Advanced digital signal processing and ranging algorithms calculate point cloud data and reflectivity of objects to enable the machine to "see" the world and to provide reliable data for localization, navigation and obstacle avoidance.
RS-LiDAR-16 installed on our autonomous vehicle, as shown in Figure 2. The related technical parameters are given in Table 2.
In this paper, the specific implementation process of the peripheral target detection and tracking method based on radar and LiDAR information fusion is to firstly conduct grid-based clustering of LiDAR point cloud data, select the target according to the rectangular box of clustering results, and identify the target centroid position. Radar can directly output the target sequence. Then, UKF is used to fuse radar and LiDAR targets.

Methodology
In this section, we describe our approach for the fusion of Radar and LiDAR. This section gives an overview of current research pertaining to the fusion of LiDAR and Radar sensor data. In this paper, the radar and liar information fusion based sensing system is designed by considering the performance of radar and LiDAR. The different detection ranges of radar and LiDAR are shown in Figure 3. The blue sector represents the LiDAR's field of view, and the purple sector represents the radar. Targets detected by LiDAR are represented by orange rectangles and radar by blue circles. The detection Angle of the radar is small, ±10° on both sides, but the detection distance is relatively far, with the maximum detection distance up to 180 m. The LiDAR has a maximum measuring distance of 150 m with a measurement accuracy of ±2 cm and a number of points up to 300000 points per second. The horizontal Angle is 360° and the vertical Angle is − 15°~15°. We found that combining the advantages of LiDAR in position perception with the advantages of radar in target speed, the information fusion system based on LiDAR and Radar can obtain more accurate target position and speed information, and effectively improve the surrounding target perception accuracy of autonomous vehicles. Moreover, our results show that the method based on LiDAR and Radar information fusion can effectively avoid the malpractice of system failure caused by the failure of any sensor and improve the robustness of the system. The following sections explain each part in more detail.

Moving Object Model
The motion state of the peripheral target of the autonomous vehicle is uncertain, so we establish a consistent model for the peripheral target [14][15][16][17]. We actually simplify the actual moving form of the peripheral target. It is assumed that the model object moves along a straight line and can also move at a fixed turning rate and a constant speed, as shown in Figure 4. The blue model is our autonomous car, and the orange model is our peripheral target. At some point, the target object is moving around the host Vehicle. The horizontal and vertical displacements of the target object from the host Vehicle are P x , P y . The P x and P y here are relative to the coordinate system of the host vehicle, that is, the host vehicle is the origin of the coordinate system, the front is the x axis, and the left side of the vehicle is the y axis. The speed of the host vehicle is v 1 and the speed of the target object is v 2 , δ 2 is the heading angle of the target object, δ 2 is the angular velocity of the target object. Then the motion state of the target object can be described as a vector x, as shown in Eq. (1). And it follows that the change in motion state of the target object ẋ is expressed as the differential equation g(x) in Eq. (1). Where, Ṗ x , Ṗ y is the rate of change of position with time, v 2 is the rate of change of velocity, and δ 2 , δ 2 are the rate of change of angle and angular acceleration. In addition, the relationship between the rate of   1) and (2), the differential Eq. (3) of the motion state change of the Target object can be deduced: Suppose discrete time step k is related to duration time value t k , discrete time step k + 1 is associated with duration time value t k+1 . The time difference between t k+1 and t k is expressed as Δt, then the prediction model can be obtained by using ẋ and its integration with time x k+1 = ẋdt.
It is assumed that turn rate ( δ 2 ) and velocity ( v 2 ) remain unchanged. Because we have noise v k in our equation of state, we put v k as a state into our state variable space, and its predictive noise includes acceleration and angular acceleration as Eq. (5): If predictive noise is considered, the state of the target is described as: There is a special case that we have to consider, that is, when δ 2 = 0, ( P x ,P y ) in our state transition function formula will become infinite, and the vehicle we are tracking is actually traveling in a straight line, so our calculation formula of ( P x ,P y ) becomes: Now that we've generated the prediction point, we need to predict the next state of the object because the object is going to move in a certain way. Here, the calculation is based on the state transition matrix. We only need to insert each prediction point into the process model Eq. (6). ).

Sensor Fusion Using UKF
Kalman filter [18][19][20] is a linear filter which can fuse multivariate uncertain information to obtain an optimal state estimation. Kalman filter in the continuous change of a linear system performance is very good, because it exists in the process of the system some interference, therefore, even if accompanied by some interference on the system and the Kalman filter can be more accurate to calculate the actual state, and can be on the system motions make reasonable forecast of the future. The prerequisite of Kalman filter is that the system is a linear Gaussian system. Generally speaking, the Gaussian noise will remain Gaussian after the state transition. If the prerequisite of linearity is not satisfied, Kalman filtering is no longer applicable. Simple Kalman filtering must be applied to systems that conform to Gaussian distributions, but not all systems in reality conform to this. In addition, the transfer result of Gaussian distribution in nonlinear system will no longer be Gaussian distribution. At this point, you need to use an extended Kalman Filter or Unscented Kalman Filter instead. Extended Kalman Filter solves nonlinear problems with local linearity. The nonlinear prediction equation and observation equation are differentiated and linearized by means of tangent lines, that is, the first order Taylor expansion is performed at the mean value.
EKF and KF [21][22][23] have the same algorithm structure, and both describe the posterior probability density in the form of Gaussian, which is obtained by calculating Bayesian recurrence formula. The difference between EKF and KF is that both the state transition matrix and the observation matrix of EKF are Jacobian matrices of state information when calculating variance. In the prediction formula section [11,[24][25][26], the Jacobian matrix of F k to f is extended by Kalman filtering. In the updating formula section, the Jacobian matrix of H k to h is extended by Kalman filtering. EKF linearizes the model through Taylor decomposition to obtain the probability mean value and variance of the prediction model. UKF can calculate the mean value and variance of the prediction model by insensitivity transformation. UKF [27] can better solve nonlinear problems through insensitivity transformation (an approximation method to calculate the moments of nonlinear random variables). By sampling and weight of certain rules, the mean value and variance can be approximately obtained. Moreover, the UKF effect can reach the effect of second-order EKF due to the high approximate accuracy of insensitive transformation to statistical moments.
We have two LiDAR and radar sensors. Rs-LiDAR-16 LiDAR measures the position coordinate (P x , P y ) of the target object. Delphi ESR measures the distance between the Target object and the host Vehicle in the coordinate system of host vehicle L as follows: The Angle between the Target Object and the X-axis is δ 2 , and the relative distance change rate between the Target Object and the host Vehicle is L . The measurement model of LiDAR is still linear, and its measurement matrix is: The prediction is mapped to the LiDAR measurement space, that is: The prediction mapping of radar to the measurement space is nonlinear, and its expression is: There are bound to be errors between the predicted data and the actual sensor data, so we need to correlate the state space vector with the data available from the sensor by measuring the transfer matrix h(x). Since the measurement transfer matrix h(x) of radar is a nonlinear function, we also need to find some prediction points and convert the predicted value � z k+1 into the measurement space through the nonlinear function x k+1|k . Our method is to substitute the generated prediction point x k+1|k into z k+1 = h(x k+1 + w k+1 ) to find the value of the measured space. Then according to each predicted value and weight, the Predicted measurement mean and measurement mean Covariance can be obtained. As shown in Eqs. (11) and (12): where w i is the weight of the average value, and w k+1 is the noise in the measurement model. Finally, Kalman gain K k+1|k and cross-correlation function T k+1|k were calculated based on the predicted values x and z, then update the state and the covariance. As shown in Eqs. (13) and (14):

Results
In this paper, the results of Radar and LiDAR fusion are validated. The results section is divided into detection results and results analysis. The experimental platform is Intel dual-core processor, 4G memory, the operating system is Ubuntu16.04 LTS, the programming software is ROS (Robot Operating System). A test vehicle as shown in Figure 6 was used for both recording the internal data and evaluating the sensor fusion and tracking effects in practice. The sensor fusion runs at 20 Hz, synchronized with the Radar.
Our methods have been evaluated on our autonomous vehicle. In the test section where the distance is measured in advance, data is collected and regarded as truth value. We designed a test section containing 6 working conditions for horizontal tracking and longitudinal tracking of surrounding targets of autonomous vehicles: (1) close to, (2) faraway, (3) turn left, (4) turn right, (5) left curve, (6) right curve. At the same time, the amount of target tracking time is counted, so as to facilitate the    the target vertically and −10~10 m behind the target laterally, so as to track the target vehicle in front. Multiple tracking targets of each tracking test were classified according to their ID Numbers and then averaged. Figures 8 and 9 are the effect experiments of longitudinal and lateral tracking of cars by intelligent driving vehicles, respectively. In Figure 8, it can be intuitively seen that the longitudinal target tracking provided by LiDAR is very stable with only a small range of fluctuation. In contrast, the longitudinal distance observed by the radar has obvious fluctuation. As can be seen in Figure 9, the horizontal target tracking provided by LiDAR is relatively stable, while radar fluctuates sharply when the horizontal distance is too large due to the limitation of field angle. In line with our initial expectations, radar and LiDAR have their advantages and disadvantages in both horizontal and vertical tracking. Table 3 lists the comparison of the final detection and tracking results of popular algorithms. As shown, the accuracy and detection rate of our approach performed best in these types of models.
Our fusion approach is especially applicable to some small networks, and the test speed improvement of small networks is more obvious.
However, it can be seen from Figures 8,9 and Table 3 that our fusion approach detection and tracking effect is closer to the truth standard value, which shows the effectiveness of the fusion algorithm in this paper.

Conclusion and Future Work
(1) Based on the research of single sensor environment awareness technology, a peripheral target detection and tracking method based on UKF LiDAR and Radar information fusion is proposed. (2) Our fusion method combines the millimeter-wave radar and LiDAR sensor in the vehicle-mounted environment. (3) Introduces the UKF nonlinear data fusion method to match the observed values, thus realizing the target detection and tracking based on the millimeterwave radar and LiDAR, which can effectively reduce the problem of incomplete attributes of peripheral targets perceived by a single sensor. (4) The actual vehicle test and tracking test of peripheral target movement under six common conditions are carried out, and the effectiveness of the fusion algorithm is verified, which can effectively improve the intelligence level of the autonomous vehicle.