CN103625475B - A kind of vehicle side inclination angle based on recurrence least square and pitch angle method of estimation - Google Patents

Abstract

The invention discloses a kind of vehicle side inclination angle based on recurrence least square and pitch angle method of estimation, this method is for land locomotion four wheeler, set up the vehicle dynamic model meeting its travelling characteristic, recursive least square method further by band forgetting factor realize to vehicle side inclination angle and pitch angle real-time, accurately estimate, and only need low cost vehicle-mounted sensor, cost is lower, and estimated angle of roll and pitch angle information can meet the needs of regarding Car integrated navigation and location.

Description

A kind of vehicle side inclination angle based on recurrence least square and pitch angle method of estimation

Technical field

The present invention relates to a kind of vehicle side inclination angle based on recurrence least square and pitch angle method of estimation, its object is to carry out suitable modeling to the dynamic process of running car, and utilize the recursive least square method of band forgetting factor to obtain vehicle side inclination angle and pitch angle estimated valve, these estimated valves can be used for Integrated Navigation for Land Vehicle and location, there is the remarkable advantages such as precision is high, cost is low, real-time is good, belong to bus location navigation field.

Background technology

In recent years, intelligent transportation system ITS(IntelligentTransportationSystems) worldwide obtain attention highly and fast speed development, the important foundation that ITS function effectively plays is the location accurately and reliably realizing vehicle.When vehicle is located accurately and reliably, ITS can more effective induction vehicle, improves operating efficiency, improves traffic safety, reduces exhaust emissions.

At present, what the application of automobile navigation positioning field was maximum is GPS technology.But owing to blocking or multipath phenomenon, GPS there will be the inaccurate problem even lost efficacy in location, cannot realize consecutive tracking accurately and reliably.For overcoming the deficiency of GPS, the research of vehicle multi-sensor combined navigation causes to be paid attention to widely, namely when GPS lost efficacy, utilizes inertial navigation sensor or dead reckoning to carry out piloting, thus obtains locating information comparatively accurately when GPS loses efficacy.

Vehicle is on real road in driving process, due to the existence of the road vertical and horizontal gradient and the motion of vehicle suspension, cause there is certain vehicle side inclination angle and pitch angle, although its value is general less, its accuracy role for piloting is difficult to ignore.In land vehicle application, the acceleration/accel of vehicle is often much smaller than acceleration due to gravity, to such an extent as to less angle of roll and pitch angle may cause producing larger error when measuring longitudinal and lateral acceleration under bodywork reference frame, these errors can accumulate the accumulated error that when causing reckoning speed and location information, generation is larger.Therefore, measure accurately the attitude angle such as angle of roll and pitch angle or estimate to extrapolate the important guarantee of locating information accurately, its accuracy is the key factor affecting vehicle combination positioning precision.

Be commonly used to determine that the method for the attitude angle such as angle of roll and pitch angle information uses complete sextuple Inertial Measurement Unit IMU(InertialMeasurementUnit), this IMU comprises 3 accelerometers and 3 rate-of-turn gyroscopes, and the attitude angle information of vehicle can be calculated by the strapdown inertial of sextuple IMU.But sextuple IMU is expensive, particularly three gyrostatic prices.Consider that many vehicles are provided with electronic stability and control or yaw stabilizing control system, part IMU signal can by the CAN(ControllerAreaNetwork of vehicle, controller local area network) bus easier obtains, these signals generally include yaw velocity, longitudinal acceleration and lateral acceleration, in order to effectively reduce costs, namely this patent utilizes these retrievable information to estimate angle of roll and the pitch angle of vehicle.

In theory, if the known and yaw velocity of vehicle of the initial condition of vehicle can obtain, the angle of roll of vehicle and pitch angle can be calculated by numerical integration method.But in fact, direct integation method is due to sensor error and inevitable numerical operation error, larger drift can be caused, particularly at the vehicle-mounted low cost MEMS(Micro-Electro-MechanicSystem of use, MEMS) sensor time, therefore, the present invention does not adopt direct integation method, but a kind of recurrence least square (RecursiveLeastSquares, RLS) algorithm in real time of proposition is estimated the angle of roll of vehicle and pitch angle.

Summary of the invention

The object of the invention is to overcome the deficiencies in the prior art, propose a kind of vehicle side inclination angle based on recurrence least square and pitch angle method of estimation, the method precision is high, cost is low, real-time is good, can bus location navigation demand.

The technical solution used in the present invention is as follows: a kind of vehicle side inclination angle based on recurrence least square and pitch angle method of estimation, it is characterized in that: the present invention is directed to land locomotion four wheeler, set up the vehicle dynamic model meeting its travelling characteristic, further by the recurrence least square (RecursiveLeastSquares of band forgetting factor, RLS) method realize to vehicle side inclination angle and pitch angle real-time, accurately estimate, and only needing low cost vehicle-mounted sensor, cost is lower; Concrete steps comprise:

1) kinetic model of vehicle traveling process is set up

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

$\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 longitudinal velocity and the side velocity of vehicle respectively, a _x, a _yrepresent longitudinal acceleration and the lateral acceleration of vehicle respectively, ω _zrepresent the yaw velocity of vehicle, above-mentioned definition is all that g represents acceleration due to gravity for bodywork reference frame, and φ, θ represent angle of roll and the pitch angle of vehicle respectively, and 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), 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 thus numerical value is less can ignore, and meanwhile, consider under most of surface conditions, the angle of roll of vehicle and pitch angle normally low-angle, namely has arcsin () ≈, then formula (2) can be reduced to:

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

2) required onboard sensor is installed

From formula (3), only need record the longitudinal velocity of vehicle, longitudinal acceleration, lateral acceleration and yaw velocity, can utilize sets up the vehicle vehicle dynamics equation after also Rational Simplification, namely formula (3) estimates pitch angle and the angle of roll of vehicle; Therefore, only need two low cost MEMS(Micro-Electro-MechanicSystem, MEMS) acceleration pick-up, a low cost MEMS gyro instrument and car speed sensor can meet measurement requirement;

Wherein two low cost MEMS acceleration pick-ups 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 lateral 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, car speed sensor is for measuring longitudinal speed of a motor vehicle, the sensor such as Hall vehicle speed sensor or wheel speed sensors 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, it can be obtained its differential to time differentiate,

3) estimate based on the vehicle side inclination angle of recurrence least square and pitch angle

Formula (3) is expressed as parameter criterion of identification form:

In formula (4), k represents discrete instants, $\mathrm{\γ}\left(k\right)=\left[\begin{array}{c}\widehat{\mathrm{\θ}}\left(k\right)\\ \widehat{\mathrm{\φ}}\left(k\right)\end{array}\right]$ Represent solve for parameter matrix, wherein, with represent vehicle pitch rate to be estimated and angle of roll respectively; $y\left(k\right)=\left[\begin{array}{c}{\stackrel{.}{v}}_{x\_m}-a_{x\_m}\\ a_{y\_m}-{\mathrm{\ω}}_{z\_m}{v}_{x\_m}\end{array}\right]$ Expression system output matrix, a _{x_m}, a _{y_m}with ω _{z_m}represent the longitudinal acceleration, lateral acceleration and the yaw velocity that utilize measured by low cost MEMS sensor respectively, v _{x_m}represent the vehicular longitudinal velocity obtained by car speed sensor, represent v _{x_m}differential, the longitudinal speed signal measured by car speed sensor obtains time differentiate, namely at each discrete instants k, has:

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

In formula (5), dt represents sampling time interval, in the present invention, and dt=0.01(second); represent input regression matrix, in the present invention, superscript T represents matrix transpose; Recurrence least square (RecursiveLeastSquares, the RLS) algorithm of band forgetting factor is then utilized to estimate that the estimating step of vehicle side inclination angle and yaw angle is as follows in real time:

(1) computing system output matrix y (k), and calculate input regression matrix

(2) calculated gains matrix K (k); wherein, variance matrix parameter lambda is forgetting factor, effectively can reduce the impact of no longer relevant to model legacy data, and prevent covariance from dispersing, and usual span is in [0.9,1], and the present invention gets 0.975;

(3) solve for parameter matrix γ (k) is calculated;

Wherein I is 2 × 2 identity matrixs, so far, can estimate vehicle side inclination angle and yaw angle in real time.

Advantage of the present invention and remarkable result:

(1) the present invention proposes the good vehicle side inclination angle of a kind of low cost, high precision, real-time and pitch angle method of estimation, can be used for Integrated Navigation for Land Vehicle and positioning field accurately calculates needs for vehicle location and velocity information;

(2) the present invention carries out Rational Simplification to vehicle dynamic model according to the vehicle feature that travels, and utilizes recursive least squares to carry out the estimation of angle of roll and pitch angle, has ensured its estimated accuracy and real-time;

(3) the present invention only requirement produce onboard sensor on car, there is the advantage that cost is low, be convenient to large-scale promotion.

Accompanying drawing explanation

Fig. 1 is emulation operating mode 1 pitch angle estimated result;

Fig. 2 is emulation operating mode 2 angle of roll estimated result;

Detailed description of the invention

Embodiment 1

In recent years, intelligent transportation system ITS(IntelligentTransportationSystems) worldwide obtain attention highly and fast speed development, the important foundation that ITS function effectively plays is the location accurately and reliably realizing vehicle.When vehicle is located accurately and reliably, ITS can more effective induction vehicle, improves operating efficiency, improves traffic safety, reduces exhaust emissions.

At present, what the application of automobile navigation positioning field was maximum is GPS technology.But owing to blocking or multipath phenomenon, GPS there will be the inaccurate problem even lost efficacy in location, cannot realize consecutive tracking accurately and reliably.For overcoming the deficiency of GPS, the research of vehicle multi-sensor combined navigation causes to be paid attention to widely, namely when GPS lost efficacy, utilizes inertial navigation sensor or dead reckoning to carry out piloting, thus obtains locating information comparatively accurately when GPS loses efficacy.

Vehicle is on real road in driving process, due to the motion of the road vertical and horizontal gradient and vehicle suspension, cause there is certain vehicle side inclination angle and pitch angle, although its value is general less, its accuracy role for piloting is difficult to ignore.In land vehicle driving process, the acceleration/accel of vehicle is often much smaller than acceleration due to gravity, to such an extent as to less angle of roll and pitch angle may cause producing larger error when measuring longitudinal and lateral acceleration under bodywork reference frame, these errors can be accumulated and be caused producing larger accumulated error when reckoning speed and location information.Therefore, measure accurately the attitude angle such as angle of roll and pitch angle or estimate to extrapolate the important guarantee of locating information accurately, its accuracy is the key factor affecting vehicle combination positioning precision.

Be commonly used to determine that the method for the attitude angle such as angle of roll and pitch angle 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 and the differential of angle information, and ignore earth rotation speed, vehicle dynamics process can be modeled as [herein can bibliography: H.EricTsenga, LiXu, DavorHrovat, Estimationoflandvehiclerollandpitchangles [J] .VehicleSystemDynamics:InternationalJournalofVehicleMech anicsandMobility, 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{\θ}---\left(2\right)\\ {\stackrel{.}{v}}_z=a_z+{\mathrm{\ω}}_y{v}_x-{\mathrm{\ω}}_x{v}_y-g\cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}$

In formula, ω _x, ω _yand ω _zrepresent the cireular frequency 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/accel along the bodywork reference frame longitudinal axis, transverse axis and vertical axle respectively; φ, θ, and ψ represents inclination, pitching and yaw three Eulerian angles respectively; G represents acceleration due to gravity, and the present invention gets 9.78.

Utilize formula (1) and (2), the attitude angle information of vehicle can be calculated by the strapdown inertial of sextuple IMU, has and relate in a large amount of vehicle location document.But sextuple IMU is expensive, particularly three gyrostatic prices.Consider that many vehicles are provided with electronic stability and control or yaw stabilizing control system, part IMU signal can by the CAN(ControllerAreaNetwork of vehicle, controller local area network) bus easier obtains, and these signals generally include yaw velocity, longitudinal acceleration and lateral acceleration; In order to effectively reduce costs, this patent namely study how to utilize these retrievable information and non-used complete 6 dimension IMU to estimate angle of roll and the pitch angle of vehicle.

By formula (1), can finding out to estimate angle of roll and pitch angle, not needing yaw angle information ψ.Simultaneously, the vertical and horizontal gradient of the traffic usually travelled due to vehicle is all less, its transverse grade rate and head fall rate are all less than 20% usually, (namely the present invention is less than the road of 20% mainly for transverse grade rate and head fall rate), the angle of roll of vehicle and pitch angle are all change slowly continuously, and its corresponding magnitude of angular velocity comparatively I is ignored, and vehicle vertical velocity is general also less, therefore, ω reasonably can be thought _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}_y+g\sin \mathrm{\θ}\\ {\stackrel{.}{v}}_y=a_y-{\mathrm{\ω}}_z{v}_x-g\sin \mathrm{\φ}\cos \mathrm{\θ}\end{array}---\left(4\right)$

According to formula (3), in theory, if the known and yaw velocity of vehicle of the initial condition of vehicle can obtain, the angle of roll of vehicle and pitch angle can be calculated by numerical integration method.But in fact, direct integation method is due to sensor error and inevitable numerical operation error, larger drift can be caused, when particularly using low cost MEMS sensor, therefore, the present invention does not adopt direct integation method, but utilizes formula (4), propose a kind of recurrence least square (RecursiveLeastSquares, RLS) algorithm in real time to estimate the angle of roll of vehicle and pitch angle.

Recurrence least square is the iterative algorithm to unknown vector, with the minimum variance of model error for target, for each sampling period, using existing sampled data to calculate unknown vector by iterating, having 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), 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 thus numerical value is less can ignore, simultaneously, consider that the vertical and horizontal gradient of the traffic usually travelled due to vehicle is all less, its transverse grade rate and head fall rate are all less than 20%, (namely the present invention is less than the road of 20% mainly for transverse grade rate and head fall rate), therefore, the angle of roll of vehicle and pitch angle normally low-angle, namely have arcsin () ≈, then formula (5) can be reduced to:

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

From formula (6), only need record the longitudinal velocity of vehicle, longitudinal acceleration, lateral acceleration and yaw velocity, can utilize sets up also Rational Simplification after vehicle vehicle dynamics equation (3) estimate pitch angle and the angle of roll of vehicle; Therefore, only need two low cost MEMS(Micro-Electro-MechanicSystem, MEMS) acceleration pick-up, a low cost MEMS gyro instrument and car speed sensor can meet measurement requirement;

Wherein two low cost MEMS acceleration pick-ups 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 lateral 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, car speed sensor is for measuring longitudinal speed of a motor vehicle, the sensor such as Hall vehicle speed sensor or wheel speed sensors 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, and it can be obtained its differential to time differentiate,

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

Formula (6) is expressed as parameter criterion of identification form:

In formula (7), k represents discrete instants, $\mathrm{\γ}\left(k\right)=\left[\begin{array}{c}\widehat{\mathrm{\θ}}\left(k\right)\\ \widehat{\mathrm{\φ}}\left(k\right)\end{array}\right]$ Represent solve for parameter matrix, wherein, with represent vehicle pitch rate to be estimated and angle of roll respectively; $y\left(k\right)=\left[\begin{array}{c}{\stackrel{.}{v}}_{x\_m}-a_{x\_m}\\ a_{y\_m}-{\mathrm{\ω}}_{z\_m}{v}_{x\_m}\end{array}\right]$ Expression system output matrix, a _{x_m}, a _{y_m}with ω _{z_m}represent the longitudinal acceleration, lateral acceleration and the yaw velocity that utilize measured by low cost MEMS sensor respectively, v _{x_m}represent the vehicular longitudinal velocity obtained by car speed sensor, represent v _{x_m}differential, the longitudinal speed signal measured by car speed sensor obtains time differentiate, namely at each discrete instants k, has:

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

In formula (8), dt represents sampling time interval, in the present invention, and dt=0.01(second); represent input regression matrix, superscript in the present invention ^trepresent matrix transpose; Recurrence least square (RecursiveLeastSquares, the RLS) algorithm of band forgetting factor is then utilized to estimate that the estimating step of vehicle side inclination angle and yaw angle is as follows in real time:

(1) computing system output matrix y (k), and calculate input regression matrix

(2) calculated gains matrix K (k); wherein, variance matrix parameter lambda is forgetting factor, effectively can reduce the impact of no longer relevant to model legacy data, and prevent covariance from dispersing, and usual span is in [0.9,1], and the present invention gets 0.975;

(3) solve for parameter matrix γ (k) is calculated;

Wherein I is 2 × 2 identity matrixs, so far, can estimate vehicle side inclination angle and yaw angle in real time.

Embodiment 2

For inspection the present invention propose based on the vehicle side inclination angle of recurrence least square and the actual effect of pitch angle method of estimation, the vehicle dynamics simulation software CarSim of specialty has carried out simulating, verifying experiment.

CarSim is the special simulation software for vehicle dynamics developed by U.S. MSC (MechanicalSimulationCorporation) company, at present by automakers numerous in the world, components supplying business adopt, be widely used in the business development of modern automobile control system, become the standard software of auto trade, enjoyed a very good reputation.Vehicle dynamic model in Carsim be by respectively to the car body of automobile, suspension, turn to, the height modeling true to nature of each subsystem such as braking and each tire realizes, there is very high degree of freedom, the closely actual information of travel condition of vehicle accurately can be provided, therefore, the reference that the travel condition of vehicle information that Carsim exports can be used as vehicle exports.

For the estimation effect of algorithm under vehicle typical case driving cycle that inspection the present invention proposes, two typical conditions are provided with in l-G simulation test, contain its straight line and curvilinear motion in operating mode, the change of road grade and the change of the speed of a motor vehicle, operating mode specifically describes in table 1.

Table 1 two kinds of exemplary simulation operating modes

Emulate the four-wheel wagon that vehicle used is a front-wheel steering, obtain the longitudinal direction of car speed of a motor vehicle and adopt wheel speed sensors, the sampling frequency of required inertial sensor and wheel speed sensors is all 100Hz, for the low cost inertial sensor based on MEMS, the standard deviation of gyrostatic zero-mean random white noise is 0.2(degree/second), the standard deviation of the zero-mean random white noise of acceleration pick-up is 0.1956(rice/(second × second)), the measurement noises of wheel speed sensors is that average is 0, standard deviation is 0.05(meter per second) Gaussian white noise.Emulation initial value arranges as follows: variance matrix initial value $P=\left[\begin{array}{cc}10& 0\\ 0& 10\end{array}\right],$ Matrix initial value to be estimated is $\mathrm{\γ}\left(0\right)=\left[\begin{array}{c}0\\ 0\end{array}\right].$

Table 2 and Fig. 1, Fig. 2 give the result of emulation experiment.Table 2 lists the statistics contrast utilizing straight survey method and the inventive method to calculate vehicle side inclination angle and pitch angle, and the error in table is all (the pitch angle error as directly surveyed method just represents the error of the pitch angle reference value that the pitch angle utilizing straight survey method to calculate exports relative to Carsim) for the corresponding reference value that Carsim exports.In addition it is noted that the concrete meaning of above-mentioned two kinds of methods is as follows: straight survey method refers to and utilizes inertial sensor output valve, is directly calculated the method obtaining pitch angle and angle of roll by embodiment 1 Chinese style (6); The inventive method refers to that the recurrence least square method of estimation utilizing the present invention to propose is to calculate the method at vehicle side inclination angle and pitch angle.

Table 2 two kinds of methods calculate angle of roll and pitch angle Contrast on effect table (unit: deg)

The result curve that Fig. 1 gives in operating mode 1 pitch angle utilizing straight survey method and the inventive method to estimate (represents straight survey method result with Original dash-dotted gray line in figure, the inventive method estimated result is indicated) with RLS black dotted line, and the reference output valve of corresponding Carsim (indicating with the real black line of Carsim in figure), the result curve that Fig. 2 gives in operating mode 2 angle of roll utilizing straight survey method and the inventive method to estimate (represents straight survey method result with Original dash-dotted gray line in figure, the inventive method estimated result is indicated) with RLS black dotted line, and the reference output valve of corresponding Carsim (indicating with the real black line of Carsim in figure).

By contrast and Fig. 1 ~ Fig. 2 of table 2, can find out that the inventive method has had relative to straight survey method precision in the reckoning of angle of roll and pitch angle and significantly improve.

Claims (1)

1. the vehicle side inclination angle based on recurrence least square and pitch angle method of estimation, it is characterized in that: for land locomotion four wheeler, set up the vehicle dynamic model meeting its travelling characteristic, further by the recurrence least square (RecursiveLeastSquares of band forgetting factor, RLS) method realize to vehicle side inclination angle and pitch angle real-time, accurately estimate, and only needing low cost vehicle-mounted sensor, cost is lower; Concrete steps comprise:

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

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

$\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 longitudinal velocity and the side velocity of vehicle respectively, a _x, a _yrepresent longitudinal acceleration and the lateral acceleration of vehicle respectively, ω _zrepresent the yaw velocity of vehicle, above-mentioned definition is all that g represents acceleration due to gravity for bodywork reference frame, and φ, θ represent angle of roll and the pitch angle of vehicle respectively, and 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), 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 thus numerical value is less can ignore, and meanwhile, consider under most of surface conditions, the angle of roll of vehicle and pitch angle normally low-angle, namely has arcsin () ≈, then formula (2) can be reduced to:

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

2) required onboard sensor is installed

From formula (3), only need record the longitudinal velocity of vehicle, longitudinal acceleration, lateral acceleration and yaw velocity, can utilize set up and Rational Simplification after vehicle vehicle dynamics equation, namely formula (3) estimates pitch angle and the angle of roll of vehicle; Therefore, only need two low cost MEMS (Micro-Electro-MechanicSystem, MEMS) acceleration pick-up, a low cost MEMS gyro instrument and car speed sensor can meet measurement requirement;

Wherein two low cost MEMS acceleration pick-ups 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 lateral 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, car speed sensor is for measuring longitudinal speed of a motor vehicle, Hall vehicle speed sensor or wheel speed sensors can be adopted, but require that vehicle speed measurement trueness error is less than 0.05 meter per second, after recording longitudinal vehicle speed signal, it can be obtained its differential to time differentiate,

3) estimate based on the vehicle side inclination angle of recurrence least square and pitch angle

Formula (3) is expressed as parameter criterion of identification form:

In formula (4), k represents discrete instants, $\mathrm{\γ}\left(k\right)=\left[\begin{array}{c}\widehat{\mathrm{\θ}}\left(k\right)\\ \widehat{\mathrm{\φ}}\left(k\right)\end{array}\right]$ Represent solve for parameter matrix, wherein, with represent vehicle pitch rate to be estimated and angle of roll respectively; $y\left(k\right)=\left[\begin{array}{c}{\stackrel{\·}{v}}_{x\_m}-a_{x\_m}\\ a_{y\_m}-{\mathrm{\ω}}_{z\_m}{v}_{x\_m}\end{array}\right]$ Expression system output matrix, a _{x_m}, a _{y_m}with ω _{z_m}represent the longitudinal acceleration, lateral acceleration and the yaw velocity that utilize measured by low cost MEMS sensor respectively, v _{x_m}represent the vehicular longitudinal velocity obtained by car speed sensor, represent v _{x_m}differential, the longitudinal speed signal measured by car speed sensor obtains time differentiate, namely at each discrete instants k, has:

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

In formula (5), dt represents sampling time interval, dt=0.01 (second); represent input regression matrix, superscript ^trepresent matrix transpose; Recurrence least square (RecursiveLeastSquares, the RLS) algorithm of band forgetting factor is then utilized to estimate that the estimating step of vehicle side inclination angle and yaw angle is as follows in real time:

(1) computing system output matrix y (k), and calculate input regression matrix

(2) calculated gains matrix K (k); wherein, variance matrix parameter lambda is forgetting factor, effectively can reduce the impact of no longer relevant to model legacy data, and prevent covariance from dispersing, and usual span is [0.9,1];

(3) solve for parameter matrix γ (k) is calculated;

Wherein I is 2 × 2 identity matrixs, so far, can estimate vehicle side inclination angle and pitch angle in real time.