System to determine the path of a vehicle

The present invention provides a system and method of selecting a most likely path of a vehicle from a list of candidate paths. If only one candidate path exists, that path is identified as the most likely path. For multiple candidate paths, cost functions determine the weight of various parameters associated with each candidate. The parameters may include lane information, the vehicle speed, the vehicle travel direction, the lateral speed of the vehicle, the state of various signals, such as the turn signals. From the various weights determined by the cost functions, the candidate path with the highest confidence level is determined to be the most likely path.

BACKGROUND

The present invention generally relates to a system to determine the path of a vehicle.

Increasingly, navigation systems have been installed in vehicles that provide guidance to the driver of the vehicle. Based on a map database and GPS, the navigation system informs the driver about the position of the vehicle on a particular route. Because the map database includes the road shape of the route, the system can inform the driver of upcoming curves along the roadway and provide warnings about the curves. However, the aforementioned systems do not provide any real threat assessment of the upcoming curves.

SUMMARY

In overcoming the above mentioned drawbacks and other limitations of the related art, the present invention provides a system and method of selecting a most likely path of a vehicle from a list of candidate paths. If only one candidate path exists, that path is identified as the most likely path. For multiple candidate paths, cost functions determine the weight of various parameters associated with each candidate. The parameters may include lane information, the vehicle speed, the vehicle travel direction, the lateral speed of the vehicle, the state of various signals, such as the turn signals. From the various weights determined by the cost functions, the candidate path with the highest confidence level is determined to be the most likely path. This information may be transmitted to a curve speed warning module of the system to alert the driver that the vehicle exceeds a safe speed for an upcoming curve in the road.

Further features and advantages of this invention will become apparent from the following description, and from the claims.

DETAILED DESCRIPTION

Referring now to the drawings, a system embodying the principles of the present invention is illustrated therein and designated at10. As its primary components, the system10includes a global positioning system (GPS) and inertial navigation system (INS) integration module12, a vehicle positioning module14, a map matching module16, a look ahead module18, and a curve speed warning module20. The system10is also provided with an inertial navigation system22, a GPS receiver24, a map database26, and yaw rate and vehicle speed sensors. The map database26includes a map data compiler28and an ADAS data access30that receives information from an ADAS data base32. The map data complier28also receives information from an SDAL database34. The map database may be a database that is commercially available.

The GPS receiver24receives satellite information36related to the vehicle GPS position. In the GPS/INS integration module12, the GPS position is augmented using, for example, a Kalman filter, with the yaw rate38and the vehicle speed40obtained through the inertial navigation system22. The information from the GPS/INS integration module12is provided to the vehicle positioning module14, where the vehicle position is calculated in a global coordinate system.

The map matching module16, implemented with a map matching algorithm, receives the hardware position estimate from the vehicle positioning module14and information from the map database26to calculate the vehicle position on the map. The look ahead module18then receives the map position information from the map matching module16, as well as information from the vehicle positioning module14and the map database26, and looks ahead in the map from the calculated map position and calculates the candidate list of probable intended driving paths, in particular, a most likely path (MLP) based on probabilities.

Once the MLP is determined, a curvature calculation algorithm residing, for example, in the look ahead module18, evaluates the most likely path to determine the curvature values, which, along with the vehicle speed from the vehicle positioning module14, are passed to the curve speed warning module20. A threat assessment algorithm implemented in the curve speed warning module20assesses the threat to the vehicle and makes a warning decision44.

The threat assessment algorithm in the curve speed warning module20evaluates the curvature values from the look ahead module18to assess the potential threat of the calculated curvature of the road based on a cost function that takes into consideration, for example, the vehicle speed, the estimated projected speed profile, the travel distance to the curvature point, the curvature, the estimated road conditions, and the driver reaction time. The estimated road conditions may be calculated from the vehicle's signals, such as brake signals, turn signals, ambient temperature, and wiper functions. The curve speed warning system20then initiates a warning level based on the calculated threat level.

The look ahead module18determines the most probable path of the vehicle using, for example, information from vehicle positioning, lane information, lateral velocity, and vehicle signals, such as turn signals, and state. The most probable path and other possible alternate paths can be predicted using the vehicle's travel direction, the direction of the road, the vehicle lane, and the predicted directional change. This information is evaluated using a cost function to weight each parameter with respect to the consideration that the parameter will have toward predicting the vehicle's most probable path.

The long ahead module18also uses the look ahead distance to assemble a candidate path subset that is projected out to a selected distance from the vehicle's current position. If only one possible candidate path exists, it will be returned with 100% confidence. Otherwise, a list of all possible candidate paths (and their associated confidence levels) within the look ahead distance will be calculated to determine the candidate path with the highest confidence level, that is, the MLP.

Provided with candidate list of paths, cost functions assign a total weight for each candidate. The candidate with the higher weight is selected as the MLP. Depending upon the application, different cost functions are used for various scenarios. For each road scenario, there are associated aided signals, or parameters, such as the vehicle lateral velocity, lateral position, turn signal, boundary types, position of the accelerator pedal, and the deceleration of the vehicle. Moreover, each aided signal has a precedence level, such that for each scenario the cost functions are calculated in ascending precedence beginning with precedence1. If a particular cost function determines a weight greater than about 0.5, then the system10does not employ the cost function of the next precedence.

For example, a scenario is shown inFIG. 2in which a vehicle100is moving along highway102with an exit ramp104. Here, the lateral velocity vLatand the lateral position dLathave a precedence level1. In this situation, the look ahead module18detects a lane change by the vehicle100toward the upcoming exit ramp104. To achieve this, the lateral velocity is defined as
vLat≡{dot over (d)}Lat≅v*Ψ
where {dot over (d)}Latis the rate of change of the lateral position, v is the vehicle speed, and Ψ is the heading angle of the vehicle with respect to the road.

The behavior of the heading angle Ψ during a lane change is shown inFIG. 3. The lane keeping behavior produces a heading angle Ψ in the range of, for example, ±0.5 degrees. Therefore, this angle can be used as a factor in the ramp cost function weight (RM_Weight) when its absolute value exceeds, for example, 0.5 degrees. However, this factor may not be able to detect a very slow lane change. To overcome this, the cost function fuses the lateral velocity measurement with the lateral distance position as shown inFIG. 4, where

Ψ⁡(deg)=vLat⁡(m⁢/⁢s)v⁡(m⁢/⁢s)*180π,
if the lateral distance is measured in meters (m) and the heading angle is determined in degrees. Specifically, the absolute values of Ψ and dLatare determined in modules106and108, respectively. Then in module110, a predefined value, such as 0.5 degrees, is subtracted from the absolute value of the heading angle. Modified values of the lateral position dLmodand the heading angle Ψmodare determined in respective limiter modules111and112, so that regardless of the input values to the modules110and112, the outputs from these modules will be within the defined minimum (min) and maximum (max) values. The RM_Weight is then calculated according to a predefined expression as shown in a module114.

Information about the boundary type is assigned a precedence level3. If the boundary type of interest is solid, or if both boundary types are solid, the RM_Weight is determined as a function of the time to reach (TTR) the ramp104(FIG. 2) as shown inFIG. 5. That is, for a TTR less than 4, the RM_Weight is 1, for TTR equal to or greater than 4, the RM_Weight decreases linearly from 1 to zero when the TTR equals 12.

The time the turn signal is on is assigned a precedence level2. In particular, as shown inFIG. 6, a resetable timer module120determines the length of time the turn signal is on. This information is forwarded to a limiter block122which provides a modified Tsmodto the calculation module124which in turn calculates the Rm_Weight using the expression associated with that module. Thus, in sum, the weight of the turn signal is a function of the time the turn signal is on. If this time is, for example, 5 seconds, the weight of the signal reaches its maximum value of 1.

Turning now toFIG. 7, the deceleration or acceleration of the vehicle is assigned a precedence level4. When the driver's foot is on the accelerator pedal, the driver is either trying to maintain the current vehicle speed or accelerate the vehicle. To decelerate, the driver's foot is taken off the accelerator pedal. The gas pedal status is expressed in the accelerator pedal percentage module130. In order to distinguish between the deceleration as a result of a vehicle decelerating in front of the host vehicle and the deceleration associated with an upcoming ramp, the weight of the accelerator pedal is proportional to the magnitude of the vehicle deceleration. The deceleration signal is estimated by the algorithm implemented in the CSW module20and is forwarded to the look ahead module18. Thus, in the present implementation, the module130generates a cost function weight RM_Weight of 1 when the accelerator pedal percentage is less than about 5%. If it is above 20%, the weight is zero. However, this weight is modified by a module132associated with the vehicle acceleration, that is, the values of the modules130and132are multiplied together in a module134. In this implementation, if the vehicle acceleration is less than about 1.7 m/sec2, the weight from the module130is multiplied by zero, such that the final RM_Weight is zero. If, however, the acceleration is more than the value of the defined function (−0.015*v+0.17), where v is the vehicle speed in m/s, the weight from the module130is multiplied by one, such that the final weight RM_Weight is 1. Note that this RM_Weight has a value if the deceleration occurs while the vehicle is on a lane that has a solid boundary type or a missing boundary (or both boundary types are solid). Therefore, this ramp weight is multiplied by the weight function shown inFIG. 5.

Scenarios involving forked roads, such as those identified at140inFIG. 8, are quite similar to those involving exit ramps (FIG. 2), such that the same cost functions in ascending precedence can be used. Note that when the upcoming curved road segments are similar, that is, the segments have about the same curvature and about the same posted advisory speeds, the deceleration-aided signal (FIG. 7) is not employed.

Another scenario, as shown inFIG. 9, involves deciding whether the vehicle100is on a highway150or on a service drive152using aided signals such as the posted/advisory speed, the vehicle speed, and road boundary types. This aids in locating the vehicle on the appropriate road, and therefore to extract the correct candidate set. If a posted/advisor speed Vp/ADexits for the candidate road exists in the map database26, the cost function shown inFIG. 10is employed and assigned a precedence level2. This cost function can be employed in any situation to help locate the vehicle on the correct road.

Specifically, the absolute value of the difference (as determined in158) of the vehicle speed v and the Vp/ADis determined in a module160. This difference vdiffis forward to a limiter162to calculate a modified difference vdif modwhich in turn is transmitted to a calculation block164to determine the service drive cost function weight (SD_Weight) employed. Thus, if the absolute difference between the vehicle speed and the posted advisory speed is greater than 7 m/sec, the weight of the road candidate is zero.

If a posted/advisory speed is not available, the cost function shown inFIG. 11is employed, where it is assumed that the average vehicle speed on a highway is greater than on a service drive. In this case, if the vehicle speed is less than 20 m/s, the weight is one. As the speed increases from about 20 m/s to a bout 30 m/s, the weight decreases from one to zero.

If the left and right boundaries are solid, it is probable that the vehicle path is a service drive. If the boundaries are dashed, it is probable that the vehicle path is a highway. Therefore, the cost function shown inFIG. 12is employed, such that, if the vehicle is on a service drive, the weight is one, and if the vehicle is on a highway, the weight is zero. If one boundary is dashed and the other is solid, the weight function is 0.5.

Moreover, an estimate of the instantaneous curvature can be used to modify the confidence in the current selected road and help in predicting the future vehicle position for a limited distance ahead. This estimate is performed in the algorithm implemented in the CSW module20and passed to the look ahead module18. This curvature estimate is compared with the instantaneous curvature coming from the map database. The difference between the two curvature values contributes to the confidence in the current selected road. The weight of the cost function varies inversely with this difference.

In a preferred implementation, the system10initially determines if the vehicles is on a highway or a serviceway by determining the weight according to the cost function shown inFIG. 12. If the weight is less than about 0.5, the system10employs either the cost function shown inFIG. 10if the advisory speed is posted or inFIG. 11if the advisory speed is not posted. If either of these functions determines a weight greater than about 0.5, than the vehicle is likely on a service road. Otherwise, the vehicle is driving along a highway.

After the system10determines the type of road on which the vehicle is moving, the system employs the cost functions in ascending precedence shown inFIG. 4,FIG. 6,FIG. 5, andFIG. 7. As each cost function is employed, if that cost function determines a weight greater than about 0.5, then the system10assumes that the vehicle is likely approaching an exit ramp to determine the most likely path, and thus there is no need to proceed to the next cost function. If the weight is less than about 0.5, then the system10proceeds to use the next cost function. Note again, that if the system10uses the cost function shown inFIG. 7, the weight calculated from that cost function is multiplied by the weight calculated from the cost function ofFIG. 5. If the cost functions ofFIG. 4,FIG. 6,FIG. 5, andFIG. 7all determine that the respective weight is less than about 0.5, than the system10assumes the vehicle is not approaching an exit ramp.