Ground Vehicle Navigation Systems

Abstract

To provide an improved terrain referenced navigation system.SOLUTION: A ground vehicle navigation system comprising: an inertial navigation system arranged to output an orientation estimate of the ground vehicle and a first position estimate of the ground vehicle; a terrain map comprising terrain data; a terrain gradient based navigation unit arranged to output a second position estimate of the ground vehicle based on a comparison between the orientation estimate and terrain gradient data extracted from the terrain map; and an iterative algorithm unit arranged to determine a system error state in each iteration. In each iteration the iterative algorithm unit is arranged to: receive the first position estimate and the second position estimate; and update the system error state for the next iteration based on the system error state, the first position estimate and the second position estimate.SELECTED DRAWING: Figure 1

Description

[Technical field]

[0001] FIELD OF THE DISCLOSURE The present disclosure relates to the field of navigation systems, particularly for use with ground vehicles. [Background technology]

[0002] Terrain Referenced Navigation (TRN) systems are utilized on many airborne platforms and integrate aircraft navigation data, radar altimeter data, and accumulated terrain altitude data to generate a navigation solution. TRN systems are often used in conjunction with Inertial Navigation Systems (INS), which use inertial measurement sensors (e.g., gyroscopes, accelerometers, etc.) to provide a navigation solution. Such systems are sometimes referred to as Inertial Reference Units (IRUs). Navigation solutions often use a Kalman filter to calibrate the errors of the INS as a means of integrating the navigation solutions provided by the TRN and INS.

[0003] Position estimates obtained from inertial navigation systems typically fluctuate at a rate of about 2 nautical miles per hour due to the accumulation of errors caused by accelerometers and gyroscopes over time. Navigation systems can correct these errors to some extent by using additional sources of position estimates, including satellite-based navigation solutions such as GPS, GNSS, etc. However, satellite-based navigation solutions are unreliable. Signals can be jammed, obstructed, blocked, etc., which can lead to problems in navigation systems that rely too heavily on these satellite-based navigation solutions. The TRN system provides an additional navigation solution that can enable navigation systems to correct INS errors over time. Summary of the Invention [Problem to be solved by the invention]

[0004] Conventional TRN systems utilize a mixed barometric-inertial altimeter and radar altimeter to estimate the terrain altitude below the aircraft. The measured terrain altitude estimate is then correlated to accumulated terrain altitude data along the aircraft's path of travel to provide a navigation solution, i.e., estimate the aircraft's position. [Means for solving the problem]

[0005] Viewed from a first aspect, the present disclosure provides a navigation system for a ground vehicle, the navigation system comprising: an inertial navigation system configured to output an estimate of an orientation of the ground vehicle and an estimate of a first position of the ground vehicle; a terrain map including terrain data; a terrain slope based navigation unit configured to output an estimate of a second position of the ground vehicle based on a comparison of the estimate of the orientation and terrain slope data extracted from the terrain map; and an iterative algorithm unit configured to determine a system error state at each iteration, the iterative algorithm unit being configured to receive the first position estimate and the second position estimate, and to update a system error state for a next iteration based on the system error state, the first position estimate, and the second position estimate. The iterative algorithm unit may thus apply the updated system error state to INS measurements to calculate a more accurate current estimate of the ground vehicle's position.

[0006] In many situations, a ground vehicle has all of its wheels / trucks / etc. in contact with the ground. As such, the orientation of the ground vehicle at any given time is indicative of the slope of the ground beneath the vehicle at that time (this is not the case for aircraft). For example, in a situation where the ground vehicle is located on a slope, the vehicle itself will typically be oriented at an angle equal to or substantially similar to the angle of the slope beneath the vehicle. This provides a possible navigation solution. The orientation of the vehicle as measured by an inertial measurement unit (IMU) can be compared, instantaneously or over time, with known (e.g., accumulated) terrain slope data to provide an estimate of the vehicle's position. In one set of embodiments, the terrain slope-based navigation unit is configured to determine a second position estimate based on a correlation between the measured orientation estimate and terrain slope data extracted from a terrain map. In one set of embodiments, the terrain slope based navigation unit is configured to determine an estimate of the second position based on a correlation between a tracking of a plurality of measured orientation estimates taken over a period of time and / or spatially and terrain slope data extracted from a terrain map.

[0007] As used herein, the term orientation is used to describe the three-dimensional angular orientation of an object relative to a fixed reference direction, and the term attitude is used to describe the two-dimensional angular orientation of an object relative to a fixed reference plane. For example, the orientation of a ground vehicle may include measurements of the vehicle's pitch, roll, and yaw, and the attitude of a ground vehicle may include measurements of the vehicle's pitch and roll. During on-road navigation, vehicle pitch measurements may prove more useful than vehicle roll measurements for comparison with terrain slope data extracted from a terrain map. This is because roads tend to be dug into the side of hills to provide a flat surface (zero roll). During off-road navigation, both vehicle pitch and roll measurements may prove equally useful for comparison with terrain slope data extracted from a terrain map.

[0008] Comparing the orientation measurements to terrain slope data in this manner provides a further navigation solution that may be used to correct for INS variations without the need for an external reference. For example, this may be used in environments where satellite-based navigation systems cannot be used (e.g., due to lack of signal, GPS signals being blocked / jammed, etc.). A further advantage is that no signal is emitted as part of the process, meaning that the vehicle in question does not reveal its presence and does not interfere with other local systems. Furthermore, the equipment (e.g., antennas) required to use radio / optical-based navigation solutions may increase the radar cross section of the vehicle, making it more detectable by radar. While current INSs are capable of providing a reliable approximation of the vehicle's position over time, they are not without variations and high-end systems can cost $50,000 or more. The system of the present disclosure may be used in such systems for further improved navigation. However, in some embodiments, the present disclosure allows for the use of less expensive, less accurate INSs with higher levels of variation. This is because the variations of these inexpensive systems can be offset using the terrain tilt-based navigation system described herein. In some embodiments, the INS comprises at least one microelectromechanical system accelerometer or gyroscope. In one set of embodiments, the INS comprises three accelerometers with orthogonal sensing axes and three gyroscopes with orthogonal sensing axes. In some embodiments, the terrain tilt matching process may be used without an INS, but with a sensor capable of measuring the distance traveled, such as an odometer. In such cases, the gyroscope or inclinometer is sufficient to determine the vehicle's orientation without the need for an accelerometer. However, since gyroscopes are often available in a cost-effective manner along with the accelerometers in the IMU, adding an INS to add navigation accuracy may be attractive.

[0009] In one set of examples, the terrain map includes terrain elevation data. A terrain map may include an array of map posts (i.e., data points at individual terrain locations), with each map post including data indicating the terrain elevation at a particular location (e.g., latitude and longitude). This type of terrain elevation data is common and typically readily available (e.g., available because it is provided according to the Digital Terrain Elevation Data (DTED) standard). In some examples, each map post of a terrain map includes a surface elevation data set that is within 40,000 m of the Earth's surface. 2 (e.g. 200m by 200m) area, preferably 10000m 2 (e.g. 100m vs 100m) areas, more preferably 2500m 2 (e.g. 50m vs 50m) and even more preferably areas under 1000m 2 (e.g., 30 m by 30 m) It should be understood that higher resolution data allows for higher potential correlations, but adds computational processing requirements.

[0010] In one set of embodiments, the navigation system includes a terrain slope calculation unit configured to calculate terrain slope data from terrain elevation data stored in a terrain map. The terrain slope calculation unit may apply a six-term two-dimensional plane fit to map posts of the terrain map to calculate the terrain slope data. This plane fit may be applied in advance to generate and store a terrain slope map including terrain slope data that can be directly used by the terrain slope based navigation unit to estimate the vehicle's position, or this plane fit may be dynamically applied from digital terrain elevation data in the terrain map for an area around the vehicle's currently estimated position. The calculated terrain slope data may include slope and / or rate of change of slope, and the terrain slope data may be continually recalculated in the area of ​​the vehicle's currently estimated position to keep the data relevant to the current measurement. The area over which the terrain slope data is calculated may be increased as the estimated uncertainty in the vehicle's currently estimated position increases. The area for which terrain slope data is dynamically calculated may be updated periodically as the vehicle moves (and thus as the vehicle's position changes), thereby ensuring that terrain slope data is available for all areas the vehicle may move to from its currently estimated position. The area for which terrain slope data is dynamically calculated may include an area less than 10 square kilometers, or less than 5 square kilometers, or less than 3 square kilometers, or less than 1 square kilometer. In one set of embodiments, the area for which terrain slope data is calculated is a substantially circular area with a radius equal to three times the currently estimated uncertainty in the current navigation solution. There may be a lower limit on the area for which terrain slope data is calculated to ensure that a sufficient amount of terrain elevation data is used to calculate the terrain slope data at any one time.

[0011] In one set of embodiments, the iterative algorithm unit is configured to further update the system error state based on the observation that the ground vehicle is not expected to move (at least not at any significant speed) in a direction perpendicular to the vehicle's forward / rearward direction (i.e., the ground vehicle generally does not move in these directions unless the ground vehicle slides or becomes airborne). This allows the iterative algorithm unit to miss some sensor readings that indicate otherwise. For example, if accelerometer readings suggest that the vehicle is moving perpendicular to the vehicle's forward / rearward direction (i.e., the direction the vehicle is facing), the iterative algorithm unit may take this into account when updating the system error state. In some embodiments, the iterative algorithm unit is configured to increase the uncertainty associated with the first position estimate when the INS measurements indicate ground vehicle movement perpendicular to the vehicle's forward / rearward direction. This observation may help reduce the variance of the INS-derived position estimate. The reason for this is that this observation provides an additional correction to reduce the variation of the INS along the axis perpendicular to the forward / rearward direction of the vehicle.

[0012] In one set of embodiments, the INS orientation estimate output may include estimates of the vehicle's pitch, roll, and yaw, from which an estimate of the vehicle's attitude, including pitch and roll, may be obtained. The terrain tilt based navigation unit may thus be used to calculate an estimate of the attitude (including pitch and roll estimates) that would be expected if the vehicle were at a given location, given the estimated vehicle front as given by the yaw estimate. The terrain tilt based navigation unit then compares the attitude estimate (i.e., pitch and roll estimates) obtained from the INS to the terrain tilt data.

[0013] In one set of embodiments, the iterative algorithm unit comprises a terrain slope based navigation unit. The iterative algorithm unit may also comprise an inertial navigation system. For example, the iterative algorithm unit may comprise a processor and a memory, and the same processor and memory may be used in the terrain slope based navigation unit and the inertial navigation system. The iterative algorithm unit may comprise a Kalman filter.

[0014] Viewed from a second aspect, the present disclosure provides an iterative method for determining a system error state for a ground vehicle, each iteration of the method including receiving a position estimate and an orientation estimate from an inertial navigation system, and receiving a second position estimate from a terrain slope based navigation unit, the second position estimate being based on a comparison between the orientation estimate and terrain slope data extracted from a terrain map including terrain data, and updating the system error state for a next iteration based on the system error state, the first position estimate, and the second position estimate.

[0015] It should be understood that all of the optional features discussed above in relation to the first aspect of the disclosure are equally applicable to this second aspect. [Brief description of the drawings]

[0016] [Figure 1] FIG. 2 shows a vehicle on a slope. [Diagram 2] FIG. 1 illustrates an embodiment of a system for terrain slope-based navigation. [Diagram 3] FIG. 2 illustrates an embodiment of system hardware. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0017] FIG. 1 shows a ground vehicle 2 moving over a terrain 6. It can be seen that the vehicle 2 is currently on a sloping part of the terrain 6 inclined at an angle θ to the horizontal. The vehicle 2 has wheels 4 in contact with the terrain 6 (but in other embodiments the vehicle 2 could have tracks or skis / runners). Because the vehicle is in contact with the terrain 6, the angle φ of the vehicle itself with respect to the horizontal corresponds to the angle of the terrain 6. A vector 8 indicating the vehicle's forward propagation direction (i.e. the direction the vehicle is moving) remains substantially parallel to the surface of the terrain 6. Thus, the orientation of the vehicle will follow the orientation (i.e. the slope) of the terrain 6 while the wheels 4 remain in contact with the terrain 6. The vehicle 2 is equipped with a ground-based navigation system 10. This navigation system 10 will be described in more detail with reference to FIGS. 2 and 3.

[0018] 2 shows a ground based navigation system 10 that includes various components such as a terrain elevation map 12, a terrain slope calculation unit 14, an inertial navigation system 16, a terrain slope based navigation unit 18, and an iterative algorithm unit (e.g., a Kalman filter) 20. It will be appreciated that these various components may be implemented separately on separate processors or they may all be implemented as part of a single system, e.g., running on a single processor with shared resources.

[0019] The inertial navigation system 16 comprises three orthogonal accelerometers 24 providing three-dimensional acceleration data for the vehicle 2 and three orthogonal gyroscopes 22 providing three-dimensional rotation information for the vehicle 2. The accelerometers 24 and gyroscopes 22 may be part of an inertial measurement unit, which may be a stand-alone component. In some embodiments, the inertial navigation system 16 is a strap-down inertial navigation system, where the accelerometers and gyroscopes are fixed in orientation with respect to the vehicle 2, such that acceleration and rotation of the vehicle 2 provide corresponding acceleration and rotation measurements in the accelerometers 24 and gyroscopes 22. The inertial navigation system 16 integrates the output of the accelerometers 24 to calculate the velocity and position of the vehicle 2, and integrates the output of the gyroscopes 22 to calculate the orientation of the vehicle 2. The accelerometer data 32 (including position and velocity) is provided to the iterative algorithm unit 20 to provide a first estimate of the vehicle's position. Additionally, the orientation data 30 is also provided to the iterative algorithm unit 20.

[0020] The terrain slope based navigation unit 18 receives accelerometer data 30, which includes an estimate of the current position of the vehicle 2. The terrain slope based navigation unit 18 receives orientation data 30, which indicates the current heading of the vehicle 2, and also receives local terrain slope data 28 from the terrain slope calculation unit 14. The terrain slope calculation unit 14 receives a currently derived position estimate 36 (representing a best estimate of the vehicle's current position) from the iterative algorithm unit 20, obtains a cache of local terrain elevation data 26 from the terrain elevation map 12 for an area around the derived position estimate 36, and calculates a map of terrain slope from the terrain elevation data 26. The terrain slope data may be calculated using a six-term two-dimensional plane fit of the terrain elevation data. In some embodiments, the terrain slope data further includes a rate of change of slope for further correlation processing. The terrain slope based navigation unit 18 thus performs a correlation process to match the orientation of the vehicle 2 with the terrain slope data 28 to estimate the position of the vehicle 2 that correlates with the terrain slope data 28. This may be a comparison of a single data point (i.e., to determine the point at which the terrain slope data 28 matches the vehicle's current orientation), but is preferably a multi-point comparison based on the vehicle's 2 orientation history (i.e., correlating the vehicle's most recent orientation profile with the terrain slope data 28). The terrain slope based navigation unit 18 then outputs a second position estimate 34 based on the correlation process. The second position estimate provides a further position estimate to the iterative algorithm unit 20 along with the INS position estimate 32. The iterative algorithm unit 20 can use both position estimates to improve its overall vehicle 2 position estimate (i.e., obtained position estimate 36).

[0021] The terrain elevation map 12 may be a database of terrain elevation data 26 for a large area. This area may be a selected area of ​​the earth's surface over which the vehicle 2 is expected to travel. This area may be the entire country or the entire world (depending on the resolution of the data and available storage capacity). The terrain elevation is typically stored as separate map posts, each map post corresponding to the height of a spot at a given location on the earth's surface. The resolution of the data may be varied by changing the distance between the map posts. For example, in some common terrain maps, the map posts are 30 meters apart and arranged in a rectangular grid. It will be appreciated that other grid shapes or arrangements of map posts, or rather other methods of storing the terrain data, may also be used.

[0022] The iterative algorithm unit 20 takes all of its inputs 30, 32, 34 and processes these inputs along with the stored system state (including the last obtained position estimate 36) to generate a new position estimate 36. The iterative algorithm unit 20 may take any suitable form, but in some embodiments is a least squares estimator. In some embodiments, the iterative algorithm unit 20 is a Kalman filter. A Kalman filter includes a predictive model that predicts the next state based on current information, and combines this prediction with new measurements (such as the first and second position estimates 32 and 34, and the orientation information 30) to generate a new estimate of the system state.

[0023] In some embodiments, the iterative algorithm unit 20 may receive an observation (or measurement) that the velocity of the vehicle 2 perpendicular to the vehicle's forward / rearward direction is zero. This may be either the horizontal (left / right) velocity or the vertical (up / down) velocity, or both. This indicates to the iterative algorithm that any changes seen in that state variable are likely due to other errors. The iterative algorithm unit is therefore configured to increase the uncertainty associated with the first position estimate 32 and / or the second position estimate 34 when updating the system's error state when the INS measurements show a significant component of velocity perpendicular to the vehicle's forward / rearward direction.

[0024] The resulting position estimate 36 may also be output externally as a position output 38. This position output may be used by the vehicle's navigation system and may be provided, for example, on a vehicle display or provided to the vehicle's autopilot and / or other vehicle systems.

[0025] FIG. 3 illustrates hardware that may be used to implement the system of FIG. 2. FIG. 3 illustrates navigation system 10 including a processor 40, memory 42, and storage 44. While in some embodiments memory 42 may be sufficient without additional storage 44, it should be understood that the size of the associated maps typically requires significant storage and therefore random access memory 42 is not cost effective. Storage 44 may be a hard disk or solid state capable of storing large amounts of map data, or the like. Additionally, navigation system 10 includes an accelerometer 22 and a gyroscope 24. Although only one of each sensor is shown in FIG. 3, it should be understood that multiple such sensors may be used, and in particular three mutually orthogonal accelerometers and gyroscopes may be provided. The processing of INS 16, terrain slope based navigation unit 18, terrain slope calculation device 14, and iterative algorithm unit 20 may all be performed on the same processor 40 (which may be a CPU or FPGA, or the like). However, in some embodiments, different processors may be used for different parts of system 10.

[0026] While the present disclosure has been described by describing one or more specific embodiments thereof, it will be understood by those skilled in the art that the disclosure is not limited to these embodiments, and many variations and modifications are possible within the scope of the appended claims.

Claims

1. 1. A navigation system for a ground vehicle, comprising: an inertial navigation system configured to output an estimate of an orientation of the ground vehicle and an estimate of a first position of the ground vehicle; a topographical map including topographical data; a terrain slope based navigation unit configured to output an estimate of a second position of the ground vehicle based on a comparison between the orientation estimate and terrain slope data extracted from the terrain map; an iterative algorithm unit configured to determine a state of the error of the system at each of the iterations; In each iteration, the iterative algorithm unit: receiving an estimate of the first location and an estimate of the second location; updating the system error state for a next iteration based on the system error state, the first position estimate, and the second position estimate; configured to: Ground vehicle navigation systems.

2. 2. The navigation system of claim 1, wherein the terrain slope based navigation unit is configured to determine the second position estimate based on a correlation between a measured orientation estimate and terrain slope data extracted from the terrain map.

3. 3. The navigation system of claim 1 or 2, wherein the terrain slope based navigation unit is configured to determine the second position estimate based on a correlation between a trace of a plurality of measured orientation estimates taken over a period of time and terrain slope data extracted from the terrain map.

4. A navigation system according to any one of claims 1 to 3, wherein the terrain map includes terrain altitude data, and the navigation system further comprises a terrain slope calculation unit configured to calculate the terrain slope data from the terrain altitude data.

5. 5. The navigation system of claim 4, wherein the terrain slope data includes slope and / or rate of change of slope, and wherein the terrain slope data is optionally calculated using a six-term two-dimensional planar fit of the terrain elevation data.

6. 6. A navigation system according to claim 4 or 5, wherein said terrain slope data is pre-calculated from said terrain elevation data and stored as a terrain slope map.

7. 6. A navigation system according to claim 4 or 5, wherein the terrain slope data is continually recalculated for an area surrounding the current approximate position of the vehicle.

8. 8. The navigation system of claim 7, wherein the area over which the terrain slope data is continually recalculated is increased as an estimated uncertainty in the current approximate position of the vehicle increases, and optionally the navigation system applies a lower limit to the area over which the terrain slope data is continually recalculated.

9. A navigation system according to any one of claims 1 to 8, wherein the iterative algorithm unit is configured to further update the error state of the system based on an observation that a ground vehicle is not expected to move in a direction perpendicular to the forward / rearward direction of the vehicle.

10. 10. The navigation system of claim 9, wherein the iterative algorithm unit is configured to increase the uncertainty of the first position estimate when INS measurements indicate a significant component of velocity perpendicular to the forward / rearward direction of the vehicle when updating the error state of the system.

11. 11. The navigation system of claim 1, wherein the orientation estimates include estimates of pitch, roll, and yaw, and the terrain tilt based navigation unit is configured to compare the measured pitch and roll estimates with the terrain tilt data.

12. A navigation system according to any preceding claim, wherein the inertial navigation system comprises at least one micro-electro-mechanical system accelerometer or gyroscope, optionally comprising three accelerometers with orthogonal sensing axes and three gyroscopes with orthogonal sensing axes, and optionally comprising an odometer and / or one or more inclinometers.

13. A navigation system according to any preceding claim, wherein said iterative algorithm unit comprises said terrain slope based navigation unit.

14. A navigation system according to any preceding claim, wherein the iterative algorithm unit comprises a Kalman filter.

15. 1. An iterative method for determining a state of error in a system relating to a ground vehicle, each iteration of the method comprising: receiving an estimate of position and an estimate of orientation from an inertial navigation system; receiving a second position estimate from a terrain slope based navigation unit, the second position estimate being based on a comparison between the orientation estimate and terrain slope data extracted from a terrain map including terrain data; updating the system error state for a next iteration based on the system error state, the first position estimate, and the second position estimate.

Images