CN104061899B - A kind of vehicle side inclination angle based on Kalman filtering and angle of pitch method of estimation - Google Patents

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

Info

Publication number
CN104061899B
CN104061899B CN201410281621.1A CN201410281621A CN104061899B CN 104061899 B CN104061899 B CN 104061899B CN 201410281621 A CN201410281621 A CN 201410281621A CN 104061899 B CN104061899 B CN 104061899B
Authority
CN
China
Prior art keywords
vehicle
angle
pitch
formula
represent
Prior art date
Application number
CN201410281621.1A
Other languages
Chinese (zh)
Other versions
CN104061899A (en
Inventor
李旭
徐启敏
宋翔
Original Assignee
东南大学
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 东南大学 filed Critical 东南大学
Priority to CN201410281621.1A priority Critical patent/CN104061899B/en
Publication of CN104061899A publication Critical patent/CN104061899A/en
Application granted granted Critical
Publication of CN104061899B publication Critical patent/CN104061899B/en

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00Measuring angles
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C9/00Measuring inclination, e.g. by clinometers, by levels

Abstract

The invention discloses a kind of vehicle side inclination angle based on Kalman filtering and angle of pitch method of estimation.This method, for land four-wheel car, is set up and is met the kinematics model of its travelling characteristic, further by Kalman filtering algorithm realize to vehicle side inclination angle and the angle of pitch real-time, accurately estimate.The inventive method is estimated still to be suitable for for the side rake angle of larger angle and the angle of pitch, meets the application demand under complex working condition, and only needs low cost vehicle-mounted sensor.Estimated side rake angle and angle of pitch information are the key parameters of Integrated Navigation for Land Vehicle and location.

Description

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

Technical field

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

Background technology

Intelligent transportation system (IntelligentTransportationSystems, 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 realize safety, the efficiently autonomous optimal selection travelled 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.In order to make up GPS at antenna by the deficiency that cannot position when blocking, development GPS/INS integrated navigation system receives 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 there is certain side rake angle and the angle of pitch in the process of moving, its value is general less, negligible under many circumstances.But in GPS/INS integrated navigation system, side rake angle and the angle of pitch are the key parameters carrying 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, less side rake angle and the angle of pitch will be caused so also to produce larger error when measuring vertical and horizontal acceleration, and the accumulation in time of these errors can cause producing larger error when reckoning speed and positional information.So, accurately real-time measurement is carried out to the attitude angle such as side rake angle and the angle of pitch or estimation is the basis of carrying out high-precision GPS/INS integrated navigation and location.

At present, based on the strap-down navigation technology of Inertial Measurement Unit (IMU, InertialMeasurementUnit) developed very ripe.An IMU generally contains the accelerometer of three single shafts and the gyro of three single shafts, and this also causes its price comparison expensive, particularly three gyrostatic prices.Patent " a kind of vehicle side inclination angle based on recurrence least square and angle of pitch method of estimation " (publication number: CN103625475A) proposes a kind of vehicle attitude angle evaluation method based on low cost MEMS (Micro-Electro-MechanicSystem, MEMS (micro electro mechanical system)) sensor.The method only needs two MEMS acceleration transducers, and a MEMS gyro instrument and vehicle speed sensor can complete the estimation of vehicle side inclination angle and the angle of pitch.But the method only considers 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, approximate process so have employed 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 comparatively large, and can not carry out approximating process, said method can be restricted in actual application.

In order to solve above-mentioned technology deficiency in the application, patent of the present invention proposes a kind of vehicle pitch rate based on Kalman filtering and side rake angle evaluation method.Kalman filter take Minimum Mean Square Error as the optimal State Estimation wave filter of criterion, it does not need to store measured value in the past, only carry out recurrence calculation 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 may be used under complex working condition, estimate larger angle the angle of pitch and side rake angle, 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 based on Kalman filtering and angle of pitch method of estimation are 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 based on Kalman filtering and angle of pitch method of estimation, it is characterized in that: the present invention is directed to land locomotion four-wheel car, set up the system motion model meeting its travelling characteristic, further by Kalman filtering algorithm realize to vehicle side inclination angle and the angle of pitch real-time, accurately estimate, may be used under complicated road environment, estimate larger angle the angle of pitch and side rake angle, and only needing low cost vehicle-mounted sensor, cost is lower.Concrete steps comprise:

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

Ignore earth rotation speed, suppose that the rate of pitch of vehicle, roll velocity and vertical velocity are zero, then can set up the kinetics equation of vehicle travel process:

v · x = a x + ω z v y + g sin θ v · y = a y - ω z v x - g sin φ cos θ - - - ( 1 )

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

By (1) Shi Ke get

θ = arcsin ( v · x - a x - ω z v y g ) φ = arcsin ( a y - ω z v x - v · y g cos θ ) - - - ( 2 )

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

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

θ = arcsin ( v · x - a x g ) φ = arcsin ( a y - ω z v x g cos θ ) - - - ( 3 )

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

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 1x 2] ' and x 1=θ, x 2=φ, i.e. X=[θ φ] ', superscript in the present invention ' represent matrix transpose; W (k-1) represent zero-mean system white Gaussian noise vector and W=[w 1w 2] ', be w wherein 1, w 2represent two system white Gaussian noise components respectively, the system noise covariance battle array Q (k-1) that W (k-1) is corresponding is: Q ( k - 1 ) = σ w 1 2 0 0 σ w 2 2 , Wherein represent system white Gaussian noise w respectively 1, w 2corresponding variance; State-transition matrix this is due in vehicle travel process, and side rake angle and the angle of pitch are all slowly change continuously, can think that the side rake angle of current sample time 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 that white noise vector observed by mutual incoherent zero-mean with W.Because observation vector and state vector all refer to side rake angle and the angle of pitch, so H ( k ) = 1 0 0 1 ; Z ( k ) = θ m ( k ) φ m ( k ) , V = n θ m n φ m , Wherein θ m(k) and φ mk () is respectively the angle of pitch and values of camber angles that are directly calculated by measurement value sensor, according to formula (3), have:

θ m = arcsin ( v · x _ m - a x _ m g ) φ m = arcsin ( a y _ m - ω z _ m v x _ m g cos θ m ) - - - ( 6 )

In formula (6), a x_m, a y_mwith ω z_mrepresent respectively and utilize low cost MEMS (Micro-Electro-MechanicSystem, MEMS (micro electro mechanical system)) longitudinal acceleration, transverse acceleration and yaw velocity measured by sensor, v x_mrepresent the vehicular longitudinal velocity obtained by vehicle speed sensor, represent v x_mdifferential, the longitudinal speed signal measured by vehicle speed sensor obtains time differentiate, namely at each discrete instants k, has:

v · x _ m ( k ) = v x _ m ( k ) - v x _ m ( k - 1 ) dt - - - ( 7 )

In formula (7), dt represents sampling time interval, in the present invention, and dt=0.01 (second); represent by formula (6) calculate obtain vehicle pitch rate observation noise and be average be 0, variance is white Gaussian noise, represent through type (6) calculate obtain vehicle side inclination angle observation noise and be average be 0, variance is white Gaussian noise; The observation noise variance matrix R that V is corresponding can be expressed as R = σ θ m 2 0 0 σ φ m 2 ;

3) required onboard sensor is installed

From formula (6), only need record the longitudinal velocity of vehicle, longitudinal acceleration, transverse acceleration and yaw velocity, the observed quantity needed for Kalman filtering observation equation can be obtained, thus 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, a low cost MEMS gyro instrument 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 gyro instrument 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 for measuring longitudinal speed of a motor vehicle, and the sensor such as Hall element or photoelectric code disk all can adopt, and does not limit at this, but requires that vehicle speed measurement trueness error is less than 0.05 meter per second.After recording longitudinal vehicle speed signal, try to achieve its differential, for calculating observation amount according to formula (7);

4) Kalman filtering recursion:

For the system state equation described by formula (4) and formula (5) and measurement equation, use kalman filtering theory, set up standard filtering recursive process below, this recursive process comprises time renewal and measurement updaue, the first two steps of recursive process are upgrade the time below, and remaining three steps are measurement updaue:

Time upgrades:

State one-step prediction equation

One-step prediction error covariance matrix

Measurement updaue:

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 X ^ ( k ) = X ^ ( k , k - 1 ) + K ( k ) [ Z ( k ) - H ( k ) · X ^ ( k , k - 1 ) ] ]

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

After above-mentioned recurrence calculation, vehicle can be estimated in real time at the side rake angle of each discrete instants k and the angle of pitch.

Advantage of the present invention and remarkable result:

(1) method of estimation that the present invention proposes can estimate vehicle side inclination angle and the angle of pitch of larger angle, while guarantee estimation precision, meets the application demand under complex working condition;

(2) the present invention carries out Rational Simplification to vehicle dynamic model according to the vehicle feature that travels, thus 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 (IntelligentTransportationSystems, 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 realize safety, the efficiently autonomous optimal selection travelled 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.In order to make up GPS at antenna by the deficiency that cannot position when blocking, development GPS/INS integrated navigation system receives the extensive concern of Chinese scholars.

Due to the existence of the road vertical and horizontal gradient and the motion of vehicle suspension, certain side rake angle and the angle of pitch can be there is in the process of moving in land four-wheel car, its value is general less, negligible under many circumstances, but but can not be ignored when carrying out the dead reckoning of GPS/INS integrated navigation.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 may cause producing larger error during measurement vertical and horizontal acceleration, the accumulation in time of these errors can cause producing larger error when reckoning speed and positional information.So measure accurately side rake angle and the angle of pitch or estimate to carry out the important leverage of GPS/INS integrated navigation, its accuracy is the key factor affecting vehicle location precision.

Be commonly used to determine that the method for the attitude angle such as side rake angle and angle of pitch information uses complete sextuple Inertial Measurement Unit IMU (InertialMeasurementUnit), 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.EricTsenga, LiXu, DavorHrovat, Estimationoflandvehiclerollandpitchangles [J] .VehicleSystemDynamics:InternationalJournalofVehicleMech anicsandMobility, 2007, 45 (5): 433-443.]:

φ · = ω x + ( ω y sin φ + ω z cos φ ) tan θ θ · = ω y cos φ - ω z sin φ ψ · = ( ω y sin φ + ω z cos φ ) / cos θ - - - ( 1 )

v · x = a x + ω z v y - ω y v z + g sin θ v · y = a y - ω z v x + ω x v z - g sin φ cos θ v · z = a z + ω y v x - ω x v y - g cos φ cos θ - - - ( 2 )

In formula, ω x, ω yand ω zrepresent the angular velocity around the bodywork reference frame longitudinal axis, transverse axis and vertical axle respectively, v x, v yand v zrepresent the linear velocity along the bodywork reference frame longitudinal axis, transverse axis and vertical axle respectively, a x, a yand a zrepresent the acceleration along the bodywork reference frame longitudinal axis, transverse axis and vertical axle respectively; φ, θ, and ψ represents inclination, pitching and yaw three Eulerian angle respectively; 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 computation of sextuple IMU, and this has and relates in a large amount of vehicle location document.But sextuple IMU is expensive, particularly three gyrostatic prices.Although the measuring accuracy of MEMS (Micro-Electro-MechanicSystem, MEMS (micro electro mechanical system)) sensor is lower compared to traditional sensors, because it has the low outstanding feature of cost, its application is still more and more extensive.Namely the present invention studies how to use as far as possible few MEMS sensor and side rake angle and the angle of pitch of accurately estimating vehicle in conjunction with data processing method, to overcome the low shortcoming of MEMS sensor precision.Meanwhile, few sensor also can reduce costs further as far as possible.The data processing method used also to ensure to the side rake angle of wide-angle and the angle of pitch applicable equally, to meet the application demand under complex working condition.

By formula (1), can finding out to estimate side rake angle and the angle of pitch, not needing yaw angle information ψ.Meanwhile, because the side rake angle of vehicle and the angle of pitch are all slowly change continuously, its corresponding magnitude of angular velocity is less, and vehicle vertical velocity is general also less, can ignore.Therefore, ω reasonably can be thought x≈ 0, ω y≈ 0, ν z≈ 0.Then formula (1) and (2) can be reduced to:

φ · = ω z cos φ tan θ θ · = - ω z sin φ - - - ( 3 )

v · x = a x + ω z v t + g sin θ v · y = a y - ω z v x - g sin φ cos θ - - - ( 4 )

Known according to formula (3), if the known and yaw velocity of vehicle of the original state of vehicle can obtain, then the side rake angle of vehicle and the angle of pitch can be tried to achieve by integration method.But in fact, direct integation method, due to sensor error and inevitable numerical operation error, can cause larger drift, particularly when using the MEMS sensor of low cost.Therefore, the present invention does not adopt direct integation method, but utilizes formula (4) to propose a kind of vehicle side inclination angle based on Kalman filtering algorithm and angle of pitch method of estimation.Kalman filter take Minimum Mean Square Error as the optimal State Estimation wave filter of criterion, it does not need to store measured value in the past, only carry out recurrence calculation 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

θ = arcsin ( v · x - a x - ω z v y g ) φ = arcsin ( a y - ω z v x - v · y g cos θ ) - - - ( 5 )

In formula (5), the differential of longitudinal speed of a motor vehicle obtains time differentiate by longitudinal speed of a motor vehicle, considers in normal vehicle operation, v ywith numerical value is less, thus can ignore, then formula (5) can be reduced to:

θ = arcsin ( v · x - a x g ) φ = arcsin ( a y - ω z v x g cos θ ) - - - ( 6 )

The vertical and horizontal gradient on road surface is the principal element affecting 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 when solving, can carry out approximating process, that is:

θ = arcsin ( v · s - a x g ) ≈ v · x - a x g φ = arcsin ( a y - ω z v x g cos θ ) ≈ a y - ω z v x g cos θ - - - ( 7 )

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 comparatively large, when solving arcsin computing, can not carry out approximating process.This patent considers application demand under complex working condition exactly, under the prerequisite of not carrying out approximating process, carry out Kalman filtering reckoning.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 1x 2] ' and x 1=θ, x 2=φ, i.e. X=[θ φ] ', superscript in the present invention ' represent matrix transpose; W (k-1) represent zero-mean system white Gaussian noise vector and W=[w 1w 2] ', be w wherein 1, w 2represent two system white Gaussian noise components respectively, the system noise covariance battle array Q (k-1) that W (k-1) is corresponding is Q ( k - 1 ) = σ w 1 2 0 0 σ w 2 2 , Wherein represent system white Gaussian noise w respectively 1, w 2corresponding variance; State-transition matrix is this is due in vehicle travel process, and its side rake angle and the angle of pitch are all slowly change continuously, 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 that white noise vector observed by mutual incoherent zero-mean with W.Because observation vector and state vector all refer to side rake angle and the angle of pitch, so H ( k ) = 1 0 0 1 ; Z ( k ) = θ m ( k ) φ m ( k ) , V = n θ m n φ m , Wherein θ m(k) and φ mk () is respectively the angle of pitch and values of camber angles that are directly calculated by measurement value sensor, have according to formula (6):

θ m = arcsin ( v · x _ m - a x _ m g ) φ m = arcsin ( a y _ m - ω z _ m v x _ m g cos θ m ) - - - ( 10 )

In formula (9), a x_m, a y_mwith ω z_mrepresent the longitudinal acceleration, transverse acceleration and the yaw velocity that utilize measured by low cost MEMS sensor respectively, v x_mrepresent the vehicular longitudinal velocity obtained by vehicle speed sensor, represent v x_mdifferential, the longitudinal speed signal measured by vehicle speed sensor obtains time differentiate, namely at each discrete instants k, has:

v · x _ m ( k ) = v x _ m ( k ) - v x _ m ( k - 1 ) dt - - - ( 11 )

In formula (11), dt represents sampling time interval, in the present invention, and dt=0.01 (second); represent by formula (10) calculate obtain vehicle pitch rate observation noise and be average be 0, variance is white Gaussian noise, represent through type (10) calculate obtain vehicle side inclination angle observation noise and be average be 0, variance is white Gaussian noise; The observation noise variance matrix R that V is corresponding can be expressed as R = σ θ m 2 0 0 σ φ m 2 ;

From formula (10), only need record the longitudinal velocity of vehicle, longitudinal acceleration, transverse acceleration and yaw velocity, the observed quantity needed for Kalman filtering observation equation can be obtained, thus 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, a low cost MEMS gyro instrument 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 gyro instrument 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 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 that vehicle speed measurement trueness error is less than 0.05 meter per second, after recording longitudinal vehicle speed signal, according to formula (11), differentiate is carried out to it and can obtain its differential,

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

For the system state equation described by formula (8) and formula (9) and measurement equation, use kalman filtering theory, set up standard filtering recursive process below, this recursive process comprises time renewal and measurement updaue, the first two steps of recursive process are upgrade the time below, and remaining three steps are measurement updaue:

Time upgrades:

State one-step prediction equation

One-step prediction error covariance matrix

Measurement updaue:

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 X ^ ( k ) = X ^ ( k , k - 1 ) + K ( k ) [ Z ( k ) - H ( k ) · X ^ ( k , k - 1 ) ] ]

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

After above-mentioned recurrence calculation, vehicle can be estimated in real time at the side rake angle of each discrete instants k and the angle of pitch.

In the process that above-mentioned whole side rake angle and the angle of pitch are estimated, this patent utilizes formula (6) but not formula (7) carries out the estimation of side rake angle and the angle of pitch, and what namely do not adopt side rake angle and the angle of pitch approximates process.Such one side can ensure the precision estimated, makes the inventive method estimate same being suitable for for the side rake angle of wide-angle with the angle of pitch on the other hand, meets the application demand under complex working condition.The inventive method provides a kind of high precision, low cost, high real-time, vehicle attitude angular estimation method applied widely for Integrated Navigation for Land Vehicle and positioning field.

Claims (1)

1. the vehicle side inclination angle based on Kalman filtering and angle of pitch method of estimation, it is characterized in that: for land locomotion four-wheel car, set up the system motion model meeting its travelling characteristic, further by Kalman filtering recursive algorithm realize to vehicle side inclination angle and the angle of pitch real-time, accurately estimate, may be used under complicated road environment, estimate larger angle the angle of pitch and side rake angle, and only need low cost vehicle-mounted sensor, concrete steps comprise:
1) the system motion model of vehicle traveling process is set up:
Ignore earth rotation speed, suppose that the rate of pitch of vehicle, roll velocity and vertical velocity are zero, then can set up the kinetics equation of vehicle travel process:
v · x = a x + ω z v y + g s i n θ v · y = a y - ω z v x - g sin φ cos θ - - - ( 1 )
In formula (1), v x, v yrepresent longitudinal velocity and the transverse velocity of vehicle respectively, a x, a yrepresent longitudinal acceleration and the transverse acceleration of vehicle respectively, ω zrepresent the yaw velocity of vehicle, the definition of above-mentioned variable all defines in bodywork reference frame, and g represents acceleration of gravity, and φ, θ represent side rake angle and the angle of pitch of vehicle respectively, and upper mark " " represents differential,
By (1) Shi Ke get
θ = a r c s i n ( v · x - a x - ω z v y g ) φ = a r c s i n ( a y - ω z v x - v · y g cos θ ) - - - ( 2 )
In formula (2), the differential of longitudinal speed of a motor vehicle obtains time differentiate by longitudinal speed of a motor vehicle,
Consider in normal vehicle operation process, v ywith numerical value all less, thus can ignore, then formula (2) can be reduced to:
θ = a r c s i n ( v · x - a x g ) φ = a r c s i n ( a y - ω z v x g cos θ ) - - - ( 3 )
2) state equation and the observation equation of Kalman filtering is set up:
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 1x 2] ' and x 1=θ, x 2=φ, i.e. X=[θ φ] ', superscript ' represent matrix transpose; W (k-1) represent zero-mean system white Gaussian noise vector and W=[w 1w 2] ', be w wherein 1, w 2represent two system white Gaussian noise components respectively, the system noise covariance battle array Q (k-1) that W (k-1) is corresponding is: Q ( k - 1 ) = σ w 1 2 0 0 σ w 2 2 , Wherein represent system white Gaussian noise w respectively 1, w 2corresponding variance; State-transition matrix this is due in vehicle travel process, and side rake angle and the angle of pitch are all slowly change continuously, can think that the side rake angle of current sample time 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 that white noise vector observed by mutual incoherent zero-mean with W, because observation vector and state vector all refer to side rake angle and the angle of pitch, so H ( k ) = 1 0 0 1 ; Z ( k ) = θ m ( k ) φ m ( k ) , V = n θ m n φ m , Wherein θ m(k) and φ mk () is respectively the angle of pitch and values of camber angles that are directly calculated by measurement value sensor, according to formula (3), have:
θ m = a r c s i n ( v · x _ m - a x _ m g ) φ m = a r c s i n ( a y _ m - ω z _ m v x _ m g cosθ m ) - - - ( 6 )
In formula (6), a x_m, a y_mwith ω z_mrepresent the longitudinal acceleration, transverse acceleration and the yaw velocity that utilize measured by low cost MEMS sensor respectively, v x_mrepresent the vehicular longitudinal velocity obtained by vehicle speed sensor, represent v x_mdifferential, the longitudinal speed signal measured by vehicle speed sensor obtains time differentiate, namely at each discrete instants k, has:
v · x _ m ( k ) = v x _ m ( k ) - v x _ m ( k - 1 ) d t - - - ( 7 )
In formula (7), dt represents sampling time interval, dt=0.01 (second); represent by formula (6) calculate obtain vehicle pitch rate observation noise and be average be 0, variance is white Gaussian noise, represent through type (6) calculate obtain vehicle side inclination angle observation noise and be average be 0, variance is white Gaussian noise; The observation noise variance matrix R that V is corresponding can be expressed as R = σ θ m 2 0 0 σ φ m 2 ;
3) required onboard sensor is installed:
From formula (6), only need record the longitudinal velocity of vehicle, longitudinal acceleration, transverse acceleration and yaw velocity, the observed quantity needed for Kalman filtering observation equation can be obtained, thus 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, a low cost MEMS gyro instrument 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 gyro instrument 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 for measuring longitudinal speed of a motor vehicle, require that vehicle speed measurement trueness error is less than 0.05 meter per second, after recording longitudinal vehicle speed signal, its differential is tried to achieve according to formula (7), for calculating observation amount,
4) Kalman filtering recursion:
For the system state equation described by formula (4) and formula (5) and measurement equation, use kalman filtering theory, set up standard filtering recursive process below, this recursive process comprises time renewal and measurement updaue, the first two steps of recursive process are upgrade the time below, and remaining three steps are measurement updaue:
Time upgrades:
State one-step prediction equation
One-step prediction error covariance matrix
Measurement updaue:
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 X ^ ( k ) = X ^ ( k , k - 1 ) + K ( k ) [ Z ( k ) - H ( k ) · X ^ ( k , k - 1 ) ] ]
Estimation error variance battle array P (k)=[I-K (k) H (k)] P (k, k-1)
After above-mentioned recurrence calculation, vehicle can be estimated in real time at the side rake angle of each discrete instants k and the angle of pitch.
CN201410281621.1A 2014-06-20 2014-06-20 A kind of vehicle side inclination angle based on Kalman filtering and angle of pitch method of estimation CN104061899B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201410281621.1A CN104061899B (en) 2014-06-20 2014-06-20 A kind of vehicle side inclination angle based on Kalman filtering and angle of pitch method of estimation

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201410281621.1A CN104061899B (en) 2014-06-20 2014-06-20 A kind of vehicle side inclination angle based on Kalman filtering and angle of pitch method of estimation

Publications (2)

Publication Number Publication Date
CN104061899A CN104061899A (en) 2014-09-24
CN104061899B true CN104061899B (en) 2016-03-30

Family

ID=51549729

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201410281621.1A CN104061899B (en) 2014-06-20 2014-06-20 A kind of vehicle side inclination angle based on Kalman filtering and angle of pitch method of estimation

Country Status (1)

Country Link
CN (1) CN104061899B (en)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB2536008B (en) * 2015-03-03 2019-06-12 Jaguar Land Rover Ltd Vehicle state estimation apparatus and method
CN105571595A (en) * 2015-12-16 2016-05-11 东南大学 Method for estimating attitude angle of rescuing wrecker based on robust filtering
CN106156481A (en) * 2015-12-16 2016-11-23 东南大学 A kind of method for estimating attitude angles of rescuing and obstacle-clearing vehicle based on lossless Kalman filtering
CN106250591B (en) * 2016-07-21 2019-05-03 辽宁工业大学 It is a kind of to consider to roll the vehicle driving state estimation method influenced
CN106197471B (en) * 2016-07-22 2019-03-22 长安大学 Road detection vehicle based on information fusion is apart from trigger device and triggering method
CN106289642B (en) * 2016-09-23 2019-05-07 交通运输部公路科学研究所 Lift by crane the method for dynamic estimation of relief car entirety gravity plane position under operating condition
CN106768638B (en) * 2017-01-19 2019-04-30 河南理工大学 A kind of passenger car height of center of mass real-time estimating method
CN108413923A (en) * 2018-01-22 2018-08-17 浙江工业大学 A kind of vehicle roll angle based on robust mixed filtering and pitch angle method of estimation
CN109050535B (en) * 2018-07-25 2020-02-14 北京理工大学 Rapid terrain condition identification method based on vehicle attitude

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102005033237B4 (en) * 2005-07-15 2007-09-20 Siemens Ag Method for determining and correcting misalignments and offsets of the sensors of an inertial measurement unit in a land vehicle
WO2009122333A2 (en) * 2008-03-31 2009-10-08 Nxp B.V. Digital modulator
US8150651B2 (en) * 2008-06-11 2012-04-03 Trimble Navigation Limited Acceleration compensated inclinometer
US9026263B2 (en) * 2011-11-30 2015-05-05 Alpine Electronics, Inc. Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis
CN103353299B (en) * 2013-06-20 2015-07-08 西安交通大学 High-precision vehicle-mounted road grade detection device and method
CN103776451B (en) * 2014-03-04 2016-11-09 哈尔滨工业大学 A kind of high-precision three-dimensional attitude inertial measurement system based on MEMS and measuring method

Also Published As

Publication number Publication date
CN104061899A (en) 2014-09-24

Similar Documents

Publication Publication Date Title
Sahlholm et al. Road grade estimation for look-ahead vehicle control using multiple measurement runs
Ahmed et al. Accurate attitude estimation of a moving land vehicle using low-cost MEMS IMU sensors
CN102829777B (en) Autonomous underwater vehicle combined navigation system and method
CN101793528B (en) System and method of lane path estimation using sensor fusion
CN101661048B (en) Velocity calculation device,velocity calculation method, and navigation device
Yi et al. IMU-based localization and slip estimation for skid-steered mobile robots
US9026263B2 (en) Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis
JP4964047B2 (en) Position detection apparatus and position detection method
Jo et al. Generation of a precise roadway map for autonomous cars
Bevly et al. Integrating INS sensors with GPS measurements for continuous estimation of vehicle sideslip, roll, and tire cornering stiffness
Rose et al. An integrated vehicle navigation system utilizing lane-detection and lateral position estimation systems in difficult environments for GPS
DE102008026397B4 (en) Radar, lidar, and camera-assisted vehicle dynamics estimation methods
Jo et al. GPS-bias correction for precise localization of autonomous vehicles
CN102538781B (en) Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method
Piyabongkarn et al. Development and experimental evaluation of a slip angle estimator for vehicle stability control
CN103076015B (en) A kind of SINS/CNS integrated navigation system based on optimum correction comprehensively and air navigation aid thereof
CN103217157B (en) A kind of inertial navigation/odometer independent combined navigation method
CN101082493B (en) Combined positioning method of agricultural machines navigation
US7711483B2 (en) Dead reckoning system
Yoon et al. Robust vehicle sideslip angle estimation through a disturbance rejection filter that integrates a magnetometer with GPS
CN101907714B (en) GPS aided positioning system and method based on multi-sensor data fusion
CN100562711C (en) Moving object method for estimating and system
CN105180933B (en) Mobile robot reckoning update the system and method based on the detection of straight trip crossing
Tao et al. Lane marking aided vehicle localization
US20100100272A1 (en) System and method for road angle estimation

Legal Events

Date Code Title Description
PB01 Publication
C06 Publication
SE01 Entry into force of request for substantive examination
C10 Entry into substantive examination
GR01 Patent grant
C14 Grant of patent or utility model