Obstacle Detection and Tracking Based on Multi-sensor Fusion

. In the obstacle detection system ， a great challenge is the perception of the surrounding environment due to the inherent limitation of the sensor. In this paper, a novel fusion methodology is proposed, which can effectively improve the accuracy of obstacle detection compared with the vision-based sys-tem and laser sensor system. This fusion methodology builds a sport model based on the type of obstacle and adopts a decentralized Kalman filter with a two-layer structure to fuse the information of LiDAR and vision sensor. We also put forward a new obstacles-tracking strategy to match the new detection with the previous one. We conducted a series of simulation experiments to calculate the performance of our algorithm and compared it with other algorithms. The results show that our algorithm has no obvious advantage when all the sensors are faultless. However, if some sensors fail, our algorithm can evidently outperform others, which can prove the effectiveness of our algorithm with higher accuracy and robustness.


Introduction
The sensor is a component or device that can sense surrounding environment information and can convert these messages into corresponding useful output signals according to certain rules [1].The development of sensor technology could drive the development of unmanned system technology to some extent, because the unmanned system's perception of the surrounding environment is based on sensor information [2].However, because of the inherent limitations of sensors (such as the low precision of the ultrasonic sensor [3], the insensitivity of the laser sensor to the transparent object and the failure of the visual sensor in the dark environment, etc.), single sensor system cannot achieve high accuracy in obstacle detection.Although improving the performance of various sensors may help solve this problem [4], a new alternative has aroused increasing attention: multi-dimensional heterogeneous data fusion technology.Compared with single sensor information, multi-sensor data fusion appears more competitive in the following aspects: fault tolerance, complementarity, real-time and economy [5], so it has been gradually applied.
In recent years, more and more people have used this technology for obstacle detection in robotics and automatic driving.Among them, Michael S. Darms et al. [6] used and fused LIDAR and RADAR sensor information from vehicles to track objects around the vehicle.Fernando García et al. [7] proposed a high-level fusion scheme that makes it possible to improve the classical ADAS (advanced driving assistance) system by integrating data provided by laser sensors and computer vision.
Our fusion system consists of LiDAR and computer vision sensors can be used to detect objects that may pose safety risks to drivers and pedestrians and this fused information provides a full understanding of the environment.
The rest of the article is organized as follows: Section II shows state of the art.Obstacle classification is introduced in Section III.In section IV, the process of tracking and data association fusion procedures based on decentralized Kalman filter with a two-layer fusion structure are described.Section V shows the obtained results.Finally, conclusions and future works are presented in Section VI.

Tracking procedures based on a decentralized Kalman filter
There are two main steps are related to tracking: estimation methods and data association techniques.The former is the estimation of the motion position at the next moment based on the previous motion state of the obstacle, and the latter deal with the matching of the new detection with the previous detection.

Estimation Method
In order to predict the position of the obstacle at the next moment more accurately, we use a decentralized Kalman filter with a two-layer fusion structure (DKF) to fuse the data of the vision sensor and LiDAR to fight the inherent limitation of sensors.We used the sequential sensor approach [8] that independently treats observations from various sensors and feeds them sequentially to the DKF estimation process.
The moving objects we are interested in (vehicles and pedestrians) have their own movement rules and constraints.For example, a pedestrian can move in any direction while the movement of the vehicle is limited by nonintegral constraints [9].Therefore, we establishes different motion models for different types of obstacles.For the vehicle, a sudden lateral movement should not be considered because of the road restrictions.We approximate it has a constant velocity motion in the two detection intervals and establish a constant velocity model for it.
The vehicles are difficult move at a constant speed, so our model should contain a certain amount of processing noise in the process of vehicle state prediction.The processing noise Q matrix is represented by the equations ( 1): For pedestrians, we use the Constant Turn Rate and Velocity (CTRV) model because of its unlimited behavior.In this model, the state transfer function of CTRV is: So, the formula for calculating the noise covariance matrix Q in the CTRV model is: After establishing motion model for obstacles, we used a distributed Kalman filter with a two-layer structure to fuse the data of multiple sensors.In Fig. 1, each sensor subsystem estimates the states and makes fault detection independently.If any sensor subsystem is faulty by detection, it is isolated and restored.Instead, it is sent to the first fusion layer with a netted parallel structure in which the estimation errors of every pair of sensors are fused to determine the cross-covariance between them, while the estimations and covariances are sent to the second fusion layer.The second fusion layer is the final fusion center in which the estimations and the cross-covariance matrices among the faultless local subsystems are fused to determine the optimal matrix weights and obtain the optimal fusion filter.On the other hand, after the faulty sensors are recovered, they can rejoin the parallel fusion structure.Due to the decentralized structure, the computational burden of fusion center is reduced, and the fault tolerance and reliability is assured.

Data Association Technology
The first step in the association process is to give up the impossible associations, so we have to create a threshold procedure for all trajectories.We use this threshold procedure to create a region containing all the most likely combinations.Associations that do not belong to the threshold area will be discarded.In this step, the probability of all possible assumptions and the connection probability is calculated.The joint incidence probability represents the probability of the associated event m in time k tracking the joint associated event of J.
Where D P is the detection probability, and FA P is the false alarm probabil- ity, M is the number of tracking targets, and n is the track assigned to clutter (no correlation).
Next, an assignment matrix should be created, in which each row denotes an observation value and each column indicates a trajectory.As a result, the probability of all combinations is calculated, and the allocation is carried out according to this matrix.This means that only one track is assigned to a given observation.All the tracks that are not assigned will increase their no-detection counters, and they will be eliminated if the counter reaches a given value.

Track Creation and Deletion Logic
Track creation and deletion policy is based on the definition of two different kind of tracks: united and non-united.Former are those tracks that were confirmed by both sensors at the same time or in subsequent scans.Later refers to tracks detected by a single sensor, it is not reliable enough since the other sensor have not confirmed it.Algorithm 1 defines the policy for track creation as well as when the track is updated.If both sensor = detected, then new united track.end

Results
We tested in a simulated environment to examine the feasibility of single-sensor system and the fusion system independently.We first tested the accuracy of the vision sensor and the LiDAR sensor, and analyzed the accuracy of the sensor by using the missed rate and the false detection rate as a measure.Table 1 shows that the performance of vision and LiDAR scanners system when tested.For LiDAR, the probability of missed obstacles is very low, which means that most obstacles can be detected.However, one problem with laser sensors is that they have higher false positives.The result of visual sensor is the opposite.Visual algorithms can be used to increase the confidence of these laser scanners because they with low false alarm rates.However, a low positive rate means that this algorithm is not robust enough, and fusion is a necessary condition for increasing robustness.

Tracking Algorithm Performance
We use three sensors in the system, in the simulation, setting T (sampling period) = 0.01.We calculated the position and velocity errors of local and decentralized filters in 0-100 seconds and compared them with a centralized filter.It can be seen from Fig. 2 that when all the sensors are not faulty, the accuracy of the optimal fusion dispersion filter is higher than that of each local filter and close the accuracy of the concentrated filter.
To test the fusion filter fault-tolerant and robust, we assume that the first sensor is faulty.The fusion filter is indicated by red solid lines, the centralized filter is indicated by blue lines, and the real value is indicated by black line.From the simulation graph Fig. 3, it can be seen that when the first sensor fails at 50 < t < 100, the centralized filter diverges, but the decentralized filter can still track the target.The results show that when any sensor fails, the information fusion dispersion filter with double-layer structure has better fault tolerance and robustness characteristics.

Fusion System Performance
After the fusion of vision and LiDAR sensor information, the accuracy of detection has been greatly improved.The fault detection rate and the miss detection reached 2.56 percent and 0.77 percent, respectively.Base on the data mentioned above, we can say the requirement for improving the detection system performance can be achieved due to the fusion procedure.

Conclusion
The main goal of the presented work was to provide sensor fusion methodology for intelligent vehicles, able to overcome the limitations of each sensor, providing a robust and reliable safety application for road environment.The results provided show that by fusing the information of the camera and LiDAR, it was possible to accomplish the complex task of safe vehicle detection in inter-urban scenarios.Furthermore, the presented systems give the possibility to increase the set of sensors thanks to its extensibility.

Fig. 1 .
Fig. 1.The decentralized Kalman filter with a two-layer fusion structure.

Algorithm 1
Track Creation and Updated Logic Case1: current state = non-united If the other sensor = detected, then track united.If the same sensor = detected, then track nonunited & track updated.If both sensor = detected, then track united.If consecutive_no_detection > 4, then track eliminated.Case 2: current state = united If single sensor = detected, then track updated.If both sensor = detected, then track updated.If consecutive_no_detection > 5, then track eliminated.Case3: current state = no match If single sensor = detected, then new non-united track.

Fig. 2 .
Fig. 2. Comparison of the precision of local filters, decentralized filter and centralized filter when all sensors are faultless: (a) filtering error of position; (b) filtering error of velocity.Local filter 1, red dotted line; local filter 2, black dotted line; local filter 3, dark blue line; centralized filter, red solid line; decentralized filer, black solid line.

Fig. 3 .
Fig. 3.The tracking performance comparison of the decentralized filter and centralized filter when the first sensor is faulty: (a) position; (b) velocity.

Table 1 .
Accuracy of vision and LiDAR