KALMAN FILTER BASED ROAD GRADE ESTIMATION METHOD USING ACCELEROMETER, GYROSCOPE, AND VEHICLE VELOCITY
20210300383 · 2021-09-30
Inventors
Cpc classification
B60W2420/905
PERFORMING OPERATIONS; TRANSPORTING
B60W2050/0031
PERFORMING OPERATIONS; TRANSPORTING
International classification
Abstract
Kalman filter based road grade estimation techniques use models of a longitudinal accelerometer, an angular pitch rate gyroscope, and a velocity sensor and their outputs, and fuses the sensor measurements to optimally estimate the road grade. The proposed Kalman filter formulation is unique in that it uses a mathematical model of the sensors where the gyroscope output is considered as the input and the combined accelerometer and velocity sensor output is considered as the output of the model whose states are to be estimated. By using this unique second-order state space model, a Kalman filter based estimation algorithm is developed to estimate road grade accurately in real-time. This estimated road grade is then being utilized by various vehicle efficiency and/or safety systems to improve vehicle efficiency and/or vehicle safety.
Claims
1. A road grade estimation system for a vehicle, the road grade estimation system comprising: a set of sensors including: an accelerometer configured to measure a longitudinal acceleration of the vehicle, a gyroscope configured to measure an angular pitch rate of the vehicle, and a velocity sensor configured to measure a longitudinal velocity of the vehicle; and a controller configured to: implement a Kalman filter that utilizes pitch rate as an input to estimate longitudinal acceleration and longitudinal velocity, receive, from the set of sensors, the measured longitudinal acceleration, angular pitch rate, and longitudinal velocity of the vehicle, estimate a grade of a road along which the vehicle is traveling using the implemented Kalman filter and the measured longitudinal acceleration, angular pitch rate, and longitudinal velocity of the vehicle, and control at least one operating parameter of the vehicle based on the estimated road grade.
2. The road grade estimation system of claim 1, wherein the controller is configured to estimate the road grade using only the Kalman filter and the measured longitudinal acceleration, angular pitch rate, and longitudinal velocity of the vehicle.
3. The road grade estimation system of claim 1, wherein the Kalman filter is based on the following second-order state space model of the set of sensors:
4. The road grade estimation system of claim 3, wherein the controller is configured to estimate the road grade using a time-varying method comprising: determining a stabilizing solution P by solving the differential Riccati equation {dot over (P)}=AP+PA.sup.T−PC.sup.TR.sup.−1CP+Q with initial value P(0) being equal to the covariance of the initial state x(0) online in real-time, where Q and R are the covariances of w and v, respectively, and P is a matrix that represents the covariance of the error x−{circumflex over (x)}; determining a gain L of the Kalman filter as L=−PC.sup.TR.sup.−1; and estimating the road grade by solving the second-order state space model:
{dot over ({circumflex over (x)})}=A{circumflex over (x)}+Bu−L(y−C{circumflex over (x)}),
ŷ=C{circumflex over (x)}, with zero initial condition.
5. The road grade estimation system of claim 3, wherein the controller is configured to estimate the road grade using a time-invariant method comprising: determining a solution P by solving the algebraic Riccati equation AP+PA.sup.T−PC.sup.TR.sup.−1CP+Q=0, where Q and R are the covariances of wand v, respectively; determining a gain L of the Kalman filter as L=−PC.sup.TR.sup.−1; and estimating the road grade by solving the second-order state space model:
{dot over ({circumflex over (x)})}=A{circumflex over (x)}+Bu−L(y−C{circumflex over (x)}),
ŷ=C{circumflex over (x)}, with zero initial condition.
6. The road grade estimation system of claim 3, wherein the controller is configured to estimate the road grade using a time-invariant method comprising: determining a solution P by solving the algebraic Riccati equation AP+PA.sup.T−PC.sup.TR.sup.−1CP+Q=0, where Q and R are the covariances of wand v, and determining a gain L of the Kalman filter with the following:
7. The road grade estimation system of claim 1, wherein the at least one operating parameter of the vehicle corresponds to a vehicle efficiency system or a vehicle safety system.
8. The road grade estimation system of claim 7, wherein the vehicle efficiency system corresponds to at least one of torque optimization, torque splitting, battery management, transmission shift scheduling, fuel tank distance-to-empty prediction, and cruise control.
9. The road grade estimation system of claim 7, wherein the vehicle safety system corresponds to at least one of anti-lock braking, active suspension control, free fall detection, unintended acceleration/deceleration detection, rollover detection, collision avoidance, stability control, and autonomous driving.
10. A road grade estimation method for a vehicle, the road grade estimation method comprising: receiving, by a controller and from a set of sensors, measured longitudinal acceleration, angular pitch rate, and longitudinal velocity of the vehicle, wherein the set of sensors comprises an accelerometer configured to measure the longitudinal acceleration of the vehicle, a gyroscope configured to measure the angular pitch rate of the vehicle, and a velocity sensor configured to measure a longitudinal velocity of the vehicle; implementing, by the controller, a Kalman filter that utilizes angular pitch rate as an input to estimate longitudinal acceleration and longitudinal velocity; estimating, by the controller, a grade of a road along which the vehicle is traveling using the implemented Kalman filter and the measured longitudinal acceleration, angular pitch rate, and longitudinal velocity of the vehicle; and controlling, by the controller, at least one operating parameter of the vehicle based on the estimated road grade.
11. The road grade estimation method of claim 10, estimating the road grade comprises estimating, by the controller, the road grade using only the Kalman filter and the measured longitudinal acceleration, angular pitch rate, and longitudinal velocity of the vehicle.
12. The road grade estimation method of claim 10, wherein the Kalman filter is based on the following second-order state space model of the set of sensors:
13. The road grade estimation method of claim 12, estimating the road grade comprises using, by the controller, a time-varying method comprising: determining a stabilizing solution P by solving the differential Riccati equation {dot over (P)}=AP+PA.sup.T−PC.sup.T R.sup.−1CP+Q with initial value P(0) being equal to the covariance of the initial state x(0) online in real-time, where Q and R are the covariances of w and v, respectively, and P is a matrix that represents the covariance of the error x−{circumflex over (x)}; determining a gain L of the Kalman filter as L=−PC.sup.TR.sup.−1; and estimating the road grade by solving the second-order state space model:
{dot over ({circumflex over (x)})}=A{circumflex over (x)}+Bu−L(y−C{circumflex over (x)}),
ŷ=C{circumflex over (x)}, with zero initial condition.
14. The road grade estimation system of claim 12, estimating the road grade comprises using, by the controller, a time-invariant method comprising: determining a solution P by solving the algebraic Riccati equation AP+PA.sup.T−PC.sup.TR.sup.−1CP+Q=0, where Q and R are the covariances of wand v, respectively; determining a gain L of the Kalman filter as L=−PC.sup.TR.sup.−1; and estimating the road grade by solving the second-order state space model:
{dot over ({circumflex over (x)})}=A{circumflex over (x)}+Bu−L(y−C{circumflex over (x)}),
ŷ=C{circumflex over (x)}, with zero initial condition.
15. The road grade estimation method of claim 12, wherein estimating the road grade comprises using, by the controller, a time-invariant method comprising: determining a solution P by solving the algebraic Riccati equation AP+PA.sup.T−PC.sup.TR.sup.−1CP+Q=0, where Q and R are the covariances of wand v, and determining the gain L of the Kalman filter with the following:
16. The road grade estimation method of claim 10, wherein the at least one operating parameter of the vehicle corresponds to a vehicle efficiency system or a vehicle safety system.
17. The road grade estimation method of claim 16, wherein the vehicle efficiency system corresponds to at least one of torque optimization, torque splitting, battery management, transmission shift scheduling, fuel tank distance-to-empty prediction, and cruise control.
18. The road grade estimation method of claim 16, wherein the vehicle safety system corresponds to at least one of anti-lock braking, active suspension control, free fall detection, unintended acceleration/deceleration detection, rollover detection, collision avoidance, stability control, and autonomous driving.
Description
BRIEF DESCRIPTION OF THE DRAWINGS
[0010]
[0011]
[0012]
[0013]
DETAILED DESCRIPTION
[0014] As previously mentioned, there exists an opportunity for more accurate/reliable and faster vehicle road grade estimation techniques. Accordingly, Kalman filter based road grade estimation techniques using accelerometer, a gyroscope, and vehicle velocity are presented. The accelerometer measures the longitudinal proper acceleration of the vehicle, and the accelerometer measurement is almost drift free but it is heavily corrupted by the accelerometer noise. The gyroscope, on the other hand, measures the angular pitch rate of the vehicle, and the gyroscope measurement is quite clean but it is substantially disturbed by the gyroscope bias. Thus, the accelerometer and the gyroscope have complementary characteristics. Lastly, the velocity sensor measures the longitudinal velocity of the vehicle, and the velocity sensor measurement is also considerably corrupted by the measurement noise.
[0015] The proposed Kalman filter based estimation techniques of the present disclosure uses the models of the sensors and their outputs, and fuses the sensor measurements to optimally estimate the road grade. In one exemplary implementation, these techniques estimate road grade using only the Kalman filter and these three sensor inputs. The proposed Kalman filter formulation is unique in that it uses a mathematical model of the sensors where the gyroscope output is considered as the input and the combined accelerometer and velocity sensor output is considered as the output of the model whose states are to be estimated. By using this unique second-order state space model, a Kalman filter based estimation algorithm is developed to estimate road grade accurately in real time. This estimated road grade could then be utilized by various vehicle efficiency and/or safety systems to improve vehicle efficiency (e.g., fuel economy) and/or vehicle safety (e.g., anti-lock braking).
[0016] Referring now to
[0017] Non-limiting examples of the functionality performed by the vehicle efficiency systems include torque optimization, torque splitting, battery management, transmission shift scheduling, fuel tank distance-to-empty prediction, and cruise control. Similarly, non-limiting examples of the functionality performed by the vehicle safety systems include anti-lock braking, active suspension control, free fall detection, unintended acceleration/deceleration detection, rollover detection, collision avoidance, stability control, and autonomous driving. For example only, the estimated road grade could be utilized to determine engine loading, to estimate wheel slip, and/or to select a next transmission gear. It will be appreciated that these are non-limiting examples and that the estimated road grade could be utilized for any other suitable vehicle functionality. It will also be appreciated that at least some of the above-described functionality could be executed by the controller 128, either alone or in coordination with the above-described systems. The specific road grade estimation techniques will now be described in greater detail with reference to
[0018] Referring now to
[0019] The accelerometer 132 can be modeled as:
a.sub.x={dot over (v)}.sub.x+g sin γ+n.sub.a.sub.
where a.sub.x is the accelerometer output, {dot over (v)}.sub.x is the longitudinal acceleration of the vehicle, and n.sub.a.sub.
ω.sub.y=−y+b.sub.y+n.sub.ω.sub.
{dot over (b)}.sub.y=n.sub.b.sub.
where ω.sub.y is the gyroscope output, b.sub.y is the gyroscope bias, n.sub.ω.sub.
s.sub.x=v.sub.x+n.sub.s.sub.
where s.sub.x is the velocity sensor output, v.sub.x is the longitudinal velocity of the vehicle, and n.sub.s.sub.
[0020] Defining θ=−y and noting that sin γ≈γ when γ is small, it follows that:
θ.sub.as=θ+w.sub.as,
ω.sub.g={dot over (θ)}+b+w.sub.g,
{dot over (b)}=w.sub.b, (4)
where θ.sub.as=({dot over (s)}.sub.x−a.sub.x)/g is the measured pitch angle, θ=−γ is the actual pitch angle, ω.sub.g=ω.sub.y is the measured angular pitch rate, b=b.sub.y is the gyroscope bias, w.sub.as=({dot over (n)}.sub.s.sub.
{dot over (x)}.sub.1=x.sub.2+u+w.sub.1,
x.sub.2=w.sub.2,
y=x.sub.1+v, (5)
or compactly as:
This second order state-space model can then be utilized to develop the Kalman filter based estimator.
[0021] With these notations, the road grade estimation problem is to estimate the pitch angle θ using the measurements θ.sub.as (from the accelerometer 132 and the velocity sensor 140) and ω.sub.g (from the gyroscope 136) or, equivalently, estimate the output Cx (road grade or pitch angle) using the input u (output of the gyroscope 136) and the output y (combined output of the accelerometer 132 and the velocity sensor 140). As previously discussed, it is known that the combined measurement provided by the accelerometer 132 and the velocity sensor 140 is almost drift free but substantially corrupted by the measurement noise due especially to the accelerometer measurement noise while the measurement provided by the gyroscope 136 is quite clean but considerably disturbed by the gyroscope bias. Equivalently, the combined accelerometer and velocity sensor measurement has a good low frequency characteristic while the gyroscope measurement has a good high frequency characteristic. Since these two measurements have complementary characteristics, they can be combined to obtain a better estimate of the road grade. For purposes of this disclosure, it is also assumed that w and v are uncorrelated zero-mean Gaussian white noise processes. Although this assumption is not strictly true, it is a fairly plausible assumption and it is usually satisfied approximately for the sensors considered.
[0022] At 208, the controller 128 obtains the measurements from the sensors, and uses a Kalman filter to fuse these measurements to estimate the road grade. The road grade estimation problem is solved using the standard Kalman filter estimation theory using the specifically developed unique mathematical model for this purpose. Both time-varying and time-invariant Kalman estimators are obtainable analytically. A simplified special case of the time-invariant Kalman filter estimator is also obtainable. The Kalman filter is an optimal method to estimate the true values of signals from imperfect measurements containing noises and other inaccuracies as encountered by conventional road grade estimation methods. It produces the estimated values from a weighted average of measurements and predictions by weighting the values with the least uncertainty (measured by the covariances of the signals) more. The Kalman filter uses the model of the system, its controlled input and measured output to form an optimal estimate of the road grade that is better than the estimate obtained by using any one of them alone.
[0023] Letting the covariances of w and v be Q and R, respectively, the Kalman filter to estimate the output Cx is characterized by
{dot over ({circumflex over (x)})}=A{circumflex over (x)}+Bu−L(y−C{circumflex over (x)}),
ŷ=C{circumflex over (x)}, (8)
where {circumflex over (x)} is the estimate of the state x (pitch angle and gyroscope bias), ŷ is the estimate of Cx (pitch angle), L is the Kalman gain
L=−PC.sup.TR.sup.−1, (9)
and P is the stabilizing solution of the differential Riccati equation
{dot over (P)}=AP+PA.sup.T−PC.sup.TR.sup.−1CP+Q (10)
with the initial value P(0) being equal to the covariance of the initial state x(0). The matrix P is the covariance of the error x−{circumflex over (x)}. This form of the Kalman filter is called the time-varying Kalman filter.
[0024] Since the model of the system is time-invariant, {dot over (P)}=0 in the steady-state and the differential Riccati equation in this case reduces to the algebraic Riccati equation:
AP+PA.sup.T−PC.sup.TR.sup.−1CP+Q=0. (11)
This resulting simplified form of the Kalman filter is called the time-invariant (or steady state) Kalman filter. Moreover, it follows that
Ŷ(s)=−C(sI−A−LC).sup.−1LY(s)+C(sI−A−LC).sup.−1BU(s), (12)
where Ŷ(s), Y(s) and U(s) are the Laplace transforms of ŷ(t), y(t) and u(t), respectively. Hence, the road grade can be estimated from the measurements using the Kalman filter described above as shown in
[0025] Assuming that w.sub.1 and w.sub.2 are also uncorrelated, the time-invariant Kalman filter estimator can be further simplified. With:
it follows that:
Note that this Kalman filter is also equivalent to using the optimal low-pass filter:
and the complementary high-pass filter:
Note further that the estimated road grade or pitch angle is formed by combining the low-pass filtered pitch angle estimate obtained from the accelerometer and velocity sensor with the high-pass filtered pitch angle estimate obtained from the gyroscope 136 as expected.
[0026] At 212, the controller 128 estimates the grade of a road along which the vehicle is traveling using the Kalman filter (time-varying or one of the time-invariant versions) and the measured longitudinal acceleration, angular pitch rate, and longitudinal velocity of the vehicle as described above. Lastly, at 216, the controller 128 controls at least one operating parameter of the vehicle based on the estimated road grade. As previously discussed, this could include controlling at least one operating parameter of the vehicle corresponding to the vehicle efficiency system(s) 116 or the vehicle safety system(s) 120, including, but not limited to, torque optimization, torque splitting, battery management, transmission shift scheduling, fuel tank distance-to-empty prediction, cruise control, anti-lock braking, active suspension control, free fall detection, unintended acceleration/deceleration detection, rollover detection, collision avoidance, stability control, and autonomous driving. The method 200 then ends or returns to 204 for one or more additional cycles.
[0027] It will be appreciated that the term “controller” as used herein refers to any suitable control device or set of multiple control devices that is/are configured to perform at least a portion of the techniques of the present disclosure. Non-limiting examples include an application-specific integrated circuit (ASIC), one or more processors and a non-transitory memory having instructions stored thereon that, when executed by the one or more processors, cause the controller to perform a set of operations corresponding to at least a portion of the techniques of the present disclosure. The one or more processors could be either a single processor or two or more processors operating in a parallel or distributed architecture.
[0028] It should be understood that the mixing and matching of features, elements, methodologies and/or functions between various examples may be expressly contemplated herein so that one skilled in the art would appreciate from the present teachings that features, elements and/or functions of one example may be incorporated into another example as appropriate, unless described otherwise above.