CN114056338A - Multi-sensor fusion vehicle state parameter estimation method - Google Patents
Multi-sensor fusion vehicle state parameter estimation method Download PDFInfo
- Publication number
- CN114056338A CN114056338A CN202111574208.0A CN202111574208A CN114056338A CN 114056338 A CN114056338 A CN 114056338A CN 202111574208 A CN202111574208 A CN 202111574208A CN 114056338 A CN114056338 A CN 114056338A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- speed
- moment
- axis
- formula
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 33
- 230000004927 fusion Effects 0.000 title claims abstract description 17
- 239000011159 matrix material Substances 0.000 claims description 52
- 230000001133 acceleration Effects 0.000 claims description 15
- 150000001875 compounds Chemical class 0.000 claims description 8
- 230000007704 transition Effects 0.000 claims description 7
- 238000000605 extraction Methods 0.000 claims description 6
- 238000004891 communication Methods 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 238000004458 analytical method Methods 0.000 claims description 2
- 238000001914 filtration Methods 0.000 claims description 2
- 230000003287 optical effect Effects 0.000 claims description 2
- 238000012546 transfer Methods 0.000 claims description 2
- 238000004364 calculation method Methods 0.000 abstract description 4
- 238000005516 engineering process Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 2
- 230000006855 networking Effects 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
- B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
- B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
- B60W40/105—Speed
Abstract
The invention discloses a multi-sensor fusion vehicle state parameter estimation method, which comprises the following steps: the simplified vehicle is a rigid body model moving on the horizontal plane, and the simplified geodetic coordinate system is a plane coordinate system of xoy; a vehicle-mounted industrial personal computer collects a sensor signal; designing a vehicle pose and speed estimation Kalman filter; estimating the pose and speed information of the vehicle by using the designed Kalman filter; solving t by the output vehicle position and speed signalskThe longitudinal running speed, the lateral sliding speed and the yaw angular speed of the vehicle in the vehicle body coordinate system at the moment; designing a vehicle lateral slip speed prediction Kalman filter; and (4) estimating the lateral slip speed of the vehicle by using the designed lateral slip speed estimation Kalman filter of the vehicle. The method has the advantages of more information of the fusion sensor, high parameter estimation precision, low calculation cost and the like.
Description
Technical Field
The invention relates to the technical field of automobile data acquisition, in particular to a multi-sensor fusion vehicle state parameter estimation method.
Background
With the great progress of technologies such as intelligent networking, artificial intelligence and the like, the automobile industry begins to develop along the trend of intellectualization and electromotion, and the automatic driving, wire-controlled chassis, intelligent networking and advanced driving assistance technologies become the current hot technology of automobiles. In order to achieve the expected target functions, the above technologies need to acquire high-precision vehicle state parameters as a basis, but the methods in the prior art are generally complex in calculation and high in cost.
Disclosure of Invention
The invention aims to solve the technical problem of how to provide a multi-sensor fusion vehicle state parameter estimation method which integrates multiple sensor information, has high parameter estimation precision and low calculation cost.
In order to solve the technical problems, the technical scheme adopted by the invention is as follows: a multi-sensor fusion vehicle state parameter estimation method is characterized by comprising the following steps:
the simplified vehicle is a rigid body model moving on the horizontal plane, and the simplified geodetic coordinate system is a plane coordinate system of xoy;
a vehicle-mounted industrial personal computer collects a sensor signal;
designing a vehicle pose and speed estimation Kalman filter;
estimating the pose and speed information of the vehicle by using the designed Kalman filter;
solving t by the output vehicle position and speed signalskThe longitudinal running speed, the lateral sliding speed and the yaw angular speed of the vehicle in the vehicle body coordinate system at the moment;
designing a vehicle lateral slip speed prediction Kalman filter;
and (4) estimating the lateral slip speed of the vehicle by using the designed lateral slip speed estimation Kalman filter of the vehicle.
Adopt the produced beneficial effect of above-mentioned technical scheme to lie in: compared with a vehicle state parameter estimation method adopting the fusion of a GPS (global positioning system), an IMU (inertial measurement unit) and a wheel speed sensor, the method adopts the mode of more sensor fusion and based on a dynamic model, realizes the redundancy design during the data acquisition of the sensor, and reduces the influence of the output error data of a single sensor caused by faults on the estimation precision of the vehicle state parameter.
Compared with the vehicle state parameter estimation method based on the multi-degree-of-freedom nonlinear vehicle dynamics model, the method adopts the two-degree-of-freedom linear vehicle dynamics model to estimate the lateral sliding speed of the vehicle, has low calculation cost, is convenient to deploy to a real vehicle, and can ensure high-precision vehicle state parameter estimation by fusing data of various sensors.
Drawings
The present invention will be described in further detail with reference to the accompanying drawings and specific embodiments.
FIG. 1 is a diagram of a rigid body model of a vehicle in xoy coordinate system in an embodiment of the present invention;
FIG. 2 is a diagram of a two-degree-of-freedom dynamic model of a vehicle according to an embodiment of the present invention;
FIG. 3 is a flow chart of a method according to an embodiment of the present invention;
Detailed Description
The technical solutions in the embodiments of the present invention are clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present invention, but the present invention may be practiced in other ways than those specifically described and will be readily apparent to those of ordinary skill in the art without departing from the spirit of the present invention, and therefore the present invention is not limited to the specific embodiments disclosed below.
As shown in fig. 3, the embodiment of the invention discloses a method for estimating vehicle state parameters by fusing multiple sensors, which comprises the following steps:
step 1: the simplified vehicle is a rigid body model moving in the horizontal plane, and the simplified geodetic coordinate system is a plane coordinate system of xoy.
Step 2: and the vehicle-mounted industrial personal computer collects a sensor signal.
The vehicle-mounted industrial personal computer acquires environment point cloud data through the laser radar, runs a feature extraction and pose matching algorithm, and obtains an x-axis coordinate x of the vehicle mass center measured by the laser radar under the xoy coordinate systemlidarY coordinate of ylidarAnd the angle between the longitudinal running direction of the vehicle and the x axis of the geodetic coordinate system, i.e. the course angle
The vehicle-mounted industrial personal computer is communicated with a vehicle-mounted RTK (Real-time kinematic) Real-time differential positioning sensor to obtain an x-axis coordinate x of a vehicle mass center determined by the RTK sensor under an xoy coordinate systemsensorY coordinate of ysensorAnd course angle
The vehicle-mounted industrial personal computer acquires image data through the camera, runs the Lucas-Kanade optical flow algorithm, and obtains the speed of the mass center of the vehicle in the x-axis direction under the xoy coordinate system
Speed in y-axis coordinate direction
The vehicle-mounted industrial personal computer obtains the yaw velocity of the vehicle under the xoy coordinate system through communication with the IMU sensor
And the lateral acceleration of the vehicle under the coordinate system of the vehicle body
And the vehicle-mounted industrial personal computer obtains the front wheel steering angle delta of the vehicle through communication with the steering wheel steering angle sensor.
And step 3: and designing a vehicle pose and speed estimation Kalman filter.
1) And constructing a system state prediction equation according to the kinematic relationship of the simplified vehicle model in the xoy coordinate system.
In the xoy coordinate system, the simplified vehicle model has three degrees of freedom, namely linear motion along the x-axis direction, linear motion along the y-axis direction and rotational motion around an axis with the center of mass perpendicular to the xy plane. Taking the vehicle moving along the x-axis direction as an example, let x (k +1), x (k)) Are each tk+1，tkThe x-axis coordinate value of the vehicle in the coordinate system xoy at the moment, and the sampling period of the system is T, then:
in the formula (I), the compound is shown in the specification,
the speed of the vehicle along the x-axis under the xoy coordinate system is represented by the following kinematic relationship:
in the formula (I), the compound is shown in the specification,
is composed of tk-1Time to tkAnd (3) replacing the formula (2) with the formula (1) according to the speed variation in the x-axis direction, and finishing to obtain:
setting the weighted value of formula (1) as q, q ∈ (0,1), and the weighted value of formula (2) as (1-q), as given by formula (1) × q + formula (2) × (1-q):
is provided with
Are each tk+1，tkThe speed of the vehicle along the x-axis direction in the coordinate system xoy at the moment is as follows:
in the formula (I), the compound is shown in the specification,
the acceleration of the vehicle along the x axis under the xoy coordinate system is represented by the following kinematic relationship:
in the formula (I), the compound is shown in the specification,
is composed of tk-1Time to tkAnd (3) substituting the formula (6) for the formula (5) according to the acceleration variation of the vehicle along the x-axis direction to obtain:
setting y (k +1), y (k) as tk+1，tkThe y-axis coordinate value of the vehicle in the xoy coordinate system at the moment,
are each tk+1，tkThe speed of the vehicle along the y axis under the xoy coordinate system at the moment satisfies the following conditions in the same way:
in the formula
Are respectively represented by tk-1Time to tkThe speed variation and the acceleration variation of the vehicle in the y-axis direction.
Setting up
Are each tk+1，tkAn included angle between the longitudinal driving direction of the vehicle and the x axis in the xoy coordinate system at any moment, namely a course angle value,
are each tk+1，tkThe yaw velocity of the vehicle at the moment satisfies the following conditions in the same way:
in the formula
Are respectively represented by tk-1Time to tkA vehicle yaw-rate change amount and a yaw-acceleration change amount.
Taking the state vector as X1(k),
Is provided with
Then
Setting a system state transition equation as follows:
X1(k+1)＝Φ1(k)X1(k)+W1(k) (12)
in the formula phi1(k) In order to be a state prediction matrix,
in the formula W1(k) In order to predict the noise, the noise is predicted,
2) and (3) combining the sensor signals acquired in the step (2) to construct a system state observation equation.
Let xlidar(k)，ylidar(k)，
Are each tkThe x-axis coordinate, the y-axis coordinate and the course angle measured by the laser radar at the moment are as follows:
in the formula, vxlidar(k)，vylidar(k)，
Are each tkThe x-axis coordinate, the y-axis coordinate and the noise of the course angle signal measured by the laser radar at the moment.
Taking an observation vector as Y1(k)，
Let the observation equation be:
Y1(k)＝H1(k)X1(k)+v1(k) (14)
in the formula, H1In order to observe the matrix, the system,
v1(k) in order to observe the noise matrix,
3) setting a system prediction noise variance matrix to be a non-negative positive definite matrix Q1Setting the system observation noise variance matrix as positive definite matrix R1。
And 4, step 4: and (4) estimating the pose and speed information of the vehicle by using the Kalman filter designed in the step (3).
1) Given a pre-estimated initial value of the system state
Sum error variance matrix initial value P1(0)。
2) From tk-1Time error variance matrix estimated value P1(k-1), solving for tkError variance matrix prediction value P of time1(k,k-1)。
3) Solving for tkTemporal filter gain matrix K1(k)。
4) Solving for tkError variance matrix estimated value P of time1(k)。
P1(k)＝[I-K1(k)H1(k)]P1(k,k-1) (17)
5) From tk-1Prediction value of system state prediction value at moment
Solving for tkTemporal system state prediction
6) Solving for tkPrediction value of system state prediction value at moment
7) Extraction of tkSystem state of moment estimation
Element of (1), output tkVehicle pose estimated at moment
And velocity
And 5: vehicle pose output by step 4
And velocity
Signal, solving for tkLongitudinal running speed V of vehicle under time vehicle body coordinate systemx1(k) Lateral slip velocity Vy1(k) And yaw angular velocity ω1(k)。
Step 6: and designing a vehicle lateral slip speed prediction Kalman filter.
1) The simplified vehicle is a two-degree-of-freedom dynamic model, and a state prediction equation is constructed.
Assuming that the steering angles of the left front wheel and the right front wheel of the vehicle are the same, and the steering angle, the tire slip angle and the centroid slip angle of the vehicle are all small, the pitching, the yawing motion and the load transfer of the vehicle are ignored, and the vehicle is simplified into a classical vehicle two-degree-of-freedom dynamic model. A coordinate system is established by taking the mass center o of the automobile as the origin of coordinates, the longitudinal movement direction of the automobile as an x axis, the lateral movement direction of the automobile as a y axis and the direction vertical to the ground as a z axis.
For the vehicle stress analysis, the following can be obtained:
where Sigma FyFor the resultant force in the y-axis direction, ∑ MzFor resultant moment about the z-axis, Fyf、FyrThe resultant forces of the front and rear tires, respectively, /)f、lrThe distances between the front and rear axles and the center of mass of the vehicle, and delta is the corner of the front wheel.
Let the acceleration at the centroid of the vehicle in the x and y directions be ax、ayThe following can be obtained:
in the formula, VxFor the longitudinal speed of the vehicle,
is the rate of change of vehicle longitudinal speed, VyIn order to obtain the transverse slip speed of the vehicle,
the lateral slip speed change rate of the vehicle, and omega is the yaw angular speed.
Since the front wheel steering angle δ is generally relatively small, cos δ is 1, which can be obtained from newton's second law:
in the formula IzIs the moment of inertia of the vehicle about the z-axis,
is the angular acceleration of the vehicle rotation about the z-axis.
Tire side force FyThe relationship to the tire slip angle α is:
Fy＝-Cα (26)
front wheel side slip angle alphafCan be expressed as:
rear wheel side slip angle alpharCan be expressed as:
the equation of the vehicle two-degree-of-freedom dynamic model is as follows:
the above equation is organized into a standard state space equation:
let Vx(k) Is tkTime of day longitudinal running speed, V, of vehicley(k+1)，Vy(k) Are each tk+1，tkThe lateral slip speed of the vehicle at the moment, omega (k +1), omega (k) are respectively tk+1，tkThe yaw rate of the vehicle at the moment and the sampling period of the system is T, then:
taking the state vector as X2(k) Is provided with
Then
Setting a system state transition equation as follows:
X2(k+1)＝Φ2(k)X2(k)+Γ2(k)δ(k)+W2(k) (32)
in the formula phi2(k) In order to be a state prediction matrix,
delta (k) is tkThe time of day, the front wheel steering angle, is the input to the system, W2(k) A noise matrix is predicted for the states.
2) And (4) combining the sensor signals acquired in the step (2) and the vehicle motion parameters obtained by solving in the step (5) to construct a state transition equation.
By the formula
And
simultaneous solution can yield:
is provided with
Is tkThe lateral acceleration of the vehicle measured by the IMU sensor at that moment is observedThe vector is Y2(k)，
Let the observation equation be:
Y2(k)＝H2(k)X2(k)+D(k)δ(k)+v2(k) (34)
in the formula
v2(k) To observe the noise matrix.
3) Setting a system prediction noise variance matrix to be a non-negative positive definite matrix Q2Setting the system observation noise variance matrix as positive definite matrix R2。
And 7: and (6) estimating the lateral slip speed of the vehicle by using the Kalman filter designed in the step 6.
1) Given a pre-estimated initial value of the system state
Sum error variance matrix initial value P2(0)。
2) Acquiring t calculated in step 5kLongitudinal running speed V of vehicle at any momentx1(k) Let Vx(k)＝Vx1(k) Updating phi2(k)，H2(k) Obtaining and updating tkThe time is the front wheel steering angle δ (k).
3) From tk-1Time error variance matrix estimated value P2(k-1), solving for tkError variance matrix prediction value P of time2(k,k-1)。
4) Solving for tkTemporal filter gain matrix K2(k)。
5) Solving for tkError variance matrix estimated value P of time2(k)。
P2(k)＝[I-K2(k)H2(k)]P2(k,k-1) (37)
6) From tk-1Prediction value of system state prediction value at moment
Solving for tkTemporal system state prediction
7) Solving for tkPrediction value of system state prediction value at moment
8) Extraction of tkSystem state of moment estimation
Element of (1), output tkVehicle lateral slip speed estimated at moment
And the vehicle yaw velocity obtained after the secondary filtering
Claims (10)
1. A multi-sensor fusion vehicle state parameter estimation method is characterized by comprising the following steps:
the simplified vehicle is a rigid body model moving on the horizontal plane, and the simplified geodetic coordinate system is a plane coordinate system of xoy;
a vehicle-mounted industrial personal computer collects a sensor signal;
designing a vehicle pose and speed estimation Kalman filter;
estimating the pose and speed information of the vehicle by using the designed Kalman filter;
solving t by the output vehicle position and speed signalskThe longitudinal running speed, the lateral sliding speed and the yaw angular speed of the vehicle in the vehicle body coordinate system at the moment;
designing a vehicle lateral slip speed prediction Kalman filter;
and (4) estimating the lateral slip speed of the vehicle by using the designed lateral slip speed estimation Kalman filter of the vehicle.
2. The method for estimating the state parameters of the multi-sensor fusion vehicle as claimed in claim 1, wherein the method for acquiring the sensor signals by the vehicle-mounted industrial personal computer in the step is as follows:
the vehicle-mounted industrial personal computer acquires environment point cloud data through the laser radar, runs a feature extraction and pose matching algorithm, and obtains an x-axis coordinate x of the vehicle mass center measured by the laser radar under the xoy coordinate systemlidarY coordinate of ylidarAnd the angle between the longitudinal running direction of the vehicle and the x axis of the geodetic coordinate system, i.e. the course angle
The vehicle-mounted industrial personal computer is communicated with the vehicle-mounted RTK real-time differential positioning sensor to obtain an x-axis coordinate x of the vehicle mass center determined by the RTK sensor under the xoy coordinate systemsensorY coordinate of ysensorAnd course angle
The vehicle-mounted industrial personal computer collects image data through the camera and runs Lucas-Kanade optical flow calculationObtaining the speed of the mass center of the vehicle in the x-axis direction under the xoy coordinate system
Speed in y-axis coordinate direction
The vehicle-mounted industrial personal computer obtains the yaw velocity of the vehicle under the xoy coordinate system through communication with the IMU sensor
And the lateral acceleration of the vehicle under the coordinate system of the vehicle body
And the vehicle-mounted industrial personal computer obtains the front wheel steering angle delta of the vehicle through communication with the steering wheel steering angle sensor.
3. The multi-sensor fusion vehicle state parameter estimation method of claim 1, wherein the method for designing the vehicle pose and speed estimation kalman filter is as follows:
constructing a system state prediction equation according to the kinematic relationship of the simplified vehicle model in the xoy coordinate system;
combining the acquired sensor signals to construct a system state observation equation;
setting a system prediction noise variance matrix to be a non-negative positive definite matrix Q1Setting the system observation noise variance matrix as positive definite matrix R1。
4. The multi-sensor fusion vehicle state parameter estimation method of claim 3, wherein the method for constructing the system state prediction equation from the kinematic relationship of the simplified vehicle model in the xoy coordinate system comprises the following steps:
in the xoy coordinate system, the simplified vehicle model has three degrees of freedom, namely along the x-axisThe linear motion is carried out towards the direction of a linear motion axis, the linear motion is carried out along the direction of a y axis, and the rotation motion is carried out around an axis of a mass center and is vertical to an xy plane; taking the vehicle moving along the x-axis as an example, let x (k +1), x (k) be tk+1，tkThe x-axis coordinate value of the vehicle in the coordinate system xoy at the moment, and the sampling period of the system is T, then:
in the formula (I), the compound is shown in the specification,
the speed of the vehicle along the x-axis under the xoy coordinate system is represented by the following kinematic relationship:
in the formula (I), the compound is shown in the specification,
is composed of tk-1Time to tkAnd (3) replacing the formula (2) with the formula (1) according to the speed variation in the x-axis direction, and finishing to obtain:
setting the weighted value of formula (1) as q, q ∈ (0,1), and the weighted value of formula (2) as (1-q), as given by formula (1) × q + formula (2) × (1-q):
is provided with
Are each tk+1，tkTime of day in coordinate system xoy along x-axisThe speed of the direction is as follows:
in the formula (I), the compound is shown in the specification,
the acceleration of the vehicle along the x axis under the xoy coordinate system is represented by the following kinematic relationship:
in the formula (I), the compound is shown in the specification,
is composed of tk-1Time to tkAnd (3) substituting the formula (6) for the formula (5) according to the acceleration variation of the vehicle along the x-axis direction to obtain:
setting y (k +1), y (k) as tk+1，tkThe y-axis coordinate value of the vehicle in the xoy coordinate system at the moment,
are each tk+1，tkThe speed of the vehicle along the y axis under the xoy coordinate system at the moment satisfies the following conditions in the same way:
in the formula
Are respectively represented by tk-1Time to tkThe speed variation and the acceleration variation of the vehicle along the y-axis direction;
setting up
Are each tk+1，tkAn included angle between the longitudinal driving direction of the vehicle and the x axis in the xoy coordinate system at any moment, namely a course angle value,
are each tk+1，tkThe yaw velocity of the vehicle at the moment satisfies the following conditions in the same way:
in the formula
Are respectively represented by tk-1Time to tkThe vehicle yaw rate variation amount and the yaw acceleration variation amount;
taking the state vector as X1(k),
Is provided with
Then
Setting a system state transition equation as follows:
X1(k+1)＝Φ1(k)X1(k)+W1(k) (12)
in the formula phi1(k) In order to be a state prediction matrix,
in the formula W1(k) In order to predict the noise, the noise is predicted,
5. the multi-sensor fusion vehicle state parameter estimation method of claim 4, wherein the collected sensor signals are combined to construct a system state observation equation by the following method:
let xlidar(k)，ylidar(k)，
Are each tkThe x-axis coordinate, the y-axis coordinate and the course angle measured by the laser radar at the moment are as follows:
in the formula, vxlidar(k)，vylidar(k)，
Are each tkThe x-axis coordinate, the y-axis coordinate and the noise of the course angle signal which are measured by the laser radar at any moment;
taking an observation vector as Y1(k)，
Let the observation equation be:
Y1(k)＝H1(k)X1(k)+v1(k) (14)
in the formula, H1In order to observe the matrix, the system,
v1(k) in order to observe the noise matrix,
6. the multi-sensor fused vehicle state parameter estimation method of claim 3, wherein the method for estimating vehicle pose and speed information is as follows:
1) given a pre-estimated initial value of the system state
Sum error variance matrix initial value P1(0)；
2) From tk-1Time error variance matrix estimated value P1(k-1), solving for tkError variance matrix prediction value P of time1(k,k-1)；
3) Solving for tkTemporal filter gain matrix K1(k)；
4) Solving for tkError variance matrix estimated value P of time1(k)；
P1(k)＝[I-K1(k)H1(k)]P1(k,k-1) (17)
5) From tk-1Prediction value of system state prediction value at moment
Solving for tkTemporal system state prediction
6) Solving for tkPrediction value of system state prediction value at moment
7) Extraction of tkSystem state of moment estimation
Element of (1), output tkVehicle pose estimated at moment
And velocity
7. The multi-sensor fusion vehicle state parameter estimation method of claim 6, wherein t is solved from the output vehicle pose and speed signalskThe method for the longitudinal running speed, the lateral sliding speed and the yaw rate of the vehicle in the vehicle body coordinate system at the moment comprises the following steps:
vehicle pose by output
And velocity
Signal, solving for tkLongitudinal running speed V of vehicle under time vehicle body coordinate systemx1(k) Lateral slip velocity Vy1(k) And yaw angular velocity ω1(k)：
8. The method for estimating the state parameters of the multi-sensor fusion vehicle as claimed in claim 6, wherein the method for designing the vehicle lateral slip speed estimation Kalman filter is as follows:
simplifying a vehicle into a two-degree-of-freedom dynamic model, and constructing a state prediction equation;
combining the acquired sensor signals and the vehicle motion parameters obtained by solving to construct a state transition equation;
setting a system prediction noise variance matrix to be a non-negative positive definite matrix Q2Setting the system observation noise variance matrix as positive definite matrix R2。
9. The method for estimating the state parameters of the multi-sensor fusion vehicle as claimed in claim 6, wherein the method for designing the vehicle lateral slip speed estimation Kalman filter is as follows:
1) simplifying the vehicle into a two-degree-of-freedom dynamic model, and constructing a state prediction equation:
assuming that the steering angles of the left front wheel and the right front wheel of the vehicle are the same, and the steering angle, the tire slip angle and the mass center slip angle of the vehicle are all very small, the pitching, the yawing motion and the load transfer of the vehicle are ignored, and the vehicle is simplified into a classical vehicle two-degree-of-freedom dynamic model; establishing a coordinate system by taking the mass center o of the automobile as an origin of coordinates, the longitudinal movement direction of the automobile as an x axis, the lateral movement direction of the automobile as a y axis and the direction vertical to the ground as a z axis;
for the vehicle stress analysis, the following can be obtained:
where Sigma FyFor the resultant force in the y-axis direction, ∑ MzFor resultant moment about the z-axis, Fyf、FyrThe resultant forces of the front and rear tires, respectively, /)f、lrThe distances between the front and rear axles and the center of mass of the vehicle are respectively, and delta is the corner of the front wheel;
let the acceleration at the centroid of the vehicle in the x and y directions be ax、ayThe following can be obtained:
in the formula, VxFor the longitudinal speed of the vehicle,
is the rate of change of vehicle longitudinal speed, VyIn order to obtain the transverse slip speed of the vehicle,
the change rate of the transverse slip speed of the vehicle is shown, and omega is the yaw angular speed;
since the front wheel steering angle δ is generally relatively small, cos δ is 1, which can be obtained from newton's second law:
in the formula IzIs the moment of inertia of the vehicle about the z-axis,
angular acceleration of the vehicle about the z-axis;
tire side force FyThe relationship to the tire slip angle α is:
Fy＝-Cα (26)
front wheel side slip angle alphafCan be expressed as:
rear wheel side slip angle alpharCan be expressed as:
the equation of the vehicle two-degree-of-freedom dynamic model is as follows:
the above equation is organized into a standard state space equation:
let Vx(k) Is tkTime of day longitudinal running speed, V, of vehicley(k+1)，Vy(k) Are each tk+1，tkThe lateral slip speed of the vehicle at the moment, omega (k +1), omega (k) are respectively tk+1，tkThe yaw rate of the vehicle at the moment and the sampling period of the system is T, then:
taking the state vector as X2(k) Is provided with
Then
Setting a system state transition equation as follows:
X2(k+1)＝Φ2(k)X2(k)+Γ2(k)δ(k)+W2(k) (32)
in the formula phi2(k) In order to be a state prediction matrix,
delta (k) is tkThe time of day, the front wheel steering angle, is the input to the system, W2(k) Predicting a noise matrix for the state;
2) combining the collected sensor signals and the vehicle motion parameters obtained by solving, and constructing a state transition equation:
by the formula
And
simultaneous solution can yield:
is provided with
Is tkThe lateral acceleration of the vehicle is measured by an IMU sensor at the moment, and an observation vector is taken as Y2(k)，
Let the observation equation be:
Y2(k)＝H2(k)X2(k)+D(k)δ(k)+v2(k) (34)
in the formula
v2(k) To observe the noise matrix;
3) setting a system prediction noise variance matrix to be a non-negative positive definite matrix Q2Setting the system observation noise variance matrix as positive definite matrix R2。
10. The multi-sensor fused vehicle state parameter estimation method of claim 9, wherein the method for predicting the lateral slip speed of the vehicle is as follows:
1) given a pre-estimated initial value of the system state
Sum error variance matrix initial value P2(0)；
2) Obtaining the calculated tkLongitudinal running speed V of vehicle at any momentx1(k) Let Vx(k)＝Vx1(k) Updating phi2(k)，H2(k) Obtaining and updating tkA time front wheel steering angle δ (k);
3) from tk-1Time error variance matrix estimated value P2(k-1), solving for tkError variance matrix prediction value P of time2(k,k-1)：
4) Solving for tkTemporal filter gain matrix K2(k)：
5) Solving for tkError variance matrix estimated value P of time2(k)：
P2(k)＝[I-K2(k)H2(k)]P2(k,k-1) (37)
6) From tk-1Prediction value of system state prediction value at moment
Solving for tkTemporal system state prediction
7) Solving for tkPrediction value of system state prediction value at moment
8) Extraction of tkSystem state of moment estimation
Element of (1), output tkVehicle lateral slip speed estimated at moment
And the vehicle yaw velocity obtained after the secondary filtering
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111574208.0A CN114056338A (en) | 2021-12-21 | 2021-12-21 | Multi-sensor fusion vehicle state parameter estimation method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111574208.0A CN114056338A (en) | 2021-12-21 | 2021-12-21 | Multi-sensor fusion vehicle state parameter estimation method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114056338A true CN114056338A (en) | 2022-02-18 |
Family
ID=80230103
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111574208.0A Pending CN114056338A (en) | 2021-12-21 | 2021-12-21 | Multi-sensor fusion vehicle state parameter estimation method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114056338A (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20070156316A1 (en) * | 2005-12-06 | 2007-07-05 | Kabushiki Kaisha Toyota Chuo Kenkyusho | Vehicle physical quantity estimation apparatus and storage medium having stored thereon computer program relating to the apparatus |
| WO2010113799A1 (en) * | 2009-03-30 | 2010-10-07 | 本田技研工業株式会社 | Device for estimating state quantity of skid motion of vehicle |
| CN104517039A (en) * | 2014-12-30 | 2015-04-15 | 吉林大学 | Tire side-tipping side-inclining steady-state aligning torque characteristic radius semi-empirical modeling method |
| CN105584485A (en) * | 2014-11-12 | 2016-05-18 | 通用汽车环球科技运作有限责任公司 | Use of participative sensing systems to enable enhanced road friction estimation |
| CN106081158A (en) * | 2016-06-21 | 2016-11-09 | 西安航空制动科技有限公司 | A kind of evaluation method of wheel slipspeed |
| RU2018127260A3 (en) * | 2018-07-25 | 2020-01-27 |
-
2021
- 2021-12-21 CN CN202111574208.0A patent/CN114056338A/en active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20070156316A1 (en) * | 2005-12-06 | 2007-07-05 | Kabushiki Kaisha Toyota Chuo Kenkyusho | Vehicle physical quantity estimation apparatus and storage medium having stored thereon computer program relating to the apparatus |
| WO2010113799A1 (en) * | 2009-03-30 | 2010-10-07 | 本田技研工業株式会社 | Device for estimating state quantity of skid motion of vehicle |
| CN105584485A (en) * | 2014-11-12 | 2016-05-18 | 通用汽车环球科技运作有限责任公司 | Use of participative sensing systems to enable enhanced road friction estimation |
| CN104517039A (en) * | 2014-12-30 | 2015-04-15 | 吉林大学 | Tire side-tipping side-inclining steady-state aligning torque characteristic radius semi-empirical modeling method |
| CN106081158A (en) * | 2016-06-21 | 2016-11-09 | 西安航空制动科技有限公司 | A kind of evaluation method of wheel slipspeed |
| RU2018127260A3 (en) * | 2018-07-25 | 2020-01-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112083726B (en) | Park-oriented automatic driving double-filter fusion positioning system | |
| CN107246876B (en) | Method and system for autonomous positioning and map construction of unmanned automobile | |
| EP3940421A1 (en) | Positioning method and device based on multi-sensor fusion | |
| KR102508843B1 (en) | Method and device for the estimation of car egomotion from surround view images | |
| Ding et al. | Event-triggered vehicle sideslip angle estimation based on low-cost sensors | |
| CN110986939B (en) | Visual inertia odometer method based on IMU (inertial measurement Unit) pre-integration | |
| CN109946732A (en) | A kind of unmanned vehicle localization method based on Fusion | |
| CN113819914A (en) | Map construction method and device | |
| JP7036080B2 (en) | Inertial navigation system | |
| Kang et al. | Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator | |
| CN112683281B (en) | Automatic driving vehicle joint positioning method based on vehicle kinematics | |
| CN110018691A (en) | Small-sized multi-rotor unmanned aerial vehicle state of flight estimating system and method | |
| CN113819905A (en) | Multi-sensor fusion-based odometer method and device | |
| Yu et al. | GPS/INS/Odometer/DR integrated navigation system aided with vehicular dynamic characteristics for autonomous vehicle application | |
| CN112433531A (en) | Trajectory tracking method and device for automatic driving vehicle and computer equipment | |
| CN115577320A (en) | Multi-sensor asynchronous data fusion method based on data interpolation | |
| Xia et al. | Automated vehicle attitude and lateral velocity estimation using a 6-D IMU aided by vehicle dynamics | |
| CN113008229B (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
| Peng et al. | Vehicle odometry with camera-lidar-IMU information fusion and factor-graph optimization | |
| Parra-Tsunekawa et al. | A kalman-filtering-based approach for improving terrain mapping in off-road autonomous vehicles | |
| CN114475581B (en) | Automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion | |
| Emran et al. | A cascaded approach for quadrotor's attitude estimation | |
| CN114440881B (en) | Unmanned vehicle positioning method integrating multi-source sensor information | |
| CN114056338A (en) | Multi-sensor fusion vehicle state parameter estimation method | |
| Zhang et al. | Learning end-to-end inertial-wheel odometry for vehicle ego-motion estimation |
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 |