TRACKING ON-ROAD VEHICLES WITH SENSORS OF DIFFERENT MODALITIES

[0001] This application relates to autonomous and/or automated vehicle or automotive systems. US Patent Application Publication Number 2011/0282581, filed May 12, 2012, discusses a method and system for detecting and tracking objects near a vehicle using a laser rangefinder, and is herein incorporated by reference in its entirety.

[0002] In a two-dimensional (2D) tracking system, targets are free to occupy any point in 2D space and hence the motion from one update cycle to another can be arbitrary and difficult to model. This can make a tracking system unstable and difficult to tune.

[0003] In one aspect of this disclosure, a tracking system can be restrained to a lane network graph (herein referred to simply as a graph) that is a predefined map, along which an object to be tracked can move. This can incorporate motion of on-road automotive vehicles and motion of other vehicles that generally abide by a lane network. Also, since only one-dimension (1D) along the graph is used to track targets, a state space for filters can be small and can be easy to tune, which can enable very efficient implementations.

[0004] In some aspects, data from different sensors is received asynchronously, and the received data can be correctly integrated together in a tracking system. Characteristics of different sensors can be leveraged to correctly filter out noisy data from the tracking system.

[0005] A vehicle system can include a first sensor and a second sensor, each having, respectively, different first and second modalities. A controller can include a processor configured to: receive a first sensor input from the first sensor and a second sensor input from the second sensor; detect, synchronously, first and second observations from, respectively, the first and second sensor inputs; project the detected first and second observations onto a graph network; associate the first and second observations with a target on the graph network, the target having a trajectory on the graph network; select either the first or the second observation as a best observation based on characteristics of the first and second sensors; and estimate a current position of the target by performing a prediction based on the best observation and a current timestamp.

[0006] The first and second observations can be associated with the target by generating a new target on the graph network when the first and second observations are not within a predefined distance of a currently tracked target.

[0007] The processor can be further configured to generate the graph network from a road map and an acquired position, such that the graph network includes one dimensional lanes as links between nodes, wherein the trajectory of the target is based on one of the one dimensional lanes.

[0008] The first sensor can be a lidar sensor, and the second sensor can be a radar sensor.

[0009] The current position of the target can be estimated by performing a Kalman filter prediction process.

[0010] The characteristics of the first and second sensors can include a time delay for processing the sensor data and an accuracy in detecting valid obstacles by the processing of the sensor data.

[0011] The target (e.g., a vehicle) can be tracked, by current position updates, through successive observations by the first and second sensors. The target can be removed from being tracked when no observation is observed for the target for a defined time period.

[0012] The detected first and second observations can be projected onto the graph network by transforming respective coordinate systems of the first and second sensor inputs into a common coordinate system of the vehicle, such as UTM.

[0013] The controller can store a list of targets in a target list in a memory of the controller, and the target list can be queried to associate observations with the targets.

[0014] Synchronous detection of observations can be restricted to points on or within a margin of error from a lane of the graph network. Similarly, tracking of targets can be restricted to points on or within a margin of error from a lane of the graph network.

[0015] The detected first and second observations can be projected onto the graph network by correcting the detected first and second observations to reflect a current timestamp. This can include adjusting a position of the first and second observations on the graph network to account for a time delay between acquiring respective sensor data and projecting resulting observations onto the graph network. An amount of the adjustment can be determined by using a calibrated (pre-defined) time delay and a velocity of the target associated with the first and second observations.

[0016] The foregoing general description of the illustrative embodiments and the following detailed description thereof are merely exemplary aspects of the teachings of this disclosure, and are not restrictive.

[0031] In the drawings, like reference numerals designate identical or corresponding parts throughout the several views.

[0032] FIG. 1 illustrates an exemplary processing system 100, and illustrates exemplary hardware found in a controller or computing system for implementing and/or executing the processes, algorithms and/or methods described in this disclosure. The system 100 can be embodied and/or implemented as an electronic control unit (ECU) or a discrete computer installed in a vehicle.

[0033] As shown in FIG. 1, a processing system 100 in accordance with this disclosure can be implemented using a microprocessor or its equivalent, such as a central processing unit (CPU) and/or at least one application specific processor ASP (not shown). The microprocessor utilizes a computer readable storage medium, such as a memory (e.g., ROM, EPROM, EEPROM, flash memory, static memory, DRAM, SDRAM, and their equivalents), configured to control the microprocessor to perform and/or control the processes and systems of this disclosure. Other storage mediums can be controlled via a controller, such as a disk controller, which can controls a hard disk drive or optical disk drive.

[0034] The microprocessor or aspects thereof, in an alternate embodiment, can include or exclusively include a logic device for augmenting or fully implementing this disclosure. Such a logic device includes, but is not limited to, an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA), a generic-array of logic (GAL), and their equivalents. The microprocessor can be a separate device or a single processing mechanism. Further, this disclosure can benefit form parallel processing capabilities of a multi-cored CPU.

[0035] In another aspect, results of processing in accordance with this disclosure can be displayed via a display controller to a monitor. The display controller would then preferably include at least one graphic processing unit for improved computational efficiency. Additionally, an I/O (input/output) interface is provided for inputting sensor data from Sensors 1, 2 . . . N, and for outputting control signals to one or more actuators to control various actuated components, including steering, brake and throttle controllers/actuators of a vehicle. A global positioning system (e.g., GPS) can be provided and connected to the I/O interface for location information of the vehicle.

[0036] Further, as to other input devices, the same can be connected to the I/O interface as a peripheral. For example, a keyboard or a pointing device (not shown) for controlling parameters of the various processes and algorithms of this disclosure can be connected to the I/O interface to provide additional functionality and configuration options, or control display characteristics. Moreover, the monitor can be provided with a touch-sensitive interface to a command/instruction interface.

[0037] The above-noted components can be coupled to a network, as shown in FIG. 1, such as the Internet or a local intranet, via a network interface for the transmission or reception of data, including controllable parameters. The network can also be a vehicle local area network (VLAN), in which input/output data (e.g., sensor, actuator data/commands, and GPS information) are transmitted via the VLAN. A central BUS is provided to connect the above hardware components together and provides at least one path for digital communication there between.

[0038] An exemplary algorithm 200 executed by the system shown in FIG. 1 is illustrated in FIG. 2. The algorithm 200 includes a process of creating a graph network at 300, a process of detecting obstacles at 400, a process of projecting the obstacles onto the graph network at 500, and performing data association and tracking at 600. Further exemplary details of the algorithm 200 are described below with reference to FIGS. 3-9.

[0039] FIG. 3 illustrates further details of process 300. In creating a graph network, a map of a road network (i.e. of an area in which the vehicle is present) is acquired or accessed at S302. The map of the road network may be pre-installed in the system 100 as part of a navigation system, or accessed by the system 100. Such access can be provided through a network connection to a navigation system or another map database stored locally or remotely (e.g., by downloading a local map from a remote server through a network connection. The network graph is generated using this local map. This graph network consists of nodes which are positions in a global coordinate system (e.g., UTM), and links which define the connectivity of these nodes. The nodes are generated at S304, and the links are generated at S306. The generation of the nodes and the links can be generated synchronously or asynchronously. Tracking of obstacles (such as other vehicles) is performed along the graph network.

[0040] Process 400 is explained in further detail by FIG. 4, which illustrates implementations with multiple sensors for vehicle detection. In particular, vehicles are detected using multiple sensors independently. In the example shown by process 400 by FIG. 4, there are two sensors, Sensor 1 and Sensor 2. However, this can be expanded include further sensors. Further, when one or more of the sensors is lidar (light detection and ranging), point cloud segmentation methods are used to detect vehicles from other obstacles. Object detection and recognition can include aspects of edge and shape detection and recognition, also incorporating other known aspects of object segmentation and recognition. When one or more of the sensors is a camera, computer vision techniques are used to detect vehicles. These techniques can include object recognition, including shape and edge detection. Both lidar and camera recognition processes can be performed based on trained and/or predefined libraries of data, with known and recognizable shapes and edges of obstacles (e.g. vehicles, cyclists, etc.). Radar sensors can report detected obstacles along with a velocity of the obstacles.

[0041] In process 400, obstacles can be detected by Sensor 1 at S402, and a velocity of the obstacles can be detected/determined at S404. Synchronously, obstacles can be detected by Sensor 2 at S406, and a velocity of the obstacles can be detected/determined at S408. That is, obstacles (e.g., vehicles, cyclists, etc.) can be synchronously detected by Sensors 1 and 2 (as well as further sensors). Obstacle velocity can be determined by, e.g., radar, as discussed above. Velocity can also be extrapolated by successive position detection measurements and calculating speed and/or direction of motion based thereon.

[0042] Projection of the obstacles onto the graph network is explained with reference to process 500 illustrated in FIG. 5. At S502, obstacles detected by each sensor are transformed into a common/same coordinate system. In particular, a calibration can be carried out for transforming data from different sensors to the vehicle's coordinate system. These transformed obstacles are then projected on the graph network at S504 by, e.g., using a nearest neighbor technique. The location of the vehicle is determined using GPS+ inertial systems, including a GPS receiver, accelerometers and gyroscopes. Other vehicle location tracking devices can be used, either in addition or supplementary to GPS or other satellite-based navigation systems.

[0043] FIG. 6 illustrates process 600 to describe data association and tracking. Each obstacle may have multiple observations from different sensors, and these multiple observations are fused. Further, since the sensors work asynchronously, a simple nearest neighbor approach is not used. In order to do a correct temporal assignment, each observation is projected and corrected along the graph network for the current timestamp. These corrected observations are then associated with a current list of targets using the nearest neighbor assignment, which is stored in a memory of the system 100. Multiple observations are assigned to the list of observations for each target, and each target has a probabilistic tracker associated with it, such as a Kalman filter.

[0044] The process 600 can include an initialization phase at S602. In the initialization, if no target is assigned for a valid observation, then a new target is created. Its Kalman state is initialized to a corrected observation. A state of the Kalman filter is a travel distance along the lane, velocity and acceleration of the obstacle. In case of observations coming from sensors which do not record the velocity, the velocity is estimated in a first couple of iterations.

[0045] An update phase is performed at S604, where a best observation is selected from a list of observations for each target. This best observation is then used to update the Kalman filter. The best observation can be determined by correlating an observation with an expected position of a target, based on velocity and the graph network. That is, targets can be expected to exist directly on the graph network and to have traveled an estimated distance along the graph network based on the determined velocity. The best observation can also be based on time-delay (delay in processing the sensor data to detect an object), and sensor accuracy. Kalman filtering can be utilized for making path and observation predictions.

[0046] At S606, if no observation is observed for a certain period of time for a given target, that target can be dropped from the tracking system. That is, that target can be removed from the list. The target may then be re-created at the initialization phase when observations resume.

[0047] In some aspects, the algorithm can be summarized as follows:

[0048] 1. Assign all observations with their corresponding timestamps to the list of observations.

[0049] 2. For each observation:

• • [0050] a. If the observation is close enough to the graph, project the observation on the graph.
  • [0051] b. For each trajectory in the list:
    • [0052] i. Estimate the position of the trajectory at the timestamp of the observations.
      • [0053] ii. Assign the observation to the trajectory if it is close to the position of the trajectory at that timestamp.
      • [0054] iii. If no trajectory is found, create a new trajectory and assign the current observation to the trajectory.

[0055] 3. For each trajectory in the list:

• • [0056] a. Select the best observation from the list of observations assigned to the trajectory. This best selection can be carried out heuristically based on sensor accuracy, time delay, etc.
  • [0057] b. Perform a Kalman filter prediction (prior) step to the timestamp of best observations. This could move the filter back in time.
  • [0058] c. Perform a Kalman filter measurement (posterior) update using the true value of the observation.
  • [0059] d. Perform a Kalman filter prediction step again to the current timestamp value to estimate the current position on the trajectory.

[0060] Several of the features provided by the above-discussed algorithm(s) are: graph based tracking instead of full 2 dimensional tracking, making the tracking more efficient and more robust against false tracking targets; correct sensor fusion for obstacles detected asynchronously using different sensors; and integrating a probabilistic tracker in the system.

[0061] Various implementations of the above-discussed algorithm(s) are shown in FIGS. 7-9, which are graphical illustrations of the tracking system operating in tracking various targets with various sensors. These figures illustrate an implementation utilizing a lidar and a radar. However, other sensors can be utilized. In FIGS. 7-9, yellow indicates a radar target, green indicates a lidar target, and the test vehicle is indicated by blue. The graph network, and specifically a lane of the graph network, is outlined in white. Various targets, which can be identified by processing only those lidar and radar targets that appear on (or within a margin of error of) the lane of the graph network, are shown in red (red diamonds).

[0062] FIG. 8 illustrates exemplary links (blue lines) and nodes (yellow diamonds), and exemplifies asynchronous/delay observations. The nodes can also be explained as a series of dots between the yellow diamonds, which are connected by the blue lines. Yellow circles are radar observations, and green circles are lidar observations, which are delayed due to processing. A final graph target (red diamond) is nonetheless correctly updated, by taking into account the delay in lidar processing compared to radar processing. FIG. 9 illustrates a highway implementation, where red diamonds show tracked targets. The display of the system 100 can be configured to display the graph network with corresponding tracked targets identified thereon, where FIGS. 7-9 can be examples of such a display.

[0063] Based on results of target tracking, the system 100 can generate appropriate outputs. For example, collision warnings can be generated and output to an audio and/or visual system of the vehicle when a tracked target is tracked as having a predefined probability of collision with the vehicle. For collision determination, path planning can be implemented using a time-horizon prediction process for predicting a path of the tracked target. Such calculations can be performed by another system in communication with the system 100. Further, results of target tracking can be input to a cruise control system, to adjust an automated speed or direction of the vehicle based on the positions of the tracked targets. That is, the system 100 can output control commands to an accelerator actuator, a brake actuator and/or a steering actuator to control the speed and direction of the vehicle based on the results of target tracking. As such, the system 100 can be implemented into a robotic autonomous vehicle capable of self-guidance and collision avoidance.

[0064] In exemplary implementations, for autonomous driving, tracking other vehicles on the road is a critical component. In order to plan safe driving maneuvers in both highway and urban traffic scenarios, information about location and motion of other vehicles on the road is required. According to aspects of this disclosure, a graph based vehicle tracking system using multiple sensors (Radar and Lidar) is provided and was successfully tested as a part of an autonomous vehicle research project.

[0065] In particular, using a map of a road network, a graph is generated as a pre-processing step. Obstacles obtained from different sensors of different modalities are tracked, by a tracker, along the graph using a probabilistic filter. Output from the tracker includes a location and expected motion of all dynamic obstacles, which is used by a planner to plan a safe driving maneuver.

[0066] A road network may be defined using a data structure called a Road Network Definition File (RNDF), which consists of a set of waypoints in the UTM coordinate system and linking information of these waypoints to create lanes, exits, merges, etc. Also, other semantic information like stop signs and traffic light positions are embedded in the data structure. This information is used by the planner to generate a route.

[0067] A graph, which is used for tracking, is generated from this RNDF data. Each node in the graph corresponds to a waypoint in the RNDF. The same linking structure is maintained to connect the nodes in the RNDF. In case of merges and exits, links, nodes are interpolated using spline-fitting between end points of links and which are added to the graph.

[0068] Additionally, travel distance from the start of a lane to which the waypoint belongs is stored in each node. The travel distance is the distance along the graph, rather than a direct Euclidian distance, which is then used for tracking obstacles along the graph.

[0069] A typical graph is shown in FIGS. 7-9, as well as FIG. 10.

[0070] An exemplary autonomous vehicle is shown in FIG. 11, which shows a test vehicle instrumented with Light Detection and Ranging (Lidar) manufactured by Velodyne, an array of long range radar sensors manufactured by Bosch, and Prosillica High Definition Cameras. The Velodyne sensor can give reasonably accurate point cloud data 360° around the vehicle up to a distance of 40 m. Obstacles are segmented out from the point cloud using 3D point cloud perception techniques. An example of point cloud segmentations is shown in FIG. 12.

[0071] An array of radar sensors (such as 8 radars) is mounted on the vehicle such that each of the radars has a different orientation. Each radar can have a field of view of ±15° at 50 m and ±6° at a range of about 200 m. Radar directly sends back the detected obstacle position and relative velocity in the coordinate frame of the radar. Since location and velocity of the obstacles are obtained in the radar coordinate frame, it is necessary to transform each obstacle to the vehicle coordinate frame. Finding the right transform for each of the radar unit can be done by a calibration process described below.

[0072] FIG. 13 illustrates schematically the transformation to be performed. All radar sensors have to be calibrated to get an accurate transformation from radar reference frame to the robot's (the car's) reference frame. A semi-automated procedure has been developed for this purpose as described below.

[0073] Radar markers which are highly radar reflective are placed in different positions around the vehicle. These positions are marked accurately using LIDAR points. Since the effective range of Velodyne LIDAR is shorter than that of the radars, the vehicle is driven within the Velodyne range to the markers, and the GPS system in the car is used to keep the marker coordinates in the same reference frame. At the same time, obstacle data from the radars is recorded.

[0074] Transformation from the radar reference frame to the robot reference frame is given by the following equations:

x_r=x_c cos θ+y_c sin θ+t_x

, where

y_r=−x_c sin θ+y_c cos θ+t_y

[0075] {x_r, y_r} are obstacle positions in radar reference frame, {x_c, y_c} are obstacle positions in the car reference frame, and t_x, t_y and θ are calibration parameters. The parameters t_x, t_y, and θ are estimated from the data collected using non-linear least square approximation using a Levenberg-Marquardt algorithm. The least square problem is formulated as a minimization of the following term:

$\mathrm{err}=\sum _i^{}{\left(x_o^i-x_e^i\right)}^2+{\left(y_o^i-y_e^i\right)}^2,$

where

[0076] {x_oⁱ, x_oⁱ} are i^th observed data point and {x_cⁱ, y_cⁱ} are estimated using the above transformation equation. A calibration accuracy obtained from this particular process is 0.01 m.

[0077] Consistent with the above, tracking can be performed by a three-phase process: association, initialization, and update.

[0078] Association Phase

[0079] Each detected obstacle is projected on to the graph. Obstacles which are not close to the graph are neglected. Each obstacle might have multiple observations from different sensors, and this information needs to be fused. Also, since the sensors work asynchronously, a simple nearest neighbor approach cannot be used. In order to do a correct temporal assignment, each observation is projected and corrected along the graph for the current timestamp. These corrected observations are then associated with the current list of targets using the nearest neighbor assignment. Multiple observations are assigned to the list of observations for each target.

[0080] Initialization Phase

[0081] If no target is assigned for a valid observation, the a new target is created. Its Kalman state is initialized to the corrected observation. A state of the Kalman filter is a travel distance along the lane, velocity and acceleration of the obstacle. In case of a Velodyne-only observation, since velocity is not observed, it is estimated in a first couple of iterations.

[0082] Update Phase

[0083] Each target has an independent Kalman filter associated with it. A best observation is selected from the list of observations for each target, and the full Kalman update is carried out as given below (see A new approach to linear filtering and prediction problems, Journal of Basic Engineering 82, by Kalman, R. E):

{circumflex over (x)}_k|k-1=F_k{circumflex over (x)}_k-1|k-1

P_k|k-1=F_kP_k-1|k-1F_k^T+Q_k

{tilde over (y)}_k=z_k−H_k{circumflex over (x)}_k|k-1

S_k=H_kP_k|k-1H_k^T+R_k

K_k=P_k|k−1H_k^TS_k⁻¹

{circumflex over (x)}_k|k={circumflex over (x)}_k|k-1+K_k{tilde over (y)}_k

P_k|k=(I−K_kH_k)P_k⊕k-1

[0084] If no observation is obtained for certain targets for some prescribed period of time, then they are dropped from the tracking system.

[0085] Pseudo-code for a tracking algorithm is presented below:

• • [0086] 1. Assign all observations with their corresponding timestamps to the list of observations
  • [0087] 2. For each observations:
    • [0088] a. If observation is close enough to the graph, project the observation on the graph.
    • [0089] b. For each trajectory in the list
      • [0090] Estimate the position of the trajectory at the timestamp of the observations.
      • [0091] Assign the observation to the trajectory if it is close to the position of the trajectory at that timestamp.
      • [0092] If no trajectory is found, create a new trajectory and assign the current observation to the trajectory.
  • [0093] 3. For each trajectory in the list:
    • [0094] a. Select the best observation from the list of observations assigned to the trajectory. This best selection can be carried out heuristically based on sensor accuracy, time delay etc.
    • [0095] b. Do Kalman filter prediction (prior) step to the timestamp of best observations. Note that this could move the filter back in time.
    • [0096] c. Do Kalman filter measurement (posterior) update using the true value of the observation.

[0097] Do Kalman filter prediction step again to the current timestamp value to estimate the current position on the trajectory.

[0098] FIGS. 14-16 illustrate various aspects of the algorithm(s) discussed above. FIG. 14 illustrates asynchronous/delay observations, where the circle is a radar observation, a square is a lidar observation, which are delayed due to processing, but a final graph track target (diamond) has been correctly updated. FIG. 15 illustrates tracking of cars on highways, where diamonds show the tracked targets. FIG. 16 illustrates tracking of cars at intersections, where diamonds show the tracked targets.

[0099] A number of implementations have been described. Nevertheless, it will be understood that various modifications may be made without departing from the spirit and scope of this disclosure. For example, advantageous results may be achieved if the steps of the disclosed techniques were performed in a different sequence, if components in the disclosed systems were combined in a different manner, or if the components were replaced or supplemented by other components. The functions, processes and algorithms described herein may be performed in hardware or software executed by hardware, including computer processors and/or programmable circuits configured to execute program code and/or computer instructions to execute the functions, processes and algorithms described herein. Additionally, some implementations may be performed on modules or hardware not identical to those described. Accordingly, other implementations are within the scope that may be claimed.