CN105571595A - Method for estimating attitude angle of rescuing wrecker based on robust filtering - Google Patents

Abstract

The invention discloses a method for estimating an attitude angle of a rescuing wrecker based on robust filtering. The method comprises the following steps: firstly, constructing a kinematic model of the rescuing wrecker according to the working characteristics of the rescuing wrecker, and then utilizing a robust filtering algorithm to acquire accurate estimated values of a pitch angle and a lateral inclination angle of the rescuing wrecker, on the basis of the model. According to the method provided by the invention, the pitch angle and the lateral inclination angle of the rescuing wrecker under the working condition of higher gradient can be estimated under the condition that the statistical properties of the interference signal cannot be accurately known. The method has the advantages of high precision, low cost, excellent instantaneity, wide application scope, etc., and can be used for monitoring the posture of the rescuing wrecker and early warning for the dangerous posture.

Description

A kind of rescue obstacles removing car pose estimation method based on robust filtering

Technical field

The present invention relates to a kind of rescue obstacles removing car pose estimation method, particularly relate to a kind of based on H _∞the rescue obstacles removing car pose estimation method of robust filtering, its object is to, for rescue obstacles removing car provides attitude monitoring and dangerous attitude early warning, belong to rescue obstacles removing car safe early warning field.

Background technology

In recent years, the rescue obstacles removing car of China obtained high speed development.But according to the statistics of relevant department, the accident rate that rescue obstacles removing car occurs to turn on one's side or tumble also increases year by year, also exist and much fail to save safely to be rescued vehicle, and the situation that self has an accident.Trace it to its cause, operating against regulations except there is operator, to lack experience and except the subjective factor such as mismanagement, the attitude monitoring device of rescue obstacles removing car is not perfect, attitude information and safe early warning accurately cannot be provided also to be unavoidable objective factors for driver.

Patent " a kind of rescue obstacles removing car pose estimation method based on Kalman filtering " (patent No.: 201410279302) propose a kind of based on the rescue obstacles removing car angle of pitch of Kalman filtering, the evaluation method of side rake angle, this method solve under complexity rescue environment, the estimation side rake angle of larger angle and the problem of the angle of pitch.The method two low cost microelectromechanical systems (MicroElectroMechanicalsystems, MEMS) acceleration transducers replace complete sextuple Inertial Measurement Unit (InertialMeasurementUnit, IMU).MEMS acceleration transducer has the advantages such as integrated, low cost, low-power consumption, high anti-overload ability and dynamic response be fast relative to general conventional inertia sensor.

Although MEMS acceleration transducer has above-mentioned advantage, itself there is highly uncertain nonlinear noise, noise signal is difficult to by accurate description.In Kalman filtering, the noise signal of MEMS acceleration transducer often represents with probabilistic model, such as Gauss-Markov model and autoregressive model.The approximate description of these models just to MEMS acceleration transducer noise.In actual application, uncertain nonlinear noise can make the noise statistics of MEMS acceleration transducer change, thus makes the noise model that presets inaccurate.But traditional Kalman filtering algorithm, only under the statistical property prerequisite of the external interference signals that knows for sure, just there is higher filtering accuracy, so in this case Kalman filtering often occur dispersing, the problem such as precision degeneration, thus cause undesirable estimation effect.

In order to solve above-mentioned technology deficiency in the application, patent of the present invention proposes a kind of based on H _∞the rescue obstacles removing car angle of pitch of robust filtering, side rake angle evaluation method.H _∞filtering is the performance index H will introduced in Robust Controller Design _∞norm is applied to filtering, and with the various uncertain problems existed in resolution system, noise is regarded as the random signal of finite energy by it, builds the H that a wave filter makes the closed loop transfer function, exported from exogenous disturbances to evaluated error _∞norm minimum or be less than given positive number γ.Therefore H _∞to the uncertainty of noise, there is very strong robustness.The method that the present invention proposes may be used for when knowing the statistical property of undesired signal for sure, estimates the angle of pitch, the side rake angle of rescuing obstacles removing car in great slope rate operating mode.Its precision is higher, and the scope of application is wider, for rescue obstacles removing car provides better attitude monitoring and dangerous attitude early warning.

Summary of the invention

The object of the invention is to propose one based on H _∞the rescue obstacles removing car pose estimation method of robust filtering, the number of sensors that the method uses is few, and can when the statistical property of undesired signal cannot be known for sure, estimate the angle of pitch, the side rake angle of rescue obstacles removing car in great slope rate operating mode, have the advantages that precision is high, cost is low, real-time is good, applied widely, attitude monitoring and dangerous attitude early warning can be provided for rescue obstacles removing car.

The technical solution used in the present invention is as follows: a kind of based on H _∞the rescue obstacles removing car pose estimation method of robust filtering, is characterized in that: according to rescue obstacles removing car work characteristics, first carry out Kinematic Model to it, then pass through H _∞filtering algorithm realize to the rescue attitude angle such as the obstacles removing car angle of pitch and side rake angle real-time, accurately estimate, the method may be used for knowing for sure the statistical property of undesired signal when, estimate the angle of pitch, the side rake angle of rescue obstacles removing car, and only need two vehicle-mounted acceleration transducers of low cost MEMS.Concrete steps comprise:

1) kinematics model of rescue obstacles removing car is set up

Because rescue obstacles removing car rate of pitch operationally, roll velocity, vertical velocity are zero, and ignore earth rotation speed, then can set up the kinematical equation of rescue obstacles removing car:

$\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 the vertical and horizontal speed of vehicle respectively, a _x, a _yrepresent the vertical and horizontal acceleration of vehicle respectively, ω _zrepresent the yaw velocity of vehicle, the definition of above-mentioned variable all defines in bodywork reference frame; θ represents the angle of pitch of vehicle, and φ represents the side rake angle of vehicle, and g represents acceleration of gravity, gets g=9.78m/s ²; Upper mark " " represents differential, represent v _xdifferential, represent v _ydifferential;

Due to rescue obstacles removing car operationally, only utilize arm to carry out the work of suing and labouring, vehicle body remains static relative to ground, namely reasonably can think v _x, v _y, and ω _zbe zero, then formula (1) can be reduced to:

a _x+gsinθ＝0(2)

a _y-gsinφcosθ＝0

Can be obtained by formula (2)

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

2) H is set up _∞the state equation of filtering and observation equation

H after discretize _∞the matrix form of filter state equation is:

In formula (4), k represents the discretize moment; X is system state vector, and X _k=[x ₁x ₂] ^tand x ₁=θ _k, x ₂=φ _k, i.e. X _k=[θ _kφ _k] ^t, superscript ^trepresent matrix transpose; W represents system noise vector, and W=[w ₁w ₂] ^t, wherein w ₁, w ₂represent two system noise components respectively; State-transition matrix is this is because rescue obstacles removing car in the course of the work the angle of pitch and side rake angle substantially remain unchanged, can think that the angle of pitch of a upper sampling instant and side rake angle equal the angle of pitch and the side rake angle of next sampling instant; Γ _k-1for system noise input matrix, get Γ _k-1for $\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$

The H of discretize _∞the matrix form of filtering observation equation is:

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

In formula (5), Z is observation vector, and H is for observing battle array, and V represents observation noise vector, V=[n _θn _φ], wherein n _θthe observation noise of the angle of pitch, n _φit is the observation noise of side rake angle; Because observation vector and state vector all refer to the angle of pitch and side rake angle, so $H_k=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];Z_k=\left[\begin{array}{c}{\mathrm{\θ}}_{m_k}\\ {\mathrm{\φ}}_{m_k}\end{array}\right],$ Wherein with be respectively and directly calculate by measurement value sensor the angle of pitch and values of camber angles that draw, according to formula (3), have:

$\begin{array}{c}{\mathrm{\θ}}_{m_k}=\arcsin \left(\frac{-a_{x\_m_k}}{g}\right)\\ {\mathrm{\φ}}_{m_k}=\arcsin \left(\frac{a_{y\_m_k}}{g{\mathrm{cos\θ}}_{m_k}}\right)\end{array}---\left(6\right)$

In formula (6), represent the longitudinal acceleration, the transverse acceleration that utilize measured by low cost MEMS acceleration transducer respectively;

3) H _∞filtering recursion:

For the system state equation described by formula (4) and formula (5) and measurement equation, set up suboptimum H _∞the recursive process of filtering, for certain positive number γ, suboptimum H _∞filtering recursive process mainly comprises following three steps:

The estimation of state linear combination

${\hat{Y}}_{k-1}=L_{k-1}{\hat{X}}_{k-1}---\left(7\right)$

y _k-1estimated value, Y _k-1for the vector to be estimated of system, x _k-1estimated value, L _k-1for given quantity of state linear combination matrix, owing to needing the amount estimated to be exactly state variable X _k-1, so get L _k-1for $\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$

Time complexity curve

State one-step prediction:

Estimation error variance battle array:

Wherein $R_{e,k-1}=\left[\begin{array}{cc}I& 0\\ 0& -{\mathrm{\γ}}^{2}I\end{array}\right]+\left[\begin{array}{c}{H}_{k-1}\\ L_{k-1}\end{array}\right]P_{k-1}\[\begin{array}{cc}{H}_{k-1}^T& L_{k-1}^T\end{array}\],$ P _kinitial value P ₀be chosen as $\left[\begin{array}{cc}{(\mathrm{\π}/180)}^2& 0\\ 0& {(\mathrm{\π}/180)}^2\end{array}\right];$

Measure and revise

Filter gain matrix: $K_k=P_k{H}_k^T{(I+H_k{P}_k{H}_k^T)}^{-1}---\left(10\right)$

State estimation: ${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})---\left(11\right)$

After above-mentioned recurrence calculation, rescue obstacles removing car can be estimated in real time at the angle of pitch of each discrete instants k and side rake angle;

Advantage of the present invention and remarkable result:

(1) the present invention proposes a kind of method of estimation of rescuing the obstacles removing car angle of pitch, side rake angle, there is the advantages such as high precision, low cost, real-time are good, applied widely, can be used for rescuing the attitude monitoring of obstacles removing car under complex working condition and dangerous attitude early warning;

(2) method that the present invention proposes is applicable to the noisiness of MEMS acceleration transducer cannot the situation of accurate modeling, and result can meet precision and the requirement of real-time of practical application;

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

Accompanying drawing explanation

Fig. 1 is the FB(flow block) of method proposed by the invention;

Fig. 2 is that the angle of pitch estimates simulation result comparison diagram;

Fig. 3 is that side rake angle estimates simulation result comparison diagram.

Embodiment

Embodiment 1

In recent years, the rescue obstacles removing car industry of China obtained high speed development.But according to the statistics of relevant department, the accident rate that rescue obstacles removing car occurs to turn on one's side or tumble also increases year by year, also exist and much fail to save safely to be rescued vehicle, and the situation that self has an accident.Trace it to its cause, operating against regulations except there is operator, to lack experience and except the subjective factor such as mismanagement, the attitude monitoring device of rescue obstacles removing car is not perfect, attitude information and safe early warning accurately cannot be provided also to be unavoidable objective factors for driver.Patent " a kind of rescue obstacles removing car pose estimation method based on Kalman filtering " (patent No.: 201410279302) propose a kind of based on the rescue obstacles removing car angle of pitch of Kalman filtering, the evaluation method of side rake angle, this method solve under complexity rescue environment, the angle of pitch of larger angle and the estimation problem of side rake angle.The method two low cost microelectromechanical systems (MicroElectroMechanicalsystems, MEMS) acceleration transducers replace complete sextuple Inertial Measurement Unit (InertialMeasurementUnit, IMU).MEMS acceleration transducer has the advantages such as integrated, low cost, low-power consumption, high anti-overload ability and dynamic response be fast relative to general conventional inertia sensor.

Although MEMS sensor has above-mentioned advantage, itself there is highly uncertain nonlinear noise, noise signal is difficult to by accurate description.In Kalman filtering, the noise signal of MEMS sensor often represents with probabilistic model, such as Gauss-Markov model and autoregressive model.The approximate description of these models just to MEMS sensor noise.In actual application, uncertain nonlinear noise can make the noise statistics of MEMS sensor change, thus makes the noise model that presets inaccurate.But traditional Kalman filtering algorithm needs just have higher filtering accuracy under the statistical property prerequisite of the external interference signals that knows for sure.So in this case Kalman filtering often occur dispersing, the problem such as precision degeneration, thus cause undesirable estimation effect.

For common vehicle, 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, 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, vehicle kinematics process can be modeled as [can list of references: H.EricTseng, LiXu, DavorHrovat, Estimationoflandvehiclerollandpitchangles [J] .VehicleSystemDynamics:InternationalJournalofVehicleMech anicsandMobility, 2007, 45 (5): 433-443.]:

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

In formula (1), ω _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; θ represents vehicle pitch rate, and φ represents vehicle side inclination angle; G represents acceleration of gravity, and the present invention gets g=9.78m/s ²; Upper mark " " represents differential, represent v _xdifferential, represent v _ydifferential, represent v _zdifferential.

Can be obtained by formula (1), the attitude angle information of vehicle can be drawn by the navigation computation of sextuple IMU, and these class methods have and relate in a large amount of vehicle location document.But sextuple IMU is expensive, particularly three gyrostatic prices.Although the measuring accuracy of MEMS acceleration transducer is lower compared to traditional sensors, because it has the low outstanding feature of cost, apply still more and more extensive.The present invention combines the work characteristics of rescue obstacles removing car, and how research utilizes few MEMS acceleration transducer as far as possible to estimate to rescue side rake angle and the angle of pitch of obstacles removing car, and reaches higher accuracy requirement.

Due to rescue obstacles removing car operationally, only utilize arm to carry out the work of suing and labouring, vehicle body remains static relative to ground, namely reasonably can think v _x, v _y, v _z, ω _x, ω _yand ω _zbe zero, then formula (1) can be reduced to:

a _x+gsinθ＝0(2)

a _y-gsinφcosθ＝0

The computing formula of the angle of pitch and side rake angle can be obtained by formula (2):

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

As can be seen from formula (3), if recorded longitudinal acceleration and the transverse acceleration of vehicle, just can the angle of pitch of rescue obstacles removing car, side rake angle be carried out preliminary estimation and measured.The longitudinal acceleration of measuring vehicle and transverse acceleration only need with the MEMS acceleration transducers of two low costs.These two MEMS acceleration transducers are installed near vehicle body (namely rescuing other parts of vehicle of removing obstacles except arm) centroid position, one and bodywork reference frame longitudinal axis, in order to measure longitudinal acceleration, another is parallel with bodywork reference frame transverse axis, in order to measure transverse acceleration.

But considering that low cost MEMS acceleration transducer contains more measurement noises, in order to obtain the high-precision angle of pitch, side rake angle, needing to adopt filtering technique to be optimized estimation to it.

In order to overcome the shortcoming of Kalman filtering, patent of the present invention proposes a kind of based on H _∞the rescue obstacles removing car angle of pitch of robust filtering and side rake angle evaluation method, this method does not do any hypothesis to undesired signal statistical property, and only it being regarded as the signal of finite energy, thus breach the restriction of Kalman filtering, is a kind of more effective method of estimation.

H _∞filtering is the performance index H will introduced in Robust Controller Design _∞norm is applied to filtering, and with the various uncertain problems existed in resolution system, noise is regarded as the random signal of finite energy by it, builds the H that a wave filter makes the closed loop transfer function, exported from exogenous disturbances to evaluated error _∞norm minimum or be less than given positive number γ.Therefore H _∞to the uncertainty of noise, there is very strong robustness.Introduce below based on H _∞the angle of pitch of robust filtering and the concrete calculation process of side rake angle evaluation method.

First, the present invention is based on (2) formula, set up H _∞the state equation of filtering.Due in actual recursive process, need the H adopting discretize _∞filtering Model, therefore will carry out sliding-model control, obtain the H after discretize _∞the matrix form of filter state equation is:

In formula (4), k represents the discretize moment; X is system state vector, and X _k=[x ₁x ₂] ^tand x ₁=θ _k, x ₂=φ _k, i.e. X _k=[θ _kφ _k] ^t, superscript ^trepresent matrix transpose; W represents system noise vector, and W=[w ₁w ₂] ^t, wherein w ₁, w ₂represent two system noise components respectively; State-transition matrix is this is because rescue obstacles removing car in the course of the work the angle of pitch and side rake angle substantially remain unchanged, can think that the angle of pitch of a upper sampling instant and side rake angle equal the angle of pitch and the side rake angle of next sampling instant; Γ _k-1for system noise input matrix, get Γ _k-1for $\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$

Set up H _∞after the state equation of filtering, discuss below and how to set up its observation equation.By (3) Shi Ke get:

$\begin{array}{c}{\mathrm{\θ}}_{m_k}=\arcsin \left(\frac{-a_{x\_m_k}}{g}\right)\\ {\mathrm{\φ}}_{m_k}=\arcsin \left(\frac{a_{y\_m_k}}{g{\mathrm{cos\θ}}_{m_k}}\right)\end{array}---\left(5\right)$

Wherein with be respectively by MEMS acceleration transducer measured value directly calculate draw containing the noisy angle of pitch and values of camber angles. represent the longitudinal acceleration, the transverse acceleration that utilize measured by low cost MEMS acceleration transducer respectively.

In the present invention, using the angle of pitch and side rake angle as H _∞filtering observed quantity.Because the angle of pitch and side rake angle are again H simultaneously _∞two states of filtering, therefore the observation equation being not difficult to set up this system, the matrix form after its discretize is:

Z _k＝H _k·X _k+V _k(6)

In formula (6), Z is observation vector, and H is for observing battle array, and V represents observation noise vector, V=[n _θn _φ], wherein n _θthe observation noise of the angle of pitch, n _φthe observation noise of side rake angle. because observation vector and state vector all refer to the angle of pitch and side rake angle, so $H_k=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];Z_k=\left[\begin{array}{c}{\mathrm{\θ}}_{m_k}\\ {\mathrm{\φ}}_{m_k}\end{array}\right].$

For the system state equation described by formula (4) and formula (6) and measurement equation, set up suboptimum H _∞the recursive process of filtering, for certain positive number γ, suboptimum H _∞filtering recursive process mainly comprises following three steps:

The estimation of state linear combination

${\hat{Y}}_{k-1}=L_{k-1}{\hat{X}}_{k-1}---\left(7\right)$

y _k-1estimated value, Y _k-1for the vector to be estimated of system, x _k-1estimated value, L _k-1for given quantity of state linear combination matrix, owing to needing the amount estimated to be exactly state variable X _k-1, so get L _k-1for $\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$

Time complexity curve

State one-step prediction:

Estimation error variance battle array:

Wherein $R_{e,k-1}=\left[\begin{array}{cc}I& 0\\ 0& -{\mathrm{\γ}}^{2}I\end{array}\right]+\left[\begin{array}{c}{H}_{k-1}\\ L_{k-1}\end{array}\right]P_{k-1}\[\begin{array}{cc}{H}_{k-1}^T& L_{k-1}^T\end{array}\],$ P _kinitial value P ₀be chosen as $\left[\begin{array}{cc}{(\mathrm{\π}/180)}^2& 0\\ 0& {(\mathrm{\π}/180)}^2\end{array}\right];$

Measure and revise

Filter gain matrix: $K_k=P_k{H}_k^T{(I+H_k{P}_k{H}_k^T)}^{-1}---\left(10\right)$

State estimation: ${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})---\left(11\right)$

In above-mentioned reckoning process, any supposition is not carried out to system noise and observation noise, rescue obstacles removing car can be estimated in real time, exactly at the angle of pitch of each discrete instants k and side rake angle.It breaches the restriction of Kalman filtering as can be seen here, is a kind of effectively method of estimation.

Embodiment 2

For inspection the present invention propose based on H _∞the rescue obstacles removing car angle of pitch of robust filtering and the effect of side rake angle method of estimation, carried out l-G simulation test in Matlab.

In order to verify the inventive method when the statistical property of undesired signal cannot be known for sure for the applicability estimating the wide-angle angle of pitch and side rake angle, by H _∞filtering and directly survey method, Kalman filtering method has carried out simulation comparison.Choose the angle of pitch and side rake angle and be respectively 35 ° and-35 °.The measurement noises of longitudinal acceleration and transverse acceleration is all set to standard deviation is in simulations 0.05m/s ²coloured noise, the distribution of this coloured noise meets second order moving average model.Setting H _∞γ value in filtering is 1.3.

Table 1 and Fig. 2, Fig. 3 give the result of emulation experiment.Table 1 lists the statistics contrast utilizing straight survey method, Kalman filtering method and the inventive method to calculate vehicle side inclination angle and the angle of pitch, and the error in table is all for reference value.Be pointed out that in addition: straight survey method refers to the value comprising noise exported with inertial sensor, is directly calculated the method for the angle of pitch and side rake angle by embodiment 1 Chinese style (5); Kalman filtering method refers to the method calculating remove obstacles the relief car angle of pitch and side rake angle by Kalman filtering; The inventive method refer to utilize the present invention to propose based on H _∞filtering calculates the method for remove obstacles the relief car angle of pitch and side rake angle.

Table 1 three kinds of methods calculate side rake angle and angle of pitch Contrast on effect table (unit: deg)

Fig. 2 gives the result curve of the angle of pitch utilizing straight survey method, Kalman filtering method and the inventive method to estimate, straight survey method result is represented with grey dotted line in figure, grey fine line represents Kalman filtering method estimated result, and black heavy line represents the inventive method estimated result.Fig. 3 gives the result curve of the side rake angle utilizing straight survey method, Kalman filtering method and the inventive method to estimate, straight survey method result is represented with grey dotted line in figure, grey fine line represents Kalman filtering method estimated result, and black heavy line represents the inventive method estimated result.

By contrast and Fig. 2, Fig. 3 of table 1, can find out that the side rake angle that the inventive method is estimated and the angle of pitch have had relative to straight survey method and Kalman filtering method in precision and significantly improve.Can estimate side rake angle and the angle of pitch of larger angle in the situation of the statistical property of the undesired signal that cannot know for sure, estimated result can meet precision and the requirement of real-time of practical application.

Claims (1)

1. the rescue obstacles removing car pose estimation method based on robust filtering, it is characterized in that: according to rescue obstacles removing car work characteristics, Kinematic Model is carried out to it, then realize rescuing the real-time of the attitude angle such as the obstacles removing car angle of pitch and side rake angle by Robust filtering algorithms, accurate estimation, the method can when knowing the statistical property of undesired signal for sure, estimate the angle of pitch of rescue obstacles removing car under great slope rate operating mode and side rake angle, and only need two low cost microelectromechanical systems (MicroElectroMechanicalsystems, MEMS) vehicle-mounted acceleration transducer, there is low cost, high-precision feature,

Concrete steps comprise:

1) kinematics model of rescue obstacles removing car is set up

Because rescue obstacles removing car rate of pitch operationally, roll velocity, vertical velocity are zero, and ignore earth rotation speed, then can set up the kinematical equation of rescue obstacles removing car:

${\stackrel{\·}{v}}_x=a_x+{\mathrm{\ω}}_z{v}_y+gsin\mathrm{\θ}---\left(1\right)$

${\stackrel{\·}{v}}_y=a_y+{\mathrm{\ω}}_z{v}_x+gs\mathrm{in\φ}\cos \mathrm{\θ}$

In formula (1), v _x, v _yrepresent the vertical and horizontal speed of vehicle respectively, a _x, a _yrepresent the vertical and horizontal acceleration of vehicle respectively, ω _zrepresent the yaw velocity of vehicle, the definition of above-mentioned variable all defines in bodywork reference frame; θ represents the angle of pitch of vehicle, and φ represents the side rake angle of vehicle; G represents acceleration of gravity, gets g=9.78m/s ²; Upper mark " " represents differential, represent v _xdifferential, represent v _ydifferential;

Due to rescue obstacles removing car operationally, only utilize arm to carry out the work of suing and labouring, vehicle body remains static relative to ground, namely reasonably can think v _x, v _y, and ω _zbe zero, then formula (1) can be reduced to:

a _x+gsinθ＝0(2)

a _y-gsinφcosθ＝0

Can be obtained by formula (2)

$\mathrm{\θ}=\arcsin \left(\frac{-a_x}{g}\right)---\left(3\right)$

$\mathrm{\φ}=\arcsin \left(\frac{a_y}{gcos\mathrm{\θ}}\right)$

2) H is set up _∞the state equation of filtering and observation equation

H after discretize _∞the matrix form of filter state equation is:

In formula (4), k represents the discretize moment; X is system state vector, and X _k=[x ₁x ₂] ^t, x ₁=θ _k, x ₂=φ _k, i.e. X _k=[θ _kφ _k] ^t, superscript ^trepresent matrix transpose; W represents system noise vector, and W=[w ₁w ₂] ^t, wherein w ₁, w ₂represent two system noise components respectively; State-transition matrix is this is because rescue obstacles removing car in the course of the work the angle of pitch and side rake angle substantially remain unchanged, can think that the angle of pitch of a upper sampling instant and side rake angle equal the angle of pitch and the side rake angle of next sampling instant; Γ _k-1for system noise input matrix, get Γ _k-1for

$\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$

H after discretize _∞the matrix form of filtering observation equation is:

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

In formula (5), Z is observation vector, and H is for observing battle array, and V represents observation noise vector, V=[n _θn _φ], wherein n _θthe observation noise of the angle of pitch, n _φit is the observation noise of side rake angle; Because observation vector and state vector all refer to the angle of pitch and side rake angle, so $H_k=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];Z_k=\left[\begin{array}{c}{\mathrm{\θ}}_{m_k}\\ {\mathrm{\φ}}_{m_k}\end{array}\right],$ Wherein with be respectively and directly calculate by measurement value sensor the angle of pitch and values of camber angles that draw, according to formula (3), have:

${\mathrm{\θ}}_{m_k}=\arcsin \left(\frac{-a_{x\_m_k}}{g}\right)---\left(6\right)$

${\mathrm{\φ}}_{m_k}=\arcsin \left(\frac{a_{y\_m_k}}{g{\mathrm{cos\θ}}_{m_k}}\right)$

In formula (6), represent the longitudinal acceleration, the transverse acceleration that utilize measured by low cost MEMS acceleration transducer respectively;

3) H _∞filtering recursion:

For the system state equation described by formula (4) and formula (5) and measurement equation, set up suboptimum H _∞the recursive process of filtering, for certain positive number γ, suboptimum H _∞filtering recursive process mainly comprises following three steps:

The estimation of state linear combination

${\hat{Y}}_{k-1}=L_{k-1}{\hat{X}}_{k-1}---\left(7\right)$

y _k-1estimated value, Y _k-1for the vector to be estimated of system, x _k-1estimated value, L _k-1for given quantity of state linear combination matrix, owing to needing the amount estimated to be exactly state variable X _k-1, so get L _k-1for $\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right];$

Time complexity curve

State one-step prediction:

Estimation error variance battle array:

Wherein $R_{e,k-1}=\left[\begin{array}{cc}I& 0\\ 0& -{\mathrm{\γ}}^{2}I\end{array}\right]+\left[\begin{array}{c}{H}_{k-1}\\ L_{k-1}\end{array}\right]P_{k-1}\[\begin{array}{cc}{H}_{k-1}^T& L_{k-1}^T\end{array}\],$ P _kinitial value P ₀be chosen as $\left[\begin{array}{cc}{(\mathrm{\π}/180)}^2& 0\\ 0& {(\mathrm{\π}/180)}^2\end{array}\right];$

Measure and revise

Filter gain matrix: $K_k=P_k{H}_k^T{(I+H_k{P}_k{H}_k^T)}^{-1}---\left(10\right)$

State estimation: ${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})---\left(11\right)$ After above-mentioned recurrence calculation, rescue obstacles removing car can be estimated in real time at the angle of pitch of each discrete instants k and side rake angle.