Trustworthy movement unit

Abstract

A trusted motion unit (202) for an autonomous vehicle, comprising: an inertial measuring unit (212); an integration circuit (214) configured to receive an output signal (207) of the inertial measuring unit (212); and a first filter (216) configured to receive an output signal (209) of the integration circuit (214) and an output signal from a second filter (220), wherein the trusted movement unit (202) is configured to provide an output signal (210) representing the position of the vehicle, the output signal (210) representing the position having a lower latency than the output signal (208) from the second filter (220).

Description

AREA OF REVELATION

[0001] This application relates to navigation systems and methods for vehicles. BACKGROUND

[0002] Navigation systems for autonomous vehicles sometimes incorporate multiple sensor types. Lidar, radar, global positioning system (GPS), inertial measurement unit (IMU), and wheel speed sensors are sometimes used as part of an autonomous vehicle navigation system. Typically, navigation systems treat the GPS sensor as the primary sensor, with its data being augmented by sensors that provide local information, such as IMU data, or data from lidar, radar, or camera systems.

[0003] US 6,408,245 B1 relates to a filter mechanization method for integrating a Global Positioning System receiver with an inertial measurement unit to generate highly accurate and highly reliable mixed GPS / IMU position, velocity, and attitude information of a carrier. The filtered GPS position and velocity data are first used individually as readings of the two local filters to generate estimates of two sets of local state vectors. Subsequently, the estimates of the two sets of local state vectors are blended by a master filter unit to generate global optimal estimates of the master state vector, including INS (Inertial Navigation System) navigation parameter errors, carrier sensor errors, and GPS-correlated position and velocity errors.The estimates of the two sets of local state vectors and master state vectors are analyzed by a GPS error detection / isolation logic module to prevent the mixed GPS / IMU position, velocity, and attitude information from being corrupted by undetected GPS errors.

[0004] US 2018 / 0095476 A1 relates to a control system that fuses various sensor data to determine the orientation of a vehicle. The control system receives the vehicle's visual heading data from a camera system, global navigation satellite system (GNSS) heading data from a GNSS system, and inertial measurement unit (IMU) heading data from an IMU. The control system can assign weights to the visual, GNSS, and IMU heading data based on the vehicle's operating conditions, which can affect the accuracy of the various visual, GNSS, and IMU data. The control system then uses the weighted visual, GNSS, and IMU data to determine a more accurate vehicle heading. SUMMARY

[0005] Navigation systems and methods for autonomous vehicles are provided. The navigation system may include multiple navigation systems, including one with an inertial measurement unit (IMU). The unit may serve as the primary unit for navigation purposes, with other subsystems treated as secondary. The other navigation systems may include global positioning system (GPS) sensors and perception sensors. In some embodiments, the navigation system may include a first filter for the IMU subsystem and separate filters for the other navigation systems. The IMU, in at least some embodiments, may operate with lower latency than the other sensors of the navigation system.

[0006] According to some embodiments, a trusted motion unit for an autonomous vehicle is provided, comprising: an inertial measurement unit (IMU), an integration circuit configured to receive an output signal of the inertial measurement unit, and a first filter configured to receive an output signal of the integration circuit and an output signal from a second filter.

[0007] According to some embodiments, a navigation system for an autonomous vehicle is provided, comprising: a plurality of navigation subsystems comprising: a first navigation subsystem comprising an inertial measurement unit (IMU) and a first filter, and a second navigation subsystem comprising a second filter, wherein the first filter is coupled to the second filter, is configured to receive an input signal from the second filter, and is configured to provide a feedback signal to the IMU.

[0008] According to some embodiments, a navigation system for an autonomous vehicle is provided, comprising: a first navigation subsystem comprising an inertial measurement unit (IMU), and a first filter coupled to the IMU in a feedback loop. The navigation system further comprises a second navigation subsystem comprising: a sensor, and a second filter coupled to receive an output signal of the sensor and coupled to an input terminal of the first filter.

[0009] According to some embodiments, a localization method is provided for an autonomously driving vehicle having a first navigation subsystem including an inertial measurement unit and a first filter, and a second navigation subsystem including a sensor and a second filter. The method includes applying an output signal of the IMU to the first filter, applying an output signal of the sensor to the second filter, and applying an output signal of the second filter to the first filter. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] Various aspects and embodiments of the application are described with reference to the following figures. It should be understood that the figures are not necessarily drawn to scale. Elements that appear in multiple figures are identified by the same reference numeral in all figures in which they appear. Fig. 1 is a block diagram of a navigation system according to a non-limiting embodiment of the present application, wherein an inertial measurement unit serves as the primary navigation sensor. Fig. Figure 2A is a non-limiting example of a more detailed implementation of the navigation system of Fig. 1. Fig. 2B is an alternative to Fig. 2A, which shows another non-limiting example of a more detailed implementation of the navigation system of Fig. 1 illustrates. Fig. 3A is a non-limiting example of a further detailed implementation of the navigation system of Fig. 1, which has a global positioning navigation subsystem and a perceptual navigation subsystem. Fig. 3B is an alternative to Fig. 3A, which shows another non-limiting example of a detailed implementation of the navigation system of Fig. 1 illustrates. Fig. 4A is a non-limiting example of a further detailed implementation of the navigation system of Fig. 3A, which has a global positioning navigation subsystem and a perceptual navigation subsystem. Fig. 4B is an alternative to Fig. 4A, which shows another non-limiting example of a detailed implementation of the navigation system of Fig. 3A illustrates. Fig. 5 illustrates a configuration of a navigation system having latency compensation for compensating for different latencies of different subsystems of the navigation system. Fig. Figure 6 illustrates a car as an example of an autonomous vehicle incorporating a navigation system of a type described here. DETAILED DESCRIPTION

[0011] According to one aspect of the present application, a system and method for vehicle localization are provided, wherein data from multiple types of sensors are used in combination, wherein an inertial measurement unit (IMU) is treated as the primary sensor and data from this unit is treated as trusted data to be corrected by data from the other sensors. Localization refers to the knowledge of the location of the vehicle. The various types of sensors can be used to provide the attitude of the vehicle in a local frame of reference and, in some embodiments, in a global frame of reference. Providing a position in a local frame of reference may mean providing a position in, for example, meters in an X, Y, Z frame with respect to a local position, such as a city. A local position may be specified with respect to north, east, and up coordinates.Providing a position in a global reference frame may mean specifying the position in terms of latitude and longitude on the globe. The IMU may provide data indicating the attitude of the vehicle in the local reference frame. The data may be communicated by data from a perceptual navigation subsystem and / or a global positioning navigation subsystem. Aspects of the present application provide improved localization in which the location of the vehicle can be determined with an accuracy of 10 cm and / or with a latency of 10 milliseconds or less. Such precise localization may be advantageous in autonomous vehicles, where precise knowledge of the vehicle's location can have a significant impact on safety.

[0012] According to one aspect of the present application, a localization system for an autonomous vehicle combines data from an IMU, one or more perception sensors, and GPS, where the IMU is treated as the primary sensor. The IMU may be a 6-degree-of-freedom (6DOF) IMU. The perception sensors may be lidar, radar, a camera, an ultrasonic sensor, a radodometry sensor, a steering direction (or heading target) sensor, and a steering wheel rotation sensor (or steering actuator input). In contrast to an approach where the GPS sensor or sensors are treated as the primary sensor, with the IMU providing correction, aspects of the present application provide improved localization by treating the IMU as the primary sensor, assisted by correction from the receiving sensor(s) and / or GPS.The inventors understood that determining location from a global level to a local level, such as by relying on GPS as the primary sensor, supported by perception systems and an IMU, is problematic and inconsistent with the reality of how human operators operate a vehicle to make decisions regarding steering, braking, or taking actions. Instead, assessing location primarily from local detectors and then correcting based on global indicators of location provides better accuracy.Because IMUs are immune to external influences, such as weather conditions or jamming, their use as the primary sensor also provides a higher degree of accuracy compared to a system that uses a GPS or perception sensor as the primary navigation system sensor, as those other types of sensors are susceptible to data dropouts and / or environmental failures. The IMU-based navigation subsystem can provide latitude, longitude, pitch, and roll in all environmental conditions. This approach of making the environmentally insensitive IMU the primary sensor means that a vehicle can travel much further safely (e.g., several seconds) than if the perception or GPS sensors were the primary sensor.Furthermore, IMUs can have lower latencies than perception sensors, other local sensors, and / or GPS technology, meaning that using an IMU as the primary sensor can result in lower-latency localization (faster localization). In general, the IMU can operate with lower latency than the other sensors in the navigation system. The IMU can have the lowest latency of all the sensors in the navigation system.

[0013] According to one aspect of the present application, a so-called trusted motion unit (TMU) is provided. A trusted motion unit is a navigation system comprising an inertial measurement unit and configured to output localization information based on data from the IMU and from a navigation subsystem comprising a different sensor type, wherein the data from the IMU is treated as primary data to be corrected by the data from the other sensor type. The trusted motion unit may comprise an integration unit and an extended Kalman filter, which may be an error-state Kalman filter. The integration circuit may receive and integrate an output signal from the IMU and provide an output signal to the extended Kalman filter. The extended Kalman filter may also receive input signals from Kalman filters of other navigation subsystems of the autonomous vehicle.For example, the autonomous vehicle may have a perceptual navigation subsystem and / or a global positioning system (GPS) navigation subsystem, each of which may have its own Kalman filter. The extended Kalman filter of the trusted motion unit may receive output signals from both of the Kalman filters. The extended Kalman filter of the trusted motion unit may also provide a feedback signal to the IMU. Finally, the integration circuit of the trusted motion unit may output a signal indicating a location of the autonomous vehicle.

[0014] Fig. 1 is a block diagram of a navigation system according to a non-limiting embodiment of the present application, wherein an inertial measurement unit serves as the primary navigation sensor. The navigation system 100 includes a trusted motion unit (TMU) 102 and a secondary navigation subsystem 104. The trusted motion unit 102, which can be considered a navigation subsystem of the larger navigation system 100, includes an IMU 112. The trusted motion unit 102 is coupled to the secondary navigation subsystem 104 to both provide an input signal 106 to the secondary navigation subsystem 104 and receive an output signal 108 from the secondary navigation subsystem 104. The trusted motion unit 102 also provides an output signal 110 representing a location of the vehicle in or on which the navigation system 100 is disposed.For example, the output signal 110 may represent a position of the vehicle in a local reference frame.

[0015] The IMU 112 may be any suitable IMU. In some embodiments, the IMU is a six-degree-of-freedom ("6DOF") IMU. In such embodiments, the IMU may produce data representing yaw, pitch, roll, and velocity along the x, y, and z directions. Alternatives to this are possible, but not all embodiments of a TMU are limited to the IMU being a 6DOF IMU. For example, the IMU may provide data regarding motion along or about one or more axes.

[0016] The secondary navigation system 104 may include a sensor of a type other than an IMU. For example, the secondary navigation subsystem 104 may include a perception sensor, such as a camera, lidar, radar, an ultrasonic sensor, and / or a wheel speed sensor. The secondary navigation subsystem 104 may include a global positioning system sensor. Examples of secondary navigation subsystems are described further below in connection with subsequent figures.

[0017] The trusted motion unit 102 may be treated as the primary navigation subsystem of the navigation system 100. In some embodiments, the navigation systems described herein may be said to perform sensor fusion. Data from the primary navigation subsystem is combined with data from the secondary navigation subsystems. The IMU 102 may have a lower latency than the sensor(s) of the secondary navigation subsystem 104. For example, the IMU may provide inertial data with a latency of a few milliseconds or less. In some embodiments, the IMU may provide inertial data with a latency of less than 1 ms. In contrast, the secondary navigation subsystem may provide localization data with a latency between 10 ms and more than 100 ms.For example, a GPS system may have a latency on the order of 100 ms, a perception system may have a latency on the order of 50 ms, and a controller area network (CAN) sensor may have a latency of 10 ms or more. Accordingly, using the IMU as the primary sensor may provide an overall system with lower latency than would be achieved by using any of the other subsystems as the primary system. The lower latency may translate into greater precision of localization. For example, navigation systems described herein may be accurate to within 10 cm or less. In at least some embodiments, the IMU may have the lowest latency of all sensors of the navigation system. Furthermore, IMUs are not susceptible to environmental conditions, such as satellite failures or various weather conditions.Although the IMU 102 is susceptible to errors that increase over time, the data from the secondary navigation subsystem can be used to update or correct the IMU data. In this way, highly precise localization data can be provided by the navigation system 100 regardless of satellite availability or weather conditions. The trusted motion unit can be treated as the root of trust for the navigation system 100.

[0018] Fig. Figure 2A is a non-limiting example of a more detailed implementation of the navigation system of Fig. 1. Fig. 2A illustrates a navigation system 200. The trusted motion unit (TMU) 202 represents an implementation example of the trusted motion unit 102. The secondary navigation subsystem 204 represents an implementation example of the secondary navigation subsystem 104.

[0019] As shown, the trusted motion unit 202 includes an IMU 212, integration circuitry 214, and a filter 216. Optionally, a feedback path 222 is provided from the filter 216 to the IMU 212 to provide a feedback signal. The IMU 212 may be any suitable IMU, such as the types described above in connection with the IMU 112. The integration circuitry 214 may be any suitable circuit for integrating data provided by the IMU 212. In some embodiments, the IMU 212 and the integration circuitry 214 may be formed within the same package, and in some embodiments, they are formed on the same semiconductor die. The filter 216, described in more detail below, may be an extended Kalman filter (EKF), a Bayesian filter, or a particle filter. In some embodiments, filter 216 is an error state Kalman filter.In some embodiments, such as those in . Fig. 2B, which is described below, the filter 216 may be omitted.

[0020] The secondary navigation subsystem 204 includes a secondary sensor 218 and a filter 220. The secondary sensor 218 may be a GPS sensor or a perception sensor. Examples of perception sensors include cameras, lidar, radar, ultrasonic sensors, wheel speed sensors, wheel odometry sensors, steering direction (or heading target) sensors, and steering wheel rotation (or steering actuator input) sensors. Wheel odometry may be acquired through any combination of wheel speed and steering data. Wheel speed sensors, wheel odometry sensors, steering direction sensors, and steering wheel rotation sensors may communicate via a controller area (CAN) bus and, accordingly, may be considered non-limiting examples of "CAN sensors."The secondary sensor 218 is secondary in that its data can be used to refine the data provided by the trusted motion unit 202, with the data from the trusted motion unit 202 being treated as the trusted data output by the navigation system 200. The secondary navigation subsystem 204 may include multiple secondary sensors 218. For example, the secondary navigation subsystem 204 may be a perceptual navigation subsystem that includes two or more of a camera, lidar, radar, an ultrasonic sensor, a wheel odometry sensor, a steering direction (or heading target) sensor, and a steering wheel rotation (or steering actuator input) sensor. The output signals from these may all be provided to the filter 220, at least in some embodiments.

[0021] In operation, the trusted motion unit 202 and the secondary navigation subsystem 204 both generate data that is exchanged. The IMU 212 may generate motion data, which is provided as a signal 207 to the integration circuitry 214. The integration circuitry 214 integrates the motion data and provides an integrated signal 209 to the filter 216.

[0022] The trusted motion unit 202 also provides an input signal 206 to the secondary navigation subsystem 204. The input signal 206 may represent the motion data also provided to the integration circuitry 214 and, accordingly, may be provided directly by the IMU 212 or may be other data generated by the IMU 212. This data may be appropriately combined with the data generated by the secondary sensor 218 and input to the filter 220 as a signal 211. The filter 220 may process the signal 211 to generate local positioning information. This local positioning information may be provided as an output signal 208 of the secondary navigation subsystem 204 to the filter 216 of the trusted motion unit.Filter 216 may process output signal 208 in combination with integrated signal 209 and provide a feedback signal on feedback path 222 back to IMU 212. The feedback signal may be used by the IMU to correct its motion data. For example, trusted motion unit 202 outputs a signal 210 representing the vehicle's position in a local reference frame. In at least some embodiments, signal 210 may be output by integration circuitry 214, although alternatives are possible.

[0023] As described above, the navigation system 200 includes two filters, filters 216 and 220, with the output of one filter (220) input to the other filter (filter 216). This configuration may be described as a stepped filter. Furthermore, as described, filter 220 may be a Kalman filter (e.g., an error-state Kalman filter) and filter 216 may be an extended Kalman filter. Using separate filters for separate sub-navigation systems may provide several advantages. For example, using separate filters allows them to be weighted differently. Controlling the weights of the filters may allow the filter as part of the trusted motion unit to be weighted more heavily than the filters of the secondary navigation subsystems, which is done in at least some embodiments.For example, a perceptual navigation subsystem may experience noise, and as a result, the navigation system may weight data from the IMU more heavily. Furthermore, using separate filters provides reduced processing latency and power consumption compared to using a single filter.

[0024] Fig. Figure 2B shows an alternative non-limiting example of a more detailed implementation of the navigation system of Fig. 1. The navigation system 250 from Fig. 2B is the same as the navigation system 200 from Fig. 2A, except that the trusted motion unit 203 differs from the trusted motion unit 202 in that the filter 216 is omitted and the output signal 208 of the filter 220 is provided to the integration circuitry 214. The feedback path 222 extends from the integration circuitry 214 to the IMU 212. The configuration of Fig. 2B is easier than those from Fig. 2A.

[0025] Fig. 3A is a non-limiting example of a further detailed implementation of the navigation system of Fig. 1, which includes a global positioning navigation subsystem and a perception navigation subsystem. The navigation system 300 includes the trusted motion unit 202, a perception subsystem 304, and a global positioning navigation subsystem 322. The perception navigation subsystem 304 and the global positioning navigation subsystem 322 may each be an implementation example of the secondary navigation subsystem 204 from Fig. 2A and Fig. 2B represent.

[0026] The trusted movement unit 202 was previously used in conjunction with Fig. 2A and is therefore not described in detail here.

[0027] The perceptual navigation subsystem 304 includes perceptual sensor(s) 318 and a filter 320. The perceptual sensor 318 may be any of the types of receiving sensors described previously herein. The receiving sensor 318 may generate data that is combined with an input signal 206 from the trusted motion unit 202 and input as signal 311 to the filter 320. The filter 320 may process the signal 311 to generate local positioning information. This local positioning information may be provided as the output signal 208 of the perceptual navigation subsystem 304 to the filter 216 of the trusted motion unit, or alternatively, directly to the integration circuitry 214.

[0028] The global positioning navigation subsystem 322 includes a global positioning system (GPS) sensor 324 and a filter 326. The GPS sensor 324 generates GPS data, which is provided as a signal 323 to the filter 326. The filter 326 may be a Kalman filter that outputs a signal 325 representing a position of the vehicle in a global frame of reference.

[0029] In operation, the trusted motion unit 202 interacts with the perception navigation subsystem 304 and the global positioning navigation subsystem 322 in the manner previously described in connection with the secondary subsystem 204 of Fig. 2A and Fig. 2B. The trusted motion unit 202 provides the input signal 206 to the perceptual navigation subsystem 304. The perceptual navigation subsystem generates the output signal 208 using the filter 320. This output signal 208 is provided as an input to the trusted motion unit's filter 216 or, alternatively, may be provided directly to the integration circuitry 214. The trusted motion unit 202 also provides an input signal 328 to the global positioning navigation subsystem 322. The input signal 328 may, in some embodiments, be the same as the input signal 206. In some embodiments, the input signal 328 is provided directly to the filter 326 and, accordingly, is an input signal to the filter 326. The filter 326 also provides an output signal 330 to the filter 216 or directly to the integration circuitry 214.Accordingly, in this non-limiting embodiment, filter 216 receives an input signal from integration circuitry 214, an output signal from filter 320, and an output signal from filter 326. Filter 216 optionally provides the previously described feedback signal to the IMU on feedback path 222, wherein the feedback signal is the result of processing the input signals from the IMU to filter 216, the receive navigation subsystem 304, and the global positioning navigation subsystem 322. As with the navigation system 200 of FIG. Fig. 2A, the navigation system 300 comprises stepped filters, with filters 320 and 326 providing input signals to filter 216. The advantages of using multiple filters have been discussed previously in connection with Fig. 2A and Fig. 2B and can equally be applied to the non-limiting embodiment of Fig. 3A apply.

[0030] Fig. 3B is an alternative to Fig. 3A, which shows another non-limiting example of a detailed implementation of the navigation system of Fig. 1. The navigation system 350 of Fig. 3B differs from the navigation system 300 Fig. 3A in that filters 320 and 326 are combined into a single filter 332, which outputs a signal 334 to filter 216. Using a combined filter can ensure that signal 334 is provided in a meaningful combined frame of reference. Global positioning navigation subsystem 335 differs from global positioning navigation subsystem 322 in that filter 326 is missing. Perceptual navigation subsystem 305 differs from perceptual navigation subsystem 304 in that filter 320 is missing.

[0031] Fig. 4A is a non-limiting example of a detailed implementation of the navigation system of Fig. 3A, which includes a global positioning navigation subsystem and a perceptual navigation subsystem. Navigation system 400 includes a trusted motion unit 402 as a navigation subsystem, a perceptual subsystem 404, and a global positioning navigation subsystem 422.

[0032] The trusted motion unit 402 includes an IMU 412, integration circuitry 414, and a filter 416. The IMU may be any suitable IMU, including any of the types described previously herein. The integration circuitry 414 may be any suitable integration circuitry for integrating the inertial data provided by the IMU, such as those types described previously herein. The filter 416 may operate as an integration filter with a reference correction to the signals it receives. The filter 416 may be configured to output changes in angle and velocity, represented by dφ and dv, respectively. The filter 416 is an extended Kalman filter in some embodiments. The filter 416 may provide a feedback signal 417 to the IMU 412 to provide bias and scale factor correction.

[0033] The perceptual navigation subsystem 404 includes a camera 418a, a lidar 418b, and a radar 418c. The perceptual navigation subsystem may optionally include a wheel speed odometer, a steering direction (or heading target) sensor, and a steering wheel rotation (or steering actuator input) sensor. The perceptual navigation subsystem 404 further includes position estimation blocks 419a, 419b, and 419c coupled to the camera 418a, the lidar 418b, and the radar 418c, respectively. The position estimation blocks 419a-419c, which may be any suitable circuitry, provide an estimate of the vehicle position in a local frame of reference based on the respective sensor data from the respective sensor (camera 418a, lidar 418b, and radar 418c) before the data is combined with the data from the other perceptual sensors. The receive navigation subsystem 404 further includes validation stages 421a, 421b and 421c and a filter 420.Filter 420 may be a local position integration filter, implemented, for example, as a Kalman filter, Bayesian filter, particle filter, or other suitable type of filter. Filter 420 is coupled to provide a signal 408 to filter 416.

[0034] The global positioning navigation subsystem 422 includes a GPS sensor 424, a filter 426, and a map block 427. The filter 426 may be configured to operate as a global position integration filter. The filter 426 may be a Kalman filter, a Bayesian filter, a particle filter, or another suitable type of filter. The global positioning navigation subsystem 422 may be configured to output a signal 425 representing the pose of the vehicle in a global reference frame. The pose information represented by the signal 425 may, in some embodiments, have an accuracy between 10 cm and 1 m and a latency between 0.1 seconds and 1 second. The signal 425 provided by the filter 426 may also be provided to the map block 427, which processes it and provides map information 429 to the filter 426.Accordingly, the filter 426 and the card block 427 may operate in a feedback loop.

[0035] In operation, the IMU 412 outputs location information as signal 406. The signal 406 is provided to the integration circuitry 414 and is also provided as a feedforward signal to position estimation blocks 419a-419c, described further below. The integration circuitry 414 integrates the output signal 406 from the IMU 412 and produces an output signal 410. The output signal 410 represents the pose of the vehicle in a local reference frame and, in some embodiments, may have an accuracy between 1 cm and 10 cm with a latency between 1 ms and 10 ms. The output signal 410 is provided to the validation stages 421a-421c, the filter 416, and the filter 426 to enable sample validation.

[0036] The camera 418a outputs a camera signal 452 to the position estimation block 419a and receives a feedback signal 454 from the position estimation block 419a. The lidar 418b outputs a lidar signal 456, which may be a point cloud, to the position estimation block 419b and receives a feedback signal 458 from the position estimation block 419b. The radar 418c outputs a radar signal 460 to the position estimation block 419c and receives a feedback signal 462 from the position estimation block 419c. The feedback signals 454, 458, and 462 can be used to make electronic or mechanical adjustments or to account for systematic changes in the types best detected by the various sensors. For example, systematic changes in altitude, heading, power, aperture, and sweep rates can be taken into account in the sensors themselves.As an example, cameras can use mechanical actuators to reduce image jitter by relying on position feedback.

[0037] The position estimation blocks 419a-419c reproduce signals representing a position of the vehicle in a local frame. In addition to receiving the signals from the respective sensors, the output signal 406 of the IMU 412 is received as a feedforward signal. The position estimation blocks process the output signal 406 in combination with the signal output from the respective sensor in developing the respective position signal, which is output to the respective validation stages 421a, 421b, and 421c.

[0038] Validation stages 421a, 421b, and 421c are provided for camera 418a, lidar 418b, and radar 418c, respectively. Validation stage 421a validates the pose estimate provided by the camera by comparing the pose estimate provided by camera 418a with the pose estimate provided by IMU 412 in the form of signal 410 output by integration circuitry 414. If the errors in the pose estimate provided by the camera are outside an acceptable range compared to the pose estimate from the IMU, then the camera pose estimate may be determined to be invalid and not pass filter 420. However, if the pose estimate provided by camera 418a is found to be valid, then the pose estimate relative to the camera data may be provided to filter 420.The sample validation stage 421b may act in the same manner with respect to the pose estimate from the lidar 418b. The sample validation stage 421c may act in the same manner with respect to the pose estimate provided by the radar 418c. The filter 420 receives valid pose estimates from the validation stages 421a-421c and outputs a localization signal 408 to the filter 416.

[0039] GPS sensor 424 produces a GPS signal 428, which is provided to filter 426. Filter 426 also receives signal 429 from map block 427 and output signal 410 from integration circuitry 414. Filter 426 produces signal 425 described above.

[0040] Accordingly, the navigation system 400 may provide both a signal 410 representing the position of the vehicle in a local frame of reference and a signal 425 representing a position of the vehicle in a global frame of reference.

[0041] Variations in configuration and operation Fig. 4A are possible. For example, in some embodiments, the integration circuitry 414 and the filters 416 and 420 and the position estimation blocks 419a-419c may operate in a local frame of reference, while the filter 426 operates in a global frame of reference, but in other embodiments, the integration circuitry 414 and the filter 416 operate in a global frame of reference. In some embodiments, the signal 410 is provided in local coordinates, but in other embodiments, it is provided in global coordinates.

[0042] Fig. 4B is an alternative to Fig. 4A, which shows another non-limiting example of a detailed implementation of the navigation system of Fig. 3A. The navigation system 450 of Fig. 4B differs from the navigation system 400 Fig. 4A in that it has a receive navigation subsystem 405 without the filter 420, and in that the filter 426 of the global positioning navigation subsystem 423 does not output the signal 425. By omitting the filter 420, the signals from the validation stages 421a-421c can be delivered directly to the filter 416. A concatenation of the filters 420 and 416, as in Fig. 4A, may in some situations result in errors input to filter 416 being correlated, which may have a negative impact on the operation of filter 416 depending on its type. Omitting filter 420 may avoid such a possibility. In addition, navigation system 450 includes a validation stage 421d in global positioning navigation subsystem 423. Validation stage 421d operates in the same manner as previously described with respect to validation stages 421a-421c, except that it operates on data from GPS sensor 424. Navigation system 400 of Fig. 4A can optionally have validation level 421d.

[0043] In the embodiments from Fig. 4A and Fig. 4B, different weights may be given to the various sensors of the navigation subsystems. For example, in one embodiment, the IMU may be given the greatest weight, followed by data from camera 418a. The other sensors, including GPS sensor 424, may be given lesser weight in pose calculations based on merging data from various sensors. Accordingly, according to one aspect of the present application, a navigation system includes a trusted motion unit as a first navigation subsystem, wherein data from the IMU of the trusted motion unit is treated as the primary navigation data, and the navigation system further includes a camera providing visual data, which is treated as the next most trusted source of data.Any additional sensors in the navigation system may produce data that is treated as less significant than the data from the trusted motion unit and the camera. However, other configurations are possible.

[0044] A noteworthy feature of the configurations from Fig. 4A and Fig. Figure 4B shows the relative timing of the signals processed by integration circuitry 414 and filter 416. As described previously, IMUs can have lower latency than other types of sensors, such as perception sensors and GPS sensors. Filter 416 receives signals from the three navigation subsystems. Fig. 4A and Fig. 4B, including the trusted motion unit 402, the perceptual navigation subsystem 404, and the global positioning navigation subsystem 422. Because these subsystems can operate at different latencies (with the trusted motion unit operating at a lower latency than others), a latency compensation block in the navigation systems can be Fig. 4A and Fig. 4B to compensate for latency differences and ensure that filter 416 operates on signals of the same time. For example, a delay may be added to the signals from the lower-latency navigation subsystems so that the overall latency is the same. However, the latency of the TMU may still be controlled by the latency of the IMU and may accordingly be lower than the latencies of the other subsystems, even if the latencies within those subsystems are equalized. Fig. Figure 5 illustrates a non-limiting example.

[0045] Fig. 5 illustrates a configuration of a navigation system 500 that includes latency compensation for compensating for various latencies of various subsystems of the navigation system. The navigation system 500 includes an IMU 502, a Global Navigation Satellite System (GNSS) sensor 504, an imager 506 (e.g., an RGB camera), and a CAN sensor 508. For illustrative purposes, these components produce output signals with latencies of 1 ms, 100 ms, 50 ms, and 10 ms, respectively, although it is understood that the listed latencies are non-limiting examples. The output signal from each of 502, 504, 506, and 508 is provided to a latency compensator 510, which applies appropriate delays to one or more signals and outputs corresponding signals with substantially equal latency.In this example, the GNSS sensor has the largest latency of 100 ms, and therefore, delays are applied by the latency compensator 510 to the signals from the IMU 502, the imager 506, and the CAN sensor 508 so that they have a latency of 100 ms. The four signals are then provided to a filter 520, which may be an extended Kalman filter. Specifically, the signal from the IMU 502 is provided to an integrator (also referred to herein as a "predictor") 524, and the signals from the GNSS sensor 504, the imager 506, and the CAN sensor 508 are provided to a corrector 522. The predictor 524 and the corrector 522 exchange information to refine the data from the IMU 502 based on the data from the other sensors. The filter 520 outputs a signal to the predictor 526, which also receives the IMU output signal from the IMU 502, and outputs the vehicle's attitude as signal 511. The configuration from . Fig. 5 therefore provides that the IMU data serves as the primary data of the navigation system, which is to be corrected by data from the other sensors of the navigation system, while allowing the filters to operate on data of the same time. The latency of the IMU controls the latency of the output signal of the navigation system (e.g., signal 511 in Fig. 5), so that the latency of the output of the predictor 526 is controlled by the latency of the IMU 502.

[0046] According to one aspect of the application, a method for localizing is provided for an autonomously driving vehicle having a first navigation subsystem comprising an inertial measurement unit and a first filter, and a second navigation subsystem comprising a sensor and a second filter. The method comprises applying an output signal of the IMU to the first filter, applying an output signal of the sensor to the second filter, and applying an output signal of the second filter to the first filter. In some embodiments, the method further comprises providing a feedback signal from the first filter to the IMU. In some embodiments, the method further comprises outputting an output signal indicative of a position of the autonomous vehicle in a local reference frame from the first navigation subsystem.In some embodiments, the method further comprises outputting an output signal indicative of a position of the autonomous vehicle in a global frame of reference from the second navigation subsystem. In some embodiments, the autonomous vehicle further comprises a third navigation subsystem having a sensor and a third filter, and the method further comprises applying an output signal of the sensor of the third navigation subsystem to the third filter and applying an output signal of the third filter to the first filter. The data from the first, second, and third navigation subsystems may be processed in combination by weighting data from the first navigation subsystem more heavily than data from the first and second navigation subsystems.

[0047] Fig.6 illustrates a car as an example of an autonomous vehicle incorporating a navigation system of a type described herein. Car 600 includes navigation system 602. Navigation system 602 may be of any of the types previously described herein. Navigation system 602 includes a trusted motion unit including an IMU. Navigation system 602 includes one or more secondary navigation subsystems, such as a perceptual navigation subsystem and / or a global positioning navigation subsystem. The trusted motion unit includes a filter, and the navigation subsystem(s) includes a filter. The trusted motion unit filter may be configured to receive an input signal from the secondary navigation subsystem filter(s).

[0048] Aspects of the present application may provide various advantages. Some non-limiting examples are described. It should be understood that this list is not exhaustive, and not all embodiments necessarily provide all listed advantages.

[0049] Aspects of the present application provide a navigation system for an autonomous vehicle that provides low latency. The latency of an IMU may be lower, and in some embodiments significantly lower, than that of the perception system and global positioning navigation systems. Accordingly, because aspects of the present application establish the IMU-based navigation subsystem as the primary navigation subsystem, the latency of the navigation system may match the latency of the IMU and, accordingly, be lower than it would result from the situation where a perception system or a global positioning navigation system were the primary navigation subsystem. For example, an IMU may operate on the order of 4 kHz, meaning that a navigation subsystem based on an IMU may provide a latency on the order of 1 millisecond.At typical travel speeds, such as highway speeds, a latency on the order of 1 millisecond can translate into a travel distance of only a few centimeters. Accordingly, aspects of the present application provide navigation systems for autonomous vehicles that can provide accurate vehicle localization to within a few centimeters, such as less than 10 cm. In contrast, using a perception-sensor-based navigation system with a latency on the order of tens of hertz can result in localization accuracy within a few meters, which is too great for safe vehicle operation. Furthermore, IMU sensors can provide sufficiently accurate output for approximately ten seconds or more without correction.This time is sufficient to obtain data from a perceptual or global positioning system sensor, which can then be used to correct errors in the IMU signal. Accordingly, structuring the IMU-based navigation system as the primary navigation subsystem can optimize the interplay between an IMU-based navigation subsystem and other types of navigation subsystems.

[0050] The terms "approximately" and "about" may be used to mean within ±20% of a target value in some embodiments, within ±10% of a target value in some embodiments, within ±5% of a target value in some embodiments, and within ±2% of a target value in some embodiments. The terms "approximately" and "about" may include the target value.

[0051] According to one aspect, navigation systems and methods for autonomous vehicles are provided. The navigation system may include multiple navigation systems, including one with an inertial measurement unit (IMU). The unit may serve as the primary unit for navigation purposes, with other subsystems treated as secondary. The other navigation systems may include global positioning system (GPS) sensors and perception sensors. In some embodiments, the navigation system may include a first filter for the IMU sensor and separate filters for the other navigation systems.

Claims

[1] Trusted motion unit (202) for an autonomous vehicle, comprising: an inertial measuring unit (212); an integration circuit (214) configured to receive an output signal (207) of the inertial measuring unit (212); and a first filter (216) configured to receive an output signal (209) of the integration circuit (214) and an output signal from a second filter (220), wherein the trusted movement unit (202) is configured to provide an output signal (210) representing the position of the vehicle, the output signal (210) representing the position having a lower latency than the output signal (208) from the second filter (220). [2] The trusted motion unit (202) of claim 1, wherein the second filter (220) forms part of a navigation subsystem (204) and wherein the output signal (208) from the second filter (220) represents local positioning data. [3] The trusted motion unit (202) of claim 1, wherein the second filter (220) forms part of a navigation subsystem (204) and wherein the output signal (208) from the second filter (220) represents global positioning data. [4] The trusted movement unit (202) of any preceding claim, wherein the first filter (216) is further configured to receive an output signal from a third filter (320), the third filter (320) forming part of a navigation subsystem (304), and the output signal (208) from the third filter (320) representing local positioning data. [5] A trusted motion unit (202) according to any preceding claim, further comprising a feedback loop (222) from the first filter (216) to the inertial measurement unit (212). [6] A trusted motion unit (202) according to any preceding claim, wherein the output signal (207) of the inertial measurement unit (212) has a lower latency than the output signal (208) from the second filter (220). [7] Trusted motion unit (202) according to any preceding claim, wherein the integration circuit (214) is configured to provide the output signal (210) of the trusted motion unit (202), the output signal (210) representing a position in a local reference frame. [8] Navigation system (100; 300) for an autonomous vehicle, comprising: several navigation subsystems that include: a first navigation subsystem comprising an inertial measurement unit (212) and a first filter (216); and a second navigation subsystem (304; 322) having a second filter (320; 326), wherein the first filter (216) is coupled to the second filter (320; 326), is configured to receive an input signal (330) from the second filter (320; 326), and is configured to provide a feedback signal (222) to the inertial measurement unit (212). [9] The navigation system (100; 300) of claim 8, wherein the second navigation subsystem (304) comprises a perception sensor (318). [10] The navigation system (100; 300) of claim 8, wherein the second navigation subsystem (322) comprises a global positioning system sensor (324). [11] Navigation system (100; 300) according to one of claims 8 to 10, wherein the inertial measurement unit (212) of the first navigation subsystem is configured to operate with a lower latency than a sensor (318; 324) of the second navigation subsystem (304; 322). [12] The navigation system (100; 300) of any one of claims 8 to 11, further comprising a third navigation subsystem (322) comprising a third filter, wherein the second navigation subsystem (304) comprises a perception sensor (318), the third navigation subsystem (322) comprises a global positioning system sensor (324), and wherein the first filter (216) is further configured to receive an input signal (330) from the third filter (326). [13] Navigation system (100; 300) according to claim 12, wherein the inertial measurement unit (212) is configured to output a position of the autonomous vehicle in a local reference frame and the third filter (326) is configured to output a position of the autonomous vehicle in a global reference frame. [14] Navigation system (100; 300) according to one of claims 8 to 13, wherein the first filter (216) is configured to weight data from the inertial measurement unit (212) more heavily than data from the second navigation subsystem (304; 322). [15] Navigation system (100; 300) for an autonomous vehicle, comprising: a first navigation subsystem comprising: an inertial measuring unit (212); and a first filter coupled to the inertial measurement unit (212) in a feedback loop (222); and a second navigation subsystem (304; 322) comprising: a sensor (318; 324); and a second filter (320; 326) coupled to receive an output signal (311; 323) of the sensor (318; 324) and further coupled to an input terminal of the first filter (216). [16] The navigation system (100; 300) of claim 15, wherein the sensor (318) is a perception sensor (318) and the output signal (311) of the sensor (318) comprises perception data. [17] The navigation system (100; 300) of claim 15 or 16, wherein the second navigation subsystem (322) comprises a global positioning system sensor (324) and the output signal (323) of the sensor (324) comprises global positioning data. [18] Navigation system (100; 300) according to one of claims 15 to 17, wherein the second filter (320; 326) is a Kalman filter and the first filter (216) is an extended Kalman filter. [19] The navigation system (100; 300) of claim 15, further comprising a third navigation subsystem (322) having a sensor (318) and a third filter (320), wherein the sensor (318) of the second navigation subsystem (304) is a perception sensor (318) and the sensor (324) of the third navigation subsystem (322) is a global positioning system sensor (324), and wherein the third filter (326) is coupled to an input terminal of the first filter (216). [20] Navigation system (100; 300) according to claim 19, wherein the inertial measuring unit (212) is configured to output a position of the autonomous vehicle in a local reference frame and the third filter (320) is configured to output a position of the autonomous vehicle in a global reference frame.

Images