CN109033017B - Vehicle roll angle and pitch angle estimation method under packet loss environment - Google Patents

Vehicle roll angle and pitch angle estimation method under packet loss environment Download PDF

Info

Publication number
CN109033017B
CN109033017B CN201810513705.1A CN201810513705A CN109033017B CN 109033017 B CN109033017 B CN 109033017B CN 201810513705 A CN201810513705 A CN 201810513705A CN 109033017 B CN109033017 B CN 109033017B
Authority
CN
China
Prior art keywords
matrix
denotes
representing
formula
equation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201810513705.1A
Other languages
Chinese (zh)
Other versions
CN109033017A (en
Inventor
冯宇
聂煊赫
陈焯明
高佳斌
俞晓天
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang University of Technology ZJUT
Original Assignee
Zhejiang University of Technology ZJUT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhejiang University of Technology ZJUT filed Critical Zhejiang University of Technology ZJUT
Priority to CN201810513705.1A priority Critical patent/CN109033017B/en
Publication of CN109033017A publication Critical patent/CN109033017A/en
Application granted granted Critical
Publication of CN109033017B publication Critical patent/CN109033017B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
    • H03H17/00—Networks using digital techniques
    • H03H17/02—Frequency selective networks
    • H03H17/0202—Two or more dimensional filters; Filters for complex signals
    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
    • H03H17/00—Networks using digital techniques
    • H03H2017/0072—Theoretical filter design

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Operations Research (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computer Hardware Design (AREA)
  • Navigation (AREA)
  • Feedback Control In General (AREA)

Abstract

The invention discloses a vehicle roll angle and pitch angle estimation method under a packet loss environment, which comprises the following steps: establishing a continuous dynamic model for the driving condition of the four-wheel vehicle; considering mixed disturbance in a complex environment and a packet loss phenomenon in wireless communication, establishing a system state equation comprising an observation equation and an output equation, and constructing a system filter; and providing a system error model, further providing the system error model based on worst non-random disturbance, and designing and solving the gain of the filter through an iterative algorithm. The method considers the influence of complex environment and data transmission packet loss phenomenon, establishes a system state equation and an observation equation aiming at a four-wheel vehicle dynamic model, further constructs a filter, and carries out real-time high-precision estimation on the roll angle alpha and the pitch angle beta of the vehicle on the premise of ensuring the robustness and the anti-interference capability of the system, the estimation result can meet the requirements of precision and real-time performance of practical application, and required related parameters can be measured by a low-cost sensor.

Description

Vehicle roll angle and pitch angle estimation method under packet loss environment
Technical Field
The invention belongs to the technical field of automobile positioning navigation, and particularly relates to a method for estimating a vehicle roll angle and a vehicle pitch angle in a packet loss environment, which considers the packet loss phenomenon in wireless communication and estimates the vehicle roll angle and the vehicle pitch angle under worst non-random interference, so as to provide high-precision estimation of the roll angle and the pitch angle under worst non-random disturbance for a navigation system.
Background
The navigation is that a navigation body is guided by a certain method or technology to safely, accurately, economically and conveniently travel to a target point according to a certain path or track, and hardware equipment for realizing the guiding function and corresponding matched software are collectively called as a navigation system. In order to make up for the defect that the GPS signal cannot be positioned when being unlocked, the GPS/INS combined navigation system can make up for the defects while exerting the respective characteristics, enhance the reliability of the navigation system and improve the navigation precision, and is the main trend of the development of the navigation technology.
Four-wheel vehicles can roll or pitch in the driving process, and in order to prevent the vehicle from rolling over, the vehicle-mounted driving posture detection system is installed on the general four-wheel vehicle. When roll or pitch motion occurs, the vertical positions of the four suspension points of the vehicle body with respect to the wheel centers of the wheels are changed, and roll angles and pitch angles are generated. The vehicle-mounted driving posture detection system receives observation data through the GPRS module, and sends out an alarm signal when detecting that the side-tipping or pitching reaches the limit working condition. Meanwhile, two parameters of the roll angle and the pitch angle are also indispensable in dead reckoning of the GPS/INS integrated navigation system. Although the values of the roll angle and the pitch angle are small in the actual vehicle running process, the acceleration of the four-wheel vehicle is far smaller than the gravity acceleration under the normal condition, so that large deviation is generated when the transverse acceleration and the longitudinal acceleration are calculated, and the real-time estimation of the roll angle and the pitch angle of the vehicle is particularly important.
When estimating the roll angle and the pitch angle, the method usually estimates by means of a Kalman filtering algorithm, and the method has the advantages of small data storage capacity, easy algorithm realization, low implementation cost and the like when processing random interference. However, the GPRS module sends the observation data to the vehicle-mounted detection system by wireless communication, so that the phenomenon that the data is not sent exists, and packet loss occurs. Meanwhile, in a complex road environment, some external random interferences (such as sensor faults, collision and other extreme conditions) exist, and the factors can influence the estimation effect of the Kalman filtering algorithm.
Disclosure of Invention
In order to solve the above problems, an object of the present invention is to provide a method for estimating a roll angle and a pitch angle of a vehicle in a packet loss environment, which ensures the robustness of the system in the packet loss environment, performs high-precision estimation on the roll angle and the pitch angle, can meet the application requirements of a four-wheel vehicle in a complex environment, and provides a navigation system with high-precision estimation of the roll angle and the pitch angle.
The present invention provides the following solutions to solve the above technical problems: the invention designs a vehicle roll angle and pitch angle estimation method under a packet loss environment. The working principle is as follows: firstly, establishing a continuous dynamic model for the four-wheel vehicle, and simulating the actual driving condition; then considering the influence of the complex environment as a mixed disturbance signal consisting of random disturbance and non-random disturbance, and simulating the packet loss phenomenon existing in the wireless communication into a Bernoulli process; further constructing a filter by using a hybrid filtering algorithm; and meanwhile, the real-time high-precision estimation is carried out on the lateral inclination angle and the pitch angle.
The method for estimating the roll angle and the pitch angle of the vehicle in the packet loss environment is characterized by comprising the following steps of:
1) establishing a continuous dynamic model for the driving condition of the four-wheel vehicle;
2) considering mixed disturbance in a complex environment and packet loss in wireless communication, establishing a system state equation comprising an observation equation and an output equation, and constructing a system filter;
3) and giving a system error model, further giving the system error model based on worst non-random disturbance in a packet loss environment, and designing and solving the filter gain through an iterative algorithm.
The method for estimating the roll angle and the pitch angle of the vehicle in the packet loss environment is characterized in that in the step 1), the method for establishing the continuous dynamic model of the driving condition of the four-wheel vehicle specifically comprises the following steps:
1.1) under the condition of not considering the rotation speed of the earth, assuming that the roll angle speed and the pitch angle speed are zero, establishing a continuous dynamic equation in the driving process of the four-wheel vehicle as shown in the formula (1):
Figure BDA0001673343750000021
wherein v isx,vyRespectively longitudinal and transverse speeds, the superscript ". cndot.x,ayRespectively denote longitudinal additionVelocity and lateral acceleration, wzRepresenting yaw angular velocity, g representing gravity acceleration, and alpha and beta representing a roll angle and a pitch angle respectively;
1.2) obtaining the expressions of the roll angle alpha and the pitch angle beta from the formula (1) as shown in the formula (2):
Figure BDA0001673343750000031
during actual vehicle travel, with respect to the longitudinal speed v of the vehiclexAnd
Figure BDA0001673343750000032
lateral velocity v of vehicleyAnd
Figure BDA0001673343750000033
are negligible, so formula (2) is simplified and shown in formula (3):
Figure BDA0001673343750000034
wherein alpha and beta respectively represent roll angle, pitch angle and longitudinal acceleration axTransverse acceleration ayLongitudinal velocity vxAnd yaw rate wzMeasured by sensors mounted on the four-wheeled vehicle, and the longitudinal speed vxBy the measured vxDerived from time.
The method for estimating the roll angle and the pitch angle of the vehicle in the packet loss environment is characterized in that in the step 2), the mixed disturbance in the complex environment and the packet loss in the sensor data transmission are considered, and the establishment of a system state equation and an observation equation comprises the following steps:
2.1) the discretized equation of state is shown in formula (4):
x(k+1)＝Ax(k)+w0(k)+w(k) (4)
where k denotes the current discretization time, k +1 denotes the next discretization time, and x denotes an estimation object including a roll angle α and a pitch angle β, that is, x ═ α [ [ α [ ]β]TThe superscript "T" denotes the transpose of the matrix, A denotes the state transition matrix of the estimation object x, w0Representing white gaussian noise with zero mean and Q variance, w representing a non-random bounded perturbation signal;
2.2) the observation equation after discretization is shown as the formula (5):
y(k)＝C2x(k)+v0(k) (5)
where k denotes the current discretization time, y denotes the observation vector, x denotes the estimation object, C2An observation matrix, v, representing an estimated object x0White gaussian noise with mean zero and variance R;
the discretized output equation is shown as equation (6):
Figure BDA0001673343750000041
where k denotes the current discretization time, z0Representing the system output, x representing the estimation object;
2.3) designing a filter based on the system, wherein the filter is represented by formula (7):
Figure BDA0001673343750000042
where k denotes the current discretization time, k +1 denotes the next discretization time, x denotes the estimation object, a denotes the state transition matrix of the estimation object x,
Figure BDA0001673343750000043
representing the estimated value of the estimated object x, L representing the filter gain to be designed, phi representing the Bernoulli process expected to be mu for simulating the packet loss environment, C2An observation matrix representing an estimation object x, y an observation vector,
Figure BDA0001673343750000044
respectively representing system outputs z, z0An estimated value of (d);
the function of the filterIs such that the estimated value
Figure BDA0001673343750000045
Proximity system output z, z0Therefore, the real-time high-precision estimation of the estimation object x, namely the roll angle alpha and the pitch angle beta is realized.
The method for estimating the roll angle and the pitch angle of the vehicle in the packet loss environment is characterized in that in the step 3), a system error model is given, the system error model based on worst non-random disturbance in the packet loss environment is further given, and the step of designing and solving the gain of the filter through an iterative algorithm specifically comprises the following steps:
3.1) obtaining an error system model according to the formula (4), the formula (5), the formula (6) and the formula (7) as shown in the formula (8):
Figure BDA0001673343750000046
where k denotes the current discretization time, k +1 denotes the next discretization time, exRepresenting the estimated object x and the corresponding estimated value
Figure BDA0001673343750000051
E represents the system output z and the corresponding estimate
Figure BDA0001673343750000052
Difference of (e)0Representing system output z0And corresponding estimated value
Figure BDA0001673343750000053
A represents a state transition matrix of an estimation object x, L represents a filter gain required to be designed, Φ represents a bernoulli process expected to be μ for simulating a packet loss environment, C2An observation matrix, w, representing an estimated object x0Denotes white Gaussian noise with mean zero and variance Q, v0Representing white gaussian noise with zero mean and R variance, w representing a non-random bounded perturbation signal;
3.2) based on the error system, defining the worst non-random bounded perturbation signal w as shown in equation (9):
w(k)＝Wex(k) (9)
substituting the formula (9) into the formula (8) to further obtain a system error model under worst non-random disturbance as shown in the formula (10):
Figure BDA0001673343750000054
defining an intermediate matrix ALAnd AWRespectively as shown in formula (11) and formula (12):
AL＝A+μLC2 (11)
AW＝A+W (12)
where k denotes the current discretization time, k +1 denotes the next discretization time, exRepresenting the estimated object x and the corresponding estimated value
Figure BDA0001673343750000055
E represents the system output z and the corresponding estimate
Figure BDA0001673343750000056
Difference of (e)0Representing system output z0And corresponding estimated value
Figure BDA0001673343750000057
A represents a state transition matrix of an estimation object x, L represents a filter gain required to be designed, Φ represents a bernoulli process expected to be μ for simulating a packet loss environment, C2An observation matrix, w, representing an estimated object x0Denotes white Gaussian noise with mean zero and variance Q, v0Denotes white Gaussian noise with mean zero and variance R, W denotes a non-random bounded perturbation signal, μ denotes the expected value of the Bernoulli process Φ, W, ALAnd AWAre all intermediate matrices;
3.3) when k is 0, for the intermediate matrix P1And P2And the intermediate matrix W and the filter gain L are assigned initial values as shown in equation (13):
P1(0),P2(0),W(0),L(0) (13)
3.4) based on error system, from H∞The filtering algorithm obtains the worst non-random bounded perturbation signal w as shown in equation (14):
w(k)＝(γ2P1 -1-In)-1ALex(k) (14)
thus, as shown in equation (15):
W＝(γ2P1 -1-In)-1AL (15)
wherein k represents the current discretization time, W represents a non-random bounded perturbation signal, gamma is a preset constant, I represents a unit matrix, a superscript of "-1" represents the inverse of the matrix, a superscript of "T" represents the transposition of the matrix, and W, ALAnd P1Are all intermediate matrices, exRepresenting the estimated object x and the corresponding estimated value
Figure BDA0001673343750000063
A difference of (d);
3.5) system error model based on worst non-random disturbance in packet loss environment, through H2The expression of the filter gain L obtained by the filtering algorithm is shown in formula (16):
Figure BDA0001673343750000061
wherein, the superscript "-1" represents the inverse of the matrix, the superscript "T" represents the transpose of the matrix, L represents the filter gain to be designed, C2An observation matrix representing an estimated object x, R representing white Gaussian noise v0Variance of (A)WAnd P2Are all intermediate matrices;
3.6) when k is 1, formula (17), formula (18), formula (19), and formula (20) are obtained from formula (11), formula (12), formula (15), and formula (16), respectively:
AW(1)＝A+W(0) (17)
AL(1)＝A+μL(0)C2 (18)
W(1)＝W＝(γ2P1 -1(0)-In)-1AL(1) (19)
Figure BDA0001673343750000062
where A represents the state transition matrix of the estimation object x, μ represents the expected value of the Bernoulli process Φ, L represents the filter gain to be designed, and C2An observation matrix representing an estimation object x, gamma is a preset constant, I represents an identity matrix, a superscript of-1 represents the inverse of the matrix, a superscript of T represents the transposition of the matrix, w represents a non-random bounded perturbation signal, and R represents white Gaussian noise v0Variance of (A), W, AL,AW,P1And P2Are all intermediate matrices;
3.7) intermediate matrix P1Satisfying the following Riccati equation as shown in equation (21):
Figure BDA0001673343750000071
thus obtaining an intermediate matrix P1(1) As shown in equation (22):
Figure BDA0001673343750000072
wherein k represents the current discretization time, k +1 represents the next discretization time, L represents the filter gain to be designed, C2An observation matrix representing an estimated object x, gamma is a preset constant, I represents an identity matrix, mu represents an expected value of a Bernoulli process phi, a superscript of '-1' represents the inverse of the matrix, a superscript of 'T' represents the transpose of the matrix, w represents a non-random bounded perturbation signal, ALAnd P1Are all intermediate matrices;
3.8) intermediate matrix P2Satisfying the following Riccati (ricartho) equation as shown in equation (23):
Figure BDA0001673343750000073
thus obtaining an intermediate matrix P2(1) As shown in equation (24):
Figure BDA0001673343750000074
where k denotes the current discretization time, k +1 denotes the next discretization time, C2An observation matrix representing an estimated object x, a superscript of "-1" representing the inverse of the matrix, a superscript of "T" representing the transpose of the matrix, and Q representing white Gaussian noise w0R represents white Gaussian noise v0The variance of (A) and [ mu ] represents the expected value of the Bernoulli process [ phi ]WAnd P2Are all intermediate matrices;
3.9) repeat step 3.6), step 3.7) and step 3.8):
if k equals T time, the matrix P1(T) and matrix P1(T-1) the two-norm difference is less than the given error, and the difference is respectively as shown in the formulas (25) and (26):
P1＝P1(T)＝P1(T-1) (25)
AL＝AL(T)＝AL(T-1) (26)
similarly, if k equals T, the matrix P2(T) and matrix P2(T-1) the difference is less than a given error in two norms,
obtaining the compounds shown in formulas (27) and (28):
P2＝P2(T)＝P2(T-1) (27)
AW＝AW(T)＝AW(T-1) (28)
wherein A isL,AW,P1And P2Are all intermediate matrices;
3.10) combining the intermediate matrices P2And intermediate matrix AWA filter gain matrix L is obtained by substituting equation (16). Therefore, the filter (7) can realize real-time high-precision estimation on the estimation object x, and the estimation object x is a roll angle alpha and a pitch angle beta.
The invention designs a vehicle roll angle and pitch angle estimation method under a packet loss environment, which solves two linear Riccati equations through an iterative algorithm, constructs a filter to realize real-time high-precision estimation of a vehicle roll angle alpha and a vehicle pitch angle beta under a worst non-random disturbance signal, and has the following beneficial effects compared with the prior art by adopting the technology: the method considers the influence of a complex environment and the phenomenon of packet loss, establishes a system state equation and an observation equation aiming at a four-wheel vehicle dynamic model, further constructs a filter, and carries out real-time high-precision estimation on the roll angle alpha and the pitch angle beta of the vehicle on the premise of ensuring the robustness and the anti-interference capability of the system. The estimation result can meet the requirements of accuracy and real-time performance of practical application, and required relevant parameters can be measured by low-cost sensors.
Drawings
FIG. 1 shows the state quantity x of the process according to the invention1Real-time simulation effect graph;
FIG. 2 shows the state quantity x of the method according to the invention2Real-time simulation effect graph;
in the figure: x is the number of1In order to be the side-tilt angle alpha,
Figure BDA0001673343750000081
is x1An estimated value of (d); x is the number of2Is a pitch angle beta, and is,
Figure BDA0001673343750000082
is x2An estimate of (d).
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more clear, the technical solutions of the present invention are further described below with reference to the accompanying drawings and simulation data.
The invention designs a vehicle roll angle and pitch angle estimation method under a packet loss environment. The working principle is as follows: firstly, establishing a continuous dynamic model for the four-wheel vehicle, and simulating the actual driving condition; then considering the influence of the complex environment as a mixed disturbance signal consisting of random disturbance and non-random disturbance, and simulating the packet loss phenomenon into a Bernoulli process; further constructing a filter by using a hybrid filtering algorithm; and meanwhile, the real-time high-precision estimation is carried out on the lateral inclination angle and the pitch angle.
The invention discloses a vehicle roll angle and pitch angle estimation method under a packet loss environment, which comprises the following specific steps:
the method for establishing the continuous dynamics model for the driving condition of the four-wheel vehicle specifically comprises the following steps:
(1.1) under the condition of not considering the rotation speed of the earth, assuming that the roll angle speed and the pitch angle speed are zero, establishing the running process of the four-wheel vehicle
The continuous kinetic equation of (a):
Figure BDA0001673343750000091
wherein v isx,vyRespectively longitudinal and transverse speeds, the superscript ". cndot.x,ayRespectively representing longitudinal and transverse acceleration, wzRepresenting yaw angular velocity, g representing gravity acceleration, and alpha and beta representing a roll angle and a pitch angle respectively;
(1.2) expressions of roll angle α and pitch angle β are obtained from formula (1):
Figure BDA0001673343750000092
during actual vehicle travel, with respect to the longitudinal speed v of the vehiclexAnd longitudinal acceleration
Figure BDA0001673343750000093
Lateral velocity v of vehicleyAnd lateral acceleration
Figure BDA0001673343750000094
It is very small and negligible, so equation (2) can be simplified as:
Figure BDA0001673343750000101
Figure BDA0001673343750000102
wherein alpha and beta respectively represent roll angle, pitch angle and longitudinal acceleration axTransverse acceleration ayLongitudinal velocity vxAnd yaw rate wzMeasured by sensors mounted on the four-wheeled vehicle, and the longitudinal speed vxBy the measured vxDerived from time;
2) considering mixed disturbance in a complex environment and a packet loss phenomenon in wireless communication, establishing a system state equation comprising an observation equation and an output equation, and constructing a system filter comprises the following steps:
(2.1) the discretized equation of state is:
x(k+1)＝Ax(k)+w0(k)+w(k) (4)
where k denotes the current discretization time, k +1 denotes the next discretization time, and the estimation object x denotes the roll angle α and the pitch angle β, that is, x ═ x1 x2]T＝[α β]TThe superscript "T" represents the transpose of the matrix, and since the roll angle and pitch angle are both continuously and slowly changing during the running of the vehicle, it can be considered that the roll angle and pitch angle at the current sampling time are similar to the roll angle and pitch angle at the next sampling time, so the state transition matrix of the estimation object x is taken as
Figure BDA0001673343750000103
w0Means mean of zero variance of
Figure BDA0001673343750000104
White gaussian noise, a non-random bounded perturbation signal is modeled by w |0.016 | sin (0.1 |).
(2.2) the observation equation after discretization is:
y(k)＝C2x(k)+v0(k) (5)
where k denotes the discretization time, y denotes the observation vector, x denotes the estimated object, and v denotes the time0The mean value is white gaussian noise with zero variance R0.05, and the observation matrix of the estimation object x is C2＝[2 2.5]。
The discretized output equation is:
Figure BDA0001673343750000105
where k denotes the discretization time, z0Representing the system output and x representing the estimation object.
(2.3) designing a filter based on the above system:
Figure BDA0001673343750000111
where k denotes the current discretization time, k +1 denotes the next discretization time,
Figure BDA0001673343750000112
represents an estimated value of an object x whose state transition matrix is
Figure BDA0001673343750000113
L denotes the filter gain to be designed, Φ denotes the bernoulli process expected to be μ ═ 0.9, and the observation matrix of the estimated object x is C2＝[2 2.5]And y represents an observation vector, and y represents,
Figure BDA0001673343750000114
respectively representing system outputs z, z0An estimated value of (d);
the effect of the filter is to make the estimate
Figure BDA0001673343750000115
Proximity system output z, z0Therefore, the real-time high-precision estimation of the estimation object x, namely the roll angle alpha and the pitch angle beta is realized;
3) providing a system error model, further providing the system error model based on worst non-random disturbance, designing and solving through an iterative algorithm
The filter gain solution specifically comprises the following steps:
(3.1) obtaining an error system model by using the following equations (4), (5), (6) and (7):
Figure BDA0001673343750000116
where k denotes the current discretization time, k +1 denotes the next discretization time, exRepresenting the estimated object x and the corresponding estimated value
Figure BDA0001673343750000117
The difference value of (a) to (b),
e represents the system output z and the corresponding estimated value
Figure BDA0001673343750000118
Difference of e0Representing system output z0And corresponding estimated value
Figure BDA0001673343750000119
Estimate a state transition matrix of the object x as
Figure BDA00016733437500001110
L denotes the filter gain that needs to be designed, Φ denotes the bernoulli process, which is expected to be μ ═ 0.9, w0Means a mean of zero variance of
Figure BDA00016733437500001111
White Gaussian noise, v0The mean value is gaussian white noise with zero variance R of 0.05, and the observation matrix of the object x is estimated to be C2＝[2 2.5]Non-random bounded perturbation signals are modeled by w |0.016 × sin (0.1 × k) |.
(3.2) defining the worst non-random bounded perturbation signal w based on an error system:
w(k)＝Wex(k) (9)
substituting the formula (9) into the formula (8) to further obtain a system error model under worst non-random disturbance:
Figure BDA0001673343750000121
defining an intermediate matrix ALAnd AW：
AL＝A+μLC2 (11)
AW＝A+W (12)
Where k denotes the current discretization time, k +1 denotes the next discretization time, and the preset constant γ is 2.5, exRepresenting the estimated object x and the corresponding estimated value
Figure BDA0001673343750000122
E represents the system output z and the corresponding estimate
Figure BDA0001673343750000123
Difference of (e)0Representing system output z0And corresponding estimated value
Figure BDA0001673343750000124
Estimate a state transition matrix of the object x as
Figure BDA0001673343750000125
L denotes the filter gain to be designed, Φ denotes the bernoulli process expected to be μ ═ 0.9, and the observation matrix of the estimated object x is C2＝[2 2.5]，v0White gaussian noise with mean zero and variance R0.05, w0Means a mean of zero variance of
Figure BDA0001673343750000126
Is white Gaussian noise, W represents a non-random bounded perturbation signal, W, ALAnd AWAre all intermediate matrices;
(3.3) when k is 0, the intermediate matrix P is aligned1And P2And the intermediate matrix W and the filter gain L are assigned initial values, namely:
Figure BDA0001673343750000127
Figure BDA0001673343750000128
(3.4) based on error system, from H∞The filtering algorithm obtains the worst non-random bounded disturbance signal w:
w(k)＝(γ2P1 -1-In)-1ALex(k) (14)
thus:
W＝(γ2P1 -1-In)-1AL (15)
wherein k represents the current discretization time, W represents a non-random bounded perturbation signal, a preset constant is gamma is 2.5, I represents a unit matrix, a superscript of "-1" represents the inverse of the matrix, a superscript of "T" represents the transpose of the matrix, and W, aLAnd P1Are all intermediate matrices, exRepresenting the estimated object x and the corresponding estimated value
Figure BDA0001673343750000131
The difference of (c).
(3.5) under the packet loss environment, based on a system error model under worst non-random disturbance, through H2The filtering algorithm obtains an expression of the filter gain L:
Figure BDA0001673343750000132
wherein, the superscript "-1" represents the inverse of the matrix, the superscript "T" represents the transpose of the matrix, L represents the filter gain to be designed, the observation matrix C of the estimation object x2＝[2 2.5]White gaussian noise v0Variance of (a) 0.05, aWAnd P2Are all intermediate matrices.
(3.6) when k is 1, the compound is obtained from formula (11), formula (12), formula (15), formula (16):
AW(1)＝A+W(0) (17)
AL(1)＝A+μL(0)C2 (18)
W(1)＝W＝(γ2P1 -1(0)-In)-1AL(1) (19)
Figure BDA0001673343750000133
wherein the state transition matrix of the estimation object x is
Figure BDA0001673343750000134
L represents the filter gain to be designed, and the observation matrix of the estimated object x is C2＝[2 2.5]The bernoulli process Φ is expected to be μ 0.9, the preset constant γ 2.5, I denotes an identity matrix, the superscript "-1" denotes the inverse of the matrix, the superscript "T" denotes the transpose of the matrix, and white gaussian noise w0Has a variance of
Figure BDA0001673343750000135
White gaussian noise v0Variance of (A) 0.05, W, AL,AW,P1And P2Are all intermediate matrices;
(3.7) intermediate matrix P1The following Riccati (ricacatti) equation is satisfied:
Figure BDA0001673343750000141
thus obtaining an intermediate matrix P1(1)：
Figure BDA0001673343750000142
Where k denotes the current discretization time, k +1 denotes the next discretization time, the expectation of the bernoulli process Φ is μ ═ 0.9, L denotes the filter gain that needs to be designed, and the observation matrix of the estimation object x is C2＝[2 2.5]The preset constant is gamma 2.5, I represents an identity matrix,the superscript "-1" represents the inverse of the matrix, the superscript "T" represents the transpose of the matrix, w represents the non-random bounded perturbation signal, ALAnd P1Are all intermediate matrices.
(3.8) intermediate matrix P2The following Riccati (ricacatti) equation is satisfied:
Figure BDA0001673343750000143
thus obtaining an intermediate matrix P2(1)：
Figure BDA0001673343750000144
Where k denotes the current discretization time, k +1 denotes the next discretization time, the superscript "-1" denotes the inverse of the matrix, the superscript "T" denotes the transpose of the matrix, the expectation of the bernoulli process Φ is μ ═ 0.9, and the observation matrix of the estimation object x is C2＝[2 2.5]White gaussian noise v0Variance of (a) 0.05, aWAnd P2Are all intermediate matrices;
(3.9) repeating the steps (3.6), (3.7) and (3.8) to obtain:
P1(k),P2(k),W(k),L(k)
when k is 40, the matrix P1(40) And matrix P1(39) The two-norm difference is less than 0.0001 error, yielding:
Figure BDA0001673343750000145
Figure BDA0001673343750000146
likewise, at time k 170, the matrix P2(40) Sum matrix P2(39) The two-norm difference is less than 0.0001 of error,
obtaining:
Figure BDA0001673343750000151
Figure BDA0001673343750000152
wherein A isL,AW,P1And P2Are all intermediate matrices;
(3.10) intermediate matrix P of formulae (27) (28)2And intermediate matrix AWA filter gain matrix L is obtained by substituting equation (16). Thus, the filter (7) realizes real-time high-precision estimation of the estimation object x (namely, the roll angle alpha and the pitch angle beta). As shown in the drawing, FIG. 1 shows a state quantity x1(i.e., roll angle α) and the estimated value
Figure BDA0001673343750000153
FIG. 2 shows the state quantity x2(i.e. pitch angle β) and the estimated value
Figure BDA0001673343750000154
From fig. 1 and fig. 2, it can be obtained that the estimated values of the roll angle and the pitch angle can well follow the state quantity, so as to achieve real-time high precision.
The invention designs a vehicle roll angle and pitch angle estimation method under a packet loss environment, which solves two linear Riccati equations through an iterative algorithm, and constructs a filter to realize real-time high-precision estimation of a vehicle roll angle alpha and a vehicle pitch angle beta under a worst non-random disturbance signal.
The invention has the advantages that: the influence of complex environment and data transmission packet loss phenomenon is considered, a system state equation and an observation equation are established for a four-wheel vehicle dynamic model, a filter is further established, and the roll angle alpha and the pitch angle beta of the vehicle are estimated in real time and high-precision on the premise of guaranteeing the robustness and the anti-interference capacity of the system. The estimation result can meet the requirements of actual application on precision and real-time performance, and required relevant parameters can be measured by a low-cost sensor.
The embodiments of the present invention have been described and illustrated in detail above with reference to the accompanying drawings, but are not limited thereto. Many variations and modifications are possible which remain within the knowledge of a person skilled in the art, given the teaching of the present invention.

Claims (1)

1. A method for estimating a roll angle and a pitch angle of a vehicle in a packet loss environment is characterized by comprising the following steps:
1) the method for establishing the continuous dynamics model for the driving condition of the four-wheel vehicle specifically comprises the following steps:
1.1) under the condition of not considering the rotation speed of the earth, assuming that the roll angle speed and the pitch angle speed are zero, establishing a continuous dynamic equation in the driving process of the four-wheel vehicle as shown in the formula (1):
Figure FDA0003580390280000011
wherein v isx,vyRespectively longitudinal and transverse speeds, the superscript ". cndot.x,ayRespectively representing longitudinal and transverse accelerations, wzRepresenting yaw angular velocity, g representing gravity acceleration, and alpha and beta representing a roll angle and a pitch angle respectively;
1.2) obtaining the expressions of the roll angle alpha and the pitch angle beta from the formula (1) as shown in the formula (2):
Figure FDA0003580390280000012
during actual vehicle travel, with respect to the longitudinal speed v of the vehiclexAnd
Figure FDA0003580390280000013
lateral velocity v of vehicleyAnd
Figure FDA0003580390280000014
are all very small and can be ignored, so that formula (2) is simplified asFormula (3):
Figure FDA0003580390280000015
wherein alpha and beta respectively represent roll angle, pitch angle and longitudinal acceleration axTransverse acceleration ayLongitudinal velocity vxAnd yaw rate wzMeasured by sensors mounted on the four-wheeled vehicle, and the longitudinal speed vxBy the measured vxDerived from time;
2) considering mixed disturbance in a complex environment and packet loss in wireless communication, establishing a system state equation comprising an observation equation and an output equation, and constructing a system filter, wherein the method specifically comprises the following steps:
2.1) the discretized equation of state is shown in formula (4):
x(k+1)＝Ax(k)+w0(k)+w(k) (4)
where k denotes the current discretization time, k +1 denotes the next discretization time, and x denotes an estimation object including a roll angle α and a pitch angle β, that is, x ═ α β]TThe superscript "T" denotes the transpose of the matrix, A denotes the state transition matrix of the estimation object x, w0Representing white gaussian noise with zero mean and Q variance, w representing a non-random bounded perturbation signal;
2.2) the observation equation after discretization is shown as the formula (5):
y(k)＝C2x(k)+v0(k) (5)
where k denotes the current discretization time, y denotes the observation vector, x denotes the estimation object, C2An observation matrix, v, representing an estimated object x0White gaussian noise with mean zero and variance R;
the discretized output equation is shown as equation (6):
Figure FDA0003580390280000021
where k denotes the current discretization time, z0Representing the system output, x representing the estimation object;
2.3) designing a filter based on the system, wherein the filter is represented by formula (7):
Figure FDA0003580390280000022
where k denotes the current discretization time, k +1 denotes the next discretization time, x denotes the estimation object, a denotes the state transition matrix of the estimation object x,
Figure FDA0003580390280000023
representing the estimated value of the estimated object x, L representing the filter gain to be designed, phi representing the Bernoulli process expected to be mu for simulating the packet loss environment, C2An observation matrix representing an estimation object x, y an observation vector,
Figure FDA0003580390280000024
respectively representing system outputs z, z0An estimated value of (d);
the effect of the filter is to make the estimate
Figure FDA0003580390280000025
Proximity system output z, z0Therefore, the real-time high-precision estimation of the estimated object x, namely the roll angle alpha and the pitch angle beta is realized;
3) providing a system error model, further providing the system error model based on worst non-random disturbance in a packet loss environment, designing and solving filter gain through an iterative algorithm, and specifically comprising the following steps:
3.1) obtaining an error system model according to the formula (4), the formula (5), the formula (6) and the formula (7) as shown in the formula (8):
Figure FDA0003580390280000031
where k denotes the current discretization time, k +1 denotes the next discretization time, exRepresenting the estimated object x and the corresponding estimated value
Figure FDA0003580390280000032
E represents the system output z and the corresponding estimate
Figure FDA0003580390280000033
Difference of (e)0Representing system output z0And corresponding estimated value
Figure FDA0003580390280000034
A represents a state transition matrix of an estimation object x, L represents a filter gain required to be designed, Φ represents a bernoulli process expected to be μ for simulating a packet loss environment, C2An observation matrix, w, representing an estimated object x0Denotes white Gaussian noise with mean zero and variance Q, v0Representing white gaussian noise with zero mean and R variance, w representing a non-random bounded perturbation signal;
3.2) based on the error system, defining the worst non-random bounded perturbation signal w as shown in equation (9):
w(k)＝Wex(k) (9)
substituting the formula (9) into the formula (8) to further obtain a system error model under worst non-random disturbance as shown in the formula (10):
Figure FDA0003580390280000035
defining an intermediate matrix ALAnd AWRespectively as shown in formula (11) and formula (12):
AL＝A+μLC2 (11)
AW＝A+W (12)
wherein k represents whenThe previous discretization time, k +1, the next discretization time, exRepresenting the estimated object x and the corresponding estimated value
Figure FDA0003580390280000036
E represents the system output z and the corresponding estimate
Figure FDA0003580390280000037
Difference of e0Representing system output z0And corresponding estimated value
Figure FDA0003580390280000041
A represents a state transition matrix of an estimation object x, L represents a filter gain required to be designed, Φ represents a bernoulli process expected to be μ for simulating a packet loss environment, C2An observation matrix, w, representing an estimated object x0Denotes white Gaussian noise with mean zero and variance Q, v0Denotes white Gaussian noise with mean zero and variance R, W denotes a non-random bounded perturbation signal, μ denotes the expected value of the Bernoulli process Φ, W, ALAnd AWAre all intermediate matrices;
3.3) when k is 0, for the intermediate matrix P1And P2And the intermediate matrix W and the filter gain L are assigned initial values as shown in equation (13):
P1(0),P2(0),W(0),L(0) (13)
3.4) based on error system, from H∞The filtering algorithm obtains the worst non-random bounded perturbation signal w as shown in equation (14):
w(k)＝(γ2P1 -1-In)-1ALex(k) (14)
thus, as shown in equation (15):
W＝(γ2P1 -1-In)-1AL (15)
wherein k represents the current discretization time, w represents a non-random bounded perturbation signal, and gamma is a preset constantNumber, I denotes the identity matrix, the superscript "-1" denotes the inverse of the matrix, the superscript "T" denotes the transpose of the matrix, W, ALAnd P1Are all intermediate matrices, exRepresenting the estimated object x and the corresponding estimated value
Figure FDA0003580390280000042
A difference of (d);
3.5) system error model based on worst non-random disturbance in packet loss environment, through H2The expression of the filter gain L obtained by the filtering algorithm is shown in formula (16):
Figure FDA0003580390280000043
wherein, the superscript "-1" represents the inverse of the matrix, the superscript "T" represents the transpose of the matrix, L represents the filter gain to be designed, C2An observation matrix representing an estimated object x, R representing white Gaussian noise v0Variance of (A)WAnd P2Are all intermediate matrices;
3.6) when k is 1, formula (17), formula (18), formula (19), and formula (20) are obtained from formula (11), formula (12), formula (15), and formula (16), respectively:
AW(1)＝A+W(0) (17)
AL(1)＝A+μL(0)C2 (18)
W(1)＝W＝(γ2P1 -1(0)-In)-1AL(1) (19)
Figure FDA0003580390280000051
where A represents the state transition matrix of the estimation object x, μ represents the expected value of the Bernoulli process Φ, L represents the filter gain to be designed, and C2An observation matrix representing an object x to be estimated, gamma being presetConstant, I denotes the identity matrix, the superscript "-1" denotes the inverse of the matrix, the superscript "T" denotes the transpose of the matrix, w denotes the non-random bounded perturbation signal, R denotes white Gaussian noise v0Variance of (A), W, AL,AW,P1And P2Are all intermediate matrices;
3.7) intermediate matrix P1Satisfying the following Riccati equation as shown in equation (21):
Figure FDA0003580390280000052
thus obtaining an intermediate matrix P1(1) As shown in equation (22):
Figure FDA0003580390280000053
wherein k represents the current discretization time, k +1 represents the next discretization time, L represents the filter gain to be designed, C2An observation matrix representing an estimated object x, gamma is a preset constant, I represents an identity matrix, mu represents an expected value of a Bernoulli process phi, a superscript of '-1' represents the inverse of the matrix, a superscript of 'T' represents the transpose of the matrix, w represents a non-random bounded perturbation signal, ALAnd P1Are all intermediate matrices;
3.8) intermediate matrix P2Satisfying the following Riccati (ricartho) equation as shown in equation (23):
Figure FDA0003580390280000054
thus obtaining an intermediate matrix P2(1) As shown in equation (24):
Figure FDA0003580390280000061
where k denotes the current discretization time, k +1 denotes the next discretization time, C2An observation matrix representing an estimated object x, a superscript of "-1" representing the inverse of the matrix, a superscript of "T" representing the transpose of the matrix, and Q representing white Gaussian noise w0R represents Gaussian white noise v0The variance of (A) and [ mu ] represents the expected value of the Bernoulli process [ phi ]WAnd P2Are all intermediate matrices;
3.9) repeat step 3.6), step 3.7) and step 3.8):
if k equals T time, the matrix P1(T) and matrix P1(T-1) the two-norm difference is less than the given error, and the difference is respectively as shown in the formulas (25) and (26):
P1＝P1(T)＝P1(T-1) (25)
AL＝AL(T)＝AL(T-1) (26)
similarly, if k equals T, the matrix P2(T) and matrix P2(T-1) the two-norm difference is less than the given error, and the difference is respectively obtained as shown in the formula (27) and the formula (28):
P2＝P2(T)＝P2(T-1) (27)
AW＝AW(T)＝AW(T-1) (28)
wherein A isL,AW,P1And P2Are all intermediate matrices;
3.10) combining the intermediate matrices P2And intermediate matrix AWA filter gain matrix L is obtained by substituting an expression (16), so that the real-time high-precision estimation of an estimation object x is realized by a filter (7), and the estimation object x is a roll angle alpha and a pitch angle beta.
CN201810513705.1A 2018-05-25 2018-05-25 Vehicle roll angle and pitch angle estimation method under packet loss environment Active CN109033017B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810513705.1A CN109033017B (en) 2018-05-25 2018-05-25 Vehicle roll angle and pitch angle estimation method under packet loss environment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810513705.1A CN109033017B (en) 2018-05-25 2018-05-25 Vehicle roll angle and pitch angle estimation method under packet loss environment

Publications (2)

Publication Number Publication Date
CN109033017A CN109033017A (en) 2018-12-18
CN109033017B true CN109033017B (en) 2022-05-13

Family

ID=64611502

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810513705.1A Active CN109033017B (en) 2018-05-25 2018-05-25 Vehicle roll angle and pitch angle estimation method under packet loss environment

Country Status (1)

Country Link
CN (1) CN109033017B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110677428A (en) * 2019-09-30 2020-01-10 上海智驾汽车科技有限公司 Vehicle control method and device based on intelligent network connection
US11544858B2 (en) * 2019-11-21 2023-01-03 GM Global Technology Operations LLC Camera-based enhancement of vehicle kinematic state estimation
CN113566777B (en) * 2020-04-29 2023-04-18 广州汽车集团股份有限公司 Vehicle pitch angle estimation method and system, computer device and storage medium

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104034332A (en) * 2014-06-20 2014-09-10 东南大学 Kalman filtering-based method for estimating attitude angle of rescue wrecker
CN104061899A (en) * 2014-06-20 2014-09-24 东南大学 Kalman filtering based method for estimating roll angle and pitching angle of vehicle
CN104168005A (en) * 2014-08-25 2014-11-26 北京理工大学 Kalman filtering method for recursive estimation under condition that observation noise covariance matrix is unknown
CN105372994A (en) * 2015-12-10 2016-03-02 中国人民解放军海军航空工程学院 Robust adaptive optimal control-based pilot manipulation behavior description method
CN105571595A (en) * 2015-12-16 2016-05-11 东南大学 Method for estimating attitude angle of rescuing wrecker based on robust filtering
CN106527139A (en) * 2016-11-29 2017-03-22 桂林航天工业学院 Robust controller design method for vehicle yawing motion under limited communication condition
CN106972949A (en) * 2017-02-16 2017-07-21 河海大学 A kind of fractional order network system situation method of estimation based on adaptive equalization technology
CN107315413A (en) * 2017-07-12 2017-11-03 北京航空航天大学 Under a kind of truck traffic environment consider vehicle between relative position many car co-located algorithms

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104034332A (en) * 2014-06-20 2014-09-10 东南大学 Kalman filtering-based method for estimating attitude angle of rescue wrecker
CN104061899A (en) * 2014-06-20 2014-09-24 东南大学 Kalman filtering based method for estimating roll angle and pitching angle of vehicle
CN104168005A (en) * 2014-08-25 2014-11-26 北京理工大学 Kalman filtering method for recursive estimation under condition that observation noise covariance matrix is unknown
CN105372994A (en) * 2015-12-10 2016-03-02 中国人民解放军海军航空工程学院 Robust adaptive optimal control-based pilot manipulation behavior description method
CN105571595A (en) * 2015-12-16 2016-05-11 东南大学 Method for estimating attitude angle of rescuing wrecker based on robust filtering
CN106527139A (en) * 2016-11-29 2017-03-22 桂林航天工业学院 Robust controller design method for vehicle yawing motion under limited communication condition
CN106972949A (en) * 2017-02-16 2017-07-21 河海大学 A kind of fractional order network system situation method of estimation based on adaptive equalization technology
CN107315413A (en) * 2017-07-12 2017-11-03 北京航空航天大学 Under a kind of truck traffic environment consider vehicle between relative position many car co-located algorithms

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Adaptive Kalman Filtering in Networked Systems;Maryam Moayedi 等;《IEEE TRANSACTIONSON SIGNAL PROCESSING》;20100331;第1-12页 *
H ∞ Gaussian filter on infinite time horizon;X. Chen 等;《IEEE Transactions on Circuits and Systems-Part I: Fundamental》;20020531;第1-6页 *
无线网络控制系统滤波器设计;胡天水;《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》;20160215;第1-11页 *

Also Published As

Publication number Publication date
CN109033017A (en) 2018-12-18

Similar Documents

Publication Publication Date Title
CN108731670B (en) Inertial/visual odometer integrated navigation positioning method based on measurement model optimization
CN109033017B (en) Vehicle roll angle and pitch angle estimation method under packet loss environment
CN111879957B (en) Vehicle dynamics determination based on fuzzy logic and enhanced machine learning
CN105151047A (en) Automobile gravity center slip angle measuring method
Li et al. Vehicle sideslip angle estimation based on hybrid Kalman filter
CN110341714B (en) Method for simultaneously estimating vehicle mass center slip angle and disturbance
CN113002549A (en) Vehicle state estimation method, device, equipment and storage medium
CN111796522B (en) Vehicle state estimation method
CN104034332A (en) Kalman filtering-based method for estimating attitude angle of rescue wrecker
CN115406446A (en) Multi-axis special vehicle state estimation method based on neural network and unscented Kalman filtering
CN111006884B (en) Method for measuring wheel axle slip angle and slip stiffness based on Fourier transform
CN112270039A (en) Distributed asynchronous fusion-based nonlinear state estimation method for drive-by-wire chassis vehicle
CN108413923B (en) Vehicle roll angle and pitch angle estimation method based on robust hybrid filtering
CN113008229B (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN104819717A (en) Multi-rotor aircraft attitude detection method based on MEMS inertial sensor group
Song et al. Reliable estimation of automotive states based on optimized neural networks and moving horizon estimator
CN113063414A (en) Vehicle dynamics pre-integration construction method for visual inertia SLAM
CN105571595A (en) Method for estimating attitude angle of rescuing wrecker based on robust filtering
CN112287289A (en) Vehicle nonlinear state fusion estimation method for cloud control intelligent chassis
CN110489807B (en) Local accurate positioning method for swing arm suspension structure inspection device
CN117068184A (en) Method, device and equipment for determining vehicle body slip angle
Huang et al. Estimation of sideslip angle based on extended Kalman filter
Zhang et al. Learning end-to-end inertial-wheel odometry for vehicle ego-motion estimation
CN114440881B (en) Unmanned vehicle positioning method integrating multi-source sensor information
CN113978476B (en) Wire-controlled automobile tire lateral force estimation method considering sensor data loss

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant