Patent Abstract:
A method for determining an actual trajectory of a vehicle using object detection data and vehicle dynamics data. An object detection system identifies point objects and extended objects in proximity to the vehicle, where the point objects are less than a meter in length and width. An updated vehicle pose is calculated which optimally transposes the point objects in the scan data to a target list of previously-identified point objects. The updated vehicle pose is further refined by iteratively calculating a pose which optimally transposes the extended objects in the scan data to a target model of previously-identified extended objects, where the iteration is used to simultaneously determine a probability coefficient relating the scan data to the target model. The updated vehicle pose is used to identify the actual trajectory of the vehicle, which is compared to a planned path in a collision avoidance system.

Full Description:
BACKGROUND OF THE INVENTION 
     1. Field of the Invention 
     This invention relates generally to enhanced trajectory tracking of an automobile and, more particularly, to a method for relative positioning of a host vehicle with respect to surrounding static obstacles and moving targets which uses object detection data along with vehicle dynamics data to compute a closed loop trajectory of the host vehicle, and uses the trajectory in path planning for collision avoidance or other purposes. 
     2. Discussion of the Related Art 
     Many vehicles now include systems which can actively control the path of the vehicle—for applications such as collision avoidance. These systems, when they detect a need to alter the path of the vehicle, will determine a desired new path for the vehicle to follow, implement braking and/or steering commands to achieve the desired path, monitor the actual trajectory of the vehicle relative to the desired path, and make further corrections to follow the desired path—all in real time. 
     In such systems, it is essential that a reliable method of monitoring the actual trajectory of the vehicle is available. Many vehicles equipped with such systems use vehicle dynamics sensors—including velocity, yaw rate and acceleration sensors—as one means of calculating actual vehicle trajectory. However, vehicle dynamics or “dead reckoning” calculations are susceptible to cumulative error. Therefore, another source of vehicle trajectory data is needed—preferably a source which does not accumulate error like the vehicle dynamics data. Global Positioning System (GPS) data is commonly used as the second source of vehicle trajectory data in vehicle path planning systems. However, some vehicles are not equipped with GPS receivers, and even vehicle which are equipped with GPS receivers will sometimes have no signal available—such as in tunnels, “urban tunnels” and roads with tall and dense surrounding tree cover. 
     SUMMARY OF THE INVENTION 
     In accordance with the teachings of the present invention, a method is disclosed for determining an actual trajectory of a vehicle using object detection data and vehicle dynamics data. An object detection system identifies point objects and extended objects in proximity to the vehicle, where the point objects are less than a meter in length and width. An updated vehicle pose is calculated which optimally transposes the point objects in the scan data to a target list of previously-identified point objects. The updated vehicle pose is further refined by iteratively calculating a pose which optimally transposes the extended objects in the scan data to a target model of previously-identified extended objects, where the iteration is used to simultaneously determine a probability coefficient relating the scan data to the target model. The updated vehicle pose is used to identify the actual trajectory of the vehicle, which is compared to a planned path in a collision avoidance system. 
     Additional features of the present invention will become apparent from the following description and appended claims, taken in conjunction with the accompanying drawings. 
    
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         FIG. 1  is a schematic diagram of a vehicle with path planning and closed-loop trajectory control using vehicle dynamics and object detection data as input; 
         FIG. 2  is a block diagram of a vehicle path planning system with closed-loop trajectory control using vehicle dynamics and object detection data as input; 
         FIG. 3  is an illustration of a vehicle determining its position relative to detected objects, including point objects and extended objects; and 
         FIG. 4  is a flowchart diagram of a method for vehicle path planning and trajectory following with closed-loop control, using vehicle dynamics and object detection data as input. 
     
    
    
     DETAILED DESCRIPTION OF THE EMBODIMENTS 
     The following discussion of the embodiments of the invention directed to a method for determining an actual trajectory of a vehicle using object detection data and vehicle dynamics data is merely exemplary in nature, and is in no way intended to limit the invention or its applications or uses. 
     Some vehicles are now equipped with collision avoidance systems which can take control of vehicle steering and braking in situations where a vehicle collision appears to be imminent. Collision avoidance systems determine a desired path for the vehicle to take in order to avoid the collision, measure actual vehicle trajectory, and make steering/braking corrections in real time. Actual vehicle trajectory is typically determined in these systems using a combination of vehicle dynamics data and GPS data. However, in vehicles which are not equipped with a GPS receiver, and in situations where GPS data is not available due to sky visibility obstructions, another source of vehicle position data is needed. 
       FIG. 1  is a schematic diagram of a vehicle  10  with path planning and closed-loop trajectory control using vehicle dynamics and object detection data as input. The vehicle  10  includes a vehicle dynamics module  20  which estimates vehicle motion based on vehicle dynamics calculations, using input from a plurality of sensors  22 . The sensors  22  (one shown) may include a vehicle speed sensor, a yaw rate sensor, lateral and longitudinal acceleration sensors, and possibly others. The vehicle  10  also includes an object range data module  30 , which provides object range data from one or more object detection sensor  32 . 
     The object detection sensor  32  may use any suitable object detection technology, including short-range radar, long-range radar, LIDAR, and/or camera images. In particular, synthetic aperture radar systems are now becoming available where the radar antenna is a thin sheet which can be placed on or inside of a vehicle body panel or bumper fascia, thus making radar-based object detection more cost-effective. Using any desirable combination of object detection sensors  32 , the object range data module  30  provides object range data which is updated at time intervals such as 100 milliseconds (ms). The vehicle  10  also includes a vehicle pose estimation module  40 , a path planning module  50  and a vehicle control module  60 , which are discussed further below. 
       FIG. 2  is a block diagram of a vehicle path planning system  70  with closed-loop trajectory control using vehicle dynamics and object detection data as input. The path planning system  70  of  FIG. 2  shows how the modules  20 - 60  are inter-related. As discussed above, the vehicle dynamics module  20  provides vehicle motion data to the vehicle pose estimation module  40 , while the object range data module  30  provides object range data to the vehicle pose estimation module  40 . The vehicle pose estimation module  40  uses the vehicle dynamics data and the object range data to estimate the updated vehicle pose, or position and orientation, at each time step. The path planning module  50  provides a desired path for the vehicle  10  to follow—such as for the purpose of collision avoidance. The actual vehicle trajectory data from the vehicle pose estimation module  40  and the desired vehicle path data from the path planning module  50  are provided to a summing junction  55 , where the difference is provided to the vehicle control module  60  which makes further adjustments to steering and braking commands in order to minimize the difference between the actual vehicle trajectory and the desired vehicle path. 
     It is to be understood that the vehicle dynamics module  20 , the object range data module  30 , the vehicle pose estimation module  40 , the path planning module  50  and the vehicle control module  60  are comprised of at least a processor and a memory module, where the processors are configured with software designed to compute desired and actual vehicle position and issue vehicle control commands based on the described inputs. The logic and calculations used in the vehicle pose estimation module  40 , in particular, will be discussed in detail below. 
     It is to be further understood that the features and calculations of the vehicle pose estimation module  40 , the path planning module  50  and the vehicle control module  60  could be allocated differently than described herein without departing from the spirit of the disclosed invention. For example, although the functions of the modules  40 ,  50  and  60  are described as being distinct throughout this disclosure, they could in fact all be programmed on the same processor. That is, all vehicle position planning, trajectory estimation and control functions could be performed in a single physical device with one or more processors. 
       FIG. 3  is an illustration of the vehicle  10  determining its position relative to detected objects, including point objects (such as light poles and tree trunks) and extended objects (such as vehicles and concrete barriers). A local world coordinate frame  100  is established with origin O and X-Y axes. The vehicle  10  is located at a position (x H ,y H ) and has a heading angle of θ H  with respect to the local world coordinate frame  100 . A method of continuously updating the vehicle pose (consisting of x H , y H  and θ H ) based on the detected objects is detailed below. 
     The calculations described in the following discussion are performed in the vehicle pose estimation module  40  unless otherwise noted. At each time step, a scan map S is received from the object range data module  30 , and S is projected to the local world coordinate frame  100  based on a known previous pose P=(x H ,y H ,θ H ). The scan map S is then grouped into a list of clusters, and each cluster is classified as a point object (which may be defined as those objects with length and width less than 1 meter) or an extended object (if larger). The objective of the method is to predict the new host vehicle pose from the observed relative motion of the scan objects. 
     A predicted new vehicle pose P′=(x′ H ,y′ H ,θ′ H ) can be computed based on vehicle dynamics and the previous pose P, as follows:
 
 x′   H   =x   H +ν xH   ΔT  
 
 y′   H   =y   H +ν yH   ΔT  
 
θ′ H =θ H +ω H   ΔT   (1)
 
Where ν xH  is the vehicle longitudinal velocity, ν yH  is the vehicle lateral velocity, ω H  is the vehicle yaw rate and ΔT is the time step increment. Throughout the following discussion, it is important to distinguish between P′, which is the preliminary predicted new vehicle pose based on vehicle dynamics, and other variables such as P* and P″, which are new vehicle poses produced by the calculations based on the object scan data.
 
     A first estimation of the new vehicle pose, producing a value P*, will be made based on the point objects, which are indicated by bracket  110  in  FIG. 3 . For each point object in the scan data (represented by circles  112 ), a center of mass o i =(x i ,y i ) is computed, and the point object is matched with a current registered target (represented by stars  114 ) in a point target list {t l }, where each target in {t l } is represented by its position (x l ,y l ). The new pose P* can be found by using a least-squares optimization, as follows: 
                     P   *     =       argmin     P   ′       ⁢           ⁢       ∑   i     ⁢                o   i     -       T     P   ′       ⁡     (     t     l   ⁡     (   i   )         )           σ     l   ⁡     (   i   )                2                 (   2   )               
Where t l(i)  is the corresponding registered target associated with the point object o i , T P′ (t l(i) ) is the projection of t l(i)  under rigid transformation P′ determined by the vehicle position, and σ l(i)  is the measurement error covariance for the corresponding target t l(i) .
 
     A hit count k l  is tracked for each registered target t l . The hit count k l  is incremented by one for each registered target t l  which is matched by a point object o i  from the scan data. If k l  is greater than a predetermined threshold, such as four, then the target t l  is marked as being matured. If o i  does not match with any registered target t l  in the map, a new target will be generated with k l =1. 
     The point target list {t l } can be updated using simultaneous localization and mapping techniques, as follows: 
                     t   l   ′     =         1     κ   l       ⁢       T     P   *       -   1       ⁡     (     o   i     )         +           κ   l     -   1       κ   l       ⁢     t   l                 (   3   )               
Where t′ l  is the new target location (x,y), T P*   −1  is the inverse transformation of P*, and o i  is the point object from the current scan which matches the registered target t l . The measurement error covariance for the corresponding target t l  is updated as:
 
                     σ   l     2   ′       =         1     κ   l       ⁢       (       t   l     -       T     P   *       -   1       ⁡     (     o   i     )         )     2       +           κ   l     -   1       κ   l       ⁢     σ   l   2                 (   4   )               
Where o i  is the point object associated with t l . Also, as mentioned above, the hit count k l  is updated by incrementing k l =k l +1.
 
     The new vehicle pose P* from Equation (2) is the output from the analysis of point objects described above. 
     A second estimation of the new vehicle pose can then be performed based on the extended objects, which are indicated by bracket  120  in  FIG. 3 . The objective of the extended object calculation is to find the best transformation from an existing model M of extended targets (point clouds) to a current scan S of objects (concatenated clusters). The extended object calculation is iterative in nature because of the ambiguity in associating individual scan points s j  in S (represented by circles  122 ) to model points m k  in M (represented by stars  124 ). 
     The iteration is started by assuming an initial value of a pose P (0)  is equal to the new vehicle pose P* from Equation (2) of the analysis of point objects. If no point objects are available in the scan data, then P (0)  can be set equal to P′ from the vehicle dynamics data. The extended object vehicle pose calculation is then iteratively computed until convergence, as follows: 
                     P     (     n   +   1     )       =       argmin     P     (   n   )         ⁢       ∑     j   ,   k       ⁢       Â     j   ⁢           ⁢   k       ⁢                s   j     -       T     P     (   n   )         ⁡     (     m   k     )         σ          2                   (   5   )               
Where P (n+1)  is the next iterative value of the vehicle pose, s j  and m k  are extended object scan points and model points, respectively, T P (x) is the operator to apply a rigid transformation P to a point x, and the coefficient matrix Â jk , computed by an Expectation-Maximization (EM) algorithm, is the probability that s j  is a measurement of m k , and Σ k Â jk =1.
 
     In Equation (5), σ is a bandwidth parameter, which is a predefined positive constant. An implementation of Â jk  can be: 
                     Â     j   ⁢           ⁢   k       =     c   ⁢           ⁢     exp   ⁡     (     -                s   j     -     m   k       σ          2       )                 (   6   )               
Where c is the normalizing factor. After convergence or a certain prescribed number of iterations, an extended object-based transformation P″ is defined as P″=P (n+1)  from Equation (5).
 
     If a model point m k  is measured by a scan point (e.g., s j ), the hit count for m k  is increased by one. When a scan point s j  does not match with any model point, the scan point s j  is added to the model M and its hit count is set equal to one. When a model point m k  is not matched by any scan point in S, its hit count is decreased by one. Model points with hit count less than one are periodically deleted. 
     The extended object target model M can be modeled as a Gaussian Mixture Model (GMM) distribution:
 
 p ( x;M )=Σ k=1   n     M     p ( x|m   k )  (7)
 
where
 
                     p   ⁡     (     x   |     m   k       )       =       1       (     2   ⁢     πσ   2       )       3   2         ⁢     exp   ⁡     (     -              x   -     m   k            2       2   ⁢     σ   2           )                 (   8   )               
And xεR 2 , a point in 2-D Cartesian coordinate frame.
 
     In the above equations, the parameter m k  has a normal distribution, 
                     p   ⁡     (     m   k     )       =     N   ⁡     (       v   k     ,       σ   2       η   k         )               (   9   )               
Where ν k  is the mean,
 
               σ   2       η   k           
is the variance, and ν k  and η k  are parameters of the model M. The update rules for ν k  and η k  are:
 
                     v   k   ′     =           ρ   k     ⁢       s   _     k       +       η   k     ⁢       T     P   ″       ⁡     (     v   k     )               ρ   k     +     η   k                 (   10   )               
and
 
η′ k =η k +ρ k   (11)
 
Where ρ k =Σ j Â jk , and  s   k =Σ j Â jk s j /ρ k .
 
     To summarize the above discussion, a vehicle pose P* is computed from vehicle dynamics data and point objects in the scan data, and a vehicle pose P″ is computed from the vehicle pose P* and extended objects in the scan data. Different algorithms can be employed for using either the vehicle pose P* or the vehicle pose P″, or a combination of the two. For example, if point objects are dominant in the scan data, then the vehicle pose P* could be used as the updated vehicle position for trajectory tracking purposes. Conversely, if extended objects are dominant in the scan data, then the vehicle pose P″ could be used as the updated vehicle position for trajectory tracking purposes. As another alternative, a fusion calculation could be performed using both point and extended objects, as follows: 
                     P   **     =       argmin     P     (   n   )         (           ⁢         ∑   i     ⁢                o   i     -       T     P     (   n   )         ⁡     (     t     l   ⁡     (   i   )         )           σ     l   ⁡     (   i   )                2       +       ∑     j   ,   k       ⁢       Â     j   ⁢           ⁢   k       ⁢                s   j     -       T     P     (   n   )         ⁡     (     m   k     )         σ          2           )             (   12   )               
Where the first summation is for the point-based objects as shown in Equation (2) and the second summation is for the extended objects as shown in Equation (5). Equation (12) is solved iteratively because of the ambiguity in associating the extended object scan points s j  to model points m k , as discussed previously. After convergence or a certain prescribed number of iterations, an overall, fused, scanned-object-based transformation P** is yielded by Equation (12).
 
       FIG. 4  is a flowchart diagram  150  of a method for vehicle path planning and trajectory following with closed-loop control, using vehicle dynamics and object detection data as input. As described previously, the method of the flowchart diagram  150  is performed by algorithms in the vehicle pose estimation module  40 . At box  152 , new scan data is obtained from the object range data module  30 . At box  154 , host vehicle motion data is provided by the vehicle dynamics module  20 . Using the host vehicle motion data and the object scan data, the ground speed of scanned targets can be estimated at box  156 . At decision diamond  158 , it can be determined if moving objects are present in the scan data. If it is determined that only moving objects are present in the scan data, then the process loops back to the box  152  to receive the next time increment of scan data, as moving objects may not be useful for the trajectory tracking purposes discussed above. If moving and stationary objects are identified, the moving objects may be disregarded. 
     At decision diamond  160 , it is determined whether point objects, extended objects, or both are present in the scan data. At box  162 , point objects in the scan data are used to calculate the new vehicle pose P* using Equation (2). The point object calculations at the box  162  also use the vehicle dynamics data from the box  154 , as discussed in detail previously. At box  164 , extended objects in the scan data are used to calculate the new vehicle pose P″ using Equation (5). The extended object calculations at the box  164  also use the point-object-based pose estimation P* from the box  162 , as discussed in detail previously. 
     At box  166 , an overall vehicle pose estimation is determined from the scanned object data, using the pose estimations from the boxes  162  and  164  as input. As discussed above, the overall vehicle pose estimation may simply use the value calculated at the box  162  or  164  if point objects or extended objects are dominant in the scan data. Alternatively, a fusion calculation can be performed at the box  166 , using Equation (12). The overall vehicle pose estimation is output from the box  166 , where it is used by the vehicle control module  60  for vehicle trajectory control. The process then loops back to the box  152  to receive the next time increment of scan data. 
     Using the methods disclosed herein, closed-loop vehicle trajectory control can be implemented using object scan data as input. By eliminating the need for GPS data for vehicle trajectory tracking feedback, system cost can be reduced, and the issue of temporary unavailability of GPS signals is avoided. Furthermore, the ability to process both point objects and extended objects provides a robust solution, while still being implementable in real-time processors onboard the vehicle. 
     The foregoing discussion discloses and describes merely exemplary embodiments of the present invention. One skilled in the art will readily recognize from such discussion and from the accompanying drawings and claims that various changes, modifications and variations can be made therein without departing from the spirit and scope of the invention as defined in the following claims.

Technology Classification (CPC): 1