Vehicle odometer estimation based on radar model

Abstract

A method for odometer estimation includes receiving first sensor data and second sensor data. The first sensor data is generated by an inertial measurement unit (IMU), a wheel speed sensor (WSS), and a steering wheel angle sensor (SAS) of the vehicle. The second sensor data is generated by a radar of the vehicle. The method also includes determining an initial longitudinal position, an initial lateral position, and an initial heading of the vehicle using the first sensor data. The method also includes determining a longitudinal position error, a lateral position error, and a heading error of the vehicle using the second sensor data. Further, the method includes correcting an initial longitudinal position, an initial lateral position, and an initial heading of the vehicle using the longitudinal position error, the lateral position error, and the heading error.

Description

TECHNICAL FIELD

[0001] The present disclosure relates to methods and systems for estimating a vehicle odometry using radar-based models. BACKGROUND

[0002] This Background section presents generally the context of the present disclosure. The work of the present named inventors, to the extent it can not be expressly described in the present Background section, as well as aspects of the description that can not be prior art at the time of filing, are not expressly or impliedly admitted to be prior art in the present disclosure.

[0003] Vehicle odometry and state estimation play a key role in vehicle advanced driver-assistance system (ADAS) control. Odometry estimation relies on different vehicle sensor data, which can change over the vehicle’s lifetime. Some vehicles use sensor data from inertial measurement units (IMUs), wheel speed sensors (WSSs), steering wheel angle sensors (SASs), and global navigation satellite system (GNSS) receivers. Radar information can provide orthogonal velocity and relative position motion of the vehicle. Therefore, there is a need to develop a system and method that uses radar data to adapt and enhance parameter learning, correct odometry adaptive filters, and enhance the accuracy of odometry estimation. SUMMARY

[0004] The present disclosure describes a method of determining a vehicle odometry using radar data. Specifically, the method uses radar data adaptation and reinforcement parameter learning, corrects an odometry adaptive filter, and improves the accuracy of odometry estimation. In one aspect of the present disclosure, the method includes receiving, by a controller of a vehicle, first sensor data. The first sensor data is generated by an inertial measurement unit (IMU), a wheel speed sensor (WSS), and a steering angle sensor (SAS) of the vehicle. The method also includes determining, by the controller of the vehicle, an initial longitudinal position, an initial lateral position, and an initial heading of the vehicle using the first sensor data. Further, the method includes receiving, by the controller of the vehicle, second sensor data. The second sensor data is generated by a radar of the vehicle. The method also includes determining, by the controller of the vehicle, a longitudinal position error, a lateral position error, and a heading error of the vehicle using the second sensor data. Further, the method includes correcting the initial longitudinal position, the initial lateral position, and the initial heading of the vehicle using the longitudinal position error, the lateral position error, and the heading error, respectively, to generate a corrected longitudinal position, a corrected lateral position, and a corrected initial heading of the vehicle. Further, the method includes controlling the vehicle using the corrected longitudinal position, the corrected lateral position, and the corrected heading. The method described in this paragraph improves vehicle technology by using improved (more accurate) vehicle position (i.e., longitudinal position and lateral position) and heading estimates to allow for robust vehicle control, thereby improving vehicle technology. Using improved vehicle position and heading estimates, the vehicle can be controlled accurately with minimal error.

[0005] In one aspect of the present disclosure, the initial longitudinal position, the initial lateral position, and the initial heading of the vehicle are determined using an adaptive filter, and the adaptive filter can be a Kalman filter. The corrected longitudinal position can be based on a longitudinal position gain. The corrected lateral position can be based on a lateral position gain. The corrected heading can be based on a heading gain. The method can also include determining a lateral velocity, a longitudinal velocity, and a yaw rate of the vehicle using the first sensor data, the second sensor data, and the adaptive filter. The adaptive filter can be implemented using the following equations:

[0006] Δx k+1 = Δx k + g k,x e x,k

[0007] Δy k+1 = Δy k + g k,y e y,k

[0008] Δψ k+1 = Δψ k + g k,ψ e ψ,k

[0009] where:

[0010] k is a time step;

[0011] Δx k+1 is a filtered term for an initial longitudinal position of the vehicle;

[0012] Δy k+1 is a filtered term for an initial lateral position of the vehicle;

[0013] Δψ k+1 is a filtered term for an initial heading of the vehicle;

[0014] Δx k is a change in longitudinal position of the vehicle at time k;

[0015] Δy k is a change in lateral position of the vehicle at time k;

[0016] Δψ k is a change in heading of the vehicle at time k;

[0017] g k,x is a longitudinal position gain at time k;

[0018] g k,y is a lateral position gain at time k;

[0019] g k,ψ is a heading gain at time k;

[0020] e x,k is a longitudinal position error at time k;

[0021] e y,k is a lateral position error at time k; and

[0022] e ψ,k is a heading error at time k.

[0023] The present disclosure also describes a system for determining a vehicle odometry. The system includes a plurality of sensors and a controller in communication with the sensors. The sensors include an inertial measurement unit (IMU), a wheel speed sensor (WSS), a steering angle sensor (SAS), and a radar. The controller is programmed to perform the method described above.

[0024] The vehicle also includes a vehicle. The vehicle includes a vehicle body, a plurality of sensors coupled to the vehicle body, and a controller in communication with the sensors. The sensors include an inertial measurement unit (IMU), a wheel speed sensor (WSS), a steering angle sensor (SAS), and a radar. The controller is connected to the vehicle body and is programmed to perform the method described above.

[0025] Other applications of the present disclosure will become apparent from the following detailed description when viewed in conjunction with the accompanying drawings. It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not intended to be restrictive of the scope of the present disclosure.

[0026] The above-mentioned and other features and advantages of the presently disclosed systems and methods, and various embodiments thereof, will be more fully understood from the detailed description when taken in conjunction with the accompanying drawings, which include: BRIEF DESCRIPTION OF DRAWINGS

[0027] The present disclosure will become more fully understood from the detailed description and the accompanying drawings, wherein:

[0028] Figure 1 is a side view schematic of a vehicle including a system for estimating a vehicle odometer using a radar-based model.

[0029] Figure 2 is a flowchart of a method for estimating a vehicle odometer using a radar-based model. DETAILED DESCRIPTION

[0030] Reference will now be made in detail to several examples of the present disclosure shown in the accompanying drawings. Wherever possible, the same or like reference numbers will be used throughout the drawings and the description to refer to the same or like parts or steps.

[0031] Referring to Figure 1 , the vehicle 10 includes (or is in communication with) a system 11 for odometer estimation. Although the vehicle 10 is shown as a coupe, it is contemplated that the vehicle 10 can be another type of vehicle, such as a pickup truck, sedan, sport utility vehicle (SUV), recreational vehicle (RV), etc. The system 11 can be used to accurately estimate the odometer of the vehicle 10. The vehicle includes a vehicle body 12 and one or more wheels 14 coupled to the vehicle body 12.

[0032] Further, the vehicle 10 comprises a controller 34 coupled to the vehicle body 12 and one or more sensors 40 in communication with the controller 34. The sensors 40 are coupled to the vehicle body 12 and collect information and generate sensor data indicative of external or internal vehicle parameters. By way of non-limiting example, the sensors 40 can include one or more inertial measurement units (IMUs), yaw rate sensors, ride height sensors, wheel speed sensors (WSSs), lidar, radar, ultrasonic sensors, steering angle sensors (SASs), global navigation satellite system (GNSS) transceivers or receivers, cameras, etc. The GNSS transceivers or receivers are configured to detect a location of the vehicle 10 on the earth. The WSSs are configured to detect a speed of the wheels 14 of the vehicle 10. The yaw rate sensors are configured to determine a heading of the vehicle 10. The cameras can have a large enough field of view to capture images of the front, back, and sides of the vehicle 10. The ride height sensors are configured to measure a right side height of the vehicle 10. The ultrasonic sensors can detect static and / or dynamic objects. The IMUs are configured to measure and report specific force, angular rate, acceleration, and orientation of the vehicle body 12 using a combination of accelerometers, gyroscopes (and sometimes magnetometers). The SASs are configured to measure a rate of rotation of the steering wheel, wheel angles, and other parameters about the steering wheel of the vehicle 10. All of the sensors 40 are configured to generate sensor data. In the present disclosure, for clarity and simplicity, the sensor data can be divided into first sensor data and second sensor data. The first sensor data refers to data generated at least in part by one or more IMUs, WSSs, and one or more steering angle sensors (SASs) of the vehicle 10. The second sensor data refers to data generated at least in part by one or more radars of the vehicle 10.

[0033] The controller 34 is in communication with the sensors 40 and is programmed to receive sensor data from the sensors 40. The controller 34 includes at least one processor 44 and a non-transitory computer readable storage device or medium 46. The processor 44 can be a custom processor, a central processing unit (CPU), a graphics processing unit (GPU), a co-processor in a plurality of processors associated with the controller 34, a semiconductor-based microprocessor (in the form of a microchip or chip set), a macroprocessor, a combination thereof, or generally a device for executing instructions. The computer readable storage device or medium 46 can include volatile and nonvolatile storage devices such as read-only memory (ROM), random access memory (RAM), and keep-alive memory (KAM) for example. KAM is a persistent or non-volatile memory that can be used to store various operating variables when the processor 44 is powered off. The computer readable storage device or medium of the controller 34 can be implemented using a plurality of storage devices such as PROM, EPROM, EEPROM, flash memory, or other electrical, magnetic, optical, or combination storage devices capable of storing data, some of which represent executable instructions used by the controller 34 in controlling the vehicle 10.

[0034] The instructions can include one or more separate programs, each of which comprises an ordered listing of executable instructions for implementing logical functions. When executed by the processor 44, the instructions receive and process signals from the cameras, perform logic, calculations, methods, and / or algorithms for automatically controlling components of the vehicle 10, and generate control signals to actuators to automatically control components of the vehicle 10 based on the logic, calculations, methods, and / or algorithms. Although Figure 1 A single controller 34 is shown in FIG. 1, but the system 11 can include multiple controllers 34 that are in communication through an appropriate communication medium or combination of communication mediums and cooperate to process sensor signals, perform logic, calculations, methods, and / or algorithms, and generate control signals to automatically control features of the system 11. In various embodiments, one or more instructions of the controller 34 are included in the system 11. The non-transitory computer readable storage device or medium 46 includes machine readable instructions (shown in FIG. 1, for example) that, when executed by the one or more processors, cause the processor 44 to perform the method 100 Figure 2 Figure 2 ) of FIG. 1.

[0035] The vehicle 10 also includes one or more vehicle actuators 42 that control one or more vehicle features such as, but not limited to, a propulsion system, a drivetrain, an accelerator pedal, a brake pedal, an electric power steering system, a steering wheel, and a braking system. The vehicle actuators 42 are in communication with the controller 34. Accordingly, the controller 34 is programmed to control operation of the vehicle actuators 42.

[0036] ​Figure 2 is a flowchart of a method 100 of odometer estimation using radar information. The method 100 utilizes radar data adaptation and augmented parameter learning, corrects an odometer adaptive filter, and enhances the accuracy of the odometer estimation. The method 100 begins at block 102. At block 102, the controller 34 receives input data or inputs. The input data includes real-time sensor data from the sensors 40, such as first sensor data and second sensor data. As described above, the first sensor data refers to data generated at least in part by one or more IMUs, WSSs, and one or more SASs of the vehicle 10, and the second sensor data refers to data generated at least in part by one or more radars of the vehicle 10. The first sensor data can also include GNSS data generated by a GNSS transceiver or receiver.

[0037] As a non-limiting example, the input data includes wheel speed over time (from a wheel speed sensor), steering angle measurement over time (which comes from a steering angle sensor), rear steer measurement over time (which comes from an active rear steer), wheel pulse inertial measurement over time (which comes from an inertial measurement unit, IMU), global navigation satellite system (GNSS) data from a GNSS transceiver, and data from a communication gateway module. The method 100 then continues to block 104.

[0038] Block 104 requires the use of adaptive filters and stochastic filters to determine, in real-time, the longitudinal position, lateral position, heading, lateral velocity, longitudinal velocity, and yaw rate of the vehicle 10 using the sensor data (i.e., the first sensor data generated by the IMUs, WSSs, GNSS receiver, and SASs of the vehicle 10, and the second sensor data generated by the radars of the vehicle 10). At block 104, suitable state observers can be used to determine the initial longitudinal position, initial lateral position, initial heading, initial lateral velocity, initial longitudinal velocity, and initial yaw rate. The state observers can be referred to as model-based observers. These model-based observers can be corrected using radar data generated by the radars of the vehicle 10. The radar data can be referred to as second sensor data. To correct the model-based observers, the controller 34 uses a velocity scale factor, an IMU scale factor, and a suspension dynamics correction.

[0039] The radar of the vehicle 10 can be used to determine the heading, longitudinal velocity, and lateral velocity of the vehicle 10 in real-time. For example, the controller 34 determines the heading of the vehicle 10 using the azimuth angle between the radar and a stationary object in front of the vehicle 10. In driving scenarios with sufficient reflections, the radar has the ability to provide the velocity of the vehicle relative to the scene. These data can be aggregated in the velocity domain and provided to us. The model-based observer uses the first sensor data to provide an odometry estimate of the relative vehicle motion. Due to unmodeled dynamics, measurement noise, and environmental uncertainty, the first sensor data can have uncertainty, which can be represented as follows:

[0040]

[0041] where:

[0042] Δx is the delta (or change) in longitudinal position (i.e., position x) of the vehicle 10;

[0043] Δy is the delta (or change) in lateral position (i.e., position y) of the vehicle 10;

[0044] Δψ is the delta (or change) in heading of the vehicle 10;

[0045] w x is the uncertainty of Δx;

[0046] w y is the uncertainty of Δy;

[0047] w ψ is the uncertainty of Δψ;

[0048] is the delta position x with uncertainty;

[0049] is the delta position y with uncertainty; and

[0050] is the delta heading with uncertainty.

[0051] The estimation error can be calculated using the radar data and the following equation:

[0052]

[0053] where:

[0054] v s is the scene velocity of the vehicle 10 provided by the radar;

[0055] e x is the error term for the x position (i.e., longitudinal position) of the vehicle 10;

[0056] e yThis is the error term for the y-position (i.e., lateral position) of vehicle 10;

[0057] e ψ This is the heading error term for vehicle 10;

[0058] Let x be the incremental position with uncertainty;

[0059] Let y be the incremental position with uncertainty;

[0060] For incremental headings with uncertainty; and

[0061] t represents time.

[0062] After calculating the error, the adaptive filter can be implemented using the following equation:

[0063] Δx k+1 =Δx k +g k,x e x,k

[0064] Δy k+1 =Δy k +g k,y e y,k

[0065] Δψ k+1 =Δψ k +g k,ψ e ψ,k

[0066] in:

[0067] k is the time step;

[0068] Δx k+1 The filter term is the initial longitudinal position of the vehicle at time k+1;

[0069] Δy k+1 The filter term is the initial lateral position of the vehicle at time k+1;

[0070] Δψ k+1 The filter term for the vehicle's initial heading at time k+1;

[0071] Δx k This represents the change in the vehicle's longitudinal position at time k;

[0072] Δy k This represents the change in the vehicle's lateral position at time k;

[0073] Δψ k The change in the vehicle's heading at time k;

[0074] g k,x The gain at the longitudinal position at time k;

[0075] g k,y The lateral position gain at time k;

[0076] g k,ψ The heading gain at time k;

[0077] e x,k The longitudinal position error at time k;

[0078] e y,k The lateral position error at time k; and

[0079] e ψ,k Let be the heading error at time k.

[0080] The longitudinal position gain, lateral position gain, and heading gain are calculated using recursive least squares. Using the adaptive filter corrected above, controller 34 corrects the vehicle's initial longitudinal position, initial longitudinal velocity, initial lateral position, initial lateral velocity, initial yaw rate, and initial heading using the longitudinal position error, lateral position error, and heading error, respectively, thereby generating the corrected longitudinal position, corrected lateral position, corrected lateral velocity, corrected longitudinal velocity, corrected yaw rate, and corrected heading for vehicle 10. Method 100 then proceeds to block 106.

[0081] At block 106, controller 34 uses radar data from the radar of vehicle 10 to perform odometer integrity monitoring. First, controller 34 detects any faulty sensor signals from sensors 40 (i.e., WSS, IMU, SAS, radar). If one or more faulty sensor signals are detected, controller 34 executes a remedial procedure. Then, controller 34 performs physical checks. For this purpose, controller 34 checks for unreasonable longitudinal speed values, unreasonable lateral speed values, unreasonable turning rates, unreasonable pitch, and unreasonable roll. Controller 34 also performs consistency checks between the yaw rate and longitudinal speed of vehicle 10, and between the yaw rate and lateral speed of vehicle 10. Controller 34 also performs estimated characteristic checks. Estimated characteristic checks include checking the diagonal elements of the negative covariance matrix and outlier detection. Method 100 then continues to block 108.

[0082] At block 108, controller 34 uses the corrected longitudinal position, corrected lateral position, corrected lateral velocity, corrected longitudinal velocity, corrected yaw rate, and corrected heading of vehicle 10 to control vehicle 10. For example, controller 34 can use the corrected longitudinal position, corrected lateral position, corrected lateral velocity, corrected longitudinal velocity, corrected yaw rate, and corrected heading of vehicle 10 to control one or more ADAS of vehicle 10.

[0083] While exemplary embodiments have been described above, it is not intended that these embodiments describe all possible forms covered by the claims. The language used in this specification is descriptive and not restrictive, and it should be understood that various changes may be made without departing from the spirit and scope of this disclosure. As previously stated, features of various embodiments may be combined to form further embodiments of the currently disclosed systems and methods that may not be explicitly described or shown. While various embodiments may have been described as providing advantages or superiority over other embodiments or prior art implementations with respect to one or more desired characteristics, those skilled in the art will recognize that trade-offs may be made to one or more features or characteristics to achieve desired overall system properties, depending on the specific application and implementation. These properties may include, but are not limited to, cost, strength, durability, lifecycle cost, merchantability, appearance, packaging, size, suitability, weight, manufacturability, ease of assembly, etc. Therefore, embodiments described as less desirable than other embodiments or prior art implementations with respect to one or more features do not exceed the scope of this disclosure and may be ideal for a particular application.

[0084] The accompanying drawings are simplified and not drawn to exact scale. For convenience and clarity only, directional terms such as top, bottom, left, right, upper, above, above, below, rear, and front may be used with respect to the drawings. These and similar directional terms should not be construed as limiting the scope of this disclosure in any way.

[0085] This document describes embodiments of the present disclosure. However, it should be understood that the disclosed embodiments are merely examples, and other embodiments may take various alternative forms. The drawings are not necessarily drawn to scale; some features may be enlarged or minimized to show details of specific components. Therefore, the specific structural and functional details disclosed herein should not be construed as limiting, but only as a representative basis for teaching those skilled in the art to employ the currently disclosed systems and methods in different ways. As will be understood by those skilled in the art, various features shown and described with reference to any of the drawings may be combined with features shown in one or more other figures to produce embodiments not explicitly shown or described. The combinations of features shown provide representative embodiments of typical applications. However, for a particular application or implementation, various combinations and modifications of features consistent with the teachings of this disclosure may be expected.

[0086] This document describes embodiments of the present disclosure based on functional and / or logical block components and various processing steps. It should be understood that such block components can be implemented by multiple hardware, software, and / or firmware components configured to perform specified functions. For example, embodiments of the present disclosure may employ various integrated circuit components, such as memory elements, digital signal processing elements, logic elements, lookup tables, etc., which can perform various functions under the control of one or more microprocessors or other control devices. Furthermore, those skilled in the art will understand that embodiments of the present disclosure can be practiced in conjunction with various systems, and the systems described herein are merely exemplary embodiments of the present disclosure.

[0087] For the sake of brevity, techniques related to signal processing, data fusion, signaling, control, and other functional aspects of the system (as well as the various operating components of the system) may not be described in detail herein. Furthermore, the connecting lines shown in the various figures included herein are intended to represent exemplary functional relationships and / or physical couplings between the various elements. It should be noted that alternative or additional functional relationships or physical connections may exist in the embodiments of this disclosure.

[0088] This description is merely illustrative in nature and is in no way intended to limit the disclosure, its application, or its use. The broad teachings of this disclosure can be implemented in many forms. Therefore, while this disclosure includes specific examples, its true scope should not be so limited, as other modifications will become apparent upon examination of the drawings, description, and appended claims.

Claims

1. A method for determining a vehicle range, comprising: receiving, by a controller of a vehicle, first sensor data; receiving, by the controller of the vehicle, second sensor data; determining, by the controller of the vehicle, an initial longitudinal position, an initial lateral position, and an initial heading of the vehicle using the first sensor data, wherein the first sensor data is generated by an inertial measurement unit (IMU) of the vehicle, a wheel speed sensor (WSS), and a steering wheel angle sensor (SAS) of the vehicle; determining, by the controller of the vehicle, a longitudinal position error, a lateral position error, and a heading error of the vehicle using the second sensor data, wherein the second sensor data is generated by a radar of the vehicle; correcting the initial longitudinal position, the initial lateral position, and the initial heading of the vehicle using the longitudinal position error, the lateral position error, and the heading error, respectively, to generate a corrected longitudinal position, a corrected lateral position, and a corrected heading of the vehicle; and controlling the vehicle using the corrected longitudinal position, the corrected lateral position, and the corrected heading.

2. The method of claim 1, wherein, The corrected longitudinal position, the corrected lateral position, and the corrected heading of the vehicle are determined using an adaptive filter.

3. The method of claim 2, wherein, The corrected longitudinal position is based on a longitudinal position gain.

4. The method of claim 3, wherein, The corrected lateral position is based on a lateral position gain.

5. The method of claim 4, wherein, The corrected heading is based on a heading gain.

6. The method of claim 5, further comprising determining a longitudinal velocity of the vehicle using the first sensor data and the second sensor data.

7. The method of claim 6, further comprising determining a lateral velocity of the vehicle using the first sensor data and the second sensor data.

8. The method of claim 7, further comprising determining a yaw rate of the vehicle using the first sensor data and the second sensor data.

9. The method of claim 8, wherein, The adaptive filter is implemented using a plurality of equations, and the plurality of equations comprises: Δx k+1 = Δx k + g k,x e x,k Δy k+1 = Δy k + g k,y e y,k Δψ k+1 = Δψ k + g k,ψ e ψ,k wherein: k is a time step; Δx k+1 filtered term for the initial longitudinal position of the vehicle; Δy k+1 a filtered term for the initial lateral position of the vehicle; Δψ k+1 a filtered term of an initial heading of the vehicle; Δx k is the change in longitudinal position of the vehicle at time k; Δy k is the change in lateral position of the vehicle at time k; Δψ k is the change in heading of the vehicle at time k; g k,x is the longitudinal position gain at time k; g k,y is the lateral position gain at time k; g k,ψ is the heading gain at time k; e x,k is the longitudinal position error at time k; e y,k is the lateral position error at time k; and e ψ,k is the heading error at time k.

10. A system, comprising: a plurality of sensors, wherein the plurality of sensors comprises an inertial measurement unit (IMU), a wheel speed sensor (WSS), a steering wheel angle sensor (SAS), and a radar; a controller in communication with the plurality of sensors, wherein the controller is programmed to: receive first sensor data, wherein the first sensor data is generated by the IMU, the WSS, and the SAS; receive second sensor data, wherein the second sensor data is generated by a radar of a vehicle; determine an initial longitudinal position, an initial lateral position, and an initial heading of the vehicle using the first sensor data; determine a longitudinal position error, a lateral position error, and a heading error of the vehicle using the second sensor data; correct the initial longitudinal position, the initial lateral position, and the initial heading of the vehicle using the longitudinal position error, the lateral position error, and the heading error, respectively, to generate a corrected longitudinal position, a corrected lateral position, and a corrected heading of the vehicle; and control the vehicle using the corrected longitudinal position, the corrected lateral position, and the corrected heading.

Images