CN104061899A - Kalman filtering based method for estimating roll angle and pitching angle of vehicle - Google Patents

Abstract

The invention discloses a Kalman filtering based method for estimating roll angle and pitching angle of a vehicle. Aiming at four-wheel land vehicles, the method establishes a kinematic model conforming to the driving characteristics of the vehicles, and further realizes real-time and accurate estimation of roll angle and pitching angle of the vehicles by a Kalman filtering method. The method provided by the invention is also applicable for estimation of large roll angle and pitching angle, meets application requirements under complicated conditions, and only requires a low cost vehicle sensor. The estimated roll angle and pitching angle information is key parameters for vehicle integrated navigation and positioning.

Description

A kind of vehicle side inclination angle and angle of pitch method of estimation based on Kalman filtering

Technical field

The present invention relates to a kind of vehicle side inclination angle and angle of pitch method of estimation based on Kalman filtering, its object is to estimate for Integrated Navigation for Land Vehicle and location provide high precision, low cost, high real-time, vehicle attitude angle applied widely, belongs to bus location navigation field.

Background technology

Intelligent transportation system (Intelligent Transportation Systems, ITS) is day by day subject to the attention of the developed countries such as Europe, Japan, the U.S. and becomes its study hotspot.Development intelligent vehicle and the autonomous driving system of vehicle are the optimal selections that realizes safety, independently travels efficiently by improving the scheme of vehicle self intelligence.An important foundation of development intelligent transportation system is Real-time Obtaining vehicle locating information accurately and reliably.The deficiency that cannot position in the time that antenna is blocked in order to make up GPS, development GPS/INS integrated navigation system has been subject to the extensive concern of Chinese scholars.

Due to the existence of the road vertical and horizontal gradient and the motion of vehicle suspension, cause land four-wheel car can have in the process of moving certain side rake angle and the angle of pitch, its value is generally less, negligible under many circumstances.But in GPS/INS integrated navigation system, side rake angle and the angle of pitch are the key parameters that carries out dead reckoning, can not be ignored.This be due to land four-wheel car in the process of moving, the acceleration of vehicle is generally much smaller than acceleration of gravity, will cause so less side rake angle and the angle of pitch also can produce larger error in the time measuring vertical and horizontal acceleration, when the accumulation in time of these errors can cause reckoning speed and positional information, produce larger error.So, the attitude angle such as side rake angle and the angle of pitch is carried out to accurately real-time measurement or estimation is the basis of carrying out high-precision GPS/INS integrated navigation location.

What at present, the strap-down navigation technology based on Inertial Measurement Unit (IMU, Inertial Measurement Unit) had developed is very ripe.An IMU has generally comprised the accelerometer of three single shafts and the gyro of three single shafts, and this also causes its price comparison costliness, particularly three gyrostatic prices.Patent " a kind of vehicle side inclination angle and angle of pitch method of estimation based on recurrence least square " (publication number: CN103625475 A) has proposed a kind of vehicle attitude angle evaluation method based on low cost MEMS (Micro-Electro-Mechanic System, MEMS (micro electro mechanical system)) sensor.The method only needs two MEMS acceleration transducers, and MEMS gyroscope and vehicle speed sensor can complete the estimation of vehicle side inclination angle and the angle of pitch.But the method is only considered the operating mode that road surface cross fall rate and head fall rate are less than 20%, and now the angle of pitch and side rake angle are all less, approximates processing so adopted in estimation procedure.But under complicated road environment, cross fall rate and the head fall rate on road surface may reach 30%.On the special road section of some proving grounds, cross fall rate and the head fall rate on road surface even can reach 55%.In these situations, the angle of pitch and side rake angle are larger, can not approximate processing, and said method can be restricted in actual application.

In order to solve the deficiency of above-mentioned technology in application, patent of the present invention proposes a kind of vehicle angle of pitch and side rake angle evaluation method based on Kalman filtering.Kalman filter is the optimal State Estimation wave filter taking Minimum Mean Square Error as criterion, it does not need to store measured value in the past, only carry out recursion calculating according to the estimated value of current observed reading and previous moment, just can realize the estimation to live signal, have the advantages that memory data output is little, algorithm is easy.The inventive method can be for estimating compared with the angle of pitch of wide-angle and side rake angle under complex working condition, and the scope of application is wider, for automobile navigation location provides one more reliable attitude angle evaluation method.

Summary of the invention

The object of the invention is to overcome the deficiencies in the prior art, a kind of vehicle side inclination angle and angle of pitch method of estimation based on Kalman filtering proposed, the method precision is high, cost is low, real-time is good, applied widely, can meet the application demand of vehicle under complex environment.

The technical solution used in the present invention is as follows: a kind of vehicle side inclination angle and angle of pitch method of estimation based on Kalman filtering, it is characterized in that: the present invention is directed to land locomotion four-wheel car, foundation meets the system motion model of its travelling characteristic, further by Kalman filtering algorithm realize to vehicle side inclination angle and the angle of pitch in real time, accurately estimate, can be for estimating compared with the angle of pitch of wide-angle and side rake angle under complicated road environment, and only need low cost vehicle-mounted sensor, cost is lower.Concrete steps comprise:

1) set up the system motion model of vehicle traveling process.

Ignore earth rotation speed, suppose that rate of pitch, roll velocity and the vertical velocity of vehicle is zero, can set up the kinetics equation of Vehicle Driving Cycle process:

$\begin{array}{c}{\stackrel{\·}{v}}_x=a_x+{\mathrm{\ω}}_z{v}_y+g\sin \mathrm{\θ}\\ {\stackrel{\·}{v}}_y=a_y-{\mathrm{\ω}}_z{v}_x-g\sin \mathrm{\φ}\cos \mathrm{\θ}\end{array}---\left(1\right)$

In formula (1), v _x, v _yrepresent respectively longitudinal velocity and the transverse velocity of vehicle, a _x, a _yrepresent respectively longitudinal acceleration and the transverse acceleration of vehicle, ω _zthe yaw velocity that represents vehicle, the definition of above-mentioned variable all defines in bodywork reference frame, and g represents acceleration of gravity, φ, θ represents respectively side rake angle and the angle of pitch of vehicle, upper mark " " represents differential, as represent v _xdifferential.

By (1) Shi Ke get

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{{\stackrel{\·}{v}}_x-a_x-{\mathrm{\ω}}_z{v}_y}{g}\right)\\ \mathrm{\φ}=\arcsin \left(\frac{a_y-{\mathrm{\ω}}_z{v}_x-{\stackrel{\·}{v}}_y}{g\cos \mathrm{\θ}}\right)\end{array}---\left(2\right)$

In formula (2), longitudinally the differential of the speed of a motor vehicle can obtain time differentiate by longitudinal speed of a motor vehicle.

Consider in normal vehicle operation process v _ywith numerical value all less, thereby can ignore, formula (2) can be reduced to:

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{{\stackrel{\·}{v}}_x-a_x}{g}\right)\\ \mathrm{\φ}=\arcsin \left(\frac{a_y-{\mathrm{\ω}}_z{v}_x}{g\cos \mathrm{\θ}}\right)\end{array}---\left(3\right)$

2) set up state equation and the observation equation of Kalman filtering.

The matrix form of the Kalman filtering state equation after discretize is:

In formula (4), k represents the discretize moment; System state vector is X=[x ₁x ₂] ' and x ₁=θ, x ₂=φ, i.e. X=[θ φ] ', superscript in the present invention ' represent matrix transpose; W (k-1) represents system white Gaussian noise vector and the W=[w of zero-mean ₁w ₂] ', be w wherein ₁, w ₂represent respectively two system white Gaussian noise components, the system noise covariance matrix Q (k-1) that W (k-1) is corresponding is: $Q(k-1)=\left[\begin{array}{cc}{\mathrm{\σ}}_{w_1}^2& 0\\ 0& {\mathrm{\σ}}_{w_2}^2\end{array}\right],$ Wherein represent respectively system white Gaussian noise w ₁, w ₂corresponding variance; State-transition matrix this is due in Vehicle Driving Cycle process, and side rake angle and the angle of pitch are all to change continuously slowly, can think that the side rake angle of current sampling instant and the angle of pitch equal side rake angle and the angle of pitch of next sampling instant.

The matrix form of Kalman filtering observation equation discretize is:

Z(k)＝H(k)·X(k)+V(k) (5)

In formula (5), Z is observation vector, and H is observation battle array, and V represents the mutual incoherent zero-mean observation white noise vector with W.Because observation vector and state vector all refer to side rake angle and the angle of pitch, so $H\left(k\right)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$ $Z\left(k\right)=\left[\begin{array}{c}{\mathrm{\θ}}_m\left(k\right)\\ {\mathrm{\φ}}_m\left(k\right)\end{array}\right],V=\left[\begin{array}{c}{n}_{{\mathrm{\θ}}_m}\\ n_{{\mathrm{\φ}}_m}\end{array}\right],$ Wherein θ _mand φ (k) _m(k) be respectively the angle of pitch and the values of camber angles that directly calculate by measurement value sensor, according to formula (3), have:

$\begin{array}{c}{\mathrm{\θ}}_m=\arcsin \left(\frac{{\stackrel{\·}{v}}_{x\_m}-a_{x\_m}}{g}\right)\\ {\mathrm{\φ}}_m=\arcsin \left(\frac{a_{y\_m}-{\mathrm{\ω}}_{z\_m}{v}_{x\_m}}{g\cos {\mathrm{\θ}}_m}\right)\end{array}---\left(6\right)$

In formula (6), a _{x_m}, a _{y_m}with ω _{z_m}represent to utilize respectively measured longitudinal acceleration, transverse acceleration and the yaw velocity of low cost MEMS (Micro-Electro-Mechanic System, MEMS (micro electro mechanical system)) sensor, v _{x_m}represent the vehicular longitudinal velocity obtaining by vehicle speed sensor, represent v _{x_m}differential, the longitudinal velocity signal measured by vehicle speed sensor obtains time differentiate,, at each discrete moment k, has:

${\stackrel{\·}{v}}_{x\_m}\left(k\right)=\frac{v_{x\_m}\left(k\right)-v_{x\_m}(k-1)}{\mathrm{dt}}---\left(7\right)$

In formula (7), dt represents sampling time interval, in the present invention, and dt=0.01 (second); represent by formula (6) calculate obtain the vehicle angle of pitch observation noise and that average is 0, variance is white Gaussian noise, represent through type (6) calculate obtain vehicle side inclination angle observation noise and that average is 0, variance is white Gaussian noise; The observation noise variance battle array R that V is corresponding can be expressed as $R=\left[\begin{array}{cc}{{\mathrm{\σ}}_{{\mathrm{\θ}}_m}}^2& 0\\ 0& {{\mathrm{\σ}}_{{\mathrm{\φ}}_m}}^2\end{array}\right];$

3) required onboard sensor is installed

From formula (6), only need record longitudinal velocity, longitudinal acceleration, transverse acceleration and the yaw velocity of vehicle, can obtain the required observed quantity of Kalman filtering observation equation, thereby utilize Kalman filtering recursive algorithm to estimate the angle of pitch and the side rake angle of vehicle; Therefore, only need two low cost MEMS acceleration transducers, low cost MEMS gyroscope and vehicle speed sensor can meet measurement requirement;

Wherein two low cost MEMS acceleration transducers are installed near vehicle centroid position, and one along the bodywork reference frame longitudinal axis, and in order to measure longitudinal acceleration, one along bodywork reference frame transverse axis, in order to measure transverse acceleration.Low cost MEMS gyroscope is also installed near vehicle centroid position, installs, in order to measure yaw velocity along the vertical axle of bodywork reference frame.Vehicle speed sensor is used for measuring longitudinal speed of a motor vehicle, and the sensors such as Hall element or photoelectric code disk all can adopt, and does not limit, but require vehicle speed measurement trueness error to be less than 0.05 meter per second at this.Recording after longitudinal vehicle speed signal, trying to achieve its differential according to formula (7), for calculating observation amount;

4) Kalman filtering recursion:

For formula (4) and the described system state equation of formula (5) and measurement equation, use kalman filtering theory, set up standard filtering recursive process below, this recursive process comprises that the time upgrades and measurement is upgraded, the first two steps of recursive process are to upgrade the time below, and remaining three steps are upgraded for measuring:

Time upgrades:

State one-step prediction equation

One-step prediction error covariance matrix

Measure and upgrade:

Filter gain matrix K (k)=P (k, k-1) H ^t(k) [H (k) P (k, k-1) H ' (k)+R (k)] ^-1

State estimation $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\·\hat{X}(k,k-1)]]$

Estimation error variance battle array P (k)=[I-K (k) H (k)] P (k, k-1)

After above-mentioned recursion is calculated, can estimate in real time side rake angle and the angle of pitch of vehicle at each discrete moment k.

Advantage of the present invention and remarkable result:

(1) method of estimation that the present invention proposes can be estimated vehicle side inclination angle and the angle of pitch compared with wide-angle, in ensureing estimation precision, meets the application demand under complex working condition;

(2) the present invention carries out Rational Simplification to vehicle dynamic model according to Vehicle Driving Cycle feature, thereby set up system state equation and the observation equation of Kalman filtering, and utilizing Kalman filtering recursive algorithm to carry out the estimation of side rake angle and the angle of pitch, estimated result can meet precision and the requirement of real-time of practical application;

(3) sensor used in the present invention is low cost sensor, is convenient to large-scale promotion.

Embodiment

Intelligent transportation system (Intelligent Transportation Systems, ITS) is day by day subject to the attention of the developed countries such as Europe, Japan, the U.S. and becomes its study hotspot.Development intelligent vehicle and the autonomous driving system of vehicle are the optimal selections that realizes safety, independently travels efficiently by improving the scheme of vehicle self intelligence.An important foundation of development intelligent transportation system is Real-time Obtaining vehicle locating information accurately and reliably.The deficiency that cannot position in the time that antenna is blocked in order to make up GPS, development GPS/INS integrated navigation system has been subject to the extensive concern of Chinese scholars.

Due to the existence of the road vertical and horizontal gradient and the motion of vehicle suspension, can there is in the process of moving certain side rake angle and the angle of pitch in land four-wheel car, its value is generally less, negligible under many circumstances, but in the time carrying out the dead reckoning of GPS/INS integrated navigation, but can not be ignored.This is because land vehicle acceleration is in the process of moving generally much smaller than acceleration of gravity, to such an extent as to less side rake angle and the angle of pitch produce larger error may cause measuring vertical and horizontal acceleration time, produce larger error when the accumulation in time of these errors can cause reckoning speed and positional information.So, the important leverage that GPS/INS integrated navigation was measured or estimated to carry out to side rake angle and the angle of pitch accurately, its accuracy is a key factor that affects vehicle location precision.

Be commonly used to determine that the method for the attitude angle information such as side rake angle and the angle of pitch is to use complete sextuple Inertial Measurement Unit IMU (Inertial Measurement Unit), this IMU comprises 3 accelerometers and 3 rate-of-turn gyroscopes, utilize the kinematic relation between IMU output quantity and the differential of angle information, and ignore earth rotation speed, dynamics of vehicle process can be modeled as [herein can list of references: H.Eric Tseng a, Li Xu, Davor Hrovat, Estimation of landvehicle roll and pitch angles[J] .Vehicle System Dynamics:International Journal of VehicleMechanics and Mobility, 2007, 45 (5): 433-443.]:

$\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}={\mathrm{\ω}}_x+({\mathrm{\ω}}_y\sin \mathrm{\φ}+{\mathrm{\ω}}_z\cos \mathrm{\φ})\tan \mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}={\mathrm{\ω}}_y\cos \mathrm{\φ}-{\mathrm{\ω}}_z\sin \mathrm{\φ}\\ \stackrel{\·}{\mathrm{\ψ}}=({\mathrm{\ω}}_y\sin \mathrm{\φ}+{\mathrm{\ω}}_z\cos \mathrm{\φ})/\cos \mathrm{\θ}\end{array}---\left(1\right)$

$\begin{array}{c}{\stackrel{\·}{v}}_x=a_x+{\mathrm{\ω}}_z{v}_y-{\mathrm{\ω}}_y{v}_z+g\sin \mathrm{\θ}\\ {\stackrel{\·}{v}}_y=a_y-{\mathrm{\ω}}_z{v}_x+{\mathrm{\ω}}_x{v}_z-g\sin \mathrm{\φ}\cos \mathrm{\θ}\\ {\stackrel{\·}{v}}_z=a_z+{\mathrm{\ω}}_y{v}_x-{\mathrm{\ω}}_x{v}_y-g\cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}---\left(2\right)$

In formula, ω _x, ω _yand ω _zrepresent respectively around the angular velocity of the bodywork reference frame longitudinal axis, transverse axis and vertical axle, v _x, v _yand v _zrepresent respectively along the linear velocity of the bodywork reference frame longitudinal axis, transverse axis and vertical axle, a _x, a _yand a _zrepresent respectively along the acceleration of the bodywork reference frame longitudinal axis, transverse axis and vertical axle; φ, θ, and ψ represents respectively inclination, pitching and three Eulerian angle of yaw; G represents acceleration of gravity; Upper mark " " represents differential, as represent v _xdifferential.

The attitude angle information of vehicle can be tried to achieve by the navigation calculation algorithm of sextuple IMU, and this has and relate in a large amount of vehicle location documents.But sextuple IMU is expensive, particularly three gyrostatic prices.Although the measuring accuracy of MEMS (Micro-Electro-Mechanic System, MEMS (micro electro mechanical system)) sensor is lower than traditional sensors, because it has the outstanding feature that cost is low, its application is still more and more extensive.The present invention studies how to use the MEMS sensor of trying one's best few side rake angle and the angle of pitch of accurately estimating vehicle in conjunction with data processing method, to overcome the shortcoming that MEMS sensor accuracy is low.Meanwhile, few sensor of trying one's best also can further reduce costs.The data processing method using also will ensure that side rake angle and the angle of pitch to wide-angle is applicable equally, to meet the application demand under complex working condition.

By formula (1), can find out in order to estimate side rake angle and the angle of pitch, do not need yaw angle information ψ.Meanwhile, because side rake angle and the angle of pitch of vehicle are all slowly to change continuously, its corresponding magnitude of angular velocity is less, and vehicle vertical velocity is general also less, can ignore.Therefore, can reasonably think ω _x≈ 0, ω _y≈ 0, ν _z≈ 0.Formula (1) and (2) can be reduced to:

$\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}={\mathrm{\ω}}_z\cos \mathrm{\φ}\tan \mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=-{\mathrm{\ω}}_z\sin \mathrm{\φ}\end{array}---\left(3\right)$

$\begin{array}{c}{\stackrel{\·}{v}}_x=a_x+{\mathrm{\ω}}_z{v}_t+g\sin \mathrm{\θ}\\ {\stackrel{\·}{v}}_y=a_y-{\mathrm{\ω}}_z{v}_x-g\sin \mathrm{\φ}\cos \mathrm{\θ}\end{array}---\left(4\right)$

Known according to formula (3), if the original state of vehicle is known and the yaw velocity of vehicle can obtain, the side rake angle of vehicle and the angle of pitch can be tried to achieve by integration method.But in fact, direct integral method, due to sensor error and inevitable numerical operation error, can cause larger drift, particularly in the time using cheaply MEMS sensor.Therefore, the present invention does not adopt direct integral method, but utilizes formula (4) to propose a kind of vehicle side inclination angle and angle of pitch method of estimation based on Kalman filtering algorithm.Kalman filter is the optimal State Estimation wave filter taking Minimum Mean Square Error as criterion, it does not need to store measured value in the past, only carry out recursion calculating according to the estimated value of current observed reading and previous moment, just can realize the estimation to live signal, have the advantages that memory data output is little, algorithm is easy.

By (4) Shi Ke get

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{{\stackrel{\·}{v}}_x-a_x-{\mathrm{\ω}}_z{v}_y}{g}\right)\\ \mathrm{\φ}=\arcsin \left(\frac{a_y-{\mathrm{\ω}}_z{v}_x-{\stackrel{\·}{v}}_y}{g\cos \mathrm{\θ}}\right)\end{array}---\left(5\right)$

In formula (5), longitudinally the differential of the speed of a motor vehicle can obtain time differentiate by longitudinal speed of a motor vehicle, considers in normal vehicle operation v _ywith numerical value is less, thereby can ignore, and formula (5) can be reduced to:

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{{\stackrel{\·}{v}}_x-a_x}{g}\right)\\ \mathrm{\φ}=\arcsin \left(\frac{a_y-{\mathrm{\ω}}_z{v}_x}{g\cos \mathrm{\θ}}\right)\end{array}---\left(6\right)$

The vertical and horizontal gradient on road surface is the principal element that affects vehicle side inclination angle and the angle of pitch, and the vertical and horizontal gradient is larger, and side rake angle and the angle of pitch are also larger.The vertical and horizontal gradient of general road is all less, carries out side rake angle and the angle of pitch while solving, can approximate processing, that is:

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{{\stackrel{\·}{v}}_s-a_x}{g}\right)\≈\frac{{\stackrel{\·}{v}}_x-a_x}{g}\\ \mathrm{\φ}=\arcsin \left(\frac{a_y-{\mathrm{\ω}}_z{v}_x}{g\cos \mathrm{\θ}}\right)\≈\frac{a_y-{\mathrm{\ω}}_z{v}_x}{g\cos \mathrm{\θ}}\end{array}---\left(7\right)$

But consider under complicated road environment, cross fall rate and the head fall rate on road surface may reach 30%.On some special road sections of proving ground, cross fall rate and the head fall rate on road surface even can reach 55%.In these situations, side rake angle and the angle of pitch are all larger, in the time solving arcsin computing, can not approximate processing.This patent is considered application demand under complex working condition exactly, carries out Kalman filtering reckoning under the prerequisite that does not approximate processing.Set up state equation and the observation equation of Kalman filtering below.

The matrix form of the Kalman filtering state equation after discretize is:

In formula (8), k represents the discretize moment; System state vector is X=[x ₁x ₂] ' and x ₁=θ, x ₂=φ, i.e. X=[θ φ] ', superscript in the present invention ' represent matrix transpose; W (k-1) represents system white Gaussian noise vector and the W=[w of zero-mean ₁w ₂] ', be w wherein ₁, w ₂represent respectively two system white Gaussian noise components, the system noise covariance matrix Q (k-1) that W (k-1) is corresponding is $Q(k-1)=\left[\begin{array}{cc}{\mathrm{\σ}}_{w_1}^2& 0\\ 0& {\mathrm{\σ}}_{w_2}^2\end{array}\right],$ Wherein represent respectively system white Gaussian noise w ₁, w ₂corresponding variance; State-transition matrix is this is due in Vehicle Driving Cycle process, and its side rake angle and the angle of pitch are all to change continuously slowly, can think that the side rake angle of a upper sampling instant and the angle of pitch equal side rake angle and the angle of pitch of next sampling instant.

The matrix form of Kalman filtering observation equation discretize is:

Z(k)＝H(k)·X(k)+V(k) (9)

In formula (9), Z is observation vector, and H is observation battle array, and V represents the mutual incoherent zero-mean observation white noise vector with W.Because observation vector and state vector all refer to side rake angle and the angle of pitch, so $H\left(k\right)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$ $Z\left(k\right)=\left[\begin{array}{c}{\mathrm{\θ}}_m\left(k\right)\\ {\mathrm{\φ}}_m\left(k\right)\end{array}\right],V=\left[\begin{array}{c}{n}_{{\mathrm{\θ}}_m}\\ n_{{\mathrm{\φ}}_m}\end{array}\right],$ Wherein θ _mand φ (k) _m(k) be respectively the angle of pitch and the values of camber angles that directly calculate by measurement value sensor, have according to formula (6):

$\begin{array}{c}{\mathrm{\θ}}_m=\arcsin \left(\frac{{\stackrel{\·}{v}}_{x\_m}-a_{x\_m}}{g}\right)\\ {\mathrm{\φ}}_m=\arcsin \left(\frac{a_{y\_m}-{\mathrm{\ω}}_{z\_m}{v}_{x\_m}}{g\cos {\mathrm{\θ}}_m}\right)\end{array}---\left(10\right)$

In formula (9), a _{x_m}, a _{y_m}with ω _{z_m}represent to utilize respectively low cost MEMS sensor measured longitudinal acceleration, transverse acceleration and yaw velocity, v _{x_m}represent the vehicular longitudinal velocity obtaining by vehicle speed sensor, represent v _{x_m}differential, the longitudinal velocity signal measured by vehicle speed sensor obtains time differentiate,, at each discrete moment k, has:

${\stackrel{\·}{v}}_{x\_m}\left(k\right)=\frac{v_{x\_m}\left(k\right)-v_{x\_m}(k-1)}{\mathrm{dt}}---\left(11\right)$

In formula (11), dt represents sampling time interval, in the present invention, and dt=0.01 (second); represent by formula (10) calculate obtain the vehicle angle of pitch observation noise and that average is 0, variance is white Gaussian noise, represent through type (10) calculate obtain vehicle side inclination angle observation noise and that average is 0, variance is white Gaussian noise; The observation noise variance battle array R that V is corresponding can be expressed as $R=\left[\begin{array}{cc}{{\mathrm{\σ}}_{{\mathrm{\θ}}_m}}^2& 0\\ 0& {{\mathrm{\σ}}_{{\mathrm{\φ}}_m}}^2\end{array}\right];$

From formula (10), only need record longitudinal velocity, longitudinal acceleration, transverse acceleration and the yaw velocity of vehicle, can obtain the required observed quantity of Kalman filtering observation equation, thereby utilize Kalman filtering recursive algorithm to estimate the angle of pitch and the side rake angle of vehicle.Therefore, only need two low cost MEMS acceleration transducers, low cost MEMS gyroscope and vehicle speed sensor can meet measurement requirement.

Wherein two low cost MEMS acceleration transducers are installed near vehicle centroid position, one along the bodywork reference frame longitudinal axis, in order to measure longitudinal acceleration, one along bodywork reference frame transverse axis, in order to measure transverse acceleration, low cost MEMS gyroscope is also installed near vehicle centroid position, install along the vertical axle of bodywork reference frame, in order to measure yaw velocity, vehicle speed sensor is used for measuring longitudinal speed of a motor vehicle, the sensor such as Hall element or photoelectric code disk all can adopt, do not limit at this, but require vehicle speed measurement trueness error to be less than 0.05 meter per second, recording after longitudinal vehicle speed signal, according to formula (11), it is carried out to differentiate and can obtain its differential,

In fact,, if vehicle is provided with electronic stability control or yaw stabilizing control system, these information can be obtained by the CAN of vehicle (Controller Area Network, controller local area network) bus.

For formula (8) and the described system state equation of formula (9) and measurement equation, use kalman filtering theory, set up standard filtering recursive process below, this recursive process comprises that the time upgrades and measurement is upgraded, the first two steps of recursive process are to upgrade the time below, and remaining three steps are upgraded for measuring:

Time upgrades:

State one-step prediction equation

One-step prediction error covariance matrix

Measure and upgrade:

Filter gain matrix K (k)=P (k, k-1) H ^t(k) [H (k) P (k, k-1) H ' (k)+R (k)] ^-1

State estimation $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\·\hat{X}(k,k-1)]]$

Estimation error variance battle array P (k)=[I-K (k) H (k)] P (k, k-1)

After above-mentioned recursion is calculated, can estimate in real time side rake angle and the angle of pitch of vehicle at each discrete moment k.

In the process of above-mentioned whole side rake angle and angle of pitch estimation, this patent utilizes formula (6) but not formula (7) is carried out the estimation of side rake angle and the angle of pitch, does not adopt the processing that approximates of side rake angle and the angle of pitch.Can ensure so on the one hand the precision of estimation, make on the other hand the inventive method estimate same being suitable for for the side rake angle of wide-angle with the angle of pitch, meet the application demand under complex working condition.The inventive method provides a kind of high precision, low cost, high real-time, vehicle attitude angle method of estimation applied widely for Integrated Navigation for Land Vehicle and positioning field.

Claims (1)

1. vehicle side inclination angle and the angle of pitch method of estimation based on Kalman filtering, it is characterized in that: the present invention is directed to land locomotion four-wheel car, foundation meets the system motion model of its travelling characteristic, further by Kalman filtering recursive algorithm realize to vehicle side inclination angle and the angle of pitch in real time, accurately estimation, can be for estimating compared with the angle of pitch of wide-angle and side rake angle under complicated road environment, and only need low cost vehicle-mounted sensor, cost is lower, and concrete steps comprise:

1) set up the system motion model of vehicle traveling process,

Ignore earth rotation speed, suppose that rate of pitch, roll velocity and the vertical velocity of vehicle is zero, can set up the kinetics equation of Vehicle Driving Cycle process:

$\begin{array}{c}{\stackrel{\·}{v}}_x=a_x+{\mathrm{\ω}}_z{v}_y+g\sin \mathrm{\θ}\\ {\stackrel{\·}{v}}_y=a_y-{\mathrm{\ω}}_z{v}_x-g\sin \mathrm{\φ}\cos \mathrm{\θ}\end{array}---\left(1\right)$

In formula (1), v _x, v _yrepresent respectively longitudinal velocity and the transverse velocity of vehicle, a _x, a _yrepresent respectively longitudinal acceleration and the transverse acceleration of vehicle, ω _zthe yaw velocity that represents vehicle, the definition of above-mentioned variable all defines in bodywork reference frame, and g represents acceleration of gravity, φ, θ represents respectively side rake angle and the angle of pitch of vehicle, upper mark " " represents differential, as represent v _xdifferential,

By (1) Shi Ke get

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{{\stackrel{\·}{v}}_x-a_x-{\mathrm{\ω}}_z{v}_y}{g}\right)\\ \mathrm{\φ}=\arcsin \left(\frac{a_y-{\mathrm{\ω}}_z{v}_x-{\stackrel{\·}{v}}_y}{g\cos \mathrm{\θ}}\right)\end{array}---\left(2\right)$

In formula (2), longitudinally the differential of the speed of a motor vehicle can obtain time differentiate by longitudinal speed of a motor vehicle,

Consider in normal vehicle operation process v _ywith numerical value all less, thereby can ignore, formula (2) can be reduced to:

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{{\stackrel{\·}{v}}_x-a_x}{g}\right)\\ \mathrm{\φ}=\arcsin \left(\frac{a_y-{\mathrm{\ω}}_z{v}_x}{g\cos \mathrm{\θ}}\right)\end{array}---\left(3\right)$

2) set up state equation and the observation equation of Kalman filtering,

The matrix form of the Kalman filtering state equation after discretize is:

In formula (4), k represents the discretize moment; System state vector is X=[x ₁x ₂] ' and x ₁=θ, x ₂=φ, i.e. X=[θ φ] ', superscript in the present invention ' represent matrix transpose; W (k-1) represents system white Gaussian noise vector and the W=[w of zero-mean ₁w ₂] ', be w wherein ₁, w ₂represent respectively two system white Gaussian noise components, the system noise covariance matrix Q (k-1) that W (k-1) is corresponding is: $Q(k-1)=\left[\begin{array}{cc}{\mathrm{\σ}}_{w_1}^2& 0\\ 0& {\mathrm{\σ}}_{w_2}^2\end{array}\right],$ Wherein represent respectively system white Gaussian noise w ₁, w ₂corresponding variance; State-transition matrix this is due in Vehicle Driving Cycle process, and side rake angle and the angle of pitch are all to change continuously slowly, can think that the side rake angle of current sampling instant and the angle of pitch equal side rake angle and the angle of pitch of next sampling instant,

The matrix form of Kalman filtering observation equation discretize is:

Z(k)＝H(k)·X(k)+V(k) (5)

In formula (5), Z is observation vector, and H is observation battle array, and V represents the mutual incoherent zero-mean observation white noise vector with W, because observation vector and state vector all refer to side rake angle and the angle of pitch, so $H\left(k\right)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$ $Z\left(k\right)=\left[\begin{array}{c}{\mathrm{\θ}}_m\left(k\right)\\ {\mathrm{\φ}}_m\left(k\right)\end{array}\right],V=\left[\begin{array}{c}{n}_{{\mathrm{\θ}}_m}\\ n_{{\mathrm{\φ}}_m}\end{array}\right],$ Wherein θ _mand φ (k) _m(k) be respectively the angle of pitch and the values of camber angles that directly calculate by measurement value sensor, according to formula (3), have:

$\begin{array}{c}{\mathrm{\θ}}_m=\arcsin \left(\frac{{\stackrel{\·}{v}}_{x\_m}-a_{x\_m}}{h}\right)\\ {\mathrm{\φ}}_m=\arcsin \left(\frac{a_{y\_m}-{\mathrm{\ω}}_{z\_m}{v}_{x\_m}}{g\cos {\mathrm{\θ}}_m}\right)\end{array}---\left(6\right)$

In formula (6), a _{x_m}, a _{y_m}with ω _{z_m}represent to utilize respectively measured longitudinal acceleration, transverse acceleration and the yaw velocity of low cost MEMS (Micro-Electro-Mechanic System, MEMS (micro electro mechanical system)) sensor, v _{x_m}represent the vehicular longitudinal velocity obtaining by vehicle speed sensor, represent v _{x_m}differential, the longitudinal velocity signal measured by vehicle speed sensor obtains time differentiate,, at each discrete moment k, has:

${\stackrel{\·}{v}}_{x\_m}\left(k\right)=\frac{v_{x\_m}\left(k\right)-v_{x-m}(k-1)}{\mathrm{dt}}---\left(7\right)$

In formula (7), dt represents sampling time interval, in the present invention, and dt=0.01 (second); represent by formula (6) calculate obtain the vehicle angle of pitch observation noise and that average is 0, variance is white Gaussian noise, represent through type (6) calculate obtain vehicle side inclination angle observation noise and that average is 0, variance is white Gaussian noise; The observation noise variance battle array R that V is corresponding can be expressed as $R=\left[\begin{array}{cc}{{\mathrm{\σ}}_{{\mathrm{\θ}}_m}}^2& 0\\ 0& {{\mathrm{\σ}}_{{\mathrm{\φ}}_m}}^2\end{array}\right];$

3) required onboard sensor is installed

From formula (6), only need record longitudinal velocity, longitudinal acceleration, transverse acceleration and the yaw velocity of vehicle, can obtain the required observed quantity of Kalman filtering observation equation, thereby utilize Kalman filtering recursive algorithm to estimate the angle of pitch and the side rake angle of vehicle; Therefore, only need two low cost MEMS acceleration transducers, low cost MEMS gyroscope and vehicle speed sensor can meet measurement requirement;

Wherein two low cost MEMS acceleration transducers are installed near vehicle centroid position, one along the bodywork reference frame longitudinal axis, in order to measure longitudinal acceleration, one along bodywork reference frame transverse axis, in order to measure transverse acceleration, low cost MEMS gyroscope is also installed near vehicle centroid position, install along the vertical axle of bodywork reference frame, in order to measure yaw velocity, vehicle speed sensor is used for measuring longitudinal speed of a motor vehicle, the sensor such as Hall element or photoelectric code disk all can adopt, do not limit at this, but require vehicle speed measurement trueness error to be less than 0.05 meter per second, recording after longitudinal vehicle speed signal, try to achieve its differential according to formula (7), for calculating observation amount,

4) Kalman filtering recursion:

For formula (4) and the described system state equation of formula (5) and measurement equation, use kalman filtering theory, set up standard filtering recursive process below, this recursive process comprises that the time upgrades and measurement is upgraded, the first two steps of recursive process are to upgrade the time below, and remaining three steps are upgraded for measuring:

Time upgrades:

State one-step prediction equation

One-step prediction error covariance matrix

Measure and upgrade:

Filter gain matrix K (k)=P (k, k-1) H ^t(k) [H (k) P (k, k-1) H ' (k)+R (k)] ^-1

State estimation $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\·\hat{X}(k,k-1)]]$

Estimation error variance battle array P (k)=[I-K (k) H (k)] P (k, k-1)

After above-mentioned recursion is calculated, can estimate in real time side rake angle and the angle of pitch of vehicle at each discrete moment k.