CN114056338B - Multi-sensor fusion vehicle state parameter prediction method - Google Patents
Multi-sensor fusion vehicle state parameter prediction method Download PDFInfo
- Publication number
- CN114056338B CN114056338B CN202111574208.0A CN202111574208A CN114056338B CN 114056338 B CN114056338 B CN 114056338B CN 202111574208 A CN202111574208 A CN 202111574208A CN 114056338 B CN114056338 B CN 114056338B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- time
- speed
- axis
- coordinate system
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 230000004927 fusion Effects 0.000 title claims abstract description 13
- 239000011159 matrix material Substances 0.000 claims description 52
- 230000001133 acceleration Effects 0.000 claims description 16
- 230000008859 change Effects 0.000 claims description 10
- 230000008569 process Effects 0.000 claims description 8
- 230000007704 transition Effects 0.000 claims description 7
- 238000005070 sampling Methods 0.000 claims description 4
- 230000007613 environmental effect Effects 0.000 claims description 2
- 238000000605 extraction Methods 0.000 claims description 2
- 238000001914 filtration Methods 0.000 claims description 2
- 230000003287 optical effect Effects 0.000 claims description 2
- 108050006002 RNA polymerase sigma factor FliA Proteins 0.000 claims 1
- 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
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
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
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Mathematical Physics (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a vehicle state parameter prediction method with multi-sensor fusion, which comprises the following steps: simplifying the vehicle into a rigid body model moving in a horizontal plane, and simplifying a plane coordinate system with a geodetic coordinate system of xoy; the vehicle-mounted industrial personal computer collects sensor signals; designing a vehicle pose and speed pre-estimated Kalman filter; estimating pose and speed information of the vehicle by using a designed Kalman filter; solving the longitudinal running speed, the lateral sliding speed and the yaw rate of the vehicle under a vehicle body coordinate system at the moment t k according to the output vehicle pose and speed signals; designing a Kalman filter for estimating the lateral slip speed of the vehicle; and estimating the lateral slip speed of the vehicle by using a designed vehicle lateral slip speed estimation Kalman filter. The method has the advantages of being more in information of the fusion sensor, high in parameter estimation precision, low in calculation cost and the like.
Description
Technical Field
The invention relates to the technical field of automobile data acquisition, in particular to a vehicle state parameter prediction method based on multi-sensor fusion.
Background
With the great progress of intelligent networking, artificial intelligence and other technologies, the automobile industry starts to develop continuously along the intelligent and electric trend, and automatic driving, wire control chassis, intelligent networking and advanced driving assistance technologies become the current automobile hot technology. In order to achieve the expected target function, the above technology needs to acquire high-precision vehicle state parameters as a basis, but the method in the prior art is generally complex in calculation and high in cost.
Disclosure of Invention
The invention aims to solve the technical problem of providing a multi-sensor fusion vehicle state parameter prediction method which has the advantages of more fusion sensor information, high parameter prediction precision and low calculation cost.
In order to solve the technical problems, the invention adopts the following technical scheme: a vehicle state parameter estimation method based on multi-sensor fusion is characterized by comprising the following steps:
Simplifying the vehicle into a rigid body model moving in a horizontal plane, and simplifying a plane coordinate system with a geodetic coordinate system of xoy;
The vehicle-mounted industrial personal computer collects sensor signals;
Designing a vehicle pose and speed pre-estimated Kalman filter;
Estimating pose and speed information of the vehicle by using a designed Kalman filter;
Solving the longitudinal running speed, the lateral sliding speed and the yaw rate of the vehicle under a vehicle body coordinate system at the moment t k according to the output vehicle pose and speed signals;
Designing a Kalman filter for estimating the lateral slip speed of the vehicle;
and estimating the lateral slip speed of the vehicle by using a designed vehicle lateral slip speed estimation Kalman filter.
The beneficial effects of adopting above-mentioned technical scheme to produce lie in: compared with a vehicle state parameter estimation method adopting GPS, IMU and wheel speed sensor fusion, the method adopts more sensor fusion and a dynamic model-based mode, thereby realizing redundant design during sensor data acquisition and reducing the influence of output error data of a single sensor on the vehicle state parameter estimation precision due to failure.
Compared with a vehicle state parameter estimation method based on a multi-degree-of-freedom nonlinear vehicle dynamics model, the vehicle state parameter estimation method based on the multi-degree-of-freedom nonlinear vehicle dynamics model is low in calculation cost, convenient to deploy to a real vehicle, and capable of guaranteeing high-precision vehicle state parameter estimation by fusing multiple types of sensor data.
Drawings
The invention will be described in further detail with reference to the drawings and the detailed description.
FIG. 1 is a diagram of a rigid body model of a vehicle in an xoy coordinate system in an embodiment of the invention;
FIG. 2 is a diagram of a two-degree-of-freedom dynamics model of a vehicle in an embodiment of the invention;
FIG. 3 is a flow chart of a method according to an embodiment of the invention;
Detailed Description
The following description of the embodiments of the present invention will be made clearly and fully with reference to the accompanying drawings, in which it is evident that the embodiments described are only some, but not all embodiments of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the 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 other than those described herein, and persons skilled in the art will readily appreciate that the present invention is not limited to the specific embodiments disclosed below.
As shown in fig. 3, the embodiment of the invention discloses a vehicle state parameter estimation method for multi-sensor fusion, 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 the planar coordinate system of xoy.
Step 2: the vehicle-mounted industrial personal computer collects sensor signals.
The vehicle-mounted industrial personal computer acquires environmental point cloud data through a laser radar, and operates a feature extraction and pose matching algorithm to acquire and obtain an x-axis coordinate x lidar, a y-axis coordinate y lidar and an included angle between the longitudinal running direction of the vehicle and the x-axis of a ground coordinate system, namely a course angle, of the centroid of the vehicle measured by the laser radar under an xoy coordinate system
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 sensor, a y-axis coordinate y sensor and a course angle of a vehicle centroid measured by the RTK sensor under an xoy coordinate system
The vehicle-mounted industrial personal computer acquires image data through a camera, runs a Lucas-Kanade optical flow algorithm, and acquires and obtains the x-axis direction speed of the vehicle centroid under an xoy coordinate systemY-axis coordinate Direction speed/>
The vehicle-mounted industrial personal computer is communicated with the IMU sensor to obtain the yaw rate of the vehicle under the xoy coordinate systemAnd vehicle lateral acceleration in the body coordinate system/>
The vehicle-mounted industrial personal computer is communicated with a steering wheel angle sensor to obtain the front wheel steering angle delta of the vehicle.
Step 3: and designing a vehicle pose and speed pre-estimated Kalman filter.
1) And constructing a system state prediction equation by the kinematic relation of the simplified vehicle model in the xoy coordinate system.
In the xoy coordinate system, the simplified vehicle model has three degrees of freedom, i.e., linear motion in the x-axis direction, linear motion in the y-axis direction, and rotational motion around an axis with the centroid perpendicular to the xy-plane. Taking the movement of the vehicle along the x-axis direction as an example, assuming that x (k+1), x (k) are x-axis coordinate values of the vehicle in a coordinate system xoy at time T k+1,tk respectively, the sampling period of the system is T, and then:
In the method, in the process of the invention, For the speed of the vehicle along the x-axis in the xoy coordinate system, the kinematic relationship is as follows:
In the method, in the process of the invention, To obtain the speed change amount in the x-axis direction from time t k-1 to t k, formula (2) is substituted into formula (1) and is arranged:
setting the weight value of formula (1) to q, q ε (0, 1), and the weight value of formula (2) to (1-q), wherein the weight value of formula (1) ×q+formula (2) × (1-q) is obtained:
Is provided with The speeds of the vehicle along the x-axis direction in the coordinate system xoy at the times t k+1,tk are:
In the method, in the process of the invention, For the acceleration of the vehicle along the x axis under the xoy coordinate system, the kinematic relationship is as follows:
In the method, in the process of the invention, To obtain the acceleration change amount of the vehicle along the x-axis direction from time t k-1 to time t k, formula (6) is substituted into formula (5) and is arranged:
setting y (k+1), wherein y (k) is the y-axis coordinate value of the vehicle under the xoy coordinate system at the time t k+1,tk, The speeds along the y axis of the vehicle in the xoy coordinate system at the time t k+1,tk are respectively as follows:
In the middle of The speed variation and the acceleration variation of the vehicle along the y-axis direction from the time t k-1 to the time t k are respectively.
Setting upThe included angles between the longitudinal running direction of the vehicle and the x-axis under the xoy coordinate system at the time t k+1,tk are course angle values,/>, respectivelyThe yaw rates of the vehicle at the time t k+1,tk respectively satisfy the following conditions:
In the middle of The vehicle yaw rate variation and the yaw acceleration variation from time t k-1 to t k, respectively.
The state vector is taken as X 1 (k),
Is provided withThen/>
The system state transition equation is set as follows:
X1(k+1)＝Φ1(k)X1(k)+W1(k) (12)
Where phi 1 (k) is the state prediction matrix,
Where W 1 (k) is the prediction noise,
2) And (3) combining the sensor signals acquired in the step (2) to construct a system state observation equation.
Let x lidar(k),ylidar (k) be the same,The x-axis coordinate, the y-axis coordinate and the course angle measured by the laser radar at the time t k are:
Wherein v xlidar(k),vylidar (k), The x-axis coordinate, the y-axis coordinate and the noise of the course angle signal measured by the laser radar at time t k are respectively.
The observation vector is taken as Y 1 (k),
Let the observation equation be:
Y1(k)＝H1(k)X1(k)+v1(k) (14)
wherein H 1 is an observation matrix,
V 1 (k) is the observation noise matrix,
3) The system prediction noise variance matrix is set to be a non-negative positive definite matrix Q 1, and the system observation noise variance matrix is set to be a positive definite matrix R 1.
Step 4: and (3) estimating the pose and speed information of the vehicle by using the Kalman filter designed in the step (3).
1) Given a pre-estimated system state initial valueAnd an error variance matrix initial value P 1 (0).
2) From the error variance matrix predicted value P 1 (k-1) at time t k-1, the error variance matrix predicted value P 1 (k, k-1) at time t k is solved.
3) The filter gain matrix K 1 (K) at time t k is solved.
4) The error variance matrix estimated value P 1 (k) at time t k is solved.
P1(k)＝[I-K1(k)H1(k)]P1(k,k-1) (17)
5) Predicted value of system state predicted value at time t k-1 Solving a system state predicted value/>, at time t k
6) Solving the predicted value of the system state predicted value at the time t k
7) Extracting a system state estimated at time t k Outputting the estimated vehicle pose at the moment t k And speed/>
Step 5: vehicle pose output by step 4And speed/>The signals are solved for the vehicle longitudinal travel speed V x1 (k), the lateral slip speed V y1 (k) and the yaw rate ω 1 (k) in the vehicle body coordinate system at time t k.
Step 6: and designing a Kalman filter for estimating the lateral slip speed of the vehicle.
1) The vehicle is simplified to be a two-degree-of-freedom dynamics model, and a state prediction equation is constructed.
Assuming that the steering angles of the left and right front wheels 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 small, the pitching, the yaw movement and the load transfer of the vehicle are ignored, and the vehicle is simplified into a classical two-degree-of-freedom dynamics model of the vehicle. And establishing a coordinate system by taking the automobile mass center o as a coordinate origin, taking the longitudinal movement direction of the automobile as an x axis, taking the lateral movement direction of the automobile as a y axis and taking the vertical ground direction as a z axis.
And (3) analyzing the stress of the vehicle, and obtaining:
Where Σf y is the resultant force in the y-axis direction, Σm z is the resultant moment about the z-axis, F yf、Fyr is the resultant lateral force of the front and rear tires, respectively, l f、lr is the distance between the front and rear axles and the vehicle center of mass, respectively, and δ is the front wheel angle.
Let the acceleration at the vehicle centroid along the x, y direction be a x、ay, it is possible to obtain:
Wherein V x is the longitudinal speed of the vehicle, V y is the vehicle lateral slip speed, which is the vehicle longitudinal speed change rateThe vehicle lateral slip speed change rate is represented by ω, which is the yaw rate.
The front wheel steering angle δ is generally small, so taking cos δ=1, it is obtained from newton's second law:
where I z is the moment of inertia of the vehicle about the z-axis, Is the angular acceleration of the vehicle about the z-axis.
The relationship between the tire side force F y and the tire side deflection angle α is:
Fy＝-Cα (26)
the front wheel slip angle α f can be expressed as:
The rear wheel slip angle α r can be expressed as:
the equations for the two-degree-of-freedom dynamics model of the vehicle are:
the above equation is organized into a standard state space equation:
Let V x (k) be the longitudinal travel speed of the vehicle at time T k, V y(k+1),Vy (k) be the lateral slip speed of the vehicle at time T k+1,tk, ω (k+1), ω (k) be the yaw rate of the vehicle at time T k+1,tk, and the sampling period of the system be T, then there are:
taking the state vector as X 2 (k) and setting Then/>The system state transition equation is set as follows:
X2(k+1)＝Φ2(k)X2(k)+Γ2(k)δ(k)+W2(k) (32)
Where phi 2 (k) is the state prediction matrix, Delta (k) is the front wheel steering angle of the vehicle at time t k, is the input quantity of the system, and W 2 (k) is the state prediction noise matrix.
2) And (5) 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.
From the formulaAnd/>The simultaneous solving can be achieved:
Is provided with For the lateral acceleration of the vehicle measured by the IMU sensor at time t k, taking the observation vector as Y 2 (k),Let the observation equation be:
Y2(k)＝H2(k)X2(k)+D(k)δ(k)+v2(k) (34)
In the middle of V 2 (k) is the observation noise matrix.
3) The system prediction noise variance matrix is set to be a non-negative positive definite matrix Q 2, and the system observation noise variance matrix is set to be a positive definite matrix R 2.
Step 7: and (3) estimating the lateral slip speed of the vehicle by using the Kalman filter designed in the step (6).
1) Given a pre-estimated system state initial valueAnd an error variance matrix initial value P 2 (0).
2) The vehicle longitudinal running speed V x1 (k) at the time t k calculated in the step 5 is obtained, V x(k)＝Vx1 (k) is enabled to update phi 2(k),H2 (k), and the front wheel rotation angle delta (k) at the time t k is obtained and updated.
3) From the error variance matrix predicted value P 2 (k-1) at time t k-1, the error variance matrix predicted value P 2 (k, k-1) at time t k is solved.
4) The filter gain matrix K 2 (K) at time t k is solved.
5) The error variance matrix estimated value P 2 (k) at time t k is solved.
P2(k)＝[I-K2(k)H2(k)]P2(k,k-1) (37)
6) Predicted value of system state predicted value at time t k-1 Solving a system state predicted value/>, at time t k
7) Solving the predicted value of the system state predicted value at the time t k
8) Extracting a system state estimated at time t k Outputting the estimated lateral slip speed/>, at the moment t k, of the vehicleAnd the yaw rate/>, obtained after the secondary filtering, of the vehicle
Claims (2)
1. A vehicle state parameter estimation method based on multi-sensor fusion is characterized by comprising the following steps:
Simplifying the vehicle into a rigid body model moving in a horizontal plane, and simplifying a plane coordinate system with a geodetic coordinate system of xoy;
The vehicle-mounted industrial personal computer collects sensor signals;
Designing a vehicle pose and speed pre-estimated Kalman filter;
Estimating pose and speed information of the vehicle by using a designed Kalman filter;
Solving the longitudinal running speed, the lateral sliding speed and the yaw rate of the vehicle under a vehicle body coordinate system at the moment t k according to the output vehicle pose and speed signals;
Designing a Kalman filter for estimating the lateral slip speed of the vehicle;
estimating the lateral slip speed of the vehicle by using a designed vehicle lateral slip speed estimation Kalman filter;
the method for designing the vehicle pose and speed pre-estimated Kalman filter comprises the following steps:
Constructing a system state prediction equation according to the kinematic relation 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 as a non-negative positive definite matrix Q 1, and setting a system observation noise variance matrix as a positive definite matrix R 1;
the method for constructing the system state prediction equation by the kinematic relation 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 linear motion along the x-axis direction, linear motion along the y-axis direction and rotational motion around an axis with the centroid perpendicular to the xy plane; taking the movement of the vehicle along the x-axis direction as an example, assuming that x (k+1), x (k) are x-axis coordinate values of the vehicle in a coordinate system xoy at time T k+1,tk respectively, the sampling period of the system is T, and then:
In the method, in the process of the invention, For the speed of the vehicle along the x-axis in the xoy coordinate system, the kinematic relationship is as follows:
In the method, in the process of the invention, To obtain the speed change amount in the x-axis direction from time t k-1 to t k, formula (2) is substituted into formula (1) and is arranged:
setting the weight value of formula (1) to q, q ε (0, 1), and the weight value of formula (2) to (1-q), wherein the weight value of formula (1) ×q+formula (2) × (1-q) is obtained:
Is provided with The speeds of the vehicle along the x-axis direction in the coordinate system xoy at the times t k+1,tk are:
In the method, in the process of the invention, For the acceleration of the vehicle along the x axis under the xoy coordinate system, the kinematic relationship is as follows:
In the method, in the process of the invention, To obtain the acceleration change amount of the vehicle along the x-axis direction from time t k-1 to time t k, formula (6) is substituted into formula (5) and is arranged:
setting y (k+1), wherein y (k) is the y-axis coordinate value of the vehicle under the xoy coordinate system at the time t k+1,tk, The speeds along the y axis of the vehicle in the xoy coordinate system at the time t k+1,tk are respectively as follows:
In the middle of The speed variation and the acceleration variation of the vehicle along the y-axis direction from the time t k-1 to the time t k are respectively;
Setting up The included angles between the longitudinal running direction of the vehicle and the x-axis under the xoy coordinate system at the time t k+1,tk are course angle values,/>, respectivelyThe yaw rates of the vehicle at the time t k+1,tk respectively satisfy the following conditions:
In the middle of The change amount of the yaw rate and the change amount of the yaw acceleration of the vehicle from the time t k-1 to the time t k are respectively;
the state vector is taken as X 1 (k),
Is provided withThen/>
The system state transition equation is set as follows:
X1(k+1)＝Φ1(k)X1(k)+W1(k) (12)
Where phi 1 (k) is the state prediction matrix,
Where W 1 (k) is the prediction noise,
The method for constructing the system state observation equation by combining the acquired sensor signals is as follows:
Let x lidar(k),ylidar (k) be the same, The x-axis coordinate, the y-axis coordinate and the course angle measured by the laser radar at the time t k are:
Wherein v xlidar(k),vylidar (k), The noise of the x-axis coordinate, the y-axis coordinate and the course angle signal measured by the laser radar at the time t k is respectively;
the observation vector is taken as Y 1 (k),
Let the observation equation be:
Y1(k)＝H1(k)X1(k)+v1(k) (14)
wherein H 1 is an observation matrix,
V 1 (k) is the observation noise matrix,
The method for estimating the pose and speed information of the vehicle comprises the following steps:
1) Given a pre-estimated system state initial value And an error variance matrix initial value P 1 (0);
2) Solving an error variance matrix predicted value P 1 (k, k-1) at the time t k by an error variance matrix predicted value P 1 (k-1) at the time t k-1;
3) Solving a filter gain matrix K 1 (K) at the time t k;
4) Solving an error variance matrix estimated value P 1 (k) at the time t k;
P1(k)＝[I-K1(k)H1(k)]P1(k,k-1) (17)
5) Predicted value of system state predicted value at time t k-1 Solving a system state predicted value at time t k
6) Solving the predicted value of the system state predicted value at the time t k
7) Extracting a system state estimated at time t k Outputting the estimated vehicle pose at the moment t k/>And speed/>
The method for solving the longitudinal running speed, the lateral sliding speed and the yaw rate of the vehicle under the vehicle body coordinate system at the time t k by the output vehicle pose and speed signals is as follows:
From the output vehicle pose And speed/>The signal is used for solving the longitudinal running speed V x1 (k), the lateral sliding speed V y1 (k) and the yaw rate omega 1 (k) of the vehicle under the vehicle body coordinate system at the moment t k:
The method for designing the vehicle lateral slip speed pre-estimation Kalman filter comprises the following steps:
simplifying the vehicle into a two-degree-of-freedom dynamics model, and constructing a state prediction equation;
Combining the acquired sensor signals and solving the acquired vehicle motion parameters to construct a state transition equation;
setting a system prediction noise variance matrix as a non-negative positive definite matrix Q 2, and setting a system observation noise variance matrix as a positive definite matrix R 2;
The method for designing the vehicle lateral slip speed pre-estimation Kalman filter comprises the following steps:
1) Simplifying the vehicle into a two-degree-of-freedom dynamics model, and constructing a state prediction equation:
assuming that the steering angles of the left and right front wheels 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 small, ignoring pitching, yaw movement and load transfer of the vehicle, and simplifying the vehicle into a classical two-degree-of-freedom dynamics model of the vehicle; taking an automobile mass center o as a coordinate origin, taking the longitudinal movement direction of the automobile as an x axis, taking the lateral movement direction of the automobile as a y axis, and taking the vertical ground direction as a z axis to establish a coordinate system;
And (3) analyzing the stress of the vehicle, and obtaining:
Wherein Sigma F y is resultant force along the y-axis direction, sigma M z is resultant moment around the z-axis, F yf、Fyr is resultant force of the front and rear tires in lateral direction, l f、lr is distance between the front and rear axles and the center of mass of the vehicle, and delta is front wheel corner;
Let the acceleration at the vehicle centroid along the x, y direction be a x、ay, it is possible to obtain:
Wherein V x is the longitudinal speed of the vehicle, V y is the vehicle lateral slip speed, which is the vehicle longitudinal speed change rateThe vehicle lateral slip speed change rate is represented by ω being the yaw rate;
the front wheel steering angle δ is generally small, so taking cos δ=1, it is obtained from newton's second law:
where I z is the moment of inertia of the vehicle about the z-axis, Angular acceleration, which is the rotation of the vehicle about the z-axis;
The relationship between the tire side force F y and the tire side deflection angle α is:
Fy＝-Cα (26)
the front wheel slip angle α f can be expressed as:
The rear wheel slip angle α r can be expressed as:
the equations for the two-degree-of-freedom dynamics model of the vehicle are:
the above equation is organized into a standard state space equation:
Let V x (k) be the longitudinal travel speed of the vehicle at time T k, V y(k+1),Vy (k) be the lateral slip speed of the vehicle at time T k+1,tk, ω (k+1), ω (k) be the yaw rate of the vehicle at time T k+1,tk, and the sampling period of the system be T, then there are:
taking the state vector as X 2 (k) and setting Then/>The system state transition equation is set as follows:
X2(k+1)＝Φ2(k)X2(k)+Γ2(k)δ(k)+W2(k) (32)
Where phi 2 (k) is the state prediction matrix, Delta (k) is the front wheel steering angle of the vehicle at time t k, is the input quantity of the system, and W 2 (k) is a state prediction noise matrix;
2) Combining the acquired sensor signals and the solved vehicle motion parameters to construct a state transition equation:
From the formula And/>The simultaneous solving can be achieved:
Is provided with For the lateral acceleration of the vehicle measured by the IMU sensor at time t k, taking the observation vector as Y 2 (k),Let the observation equation be:
Y2(k)＝H2(k)X2(k)+D(k)δ(k)+v2(k) (34)
In the middle of V 2 (k) is the observation noise matrix;
3) Setting a system prediction noise variance matrix as a non-negative positive definite matrix Q 2, and setting a system observation noise variance matrix as a positive definite matrix R 2;
The method for predicting the lateral slip speed of the vehicle comprises the following steps:
1) Given a pre-estimated system state initial value And an error variance matrix initial value P 2 (0);
2) Obtaining the calculated vehicle longitudinal running speed V x1 (k) at the moment t k, enabling V x(k)＝Vx1 (k) to update phi 2(k),H2 (k), and obtaining and updating the front wheel rotation angle delta (k) at the moment t k;
3) Solving an error variance matrix predicted value P 2 (k, k-1) at the time t k by an error variance matrix predicted value P 2 (k-1) at the time t k-1:
4) Solving a filter gain matrix K 2 (K) at time t k:
5) Solving an error variance matrix estimated value P 2 (k) at time t k:
P2(k)＝[I-K2(k)H2(k)]P2(k,k-1) (37)
6) Predicted value of system state predicted value at time t k-1 Solving a system state predicted value at time t k
7) Solving the predicted value of the system state predicted value at the time t k
8) Extracting a system state estimated at time t k Outputting the estimated lateral slip speed of the vehicle at the time t k And the yaw rate/>, obtained after the secondary filtering, of the vehicle
2. The method for estimating vehicle state parameters by fusion of multiple sensors according to claim 1, wherein the method for acquiring sensor signals by the vehicle-mounted industrial personal computer comprises the following steps:
The vehicle-mounted industrial personal computer acquires environmental point cloud data through a laser radar, and operates a feature extraction and pose matching algorithm to acquire and obtain an x-axis coordinate x lidar, a y-axis coordinate y lidar and an included angle between the longitudinal running direction of the vehicle and the x-axis of a ground coordinate system, namely a course angle, of the centroid of the vehicle measured by the laser radar under an xoy coordinate system
The vehicle-mounted industrial personal computer is communicated with a vehicle-mounted RTK real-time differential positioning sensor to obtain an x-axis coordinate x sensor, a y-axis coordinate y sensor and a course angle of a vehicle centroid measured by the RTK sensor under an xoy coordinate system
The vehicle-mounted industrial personal computer acquires image data through a camera, runs a Lucas-Kanade optical flow algorithm, and acquires and obtains the x-axis direction speed of the vehicle centroid under an xoy coordinate systemY-axis coordinate Direction speed/>
The vehicle-mounted industrial personal computer is communicated with the IMU sensor to obtain the yaw rate of the vehicle under the xoy coordinate systemAnd vehicle lateral acceleration in the body coordinate system/>
The vehicle-mounted industrial personal computer is communicated with a steering wheel angle sensor to obtain the front wheel steering angle delta of the vehicle.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111574208.0A CN114056338B (en) | 2021-12-21 | 2021-12-21 | Multi-sensor fusion vehicle state parameter prediction method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111574208.0A CN114056338B (en) | 2021-12-21 | 2021-12-21 | Multi-sensor fusion vehicle state parameter prediction method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114056338A CN114056338A (en) | 2022-02-18 |
| CN114056338B true CN114056338B (en) | 2024-05-24 |
Family
ID=80230103
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111574208.0A Active CN114056338B (en) | 2021-12-21 | 2021-12-21 | Multi-sensor fusion vehicle state parameter prediction method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114056338B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4655004B2 (en) * | 2005-12-06 | 2011-03-23 | 株式会社豊田中央研究所 | Vehicle physical quantity estimation device and program |
-
2021
- 2021-12-21 CN CN202111574208.0A patent/CN114056338B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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 |
Non-Patent Citations (1)
| Title |
|---|
| 林柏忠等.华南理工大学学报.第44卷(第9期),131-136. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114056338A (en) | 2022-02-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107246876B (en) | Method and system for autonomous positioning and map construction of unmanned automobile | |
| CN112083726B (en) | Park-oriented automatic driving double-filter fusion positioning system | |
| Ding et al. | Event-triggered vehicle sideslip angle estimation based on low-cost sensors | |
| Broggi et al. | Autonomous vehicles control in the VisLab intercontinental autonomous challenge | |
| CN113945206A (en) | Positioning method and device based on multi-sensor fusion | |
| CN113819914A (en) | Map construction method and device | |
| CN110162046B (en) | Unmanned vehicle path following method based on event trigger type model predictive control | |
| CN113386781A (en) | Intelligent vehicle trajectory tracking control method based on data-driven vehicle dynamics model | |
| CN111142091B (en) | Automatic driving system laser radar online calibration method fusing vehicle-mounted information | |
| CN113819905B (en) | Mileage metering method and device based on multi-sensor fusion | |
| Kang et al. | Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator | |
| CN108759822B (en) | Mobile robot 3D positioning system | |
| Xia et al. | Automated vehicle attitude and lateral velocity estimation using a 6-D IMU aided by vehicle dynamics | |
| CN114056338B (en) | Multi-sensor fusion vehicle state parameter prediction method | |
| Liu et al. | Vehicle state estimation based on unscented kalman filtering and a genetic algorithm | |
| CN108860137A (en) | Control method, device and the intelligent vehicle of unstability vehicle | |
| CN114475581B (en) | Automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion | |
| Junqueira et al. | A Model-less Approach for Estimating Vehicles Sideslip Angle by a Neural Network Concept | |
| CN108413923B (en) | Vehicle roll angle and pitch angle estimation method based on robust hybrid filtering | |
| CN115826583A (en) | Automatic driving vehicle formation method based on point cloud map | |
| Sun et al. | Dimensionless model-based system tracking via augmented Kalman filter for multiscale unmanned ground vehicles | |
| CN114932909A (en) | Slope estimation method for realizing acceleration correction based on complementary filtering | |
| CN111812668B (en) | Winding inspection device, positioning method thereof and storage medium | |
| Chen et al. | An integrated GNSS/INS/DR positioning strategy considering nonholonomic constraints for intelligent vehicle | |
| Tian et al. | Autonomous formula racecar: Overall system design and experimental validation |
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 |