2010 IEEE Intelligent Vehicles Symposium University of California, San Diego, CA, USA June 21-24, 2010
ThB1.16
Multiple Hypothesis Tracking for Automated Vehicle Perception George Thomaidis, Leonidas Spinoulas, Panagiotis Lytrivis, Malte Ahrholdt, Grant Grubb, Angelos Amditis Member, IEEE Figr
Abstract- The use of multiple hypothesis tracking has proven to provide significant performance benefits over the single hypothesis GNN or the PDA algorithm. Automotive sensors like radars, laser-scanners or vision systems are being integrated into vehicles for commercial or scientific purposes, in increasing numbers over the last years. As a result, there is profound literature on this area and several approaches have been proposed to the problem of multi-target, multi-sensor target tracking. The most advanced vehicle applications allow the use of highly or even fully automated driving. Of course, these applications require an accurate, robust and reliable perception output so that the vehicle can be driven autonomously. The HAVEit EU project investigates the application and validation of automated vehicles applications, technologies that are going to have great impact in transport safety and comfort. In this paper the MHT algorithm is applied to real sensor data, installed in Volvo Technology vehicle demonstrating Automated Queue Assistance. In conjunction with simulated scenarios, the benefits in tracking performance compared to conventional GNN tracking are presented.
I.
INTRODUCTION
T
he use of advance driver assistance systems requires a wide field of view from the perception system. As a result, a variety of sensors are used in order to cover different areas around the vehicle. These systems pose significant challenges in the design of multi-target, multisensor data fusion system. The tracking system should associate measurements that origin from the same target, and also be able to maintain the track ID when a target is moving from one sensor’s field of view to another’s. Finally, the local track estimates from independent sensor have to be effectively fused in order to exploit the benefits of a multisensor system. A critical part of a tracking system is the measurement to track association. In automotive sensors, this procedure is hindered by the fact that targets may be closely spaced and Manuscript received February 12, 2010 This work was supported by the European Commission under the EU 7th Framework programme ICT2007.6.1, IP HAVEit Grant agreement no.: 212154 G. Thomaidis, P. Lytrivis and A.Amditis are with Institute of Communications and Computer Systems (ICCS), I-SENSE group, Iroon Polytechniou St. 9, 15773 Athens, Greece, (phone: +30 210 7723865; +30 210 7722291; e-mails:
[email protected],
[email protected],
[email protected]). L.Spinoulas is with National Technical University of Athens (NTUA), Department of Electrical and Computer Engineering; e-mail:
[email protected] M. Ahrholdt and G. Grubb are with Volvo Technology Corporation, Intelligent Vehicle Technologies, Dept. 6320, M1.6, Götaverksgatan 10, SE-405 08 Göteborg, Sweden, (e-mails:
[email protected],
[email protected])
978-1-4244-7868-2/10/$26.00 ©2010 IEEE
rapidly change kinematic state due to maneuvers, like lane change or overtaking. The measurement to track assignment phase of the tracking procedure is the most common source of errors in the estimation of a target’s state. Erroneous association may result not only to track ID loss and track re-initialization, when a track is not updated, but also degrade the overall tracking performance in cases where a track is updated with a wrong measurement. In [2] it is indicated that even a single erroneous update often results in track loss, and two consecutive false updates definitely lead in losing the track. Global Nearest Neighbor algorithm updates each track with the measurement it has been associated. The JPDA method on the other hand is able to deal with uncertain association conditions by updating a track with the weighted sum of all the measurements inside the gate [4]. GNN method selects only one data association hypothesis, whereas JPDA combines all the possible hypotheses. On the contrary, MHT [3] forms all possible data association hypotheses and uses the next observations to resolve the assignment ambiguities that occurred in the past. This paper presents an implementation of a multiple hypothesis tracker, applied on a vehicle, used for automated driving applications. These systems pose more demanding requirements than preventive or interventive applications, which means: need for better accuracy, tracking stability and a minimum number of false alarms-missed detections. The structure of the paper is the following: Section II gives an overview of the MHT algorithm. In Section III, vehicle demonstrator, its sensors topologies and the automated driving applications are described. A description of the implemented MHT tracker is given in Section IV. The experimental results from simulation and real data are presented in Section V. II.
MHT OVERVIEW
Reid was the first to present in [1] an algorithmic implementation of the MHT. In general there are measurements at scan and is the set of hypotheses formed after the measurements of scan are received. Each one of the measurements forms a new set of hypotheses for each prior hypothesis , with and . Assuming we have a scenario depicted in Fig. 1, in which two vehicles are moving in adjacent lanes, there aretwo preexisting tracks from the previous scan . This can be a hypothesis from the set of hypotheses that have been
1122
formed in the previous scan, or it can bee aassumed that it is the only one. The track states are predicted for scan , the measurements are gated with the tracks and track prices are calculated. If a measurement falls inside de the gate of track , an association hypothesis is formed.
Dϭ dϭ Dϯ
DϮ
dϮ Fig. 1. Gating of measurements in scan
k
with the trac racks of hypothesis
TABLE I, MATRIX IX REPRESENTATION OFTHE HYP YPOTHESIS SET 0 0 0 T1 T1 T2 T2 T2 T4 T4 T4 0 0 T2 T2 T4 T4 0 0 0 T1 T1 T4 T4 T4 0 0 0 T1 T1 T2 T2 T2 T4 T4 T4
III.
AUTOMATED DRIVIN ING VEHICLE
In the European FP7 research ch initiative HAVEit, an automatic queue assistance funct nction (AQuA) is being developed, which is the target applic plication of the data fusion algorithms presented in this contribu ibution. The primary focus of AQuA is to provide completee driver d support when the vehicle is in a queue scenario. This includes i the combination of lateral and longitudinal vehicle icle control – essentially providing automated steering, acceleration acc and braking control. Furthermore, support will not merely m be available in a simple on/off manner. Rather, thee system will provide the most suitable level of support based ed on the state of the road environment, vehicle and driver.. An A important aspect of AQuA is to develop the functionalit ality for commercial heavy goods vehicles rather than passen senger cars. Commercial vehicle drivers have special requir uirements, since they are typically professional drivers. Thiss puts p specific demands on how the system should interact with ith the driver. The AQuA concept will be demonstrated usin sing a Volvo FH12 truck equipped with suitable sensing systems, sys processing units, and control actuators. The demonstrator truck is equippe ped with an exterioceptive sensor system comprising forward rd-looking laser scanners and vision system, side-looking g radars and additional information from electronic map p and an infrared V2V communication link, as shown in Fig. Fig 3.
In this example, 36 hypotheses are formed. d. The representation of the set can be shown as a tree (FFig. 2) or as a matrix (TABLE I)
0 T1 T3 0 T3 0 T1 T3 0 T1 T3 0 T3 0 T3 0 T3 0 T1 T3 0 T3 0 T1 T3 0 T1 T3 0 T3 0 T1 T3 0 T1 T3
either discard or merge hypothese eses. The most common pruning method is to discard hypot othesis with a probability lower than a threshold or the use off a N-scan sliding window where a single root (hypothesis) is chosen for the k -Nth scan. Alternative implementa ntations include the multidimensional assignment method hod [5], [6] with the use of Lagrangian Relaxation. Track orie riented approach discards hypotheses of scan 1, whereass in i [7] Murty’s algorithm [8] is used for finding the k-best hyp ypotheses. The hypotheses merging is usually done by combin bining the hypotheses that share the same measurements or by combining similar hypotheses, i.e. hypotheses that have hav the same number of track estimates and these estimatess are a sufficiently close.
0 0 0 0 0 0 0 0 0 0 0 T1 T1 T1 T1 T1 T1 T2 T2 T2 T2 T2 T2 T2 T2 T5 T5 T5 T5 T5 T5 T5 T5 T5 T5 T5
Fig. 2 Tree Representation of the hypothesis set
For each hypothesis formed from hypo ypothesis a probability is calculated, as proposed in [1]:
Pi k =
( N NGT − N DT ) N 1 N DT N NT PD 1 − PD β FTFT β NT c N DT × ∏ N Z m − H x, B Pgk −1 m =1
(
)
(
)
1
As far as hypothesis reduction is regarded ded, a large number of methods have been proposed dependi nding also on the implementation of the MHT. In general, ral, these methods
Fig. 3 AQuA Demonstrator vehicle
1123
IV.
ALGORITHM
The algorithm is using the Multiple Hypothesis Tracking technique, in combination with Kalman filtering for the track state estimation. The motion model being used is the constant velocity (CV) model which considers constant velocity movement in both x and y axis as well as zero acceleration. The CV model requires small size of vectors and matrices for the Kalman filtering procedure, thus making it ideal for use in the MHT algorithm. The algorithm consists of several scripts and functions which are going to be presented separately. A. Initialization Procedure The measurements of the first scan are used to initialize the state vector of the first track(s). Moreover, measurement noise, process noise and other Kalman matrices are initialized. B. State vector prediction – Gating - Association When each data set is received, a Kalman filtering is temporarily performed to each one of the N pre-existing targets represented by mean state vector and covariance using the equations:
!
(1) (2)
The N existing tracks are associated with the M measurements of the data set using the gating function. If a measurement " # $ lies within a “η-sigma” validation region of track " % # &:
' ( ) ( * + ) ( ( ,
(3) (4)
where,, Measurement error covariance
The measurement receives an association score - . ' where . is a fixed value as described in [4]. An assignment matrix /012 is created, containing the prices - for measurement – track combinations.
C. Hypothesis Matrix Generation – Multiple Scans The hypothesis matrix is generated using the measurement-oriented approach that accounts for all possible origins of each measurement. Using the assignment matrix it is hypothesized that every new measurement: (i) is a false alarm (ID = 0), (ii) belongs to a pre-existing track (track ID) or initializes a new track (new track ID). If more than one measurement is received in each scan, only one measurement can be associated with each track in each hypothesis. With this branching technique, each previous hypothesis creates several new hypotheses, which leads to the formation of a hypotheses tree. This tree is represented by a &345 1 $ matrix, where &345 is the number of hypotheses and $ is the number of measurements in each
scan. The entry in the matrix is the hypothesized origin of the measurement for that particular hypothesis. All submatrices of new hypotheses created from each previous hypothesis are concatenated horizontally and vertically in a recursive procedure that forms the final hypothesis matrix for each scan. For better track prediction it is suggested in [1] that subsequent measurements and hypotheses should be correlated to prior ones. Therefore, the hypothesis matrix is growing for a number of consecutive scans before reducing itself with pruning methods which are presented in the next section. The algorithm allows the creation of multiple-scan hypothesis matrix using the previous procedure recursively for consecutive scans. Again, hypothesis matrices for each scan are concatenated to produce the multiple-scan hypothesis matrix. The number of scans for the N-scan sliding window hypothesis pruning was chosen & 6, but in general this value varies depending on the complexity of the test case. (Usually, closely spaced targets require a larger number for their tracks to be correctly predicted but this increase leads to speed reduction of the algorithm). D. Track state update After each scan, the hypothesis matrix is formed and the state vectors and covariance matrices of each track of all hypotheses are updated using the Kalman filter. If the hypothesis suggests that a track is not associated with a measurement, the state vector is just predicted using (1). If a measurement is associated with a previous track of the hypothesis, this track is updated using the current measurement. The mean and covariance are calculated as shown in section B. The conditional mean 7 and covariance are calculated using the equations:
7 ( , 89 ( : ( ( ( , (
(5) (6)
Last, if the hypothesis suggests that the measurement initializes a new track, the current measurement is used to initialize this new track and the covariance matrix is set to unitary matrix. If there is no input information about the target velocity, initial velocity is set to zero. In any case the Hypothesis Structure’s elements are updated as necessary. E. Probability of each hypothesis After the formation of the hypothesis matrix, probabilities are calculated for each hypothesis and form the hypothesis probability matrix. The basic variables being used are: ; Probability of detection ?
OPQ AL Q
When the tracking procedure starts, all probabilities are initialized with the value ; 0RSR where & T is the number of previously known targets within the sensors’ area of coverage, as proposed by Reid, in [1]. In the following scans, the previous probability of each hypothesis is first multiplied by ; 0RSR and then multiplied by one of the following values depending on the characterization of each measurement:
