Camera-based enhancement of vehicle kinematic state estimation

Abstract

Methods and systems implemented in a vehicle involve obtaining a single camera image from a camera arranged on the vehicle. The image indicates a heading angle ψ.sub.0 between a vehicle heading x and a tangent line that is tangential to road curvature of a road on which the vehicle is traveling and also indicates a perpendicular distance y.sub.0 from a center of the vehicle to the tangent line. An exemplary method includes obtaining two or more inputs from two or more vehicle sensors, and estimating kinematic states of the vehicle based on applying a Kalman filter to the single camera image and the two or more inputs to solve kinematic equations. The kinematic states include roll angle and pitch angle of the vehicle.

Claims

1. A method implemented in a vehicle, comprising: obtaining, using a processor, a single camera image from a camera arranged on the vehicle, the image indicating a heading angle ψ.sub.0 between a vehicle heading x of the vehicle and a tangent line that is tangential to road curvature of a road on which the vehicle is traveling and also indicating a perpendicular distance y.sub.0 from a center of the vehicle to the tangent line; obtaining, using the processor, two or more inputs from two or more vehicle sensors of the vehicle; estimating, using the processor, kinematic states of the vehicle based on applying a Kalman filter to the single camera image and the two or more inputs to solve kinematic equations, wherein the kinematic states include roll angle and pitch angle of the vehicle; and implementing autonomous or semi-autonomous control of the vehicle based on the kinematic states.

2. The method according to claim 1, wherein the obtaining the two or more inputs includes obtaining acceleration components a.sub.x, a.sub.y of the vehicle from an inertial measurement unit (IMU).

3. The method according to claim 2, wherein the obtaining the two or more inputs includes additionally obtaining roll rate ω.sub.x, pitch rate ω.sub.y, and yaw rate ω.sub.z from the IMU.

4. The method according to claim 3, wherein the estimating the kinematic states includes estimating a velocity component v.sub.y of the vehicle based on the yaw rate ω.sub.z.

5. The method according to claim 1, wherein the obtaining the two or more inputs includes obtaining a velocity component v.sub.x of the vehicle from an odometer.

6. The method according to claim 1, wherein the obtaining the two or more inputs includes obtaining a position X, Y of the vehicle on a global frame from a global navigation satellite system (GNSS).

7. The method according to claim 6, wherein the estimating the kinematic states includes estimating global heading {circumflex over (ψ)} based on the position X, Y of the vehicle.

8. The method according to claim 1, further comprising implementing a dynamic model using the kinematic states estimated using the single camera image and the two or more inputs and steering angle of the vehicle.

9. The method according to claim 8, further comprising obtaining tire forces from the dynamic model and performing a force calculation to determine external disturbances on the vehicle, wherein the external disturbances include crosswinds.

10. The method according to claim 9, further comprising compensating for the external disturbances.

11. A system in a vehicle, the system comprising: a camera arranged on the vehicle and configured to obtain a single camera image, the image indicating a heading angle ψ.sub.0 between a vehicle heading x and a tangent line that is tangential to road curvature of a road on which the vehicle is traveling and also indicating a perpendicular distance y.sub.0 from a center of the vehicle to the tangent line; and a processor configured to obtain the single camera image, to obtain two or more inputs from two or more vehicle sensors of the vehicle, to estimate kinematic states of the vehicle based on applying a Kalman filter to the single camera image and the two or more inputs to solve kinematic equations, wherein the kinematic states include roll angle and pitch angle of the vehicle; and to implement autonomous or semi-autonomous control of the vehicle based on the kinematic states.

12. The system according to claim 11, wherein the processor is configured to obtain acceleration components a.sub.x, a.sub.y of the vehicle from an inertial measurement unit (IMU) as the two or more inputs.

13. The system according to claim 12, wherein the processor is additionally configured to obtain roll rate ω.sub.x, pitch rate ω.sub.y, and yaw rate ω.sub.z from the IMU as additional ones of the two or more inputs.

14. The system according to claim 13, wherein the processor is configured to estimate the kinematic states by estimating a velocity component v.sub.y of the vehicle based on the yaw rate ω.sub.z.

15. The system according to claim 11, wherein the processor is configured to obtain a velocity component v.sub.x of the vehicle from an odometer as one of the two or more inputs.

16. The system according to claim 11, wherein the processor is configured to obtain a position X, Y of the vehicle on a global frame from a global navigation satellite system (GNSS) as the two or more inputs.

17. The system according to claim 16, wherein the processor is configured to estimate the kinematic states by estimating global heading {circumflex over (ψ)} based on the position X, Y of the vehicle.

18. The system according to claim 11, wherein the processor is additionally configured to implement a dynamic model using the kinematic states estimated using the single camera image and the two or more inputs and steering angle of the vehicle.

19. The system according to claim 18, wherein the processor is additionally configured to obtain tire forces from the dynamic model and perform a force calculation to determine external disturbances on the vehicle, wherein the external disturbances include crosswinds.

20. The system according to claim 19, wherein the processor is further configured to compensate for the external disturbances.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) Other features, advantages and details appear, by way of example only, in the following detailed description, the detailed description referring to the drawings in which:

(2) FIG. 1 is a block diagram showing aspects of camera-based enhancement of vehicle kinematic state estimation according to one or more embodiments;

(3) FIG. 2 illustrates an exemplary scenario that results in a roll angle determined using camera-based enhancement of vehicle kinematic state estimation according to one or more embodiments;

(4) FIG. 3 illustrates an exemplary scenario that results in a pitch angle determined using camera-based enhancement of vehicle kinematic state estimation according to one or more embodiments;

(5) FIG. 4 is a block diagram indicating the inputs and outputs involved in camera-based enhancement of vehicle kinematic state estimations according to one or more embodiments; and

(6) FIG. 5 is a process flow of an exemplary method of applying camera-based enhancement of vehicle kinematic state estimations, according to one or more embodiments, to control a vehicle.

DETAILED DESCRIPTION

(7) The following description is merely exemplary in nature and is not intended to limit the present disclosure, its application or uses. It should be understood that throughout the drawings, corresponding reference numerals indicate like or corresponding parts and features.

(8) As previously noted, information about vehicle behavior and vehicle environment are used to determine autonomous or semi-autonomous control over vehicle operation. Vehicle kinematic state provides information about vehicle behavior. Kinematic state refers to longitudinal and latitudinal velocity of the vehicle and roll and pitch angles, for example. Prior approaches to estimating kinematic state include using an IMU and odometer. The information obtained according to this approach may not properly convey vehicle behavior relative to the road. That is, estimated roll and pitch angles may not be obtained at all or may be inaccurate by corresponding with road banking or road inclination or with vehicle roll or pitch relative to the road alone rather than with a sum of both road-related and vehicle-related factors. Another approach uses optic flow based on obtaining sequential images. Embodiments of the systems and methods detailed herein relate to a camera-based enhancement of vehicle kinematic state estimations. Kinematics relative to a reference path are obtained with a single image rather than two or more sequential images.

(9) In accordance with an exemplary embodiment, FIG. 1 is a block diagram showing aspects of camera-based enhancement of vehicle kinematic state estimation. The exemplary vehicle 100 shown in FIG. 1 is an automobile 101. The vehicle 100 may include one or more cameras 110 and other sensors 130 (e.g., radar system, lidar system). In addition to the exemplary arrangement shown for explanatory purposes, the cameras 110 and other sensors 130 may be located anywhere within or on the vehicle 100. In addition to the camera 110 and sensors 130 that provide information from outside the vehicle 100, known vehicle sensors 140 may provide information about the vehicle 100. For example, the odometer 125 determines velocity component v.sub.x of the vehicle 100 along the direction of travel. The IMU 105 tracks the acceleration components a.sub.x, a.sub.y of the vehicle 100 and may additionally provide roll rate ω.sub.x, pitch rate ω.sub.y, and yaw rate ω.sub.z. If a global navigation satellite system (GNSS) 115 (e.g., global positioning system (GPS)) is also available as one of the vehicle sensors 140, it provides the position of the vehicle 100 on a global X and Y grid.

(10) A reference path R indicates the road curvature that the vehicle 100 should be following. The reference path R may be a road center line (i.e., lane marker) or edge of the road, for example. A tangent T to the reference path R is also indicated and shown relative to the vehicle as T′. The x and y axes shown in FIG. 1 indicate the vehicle frame of reference. If a GNSS 115 is available, the global position X, Y of the vehicle 100, relative to a global frame of reference, is known. The vehicle position relative to the reference path R is obtained based on a camera 110 (e.g., the camera 110 at the front center of the vehicle 100). Specifically, the angle ψ.sub.0 is between the vehicle heading x and the tangent line T′ and the distance y.sub.0 is the perpendicular distance from the center of the vehicle 100 to the tangent line T. The velocity v of the vehicle 100 is indicated. As previously noted, the velocity component v.sub.x along the direction of travel (i.e., along the x axis, as shown in FIG. 1) is determined by the odometer 125. The angle β is the side slip angle. That is, the angle β indicates the angle between the x axis and the velocity vector v. Put another way, the tangent of the angle β is v.sub.y/v.sub.x, where the velocity component v.sub.y is along the y axis. The heading angle ψ is the angle between the X axis, the global reference line, and the x axis, which is the vehicle 100 reference line.

(11) The vehicle 100 also includes a controller 120. The controller 120 may obtain and process data from the camera 110, sensors 130, and vehicle sensors 140 to obtain information about the vehicle 100 and its environment. This information includes a roll angle estimate {circumflex over (φ)}. Roll is illustrated in FIG. 2. The information also includes a pitch angle estimate {circumflex over (θ)}. Pitch is illustrated in FIG. 3. The controller 120 may control one or more aspects of the operation of the vehicle 100 based on the information. According to one or more embodiments, the controller 120 performs vehicle kinematic state estimation that is enhanced based on the camera 110. The controller includes processing circuitry that may include an application specific integrated circuit (ASIC), an electronic circuit, a processor (shared, dedicated, or group) and memory that executes one or more software or firmware programs, a combinational logic circuit, and/or other suitable components that provide the described functionality.

(12) FIG. 2 illustrates an exemplary scenario that results in a roll angle φ. The road 210 is banked (i.e., curves up on one edge), as shown in FIG. 2. In addition the vehicle 100 is inclined on one side with respect to the road 210. The roll angle φ is a sum of the banking of the road 210 and the incline of the vehicle 100 relative to the road 210. FIG. 3 illustrates an exemplary scenario that results in a pitch angle θ. The road 310 is inclined (i.e., slopes up), as shown in FIG. 3. In addition, the vehicle 100 is inclined with respect to the road 310 in the front. The pitch angle θ is a sum of the inclination of the road 310 and the incline of the vehicle 100 relative to the road 310. The controller 120 obtains a roll angle estimate {circumflex over (φ)}, a pitch angle estimate {circumflex over (θ)}, and velocity component estimates and along the x and y axes, respectively. If a global X, Y reference is available through a GNSS 115, the controller 120 also obtains global position estimates {circumflex over (X)} and Ŷ along with heading angle estimate {circumflex over (ψ)}. The controller 120 implements a Kalman filter to solve kinematic equations, as detailed with reference to FIG. 4.

(13) FIG. 4 is a block diagram indicating the inputs and outputs involved in camera-based enhancement of vehicle kinematic state estimations according to one or more embodiments. Inputs and outputs shown in brackets are optional and may not be available, as further discussed. The kinematic equations are indicated as being solved by the controller 120. A Kalman filter uses observable states (i.e., inputs to the controller 120) to obtain unobservable states in the kinematic equations. The Kalman filter is a multi-rate Kalman filter that takes into account the different rates at which different vehicle sensors 140 provide data (i.e., the observable states) to the controller 120. The controller obtains the angle ψ.sub.0 and the distance y.sub.0 from the camera 110. As previously noted, one image is sufficient. Even though the camera 110 may be located at the front of the vehicle 100 to be forward-facing, for example, information may be transformed to the center of the vehicle 100, for example, to obtain the distance y.sub.0 from the center of the vehicle 100 to the tangent line T. From the IMU 105, the controller 120 obtains acceleration components a.sub.x, a.sub.y of the vehicle 100 and may additionally obtain roll rate ω.sub.x, pitch rate ω.sub.y, and yaw rate ω.sub.z. If the vehicle sensors 140 include a GNSS 115, the controller 120 obtains a global position X, Y of the vehicle 100 and a global frame of reference. From the odometer 125, the controller obtains the velocity component v.sub.x along the direction of travel.

(14) Each of the kinematic equations is discussed. Generally, the “⋅” above a parameter indicates rate (e.g., {dot over (θ)} indicates pitch rate) while, as previously indicated, “{circumflex over ( )}” above a parameter indicates an estimate (e.g., {circumflex over (θ)} indicates pitch estimate). For example, {dot over (θ)} indicates pitch rate, and {circumflex over (θ)} indicates pitch estimate determined using the Kalman filter and the observable states included in the equation for {dot over (v)}.sub.x in FIG. 4. The first kinematic equation relates to rate of displacement and is:
{dot over (y)}.sub.0≈v.sub.x sin(ψ.sub.0+β)  [EQ. 1]
As previously noted, velocity component v.sub.x along the direction of travel is an observable state provided by the odometer 125, and the angle ψ.sub.0 and the distance y.sub.0 are provided by the camera 110. Thus, the side slip angle β is determined using EQ. 1. As indicated in FIG. 1, the angle β is the angle between the x axis (of the vehicle 100 frame of reference) and the velocity vector v. While the velocity component v.sub.x is an input, as previously noted, the angle β may be used to determine the velocity component v.sub.y along they axis of the vehicle 100 frame of reference.

(15) The second equation shown in FIG. 4 is:
{dot over (v)}.sub.x=a.sub.x+v.sub.y{dot over (ψ)}+g sin θ  [EQ. 2]
In EQ. 2, g is the acceleration due to gravity. As previously noted, the velocity component v.sub.x is an input (i.e., observable) obtained from the odometer 125 and the acceleration component a.sub.x is obtained from the IMU 105. If the IMU 105 additionally provides pitch rate ω.sub.y ({dot over (θ)}) and yaw rate ω.sub.z, then pitch θ (time derivative of pitch rate {dot over (θ)}) and heading rate {dot over (ψ)} (same as yaw rate ω.sub.z) are available as observable values, as well. The velocity component v.sub.y may be solved by the Kalman filter.

(16) The third equation in FIG. 4 is:
{dot over (v)}.sub.y=a.sub.y−v.sub.x{dot over (ψ)}−g sin φ  [EQ. 3]
As previously noted, the acceleration component a.sub.y is obtained from the IMU 105, and the velocity component v.sub.x is obtained from the odometer 125. If the IMU 105 additionally provides roll rate ω.sub.x ({dot over (φ)}) and yaw rate ω.sub.z, then roll φ (time derivative of roll rate {dot over (φ)}) and heading rate {dot over (ψ)} (same as yaw rate ω.sub.z) are available as observables, as well. As noted with reference to EQ. 2, the velocity component v.sub.y may be solved by the Kalman filter. If the roll rate ω.sub.x ({dot over (φ)}) and pitch rate ω.sub.y ({dot over (θ)}) are provided by the IMU 105, then the Kalman filter will converge more quickly in EQS. 2 and 3. The yaw rate ω.sub.z is generally available. However, even if the IMU 105 does not provide the roll rate cox, pitch rate ω.sub.y, EQS. 2 and 3 may be used with zero as the initial values for roll rate ω.sub.x ({dot over (φ)}) and pitch rate ω.sub.y ({dot over (θ)}).

(17) If the GNSS 115 information in the global frame of reference is available and the IMU 105 provides roll rate ω.sub.x, pitch rate ω.sub.y, and yaw rate ω.sub.z, in addition to the acceleration components a.sub.x, a.sub.y, the following additional equations may be solved:
{dot over (X)}=v.sub.x sin ψ−v.sub.y cos ψ  [EQ. 4]
{dot over (Y)}=v.sub.x cos ψ+v.sub.y sin ψ  [EQ. 5]
ψ=ω.sub.z  [EQ. 6]
As previously noted, the controller 120 obtaining additional information from the IMU 105 facilitates estimation of heading angle ψ directly from the yaw rate ω.sub.z as in EQ. 4. In addition, from the heading angle ψ and velocity components v.sub.x, v.sub.x, the displacement of the vehicle 100 based on global position {dot over (X)} and {dot over (Y)} can be determined according to EQS. 4 and 5.

(18) FIG. 5 is a process flow of an exemplary method 500 of applying camera-based enhancement of vehicle kinematic state estimations, according to one or more embodiments, to control a vehicle 100. The processes shown in FIG. 5 may all be implemented by the controller 120 or by a combination of the controller 120 and other processors of the vehicle 100. At block 510, obtaining inputs and determining kinematic states includes the inputs and kinematic equations discussed with reference to FIG. 4. The kinematic states (e.g., pitch θ, roll φ) are provided along with steering angle δ and, optionally, an estimate of friction {circumflex over (μ)} to the process at block 520. At block 520, implementing a dynamic model results in an output of tire forces on the tires of the vehicle 100. At block 530, performing a force calculation results in a determination of external disturbances. At block 540, the external disturbances determined at block 530 are used to implement compensating for external disturbances (e.g., crosswinds) that the vehicle 100 is subjected to.

(19) While the above disclosure has been described with reference to exemplary embodiments, it will be understood by those skilled in the art that various changes may be made and equivalents may be substituted for elements thereof without departing from its scope. In addition, many modifications may be made to adapt a particular situation or material to the teachings of the disclosure without departing from the essential scope thereof. Therefore, it is intended that the present disclosure not be limited to the particular embodiments disclosed, but will include all embodiments falling within the scope thereof.