Embodiments of the invention generally relate to global positioning system (GPS) based vehicle location systems and, more particularly to an augmented GPS based vehicle location system configured to provide vehicle position estimates in GPS dead zones.
Conventional vehicle location systems include GPS to locate the position of the vehicle on the surface of the earth. Such systems generally include an antenna and a receiver for receiving signals from GPS satellites and determining a location of the vehicle based on the signals.
The ability of the GPS to determine a solution for the location of the vehicle is dependent upon an unobstructed line of sight between the antenna and multiple GPS satellites. Unfortunately, in many roadway environments, short term (less than 200 meter) GPS dead zones prevent GPS position solutions. GPS dead zones occur where the antenna is obstructed from receiving the satellite signals, such as under bridges, on roads having tree canopies, roads through urban canyons, and other locations where line-of-sight view from the satellite to the antenna is obstructed. Degraded GPS solutions, ranging from no solution to solution qualities inferior to fixed integer carrier phase solutions, can last from a few seconds to minutes.
Embodiments of the invention are directed to systems and methods for providing vehicle position estimates in GPS dead zones. One embodiment of the system comprises a mobile vehicle, a global positioning system (GPS) based vehicle position and heading system, at least one two-dimensional (2D) velocity sensor, a yaw rate system, and a vehicle position and heading estimator. The GPS based vehicle position and heading system is supported on the vehicle and measures global easting and global northing (measured position) of the vehicle, and determines a heading (measured heading) of the vehicle. The 2D velocity sensor measures the velocity of the vehicle with respect to the ground, over which the vehicle travels, in two orthogonal directions (measured velocity). The yaw rate system is supported on the vehicle and measures a yaw rate of the vehicle (yaw rate measurement). The vehicle position and heading estimator comprises at least one processor that computes a position of the vehicle (estimated position) and a heading of the vehicle (estimated heading) based on the measured position, the measured heading, the measured velocity and the yaw rate measurement.
In one embodiment of the method, a mobile vehicle is moved. Global positioning system (GPS) based measurements are then performed at a first frequency using at least one GPS receiver and at least one antenna supported on the vehicle including measuring a position of the vehicle (measured position) and a heading of the vehicle (measured heading). Between successive GPS based measurements, a position of the vehicle is estimated based on the measured position, the measured heading, a two-dimensional velocity measurement of the vehicle and a yaw rate measurement of the vehicle, using at least one processor supported on the vehicle.
Other features and benefits that characterize embodiments of the present invention will be apparent upon reading the following detailed description and review of the associated drawings.
Embodiments of the invention relate to an augmented vehicle location system that calculates real-time, high accuracy (e.g., centimeter level) estimates of a global vehicle position by fusing GPS position measurements, vehicle heading measurements, yaw rate measurements, and/or two-dimensional velocity measurements. In one embodiment, the augmented vehicle location system estimates a heading of the vehicle based upon one or more of these measurements. The augmented vehicle location system is useful in providing vehicle position estimates in environments where spatially periodic GPS dead zones exist.
In one embodiment, the system 100 provides vehicle position information 104 to a vehicle position dependent system 106. The system 106 may be in the form of a navigational system, a vehicle automation system, a lane departure warning system, a crash avoidance system, a mobility assist device (see, for example, U.S. Pat. No. 6,977,630) or other vehicle position dependent system that may benefit from the position information 104 generated by the augmented vehicle location system 100.
The position information 104 generated by the augmented vehicle location system 100 may also be provided to GPS receivers to accelerate the convergence of a vehicle position solution after a loss of satellite lock (i.e., the passage through a dead zone). With a typical GPS outage duration of fifteen seconds, the system 100 can provide accurate position estimates that reduce the size of the error sphere associated with the seed of the convergence algorithm. By minimizing the size of the initial search space, a more rapid convergence can be achieved. Thus, in one embodiment, the vehicle position dependent system 106 represents one or more GPS receivers, which may include those used by the system 100.
Embodiments of the system 100 comprise a GPS based vehicle position and heading system 108, at least one two-dimensional (2D) velocity sensor 130, and a yaw rate system 132. The GPS based vehicle position and heading system 108 measures global easting and global northing (hereinafter “measured position”) of the mobile vehicle 102, and determines a measured heading of the vehicle 102. The 2D velocity sensor 130 measures the velocity of the vehicle 102 relative to the ground in two orthogonal directions. The yaw rate system measures a yaw rate of the vehicle 102.
One embodiment of the GPS based vehicle position and heading system 108 includes an antenna 110 and a GPS receiver or unit 112. In one embodiment, the antenna 110 is mounted at a point of interest 115 (
As mentioned above, the GPS based vehicle position and heading system 108 is supported on the mobile vehicle 102 and determines the measured heading Ψ of the vehicle 102 (
In one embodiment, the antennas 118 and 120 are each L1-frequency antennas, which are supported on the vehicle 102, as illustrated in the simplified top view provided in
In one embodiment, the heading calculator 124 represents one or more processors, memory, program instructions and other components that may be used to determine, using conventional techniques, the measured heading based on GPS signals received by the GPS receiver 122 through the antennas 118 and 120. In one embodiment, the GPS receiver 122 determines the measured heading by computing the arctangent of the vector between the position solution for antenna 118 and the position solution for antenna 120. One suitable heading calculator 124 is the Hemisphere Crescent Vector GPS receiver, which can provide both the measured position and heading.
In accordance with another embodiment, the system 108 includes antennas 110 and 118 and the GPS receivers 112 and 122, which may be components of a dual head receiver. The measured heading of the vehicle 102 is determine based on the signals received by the GPS receivers 112 and 122 through the antennas 110 and 118, respectively, in accordance with conventional techniques. Such techniques may involve, for example, the comparison of phase and timing information in addition to the calculation of the arctangent of the vector between the position solution for antenna 110 and the position solution for antenna 118.
In yet another embodiment, the heading calculator 124 of the system 108 calculates the heading of the vehicle 102 (i.e., the measured heading) using the measured position using the antenna 110 and the GPS receiver 112. The heading calculator 124 either includes a processor or utilizes another processor of the system 100 to calculate the measured heading using a “back-looking” propagation algorithm. The back propagation technique uses previously determined measured positions using the GPS receiver 112 to accurately determine the heading angle.
Consider the trajectory shown in the simplified diagram of
where “Path Length” is the sum of the length of the trajectory line segments from time tn-k to tn.
The heading estimate error computed at time tn is applied to the heading estimate at time tn−(k+1) to produce the optimal estimate of heading at time tn-k. Using the optimal estimate of heading at time tn-k as a new initial condition, the system 108 uses 2D-velocity and yaw rate measurements from the sensor 130 and the system 132 from time tn-k to tn to propagate forward in time to produce the optimal estimate of the heading of the vehicle 102 at time tn.
At each time step tk, this process is repeated, thereby providing a continuous stream of accurate measured heading estimates without the need for a separate specific, GPS-based heading estimator.
The 2D velocity sensor 130 is supported on the mobile vehicle 102 and measures the velocity of the vehicle 102 relative to the ground in two orthogonal directions (hereinafter “measured velocity”). As used herein, the sensor 130 represents one or more velocity sensors or other components that are used to obtain the velocity of the vehicle 102 relative to the ground in two orthogonal directions. This may be accomplished by measuring the velocity of the ground relative to the vehicle 102 and/or measuring the velocity of objects to the side of the vehicle 102, such as a guardrail, a wall, and/or an embankment, for example. One exemplary 2D velocity sensor 130 that is suitable for determining the measured velocity of the vehicle 102 is the Correvit S-350 Aqua Two-Dimensional Velocity Sensor.
In one embodiment, the velocity sensor 130 is mounted such that its coordinate frame is aligned with the local vehicle coordinate frame. In the event that the coordinate frame of the velocity sensor 130 is not aligned with a local vehicle frame, the measured velocity can be translated to the desired local coordinate frame using conventional techniques. In one embodiment, the sensor 130 is mounted at the front 136 of the vehicle 102.
The yaw rate system 132 that is supported on the vehicle 102 and measures a yaw rate Ψ of the vehicle (hereinafter “yaw rate measurement”), which is the rate of angular movement of the vehicle 102 about the z-axis (not shown), which is orthogonal to the x- and y-axes of the local coordinate frame (
In accordance with another embodiment, the yaw rate system 132 comprises at least two 2D velocity sensors 138A and 138B supported on the mobile vehicle 102, as illustrated in the simplified diagram provided in
Consider the kinematics of a solid body translating and rotating on a plane, as shown in
{right arrow over (V)}
b
={right arrow over (V)}
a
+{right arrow over (r)}
b/a×ω (Eq. 1)
With {right arrow over (r)}b/a known (the vehicle manufacturer knows where the sensors are located on the vehicle), Equation 1 is solved to determine {right arrow over (ω)}, the yaw rate of the vehicle.
In one embodiment, the yaw rate system 132 includes an array of more than two 2D sensors 138, as shown in
It is understood that the exemplary components described above that are used to determine the measured heading of the vehicle 102, the measured position of the vehicle 102, the measured velocity of the vehicle 102 and the yaw rate measurement of the vehicle 102, may be substituted with other components that are capable of providing the desired measurements. Embodiments of the invention include the use of such substituted components. These other components may be capable of handling a combination of the desired measurements. For instance, an integrated digital GPS and inertial measurement unit, such as the Novatel UIMU-HG utilizing their Synchronous Position, Attitude and Navigation (SPAN) technology, may be used to determine the measured position, measured heading and yaw rate of the vehicle 102. Thus, while the diagram of
Additionally, more accurate technologies may be utilized as they are developed to obtain the desired measurements. For instance, embodiments of the invention may make use of current and future GPS technologies such as L1, L2 and L5 technologies, to provide the desired measured position accuracy. Thus, for example, the L5 technology may be used to provide high accuracy position measurements (˜10-30 cm) without the need for differential GPS corrections.
In one embodiment, the system 100 includes a vehicle position and heading estimator 140, which comprises at least one processor 142. In one embodiment, the vehicle position and heading estimator 140 includes a memory 144. In one embodiment, the memory 144 includes program instructions that are executable by the processor 142 to process data and perform method steps described herein.
The general convention used herein is to cap a measurement variable with a tilde (˜) and an estimated value with a hat (̂). If the value is not capped it denotes the true value. For example, XG is the global easting position in state plane coordinates, so {tilde over (X)}G is the measured value from GPS and {circumflex over (X)}G is the estimated value of global easting from the estimator 140. The following will be the variable notation used herein.
1. Coordinate Frames
2. States
3. Measurements
4. Inputs
In accordance with one embodiment, as the vehicle 102 is moved, the GPS based vehicle position and heading system 108 determines the measured position ({tilde over (X)}G, {tilde over (Y)}G) of the vehicle 102 and the measured heading ({tilde over (Ψ)}) of the vehicle at a first frequency (e.g., 10 Hz) based on GPS measurements (i.e., processing of GPS satellite signals). Between successive GPS based measurements using the system 108, the vehicle position and heading estimator 140 computes an estimated position ({circumflex over (X)}G, ŶG) of the vehicle 102 and an estimated heading ({circumflex over (Ψ)}) of the vehicle 102 based on the measured position and heading from the GPS based position and heading system 108, the measured velocity ({dot over ({tilde over (x)}, {dot over ({tilde over (y)}) from the 2D velocity sensor 130, and the yaw rate measurement ({dot over ({tilde over (Ψ)}) from the yaw rate system 132, using the processor 142. Details of the various measurements and the processing steps used to calculate the estimated vehicle position are provided below.
The estimator 140 can generally be split into three separate parts; a heuristic filter 150, a linear Kalman filter 152, and a position propagator 154, which are illustrated in the simplified block diagram of
In between GPS based vehicle position measurement updates by the system 108, the position estimate is propagated from the yaw rate system 132 measurement and the 2D velocity sensor 130 measurements at a second frequency (e.g., 100 Hz) by the position propagator 154. This process is generally depicted in
The position state equations are used by the position propagator 154 to propagate the state estimates XG,k and YG,k between GPS measurements of the system 108. The other states, Ψk and {dot over (Ψ)}b,k, are updated by the linear Kalman filter 152. The state matrix for the position propagator 154 is defined as,
Equations 3 and 4 are the derivation of the state equations for the system.
Here {circumflex over (x)}p,k+1 is the state matrix estimate at time step k+1, Δtk is the difference in time between time steps k and k+1, and {dot over ({circumflex over (x)} is the rate of the change of the estimated state matrix with respect to time.
The 2D velocity sensor 130 measures its velocity vector
at its location on the vehicle. Note that the speed in the z dimension is not measured by the 2D speed sensor, thus it is shown as zero. Equation 5 translates the measured velocity at the velocity sensor 130, {right arrow over ({tilde over (V)}2D, to the velocity at the local vehicle coordinate frame at time step k, {right arrow over ({tilde over (V)}v,k,
where
and rx and ry are shown in
Moreover, {dot over ({circumflex over (Ψ)}k={dot over ({tilde over (Ψ)}k−{dot over ({circumflex over (Ψ)}b
To transform the local vehicle velocity, {right arrow over (V)}v,k, to the global state plane velocity, we must pre-multiply {right arrow over (V)}v,k by the rotation matrix,
Putting the state equations into discrete matrix form leads to Equation 8.
The Kalman filter 152 is responsible for producing a stochastically optimal estimate of the vehicle heading, {circumflex over (Ψ)}k, and the yaw rate bias, {dot over ({circumflex over (Ψ)}k. The state matrix of the linear Kalman filter 152 is provided in Equation 9.
The discrete system model is of the form,
x
KF,k+1=Φ
k
x
KF,k
+Γu
k
+Yq
k, wk˜N(0,Qk) (Eq. 10)
{tilde over (y)}
k
=Hx
KF,k
+v
k
, v
k
˜N(0,Rk) (Eq. 11)
where wk and vk are the input noise and measurement noise respectively; wk and vk are modeled by zero-mean Gaussian distributions. The input noise error covariance, Qk, is related to the error characteristics of the yaw rotational rate measurement from the inertial measurement unit of the yaw rate system 132. For the Crossbow Inertial Measurement Unit measuring vehicle yaw rate, the value Qk was determined to be 0.0045 rad2/s2. Other values may be used depending on the inertial measurement unit. The observation noise, Rk, is related to the quality of the GPS measurements, and is determined by the heuristic filter 150, as explained below.
The system model for the linear Kalman filter 152 is provided in Equations 12 and 13.
The state estimate {circumflex over (x)}KF, and the state error covariance, Pk, are both propagated when a new yaw rate measurement is available (e.g., 100 Hz) and a measurement update is performed when a GPS measurement is available (e.g., 10 Hz). A summary of the state and state error covariance propagation, gain computation, and measurement update is shown in Table 1.
The heuristic filter block 150, shown in
to update the position estimates,
using Equation 19.
The position update gain and vehicle heading error covariance are selected based on the following metrics:
In one embodiment, the position update gain is selected based upon the front GPS quality metric. In one embodiment, if the front GPS quality metric is not “fix”, the position update gain is set to zero. If the front GPS quality metric is “fix”, the position update gain is selected based on the difference between the GPS position measurement and the position estimate dk.
Although the present invention has been described with reference to preferred embodiments, workers skilled in the art will recognize that changes may be made in form and detail without departing from the spirit and scope of the invention. It is understood that embodiments of the invention are directed to real-world applications, as opposed to a simulator or virtual world environments. That is, embodiments of the invention are for use on a mobile vehicle traveling over the surface of the earth. Additionally, it is understood that embodiments of the invention include the performance of the method steps and function blocks described herein using a processor through the execution of instructions stored in memory in the form of a tangible data storage medium.
The present application is based on and claims the benefit of U.S. provisional patent application Ser. No. 61/289,757, filed Dec. 23, 2009 and U.S. provisional patent application Ser. No. 61/297,111, filed Jan. 21, 2010, the content of each provisional application, is hereby incorporated by reference in its entirety.
Number | Date | Country | |
---|---|---|---|
61289757 | Dec 2009 | US | |
61297111 | Jan 2010 | US |