CN103616013A - Method for estimating attitude angles of rescuing and obstacle-clearing vehicle - Google Patents

Abstract

The invention discloses a method for estimating the attitude angles of a rescuing and obstacle-clearing vehicle. According to the method, the rescuing and obstacle-clearing vehicle is subjected to dynamics modeling according to the work characteristics of the rescuing and obstacle-clearing vehicle; the estimated values of the attitude angles, such as side rake angle, pitch angle and the like, of the rescuing and obstacle-clearing vehicle are obtained by a recursive least-square method, and can be applied to attitude monitoring and dangerous attitude early warning of the rescuing and obstacle-clearing vehicle, so the method has the significant advantages of high precision, low cost, high real-time property and the like.

Description

A kind of rescue obstacles removing car pose estimation method

Technical field

The present invention relates to a kind of rescue obstacles removing car pose estimation method, its object is according to rescue obstacles removing car work characteristics, it is carried out to Dynamic Modeling to it, and the recursive least square method of utilizing belt variable forgetting factor obtains the estimated value of the attitude angle such as rescue obstacles removing car side rake angle and the angle of pitch, these estimated values can be used for rescuing obstacles removing car attitude monitoring and dangerous attitude early warning, there is the remarkable advantages such as precision is high, cost is low, real-time is good, belong to rescue obstacles removing car safe early warning field.

Background technology

According to the statistics made by the departments concerned, rescued in recent years the Frequent Accidents that obstacles removing car is tumbled, often exist and fail to save and rescued vehicle safely, and rescue the vehicle self of removing obstacles, there is rollover or overthrow accident, trace it to its cause, except operator operates against regulations, lack experience and the subjective factor such as mismanagement outside, rescue obstacles removing car there is no perfect attitude monitoring device, cannot be a large defect for driver provides attitude information accurately, if can estimate in real time and monitor for attitude informations such as rescue obstacles removing car side rake angle and the angles of pitch, when it gives driver corresponding information during in dangerous attitude, can avoid a large amount of overthrow accidents.The attitude angle of general vehicle comprises side rake angle and yaw angle, the angle of pitch, work characteristics due to rescue obstacles removing car, while removing obstacles rescue work, only utilize the arm work of suing and labouring, and vehicle body and ground are in relative static conditions, therefore may cause the attitude angle that overthrow accident occurs only to comprise side rake angle and the angle of pitch, yaw angle is without impact, (this patent is estimated for side rake angle and these two attitude angle of the angle of pitch of rescue obstacles removing car), the excessive rollover accident that easily causes of side rake angle, the excessive overthrow accident that easily causes fore-and-aft direction of the angle of pitch, but the correlation technique that the attitude informations such as the current side rake angle for rescue obstacles removing car and the angle of pitch are estimated there is not yet research.

For common vehicle, 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, and the attitude angle information of vehicle can calculate by the strapdown algorithm of sextuple IMU.Yet sextuple IMU is expensive, three gyrostatic prices particularly.In theory, 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 calculate by numerical integration method.Yet in fact,, direct integral method, due to sensor error and inevitable numerical operation error, can cause larger drift, is particularly using vehicle-mounted low-cost MEMS(Micro-Electro-Mechanic System, MEMS (micro electro mechanical system)) during sensor.

And for rescue obstacles removing car, when it removes obstacles rescue work, only utilize the arm work of suing and labouring, and vehicle body and ground are in relative static conditions, thereby can utilize this work characteristics to carry out further Rational Simplification to its kinetics equation, in real time, for the attitude angle such as side rake angle and the angle of pitch of rescue obstacles removing car, estimate accurately, to rescuing obstacles removing car, carry out attitude monitoring, but have not yet to see correlative study.

Summary of the invention

The object of the invention is to fill up the blank of prior art, a kind of rescue obstacles removing car pose estimation method is proposed, the method precision is high, cost is low, real-time is good, can meet the demand of rescue obstacles removing car attitude monitoring and dangerous attitude early warning, for rescue obstacles removing car trouble free service escorts.

The technical solution used in the present invention is as follows: a kind of rescue obstacles removing car pose estimation method, it is characterized in that: the present invention is according to rescue obstacles removing car work characteristics, it is carried out to Dynamic Modeling, further by recurrence least square (the Recursive Least Squares of belt variable forgetting factor, RLS) method realize to the attitude angle such as rescue obstacles removing car side rake angle and the angle of pitch in real time, accurately estimation, and only need two low-cost MEMS(Micro-Electro-Mechanic System, MEMS (micro electro mechanical system)) vehicle-mounted acceleration transducer; Concrete steps comprise:

1) set up the kinetic model of rescue obstacles removing car

While removing obstacles rescue work due to rescue obstacles removing car, what this patent was paid close attention to is side rake angle and these two attitude angle of the angle of pitch of rescue obstacles removing car; Ignore earth rotation speed, while simultaneously removing obstacles rescue work due to rescue obstacles removing car, only utilize arm work, vehicle body in ground relative static conditions, the rate of pitch of vehicle, roll velocity and vertical velocity are zero, can set up dynamics of vehicle equation and be:

$\begin{array}{c}{\stackrel{.}{v}}_x=a_x{\mathrm{\ω}}_z{v}_y+g\sin \\ {\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 _ylongitudinal and the side velocity that represents respectively vehicle, a _x, a _ylongitudinal and the side acceleration that represents respectively vehicle, ω _zthe yaw velocity that represents vehicle, above-mentioned definition is all that g represents acceleration of gravity for bodywork reference frame, φ, θ represents respectively side rake angle and the angle of pitch of vehicle, upper sign " " represents differential, as

expression is to 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 conjunction with rescue obstacles removing car work characteristics, formula (2) is analyzed, while removing obstacles rescue work due to rescue obstacles removing car, only utilize arm work, and vehicle body in ground relative static conditions, longitudinally the speed of a motor vehicle, side velocity and yaw velocity are 0, simultaneously, consider in the road surface situation of working at rescue obstacles removing car, the side rake angle of vehicle and the angle of pitch be low-angle normally, has arcsin () ≈, and formula (2) can be reduced to:

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{{-a}_x}{g}\right)\≈\frac{{-a}_x}{g}\\ \mathrm{\φ}=\arcsin \left(\frac{a_y}{g\cos \mathrm{\θ}}\right)\≈\frac{a_y}{g\cos \mathrm{\θ}}\end{array}---\left(3\right)$

2) required onboard sensor is installed

From formula (3), only need record longitudinal direction of car acceleration and side acceleration, can estimate attitude angle such as the angle of pitch of rescue obstacles removing car and side rake angle; Therefore, only needing two low-cost MEMS(Micro-Electro-Mechanic System, MEMS (micro electro mechanical system)) acceleration transducer can meet measurement requirement; Two low-cost MEMS acceleration transducers are installed near vehicle body (rescuing other parts of the vehicle of removing obstacles except arm) centroid position, one parallel with the bodywork reference frame longitudinal axis, in order to measure longitudinal acceleration, one and bodywork reference frame transverse axis, in order to measure into side acceleration;

3) the rescue obstacles removing car pose estimation based on recurrence least square

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

y(k)＝φ ^T(k)γ(k)（4）

In formula (4), k represents the discrete moment,

represent solve for parameter matrix, wherein, with

represent respectively the vehicle angle of pitch and side rake angle to be estimated;

expression system output matrix, a _{x_m}, a _{y_m}represent to utilize respectively low-cost MEMS sensor measured longitudinal acceleration, side acceleration;

represent input regression matrix, in the present invention, superscript T represents matrix transpose; Utilize recurrence least square (Recursive Least Squares, the RLS) algorithm of belt variable forgetting factor to estimate that in real time the estimating step of vehicle side inclination angle and yaw angle is as follows:

(1) computing system output matrix y (k), and calculate input regression matrix φ ^t(k);

(2) calculated gains matrix K (k);

Wherein, variance matrix parameter lambda is forgetting factor, can effectively reduce the impact of no longer relevant to model legacy data, and prevent that covariance from dispersing, conventionally span is [0.9,1], the present invention in the early stage iteration of 0-0.5 second gets 0.96 in the phase, and is taken as 0.99 after 0.5 second, and iteration cycle is taken as 0.01 second (from k-1 time be carved into the k time interval constantly);

(3) calculate solve for parameter matrix γ (k);

γ(k)＝γ(k-1)+Κ(k)[y(k)-φ ^T(k)γ(k-1)]

Wherein I is 2 * 2 unit matrixs, so far, can estimate in real time the attitude angle such as rescue obstacles removing car side rake angle and the angle of pitch.

Advantage of the present invention and remarkable result:

(1) the present invention proposes a kind of low cost, high precision, rescue obstacles removing car side rake angle and angle of pitch method of estimation that real-time is good, can be used for rescuing the needs of obstacles removing car attitude monitoring and dangerous attitude early warning;

(2) the present invention, according to rescue obstacles removing car work characteristics, carries out Rational Simplification to its kinetic model, and utilizes the recursive least squares of belt variable forgetting factor to carry out the estimation of side rake angle and the angle of pitch, has ensured its estimated accuracy and real-time;

(3) the present invention only needs two vehicle-mounted acceleration transducers of low-cost MEMS, has advantages of that cost is low, is convenient to large-scale promotion.

Embodiment

According to the statistics made by the departments concerned, rescued in recent years the Frequent Accidents that obstacles removing car is tumbled, often exist and fail to save and rescued vehicle safely, and rescue the vehicle self of removing obstacles, there is rollover or overthrow accident, trace it to its cause, except operator operates against regulations, lack experience and the subjective factor such as mismanagement outside, rescue obstacles removing car there is no perfect attitude monitoring device, cannot be a large defect for driver provides attitude information accurately, if can estimate in real time and monitor for attitude informations such as rescue obstacles removing car side rake angle and the angles of pitch, when it gives driver corresponding information during in dangerous attitude, can avoid a large amount of overthrow accidents.The attitude angle of vehicle comprises side rake angle and yaw angle, the angle of pitch, work characteristics due to rescue obstacles removing car, while removing obstacles rescue work, only utilize the arm work of suing and labouring, and vehicle body and ground are in relative static conditions, therefore may cause the attitude angle that overthrow accident occurs only to comprise side rake angle and the angle of pitch, yaw angle is without impact, (this patent is estimated for side rake angle and these two attitude angle of the angle of pitch of rescue obstacles removing car), the attitude angle that the present invention pays close attention to only comprises side rake angle and the angle of pitch, the excessive rollover accident that easily causes of side rake angle, the excessive overthrow accident that easily causes fore-and-aft direction of the angle of pitch, in general, side rake angle or the angle of pitch are greater than 15 degree and should report to the police accordingly, to remind operating personnel to take care, sue and labour, but the correlation technique that the attitude informations such as the current side rake angle for rescue obstacles removing car and the angle of pitch are estimated there is not yet research.

For common vehicle, 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, Li Xu, Davor Hrovat, Estimation of land vehicle roll and pitch angles[J] .Vehicle System Dynamics:International Journal of Vehicle Mechanics and Mobility, 2007, 45 (5): 433-443.]:

$\begin{array}{c}\stackrel{.}{\mathrm{\φ}}={\mathrm{\ω}}_x+\left({\mathrm{\ω}}_y\sin \mathrm{\φ}{\mathrm{\ω}}_z\cos \mathrm{\φ}\right)\tan \mathrm{\θ}\\ \stackrel{.}{\mathrm{\θ}}=\cos \mathrm{\φ}-{\mathrm{\ω}}_z\sin \mathrm{\φ}\\ \stackrel{.}{\mathrm{\ψ}}=\left({\mathrm{\ω}}_y\sin \mathrm{\φ}{\mathrm{\ω}}_z\cos \mathrm{\φ}\right)/\cos \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{\φ\θ}\\ {\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, and the present invention gets 9.78.

Utilize formula (1) and (2), the attitude angle information of vehicle can calculate by the strapdown algorithm of sextuple IMU, in a large amount of vehicle location documents, has and relates to.Yet sextuple IMU is expensive, three gyrostatic prices particularly.Consider that most of vehicles are all provided with electronic stability and control or yaw stabilizing control system, part IMU signal can pass through the CAN(Controller Area Network of vehicle, controller local area network) bus is easier obtained, and these signals generally include ω _z, a _xand a _y.In order effectively to reduce costs, this patent is studied and how to be utilized these retrievable information but not utilize complete 6 dimension IMU to estimate side rake angle and the angle of pitch of vehicle.

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 rescue obstacles removing car is while removing obstacles rescue work, only utilize arm work, vehicle body in ground relative static conditions, it is zero that the rate of pitch of vehicle, roll velocity and vertical velocity can be approximately, and therefore, can reasonably think ω _x≈ 0, ω _y≈ 0, ν _z0. formula of ≈ (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 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 calculate by numerical integration method.Yet in fact, direct integral method is due to sensor error and inevitable numerical operation error, can cause larger drift, particularly use low-cost MEMS sensor, therefore, the present invention does not adopt direct integral method, but utilizes formula (4), real-time recurrence least square (Recursive Least Squares, the RLS) algorithm that proposes a kind of belt variable forgetting factor is estimated the side rake angle of vehicle and the angle of pitch.

Recurrence least square is the iterative algorithm to unknown vector, and the minimum variance of model error of take is target, for each sampling period, uses existing sampled data to calculate unknown vector by iterating, and has 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)$

Feature in conjunction with the work of rescue obstacles removing car is analyzed formula (5), while removing obstacles rescue work due to rescue obstacles removing car, only utilize arm work, and vehicle body in ground relative static conditions, it is longitudinal speed of a motor vehicle, side velocity and yaw velocity are 0, simultaneously, consider that the road surface of working at rescue obstacles removing car is comparatively smooth, the vertical and horizontal gradient of road is all less, the vehicle angle of pitch and the side rake angle that under normal condition, cause are little, side rake angle and the angle of pitch while rescuing obstacles removing car normal operation are conventionally little, there is arcsin () ≈, formula (5) can be reduced to:

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{{-a}_x}{g}\right)\≈\frac{{-a}_x}{g}\\ \mathrm{\φ}=\arcsin \left(\frac{a_y}{g\cos \mathrm{\θ}}\right)\≈\frac{a_y}{g\cos \mathrm{\θ}}\end{array}---\left(6\right)$

From formula (6), only need record longitudinal direction of car acceleration and side acceleration, can utilize rescue obstacles removing car kinetics equation (6) after sets up Rational Simplification also to estimate to rescue the attitude angle such as the angle of pitch of obstacles removing car and side rake angle; Therefore, only needing two low-cost MEMS(Micro-Electro-Mechanic System, MEMS (micro electro mechanical system)) acceleration transducer can meet measurement requirement; Two low-cost MEMS acceleration transducers are installed on vehicle body and (remove outer other parts of rescuing the vehicle of removing obstacles of arm, also can be described as the remove obstacles base of vehicle of rescue) near centroid position, one parallel with the bodywork reference frame longitudinal axis, in order to measure into longitudinal acceleration, one and bodywork reference frame transverse axis, in order to measure into side acceleration.

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

y(k)＝φT(k)γ(k)（7）

In formula (7), k represents the discrete moment,

represent solve for parameter matrix, wherein, with

represent respectively the vehicle angle of pitch and side rake angle to be estimated;

expression system output matrix, a _{x_m}, a _{y_m}represent to utilize respectively low-cost MEMS sensor measured longitudinal acceleration, side acceleration;

represent input regression matrix, in the present invention, superscript T represents matrix transpose; Utilize recurrence least square (Recursive Least Squares, the RLS) algorithm of belt variable forgetting factor to estimate that in real time the estimating step of vehicle side inclination angle and yaw angle is as follows:

(1) computing system output matrix y (k), and calculate input regression matrix φ ^t(k);

(2) calculated gains matrix K (k);

Wherein, variance matrix

parameter lambda is forgetting factor, can effectively reduce the impact of no longer relevant to model legacy data, and prevent that covariance from dispersing, span is [0.9 conventionally, 1], the present invention considers concrete application, adopts the variable method of forgetting factor, and the iteration of 0-0.5 second gets 0.96 in the phase in the early stage, so that estimated value Fast Convergent, and be taken as 0.99 after 0.5 second, to have good denoising ability, iteration cycle is taken as 0.01 second (being carved into the k time interval constantly during from k-1) conventionally;

(3) calculate solve for parameter matrix γ (k);

γ(k)＝γ(k-1)+Κ(k)[y(k)-φ ^T(k)γ(k-1)]

Wherein I is 2 * 2 unit matrixs, so far, can estimate in real time the attitude angle such as rescue obstacles removing car side rake angle and the angle of pitch, should be noted, the initial value of algorithm can be set to: variance matrix initial value

matrix initial value to be estimated is

$\mathrm{\γ}\left(0\right)=\left[\begin{array}{c}0\\ 0\end{array}\right].$

Claims (1)

1. a rescue obstacles removing car pose estimation method, it is characterized in that: the present invention is according to rescue obstacles removing car work characteristics, it is carried out to Dynamic Modeling, further by recurrence least square (the Recursive Least Squares of belt variable forgetting factor, RLS) method realize to the attitude angle such as rescue obstacles removing car side rake angle and the angle of pitch in real time, accurately estimation, and only need two low-cost MEMS(Micro-Electro-Mechanic System, MEMS (micro electro mechanical system)) vehicle-mounted acceleration transducer; Concrete steps comprise:

1) set up the kinetic model of rescue obstacles removing car

While removing obstacles rescue work due to rescue obstacles removing car, what this patent was paid close attention to is side rake angle and these two attitude angle of the angle of pitch of rescue obstacles removing car; Ignore earth rotation speed, while simultaneously removing obstacles rescue work due to rescue obstacles removing car, only utilize arm work, vehicle body in ground relative static conditions, the rate of pitch of vehicle, roll velocity and vertical velocity are zero, can set up dynamics of vehicle equation and be:

$\begin{array}{c}{\stackrel{.}{v}}_x=a_x{\mathrm{\ω}}_z{v}_y+g\sin \\ {\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 _ylongitudinal and the side velocity that represents respectively vehicle, a _x, a _ylongitudinal and the side acceleration that represents respectively vehicle, ω _zthe yaw velocity that represents vehicle, above-mentioned definition is all that g represents acceleration of gravity for bodywork reference frame, φ, θ represents respectively side rake angle and the angle of pitch of vehicle, upper sign " " represents differential, as

expression is to 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 conjunction with rescue obstacles removing car work characteristics, formula (2) is analyzed, while removing obstacles rescue work due to rescue obstacles removing car, only utilize arm work, and vehicle body in ground relative static conditions, longitudinally the speed of a motor vehicle, side velocity and yaw velocity are 0, simultaneously, consider in the road surface situation of working at rescue obstacles removing car, the side rake angle of vehicle and the angle of pitch be low-angle normally, has arcsin () ≈, and formula (2) can be reduced to:

$\begin{array}{c}\mathrm{\θ}=\arcsin \left(\frac{{-a}_x}{g}\right)\≈\frac{{-a}_x}{g}\\ \mathrm{\φ}=\arcsin \left(\frac{a_y}{g\cos \mathrm{\θ}}\right)\≈\frac{a_y}{g\cos \mathrm{\θ}}\end{array}---\left(3\right)$

2) required onboard sensor is installed

From formula (3), only need record longitudinal direction of car acceleration and side acceleration, can estimate attitude angle such as the angle of pitch of rescue obstacles removing car and side rake angle; Therefore, only needing two low-cost MEMS(Micro-Electro-Mechanic System, MEMS (micro electro mechanical system)) acceleration transducer can meet measurement requirement; Two low-cost MEMS acceleration transducers are installed near vehicle body (rescuing other parts of the vehicle of removing obstacles except arm) centroid position, one parallel with the bodywork reference frame longitudinal axis, in order to measure longitudinal acceleration, one and bodywork reference frame transverse axis, in order to measure into side acceleration;

3) the rescue obstacles removing car pose estimation based on recurrence least square

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

y(k)＝φ ^T(k)γ(k)（4）

In formula (4), k represents the discrete moment, represent solve for parameter matrix, wherein, with represent respectively the vehicle angle of pitch and side rake angle to be estimated;

expression system output matrix, a _{x_m}, a _{y_m}represent to utilize respectively low-cost MEMS sensor measured longitudinal acceleration, side acceleration;

represent input regression matrix, superscript in the present invention ^texpression is to matrix transpose; Utilize recurrence least square (Recursive Least Squares, the RLS) algorithm of belt variable forgetting factor to estimate that in real time the estimating step of vehicle side inclination angle and yaw angle is as follows:

(1) computing system output matrix y (k), and calculate input regression matrix φ ^t(k);

(2) calculated gains matrix K (k);

Wherein, variance matrix parameter lambda is forgetting factor, can effectively reduce the impact of no longer relevant to model legacy data, and prevent that covariance from dispersing, conventionally span is [0.9,1], the present invention in the early stage iteration of 0-0.5 second gets 0.96 in the phase, and is taken as 0.99 after 0.5 second, and iteration cycle is taken as 0.01 second (from k-1 time be carved into the k time interval constantly);

(3) calculate solve for parameter matrix γ (k);

γ(k)＝γ(k-1)+Κ(k)[y(k)-φ ^T(k)γ(k-1)]

Wherein I is 2 * 2 unit matrixs, so far, can estimate in real time the attitude angle such as rescue obstacles removing car side rake angle and the angle of pitch.