Patent Application
20170052261
The disclosure is generally related to navigation systems for land vehicles.
Inertial navigation supplements satellite navigation, especially when satellite signals are weak or obstructed. A global navigational satellite system (GNSS) receiver in a car or truck may have difficulty producing accurate position and velocity estimates near tall buildings or in tunnels, for example.
Many vehicle navigation systems receive sensor inputs from the vehicle in which they are installed. A common example is wheel rotation information sensed at a vehicle wheel and transmitted over a vehicle information bus. However, in some cases it is desirable for a navigation system to be “cordless”; i.e. not to have to rely on a connection to vehicle-supplied information.
One prior art solution (“Cordless inertial vehicle navigation”, U.S. Pat. No. 8,406,996) relies on inherent motion constraints of cars and trucks, and uses self-contained barometric altimeter measurements as an input to a navigation filter. That system ignores roll rates, lateral acceleration and vertical acceleration. Despite the success of that approach, further improvements are possible and often desirable for high performance cordless inertial navigation.
Cordless inertial vehicle navigation with elevation data input is a navigation solution appropriate for land vehicles, meaning vehicles such as automobiles and trucks that travel on the surface of the earth. The solution includes an elevation database that provides a source of vertical position measurements for a Kalman filter. This and other developments reduce or eliminate small error sources that have been found in prior art solutions through extensive testing.
In particular, three unexpected sources of small errors have been discovered in navigation systems such as those described in “Cordless inertial vehicle navigation”, U.S. Pat. No. 8,406,996. First, a no-roll assumption is not always valid for cars and trucks. Second, altitude measurements made in a vehicle cabin may not be reliable. Third, longitudinal accelerometer arm, L, can vary due to sideslip.
Although small, temporary roll rates can occur in cars and trucks due to their soft suspensions. When a roll rate occurs simultaneously with yaw, some of the roll may be observed as a pitch rate. This leads to pitch error and ultimately to positioning error. As a practical example, consider backing and turning out of a parking space. The small rolling motion that occurs when this maneuver is performed in a light cargo van, for example, has been observed to cause altitude errors as large as two meters in an inertial navigation system.
Micro-electromechanical systems (MEMS) pressure altimeters are reasonably accurate sources of barometric data, but their pressure estimates are only as useful as what might be called the “fidelity of the pressure sampling environment.” Ideally pressure is sampled from a static port with an opening parallel to significant airflow. In the worst case, the opening is pointed into the airflow and is greatly affected by dynamic pressure of air caused by motion of a vehicle. Open windows or even operation of an air conditioner fan can cause pressure changes in the cabin of a vehicle that are unrelated to elevation changes. The problem is worse at high speeds and can lead to altitude errors of ten meters or more.
When only longitudinal acceleration is measured, and lateral and vertical acceleration are ignored, one must take into account the distance of a longitudinal accelerometer ahead or behind a vehicle's pivot point. This “accelerometer arm”, L, affects longitudinal acceleration measurements during turns. The prior art describes a way to compensate for non-zero accelerometer arm but assumes that L is constant. In reality, effective accelerometer arm can change when entering or leaving a turn because of sideslip. In car racing parlance, when a vehicle is “loose” (yaw rate is greater than expected given steerable wheel angle and speed; rear wheels skid; over steer), then effective L is less than actual L when entering a turn. The effect is opposite in a vehicle that is “tight” (front wheels skid; understeer). This kind of error can lead to two or three meters lateral positioning error when entering a turn at highway speeds.
The three error sources just described are eliminated when elevation data input is used with cordless inertial navigation. Inertial navigation depends on accurate rotation rate and acceleration measurements. In particular, rate gyroscopes are used to keep track of the orientation of vertical; i.e. the direction of gravity or the direction to the center of the earth. Since gravitational acceleration (1 g at the surface of the earth, by definition) is usually the largest magnitude sustained acceleration of a land vehicle, small errors in an estimate of vertical become significant when accelerations are integrated once to estimate velocity and again to estimate position. Vehicle attitude errors lead to acceleration (and consequently position) errors, but an independent source of elevation data can be used to limit them.
An elevation database provides terrain height U (above sea level or some other suitable datum) as a function of position (E, N) on the earth's surface; one may write U=h(E, N) where E, N, U are east, north, up dimensions of a local coordinate system. As explained below, for purposes of a Kalman filter, obtaining U from an elevation database is considered to be a measurement on the database.
The structure and operation of a cordless inertial vehicle navigation system with elevation data input are now described in conjunction with the figures.
In
Navigation filter 105 includes a microprocessor and associated memory and input/output facilities. The navigation filter implements a Kalman filter in firmware to synthesize an optimal estimate of vehicle state based on available measurements and a state dynamics model. GNSS receiver 120 may receive signals from satellites of the NAVSTAR global positioning system (GPS), Galileo, Glonass, Beidou and/or other global or regional navigational satellite systems. Rate gyroscopes 125 and accelerometers 130 are preferably MEMS devices having three orthogonal sensing axes each.
Elevation database 135 is an electronic store of elevation information; i.e. height tabulated versus lateral position. As an example, an elevation database may contain road surface height above sea level for a range of east and north coordinates. When an elevation database is queried with a lateral position (E, N), it returns elevation U at that position. A road database returns road height, for example. Only in the last few years have elevation databases become available that have sufficient accuracy and precision to be useful for providing U measurements to a Kalman filter for land vehicle navigation. An example of state-of-the-art elevation data, with accuracy better than about one meter, is HERE HD Map data which provides lane-level precision on roads and highways. (“HERE” is the name of a mapping company.) As discussed further below, an elevation database may also provide tilt (θ, φ)) in addition to height (U) data.
In a system such as that of
The details of a Kalman filter appropriate to cordless inertial vehicle navigation with elevation data input are now described in conjunction with Tables 1-6:
= (e, n, u)
=
+
= 0 +
The state variables for inertial navigation in the Kalman filter presented in Tables 1-6 include position, velocity and attitude of a GNSS antenna on a vehicle, and sensor biases. Vehicle attitude (i.e. yaw, pitch, roll) is represented by a direction cosine matrix, TB2L, that represents the orientation of a vehicle body reference frame with respect to a local (east, north, up) coordinate system. Other representations such as Euler angles, Tait-Bryan angles or quaternions are suitable alternatives to the direction cosine matrix. The time rate of change of the direction cosine matrix is proportional to a Tait-Bryan rotation matrix, ΩB*, that includes rate gyroscope measurements, biases and noise:
The measurement in the Kalman filter presented above is a measurement of vehicle GNSS antenna height. The measurement equation, zh*=u−h(e,n), expresses the idea that the height of the antenna above ground is the antenna's u coordinate minus the ground elevation. For example, if the antenna u coordinate is 10 meters and the ground elevation is 8 meters, then the antenna height above the ground is 2 meters. Ground elevation is provided by an elevation database, h(e,n), that returns u given lateral position (e, n).
The measured GNSS antenna height zh* is a constant because a land vehicle is assumed not to leave the ground. Intuitively one may appreciate that the database input places a bound on the Kalman filter's estimate for u. This in turn limits the effects of errors in estimates of the orientation of vertical that come from rate gyroscope drift.
The model presented above doesn't include second order effects due to east and north coordinates of a land vehicle antenna being slightly different from (e, n) “under” the antenna when the ground is not level; however, these effects may be accounted for by considering direction cosines between a vehicle's yaw axis and the up unit vector in the local (e, n, u) reference frame. The rotation of the earth, which is less than 0.1 mrad/s, is also not included in the model.
The estimate of the measurement, f, is defined by:
f=û−h(ê,{circumflex over (n)})
The Kalman filter innovation, or measurement minus estimate of the measurement, is:
D, a constant, is the design antenna height on a vehicle, typically 1 to 2 meters. In the equations above, {circumflex over (x)} is the estimated position vector (ê, {circumflex over (n)}, û) and H is the Kalman observation matrix (a vector in this case),
vh is the noise associated with measurements on the elevation database. If pitch and bank information can't be determined from a database, then one may assume
When GNSS data is available, state variables for position become e(GNSS), n(GNSS) and u(GNSS) where the (e, n, u) coordinate system (a linearized approximation around a latitude, longitude, altitude) is useful, but not an essential choice. If GNSS pseudorange and Doppler measurements are used instead of computed GNSS fixes, then GNSS clock bias and frequency are included in Kalman filter state equations as presented in Table 6.
Some elevation databases may provide additional information beyond basic elevation data.
Tilt angles supplied by an elevation database (or computed from elevation data) reveal the sensitivity of elevation measurements to horizontal displacement. If a section of terrain is level, or nearly so, then h(e, n)≅h(e+δ, n+ε) where δ and δ are small displacements. On the other hand if the section of terrain is tilted, then tilt angles
may be included in a Kalman filter to more accurately compute state variable covariances.
A common situation is a database from which pitch information in the direction of travel along a road (“along track”) is provided (or can be computed), but no road bank information is available. In the absence of road bank information, vehicle roll may be averaged over a minute or two to find roll rate gyroscope bias. This procedure is based on the assumption that over long periods of time, the average roll angle of a land vehicle is zero; i.e. on average, it is not rotated away from level about the roll axis.
Map matching, e.g. “snapping” GPS position to the nearest plausible roadway in a database, may be used in conjunction with the techniques described above to further bound lateral position estimates.
The above description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the principles defined herein may be applied to other embodiments without departing from the scope of the disclosure. Thus, the disclosure is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
