Abstract
Tracking the actions of vehicles at crossroads and planning safe trajectories will be an effective method to reduce the rate of traffic accident at intersections. It is to resolve the problem of the abrupt change because of the existence of drivers' voluntary choices. In this paper, we make approach of an improved IMM tracking method based on trajectory generation, abstracted by trajectory generation algorithm, to improve this situation. Because of the similarity between human-driving trajectory and programming trajectory which is generated by trajectory-generated algorithm, the improved IMM method performs well in tracking moving vehicles with some sudden changes of its movement. A set of data is collected for experiments when an object vehicle takes a sudden left turn in intersection scenario. To compare the experiment results between IMM method with trajectory generation model and the one without, tracking error of the former decreases by 75% in particular scenario.
1. Problem Statement
In urban environments, private automobile use has led to severe problems with respect to congestions, pollution, and safety. Perhaps safety is the most serious problem. In China alone, 238,000 accidents have happened in 2009, which accounted for 275125 nonfatal injuries and 67759 deaths [1], and many of these accidents happened at intersections.We can easily find the reasons for the insecurity at intersections from the accidents and actions that happened at crossroads. Obviously, it is different from lane because vehicles and pedestrians have various choices at intersections. A large effort has been put in industrial countries into developing new types of transportation systems to solve safety problem in intersection scenarios. Tracking the actions of vehicles at crossroads and then planning safe feasible path based on it is an effective approach to resolve this.
However, because of the voluntary behaviors of driving, the existing models used in interacting multiple model algorithm (IMM) cannot track the object vehicle accurately and effectively especially when abrupt motion change happens. And the reason of this defect lies in the lack of foresee abilities of the existing models, such as uniform motion, constant acceleration motion mode, and coordinated turn model (further details can be found in [2, 3]). Using these regular models can only estimate the vehicle state of next sampling time based on the currently state. If a big change on motion happens on the next sampling time, the estimate based on the currently state will appear big errors.
In fact, before an abrupt motion change happening, some signs will appear alongside, like the change of traffic lights and the velocity deceleration before turns. Distinguishing these signs to assist the tracking algorithm is a potential method to resolve the problem above. Then the hypothesis will be varied still further. If a human-drive-like trajectory of the object vehicle can be generated based on the signs and some other situations we can perceive, and also a new model can be able to abstract from it, we can use this new model to replace the regular models in IMM method to obtain the ability of prediction.
Given the above consideration, we propose a new model, which is abstracted from trajectory generation algorithm, to add into IMM method to resolve the tracking of abrupt motion-changing problem. In this paper, we take intersection scenario in urban environment as an example to expound.
2. State of Art
In prior work, two sorts of methods are proposed mostly to resolve this problem, which are vehicle-to-vehicle communication (V2V) and vehicle tracking. The advantage of V2V method which is coupled with perception systems lies in that the positions and motions of other vehicles are available via V2V system and then trajectories can be planned based on it. The projects focus on V2V communication for road safety in general; we can find for example eSafety program [4] that aims at accelerating the development, deployment, and use of systems that use information and communication technologies to increase road safety. Finally, the V2V Communication Consortium [5] addresses similar problems, focusing on the creation of a standard for active safety applications. In [6], another tactics with V2V is introduced. In the tactics, taking vehicles’ speeds into account, fuzzy logic is used to make decisions about whether cross the intersection or not.
However, the disadvantage of V2V is also obvious. To realize the V2V system, all vehicles participated in traffic should be equipped with communication equipment, which costs enormously. For this reason, vehicle tracking method would be a good choice. Currently, IMM method is one of the most effective method, which uses both incivil and military applications.
In [7], VS-IMM-PF (variable structure-IMM-particle filter) algorithm is proposed, which is based on state estimation process and process noise correction according to the characteristics of ground moving target. The simulation results show that the proposed algorithm can track ground moving target with a higher accuracy and low complexities in calculation than some other algorithms which are based on process noise correction only. Another algorithm named IMM-PF (IMM-particle filter) is discussed in [8], which keeps a constant number of particles for all modes in the filter irrespective of the posterior mode probabilities. In [9], the DIMM (diagonal interacting multiple model) algorithm is presented. The DIMM algorithm is reweighted by diagonal matrices based on a multisensor optimal information fusion rule in the linear minimum variance sense instead of Bayes’ rule. Simultaneously, the algorithm can avoid the hybrid computation of likelihood function and probability mass and distinguish the effects produced by different dimensions of state.
In view of the previous methods, we present a new approach which has the following distinctive characteristics.(1)Trajectory generation model has the same form with the regular coordinated turn model, which makes it much easier to replace the regular ones and added into IMM method, without changing the calculation procedure of IMM method. (2)Trajectory generation model is abstracted from trajectory generation algorithm, so when it is been added into IMM method, the tracking accuracy will be improved remarkably because of the characteristic of prediction ability that the trajectory generation algorithm has.
3. Algorithm Theory
We abstract our trajectory generation model from trajectory generation algorithm, which is using cubic curvature polynomials to generate trajectory sets. The form of the coordinated turn model is reserved in trajectory generation model, and only the turning rate parameter is denoted in heading , which is calculated by trajectory generation algorithm and regarded as very approximate to truth value. We also proposed an effective method to calculate the related parameters of trajectory generation algorithm and meanwhile resolve the obstacles avoidance problem.
3.1. Trajectory Generation Model
The trajectory generation model is abstracted from the trajectory generation method to be used in IMM method to simulate the voluntary behaviors, mainly turning abruptly, when crossing intersections. The trajectory generation algorithm will be introduced first, and then we build the trajectory generation model based on it.
Trajectory generation method generates a set of trajectory between two points, which should be expressed as where is the vehicle state, with position , heading , and curvature . Considering that the postures of start point and end point are described as the four degrees of freedom in state space, the solution to the state equations for a curvature which is a third-order polynomial in arc length: The dynamic modeling of the Kalman filter kernels used in the IMM method determines the constraints of the dynamic process. Referring to the forms of coordinated turn model, the joint system vector of trajectory generation model can be expressed in the same form as where is the position in sampling time , the velocity in -axis and -axis, respectively, and the turning rate parameter.
As we know, heading can be expressed as where is sample interval of sensors. Then solving the two equations above simultaneously for a function about turning rate parameters and , If , then . Under this condition, trajectory generation model turns into coordinated turn model.
Hence, the dynamic model for the trajectory generation is where is univariate white Gaussian process noise for the turning rate parameter.
In (6), heading is updated with the value of observation, while and should be obtained from trajectory generation method, which is expressed as where .
Headings and which are obtained from trajectory generation method are regarded as very approximate to truth values because of the existence of driver’s initiative. Obviously, calculated by (5) is also a better estimate of turning rate than a regular estimate value in coordinated turn model which is expressed as follows:
The trajectory generation model is built based on the turning rate parameter , so the increased estimate accuracy of will also increase the accuracy of this model.
3.2. Intersection Path Planning
As restrictions of the traffic lights, situations at the intersection can be divided into three cases: straight, turn left, and turn right. According to the traffic signals, the moving direction of the vehicle can be easily obtained. As the paper mainly considers the situation of a sudden turn, we can take the situation of turning left as an illustration. Set the entrance of the left road as the coordinate of the end of the planning path, so that parameters of path planning method are computable using the following algorithm.
Solving the equations in (2) and getting answers of seem a method to solve the problem. Unfortunately, the last two equations of (2) are not easy to get solutions.
An effective method of trajectory generation is introduced by Nagy and Kelly [10]. The arc length is based on an approximation of the observed average relationship between and the total change in heading between the start and end postures over a large sample set. And we assume the parameter to be zero, then we solve the and equations simultaneously for and . The actual equations are as follows: We proposed a parameters circulation method (PCM) to calculate the exact solutions of and solve the obstacle avoidance problem at the same time.
In PCM, we define small variation ranges for parameters , , , and , which are calculated by (9) and then build a fourfold circulation as the parameters changing. A set of trajectories will be generated and the optimal one picked out and executed. Then we generate a cost function that considers some mixture of risk, smoothness, length, and other “costs” that can be associated with a trajectory to pick out the optimal one, and the cost function is as follows: The position of obstacle vehicle perceived by Velodyne is projected onto some cm grids on -plane. We track and predict the motion of obstacle vehicles by IMM method and calculate its position when the trajectories extend nearby, which is marked up on the grids as actual obstacles. Once the trajectory crosses the grid cell which is occupied by actual obstacles, is set as infinity. Also the termination errors are taken into consideration: The PCM algorithm is listed in Algorithm 1.
| ||||||||||||||||||||||||||
3.3. Interacting Multiple Model Algorithm
The basic idea of IMM algorithm is to use different motion models to match the different states of object’s motion. This can overcome the error caused by target movement state not matching the model when using a single motion model. Markov chain is used to converse between various motion models. More details are discussed in [11–13].
The IMM algorithm is composed of three main parts.
(1) Interactive Input
The entries of the Markov matrix are used with the model probability of the previous cycle to calculate the predicted model probability
The mixing of the state estimates and their covariances can be performed:
(2) Model-Specific Filtering
The predicted state estimates and their covariances can be calculated by using the different dynamic models. The measurement residual is and is the residual covariance. The likelihood for the observation is calculated from and :
The new model probabilities are calculated:
(3) Interactive Output
Finally the overall state estimate and its covariance are as follows:
The flowchart of IMM algorithm is shown in Figure 1.
4. Experiments
We perform experiments on the datasets captured by our autonomous vehicle—SmartV-II (show in Figure 2) which is equipped with Velodyne HDL-64 sensor on the roof. It scans around at 10 Hz, composes 64 scan lines at different sizes of circle, yields a 360° horizontal field of view and a 26.8° vertical field of view, and provides more than 1.33 million points per second (More details of Velodyne HDL-64 can be found in [14]). GPS (Global Position System) and IMU (Inertial Measurement Unit) are also used to get accurate position and heading of SmartV-II.
The test data presented here was collected by SmartV-II on the campus of Wuhan University. We choose a three-way intersection scene to collect experimental data. The object vehicle is followed by SmartV-II, and the former one turns left abruptly at the intersection.
Firstly, we process the test data to detect the target vehicle and several steps are executed in detection method, successively as labeling connected regions, coarsely extraction, and accurate extraction. The algorithms we used are referred in [15, 16]. The effects after each step are shown in Figure 3. And the object vehicle is shown as a green rectangle on the very right side in Figure 3(c).
(a)
(b)
(c)
Secondly, processing the data from Velodyne, we can get coordinates of the object vehicle in SmartV-II coordinate system. Then global coordinates of the object vehicle are easy to obtain after some transformations because the global positions of SmartV-II are known.
Thirdly, two models are chosen to tracking the object vehicle in experiment 1 (E1): standard wiener process velocity model and trajectory generation model. The experiment selects 40 pairs of data which follow right after the abrupt turn. And two waypoints of the trajectory generation algorithm are set as (−423.5, 203, 90, 0) and (−405, 207, 60, 0), which are based on the results after coordinate transformation.
The standard wiener process velocity model can be expressed as follows: where and , . The parameters of process noise covariance matrix are set as In trajectory generation model which is expressed as (6), the process noise of turning rate is set as And we also design a check experiment (E2) which chooses the regular coordinated turn model, described in (8), to replace the trajectory generation model, and the correlation parameter is equal to 0.05.
The comparison between E1 and E2 is shown in Figure 4. The mean square errors of both the experiments are listed in Table 1.
(a)
(b)
Comparing two figures in Figure 4, the tracking trajectory converges faster at the very beginning and coincides with the measurement trajectory perfectly after that. And the average MSEs of E1 decreased by 75% on the basis of E2. This leads to the conclusion that the trajectory generation model promotes the tracking accuracy because it corresponds much better to drivers’ behavior than coordinated turn model does, especially when abrupt change happens.
The experiment above is repeated more than 15 times with different data, and the results are satisfactory without exception.
5. Conclusion and Future Work
In this paper, we proposed the problem of improve precision of tracking vehicles with abrupt changing motions, and a new model was built to resolve this. The main ideas are as follows. By abstracting trajectory generation model, from trajectory generation algorithm which has the ability to simulate drivers’ behaviors, and using it in IMM method, we effectively track the object vehicle in intersection scenario with uncommonly improved accuracy. The approach we used to abstract model can be viewed as a two-step procedure. First, the relations between trajectory generation model and trajectory generation algorithm are established through the differential relationship between the heading and the turning rate parameter. In the second step, the value of is calculated by trajectory generation algorithm to resolve , which is the basis of the new model.
Our approach assumes that the state of the end point is divinable because of the existence of signs, followed by abrupt motion change. In fact, the research of our work in detecting these signs is not quite enough because of the underlying difficulty. This will be the key point of our future work.
Acknowledgments
This paper is supported by the National Natural Science Foundation of China under Grant no. 41050110437 and Grant no. 41001306, the National Innovation Team under Grant no. 40721001, the Ph. D. Programs Foundation under Grant no. 20070486001, and the Independent Research Fund of Wuhan University under Grant no. 3101019.