Transverse and longitudinal acceleration compensation method considering vehicle attitude and IMU installation angle and computer program

Abstract

The invention discloses a transverse and longitudinal acceleration compensation method considering a vehicle attitude and an IMU (Inertial Measurement Unit) installation angle, which comprises the following steps: establishing an accelerometer error model based on a vehicle-mounted IMU, and measuring errors caused by the violent change of the IMU equipment installation angle and the vehicle motion attitude; a vehicle transverse and longitudinal motion state space model is established, an accelerometer error correction model considering the vehicle attitude and the IMU installation angle is deduced based on coordinate conversion and acceleration synthesis, the calculated vehicle displacement and compensated acceleration data are input into a Kalman observer, and the acceleration of the vehicle is accurately estimated. The method has the advantages that a more accurate acceleration estimation result is obtained, and therefore the state sensing capacity of the vehicle under the emergency working condition is improved.

Description

Technical Field

[0001] The present invention relates to the field of intelligent driving, and in particular to a lateral and longitudinal acceleration compensation method and a computer program taking into account vehicle posture and IMU installation angle. Background Art

[0002] Intelligent driving has become a research focus in the automotive industry. With the advancement of intelligent driving functions, it is particularly important to obtain vehicle status information such as speed, acceleration, and posture in real time and accurately. Among the many vehicle status information, acceleration is an important and basic data. At present, most vehicles use accelerometers in inertial measurement units (IMUs) to obtain lateral, longitudinal, and vertical accelerations. Since IMUs are usually composed of microelectromechanical systems (MEMS), they have the advantages of small size and low cost and are widely used in automobiles, making acceleration information one of the easier vehicle status information to obtain. Acceleration information is not only the basis for estimating other state quantities that are not easy to measure directly, but also the direct input of many vehicle electronic control algorithms. Its accuracy has an important impact on the safety, intelligence, and comfort of the vehicle.

[0003] However, the complexity of vehicle operating conditions leads to errors in IMU accelerometer measurements, which mainly come from two aspects. First, changes in vehicle posture (such as pitch and roll motion) will cause the measurement results to include some gravity acceleration components, especially when the posture changes drastically, the error is more significant. Second, the angle at which the IMU is installed is difficult to be completely aligned with the vehicle coordinate system, and there is a slight angular deviation between the three measurement axes and the vehicle coordinate axes, which introduces errors.

[0004] Therefore, in order to meet the requirements of smart cars in terms of safety, intelligence and comfort, it is particularly necessary to analyze the error sources of vehicle acceleration measurement and compensate for them. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide a lateral and longitudinal acceleration compensation method and a computer program that take into account the vehicle posture and the IMU installation angle, so as to obtain a more accurate acceleration estimation result, thereby improving the vehicle's state perception ability under emergency conditions.

[0006] In order to solve the above technical problems, the present invention provides a lateral and longitudinal acceleration compensation method considering the vehicle posture and the IMU installation angle, comprising the following specific steps:

[0007] Step S1, establishing an accelerometer error model based on the vehicle-mounted IMU, which mainly considers the measurement error caused by the drastic changes in the installation angle of the IMU device and the vehicle's motion posture;

[0008] Step S2: Establish a state space model of the vehicle's lateral and longitudinal motion, and derive an accelerometer error correction model that takes into account the vehicle's posture and the IMU installation angle based on coordinate transformation and acceleration synthesis, and input the calculated vehicle displacement and compensated acceleration data into the Kalman observer to accurately estimate the vehicle's acceleration.

[0009] The measurement errors caused by the drastic changes in the IMU device installation angle and the vehicle's motion posture include the following:

[0010] (1) IMU installation error: During the installation of the IMU device, the three axes of the IMU device coordinate system and the vehicle coordinate system do not coincide. This angle difference will cause acceleration measurement errors. Compensation is performed as shown in formula (1);

[0011]

[0012] Where: θ is the pitch angle of the IMU installation, ψ is the yaw angle of the IMU installation;

[0013] Further, we can get formula (2), and the lateral and longitudinal acceleration compensation values ​​of the installation error are a vy1 、a vx1 ,

[0014]

[0015] Among them: a by 、a bx It is the lateral and longitudinal acceleration values ​​measured by IMU;

[0016] (2) Errors caused by changes in vehicle posture;

[0017] When the vehicle is in emergency braking or steering, its posture will change dramatically, which will affect the measurement accuracy of the IMU. The longitudinal acceleration a measured by the IMU bx Contains the actual longitudinal acceleration of the vehicle a vx2 The component of and the component of the reverse gravitational acceleration g are shown in formula (3):

[0018] a bx =-g sinβ+a vx2 cosβ (3);

[0019] Where: β is the vehicle pitch angle;

[0020] The lateral acceleration a measured by the IMU by The actual lateral acceleration a vy2 The relationship between is shown in formula (4):

[0021] a by =-g sinα+a vy2 cosα (4);

[0022] Where: α is the vehicle roll angle;

[0023] Substituting equation (2) into equation (3) and equation (4), we get the accelerometer error model based on the vehicle-mounted IMU, as shown in equation (5):

[0024]

[0025] Among them: the IMU installation pitch angle θ and the installation yaw angle ψ are calculated by an algorithm that estimates the installation angle between the IMU and the vehicle, and the vehicle roll angle α and the vehicle pitch angle β are collected in real time by the IMU.

[0026] The algorithm for estimating the installation angle between the IMU and the vehicle adopts a two-layer structure, and the output of the first layer serves as the input of the second layer.

[0027] The first layer obtains vehicle position measurements through GNSS / INS fusion Position increment and the IMU attitude matrix In the second layer, the attitude matrix and position increment input are imported into the dead reckoning module to obtain the position estimate The difference between the position estimate and the measured value reflects the effect caused by the IMU installation angle. The position residual is Kalman filtered to obtain the error state matrix And feed it back to the dead reckoning module to obtain the estimated values ​​of the pitch angle θ and yaw angle ψ.

[0028] The step is based on the acceleration estimation algorithm of the Kalman filter, including the following:

[0029] (1) Establish the vehicle's lateral and longitudinal motion state space model;

[0030] Select the vehicle's displacement x and acceleration As the observation, that is, the observation matrix

[0031] Where: displacement x is calculated from the positioning data output by the integrated navigator, acceleration The output of the IMU accelerometer is calculated using the error compensation formula (5);

[0032] X(k) is the displacement, velocity and acceleration of the vehicle, that is,

[0033] The vehicle lateral and longitudinal motion state space model is obtained as follows:

[0034]

[0035] Y(k)=HX(k)+V(k) (7);

[0036] Where: Φ is the state transfer matrix, W(k) is the process noise matrix, H is the observation matrix, and V(k) is the observation noise matrix;

[0037] The values ​​of the state transfer matrix and the observation matrix are as follows:

[0038]

[0039] (2), iteration of Kalman filter;

[0040] By continuously iterating the state of the previous moment, the state estimate of the current moment is optimized to make the estimate closer to the actual state. Each iteration process is summarized as follows: state prediction equation, Kalman gain equation and state update equation;

[0041] 1) State prediction equation: including state extrapolation equation and covariance extrapolation equation;

[0042] The state extrapolation equation is (6), Its meaning is based on the current state estimate Predict the state of the system at the next moment

[0043] The covariance extrapolation equation is as follows:

[0044] P(k+1)=ΦP(k)Φ T +Q (8);

[0045] Where: P(k) is the covariance matrix of the current state estimate, Q is the covariance matrix of the process noise, and P(k+1) is the estimated value of the covariance matrix of the state estimate at the next moment;

[0046] 2) Kalman gain equation:

[0047]

[0048] Where K(k+1) is the Kalman gain, and R is the covariance matrix of the observation noise;

[0049] 3) State update equation: including state update equation and state covariance update equation;

[0050] The state update equation is as follows:

[0051]

[0052] Where: is the a posteriori state estimate. In the next iteration, the state calculated by this formula will be As new

[0053] The state covariance update equation is as follows:

[0054] P'(k+1)=[IK(k+1)H]P(k+1)[IK(k+1)H] T +K(k+1)R(11);

[0055] Where: I is the unit matrix;

[0056] Where: P′(k+1) is used as the new P(k) in the next iteration;

[0057] 4) Result storage: Use column vector A to store the processed acceleration;

[0058]

[0059] The present invention also provides a computer program, including a memory, a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the lateral and longitudinal acceleration compensation method considering the vehicle posture and the IMU installation angle.

[0060] The beneficial effects of the present invention are as follows: by analyzing the error characteristics of the IMU measured acceleration data under emergency driving conditions, an error model considering the vehicle posture and the IMU installation angle is established, and by introducing the Kalman observer to compensate for the vehicle's lateral and longitudinal accelerations, the acceleration measurement accuracy is further improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] The drawings constituting a part of the present application are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0062] Figure 1 A schematic diagram of the vehicle-mounted IMU installation error of a specific embodiment of the present invention;

[0063] Figure 2 The relationship between various accelerations during emergency braking of a vehicle according to a specific embodiment of the present invention;

[0064] Figure 3 Schematic diagram of the vehicle-mounted IMU installation angle estimation algorithm of a specific embodiment of the present invention;

[0065] Figure 4 The iterative process of the Kalman filter of the specific embodiment of the present invention. Specific embodiments

[0066] The embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0067] The present invention provides a method for compensating lateral and longitudinal accelerations taking into account vehicle posture and IMU installation angle, comprising the following specific steps:

[0068] Step S1, IMU installation error analysis;

[0069] (1) IMU installation error: Figure 1 As shown in the figure, during the equipment installation process, the IMU equipment coordinate system (called b system) often does not coincide with the three axes of the vehicle coordinate system (v system). This angle difference will cause acceleration measurement errors (i.e., pitch angle θ and yaw angle ψ); by converting the conversion matrix from b system to v system Compensation is performed as shown in formula (1); further formula (2) is obtained, which is the lateral and longitudinal acceleration compensation value a considering only the installation error. vy1 、a vx1 , where a by 、a bx It is the lateral and longitudinal acceleration values ​​measured by IMU;

[0070]

[0071] (2) Errors caused by changes in vehicle posture;

[0072] When a vehicle is in emergency braking or steering, its posture will change dramatically, which will affect the measurement accuracy of the IMU. Figure 2 The figure shows the emergency braking scenario of the vehicle. During this process, the pitch angle of the vehicle is β, and the longitudinal acceleration a measured by the IMU is bx Contains the actual longitudinal acceleration of the vehicle a vx2 The component of and the component of the reverse gravitational acceleration g, that is:

[0073] a bx =-g sinβ+a vx2 cosβ (3);

[0074] Formula (3) is extended to describe lateral motion. When the vehicle has a roll angle α, the actual lateral acceleration a vy2 The lateral acceleration a measured by the IMU by The relationship between them is:

[0075] a by = -g sin α + a vy2 cos α (4);

[0076] Substituting formula (2) into (3) and (4), we get formula (5):

[0077]

[0078] Among them: the IMU installation pitch angle θ and the installation yaw angle ψ are calculated by the design model algorithm, and the vehicle roll angle α and the vehicle pitch angle β are collected in real time by the IMU.

[0079] Step S2, IMU installation error calibration

[0080] like Figure 3 As shown in Figure 1, an algorithm for estimating the angle between the IMU and the vehicle, that is, the pitch angle θ and the yaw angle ψ, is used. The algorithm adopts a two-layer structure, in which the output of the first layer is used as the input of the second layer. Specifically, the first layer obtains the vehicle position measurement value through GNSS / INS fusion Position increment and the IMU attitude matrix In the second layer, the attitude matrix and position increment input are imported into the dead reckoning module, and the position estimate is obtained with the help of the kinematic model. The difference between the position estimate and the measured value reflects the effect caused by the IMU installation angle. The position residual is Kalman filtered to obtain the error state matrix And feed it back to the dead reckoning module to obtain the estimated values ​​of the pitch angle θ and yaw angle ψ.

[0081] Step S3, acceleration estimation algorithm based on Kalman filter;

[0082] Step S31, establishing a vehicle lateral and longitudinal motion state space model;

[0083] Select the vehicle's displacement x and acceleration As the observation, that is, the observation matrix

[0084] Where: displacement x is calculated from the positioning data output by the integrated navigator, acceleration The output of the IMU accelerometer is calculated using the error compensation formula (5):

[0085] X(k) is the displacement, velocity and acceleration of the vehicle, that is,

[0086] The final state space model is:

[0087]

[0088] Y(k)=HX(k)+V(k) (7)

[0089] Where: Φ is the state transfer matrix, W(k) is the process noise matrix, H is the observation matrix, and V(k) is the observation noise matrix;

[0090] The values ​​of the state transfer matrix and the observation matrix are as follows:

[0091]

[0092] Step S32, iteration of Kalman filter;

[0093] like Figure 4 The flowchart of using Kalman filter algorithm to predict acceleration is shown. By continuously iterating the state of the previous moment, the state estimation of the current moment is optimized to make the estimation closer to the actual state. Each iteration process is summarized as follows: state prediction equation, Kalman gain equation and state update equation;

[0094] ① State prediction equation: including state extrapolation equation and covariance extrapolation equation;

[0095] The state extrapolation equation used is equation (6), which means that based on the current state estimate Predict the state of the system at the next moment

[0096] The covariance extrapolation equation takes the form:

[0097] P(k+1)=ΦP(k)Φ T +Q (8)

[0098] Where: P(k) is the covariance matrix of the current state estimate, Q is the covariance matrix of the process noise, and P(k+1) is the estimated value of the covariance matrix of the state estimate at the next moment;

[0099] ② Kalman gain equation:

[0100]

[0101] Where K(k+1) is the Kalman gain, and R is the covariance matrix of the observation noise;

[0102] ③State update equation: including state update equation and state covariance update equation;

[0103] The state update equation is as follows:

[0104]

[0105] Where: is the a posteriori state estimate. In the next iteration, the state calculated by this formula will be As new

[0106] Get the posterior state estimate After that, the state covariance update equation needs to be used to calculate Uncertainty:

[0107] P'(k+1)=[IK(k+1)H]P(k+1)[IK(k+1)H] T +K(k+1)R( 11 )

[0108] Where: I is the unit matrix;

[0109] Similarly, P′(k+1) calculated by equation (11) will be used as the new P(k) in the next iteration.

[0110] ④Result storage: Use column vector A to store the processed acceleration;

[0111]

[0112] In summary, an accelerometer error correction model considering vehicle posture and IMU installation angle is derived based on coordinate transformation and acceleration synthesis, and the calculated vehicle displacement and compensated acceleration data are input into the Kalman observer to achieve accurate estimation of vehicle acceleration.

[0113] The present invention also provides a computer program, including a memory, a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the lateral and longitudinal acceleration compensation method considering the vehicle posture and the IMU installation angle.

[0114] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for compensating lateral and longitudinal accelerations taking into account vehicle posture and IMU installation angles, comprising the following steps: Step S1, establishing an accelerometer error model based on the vehicle-mounted IMU, wherein the measurement error caused by the drastic change of the IMU device installation angle and the vehicle motion posture; Step S2: Establish a state space model of the vehicle's lateral and longitudinal motion, and derive an accelerometer error correction model that takes into account the vehicle's posture and the IMU installation angle based on coordinate transformation and acceleration synthesis, and input the calculated vehicle displacement and compensated acceleration data into the Kalman observer to accurately estimate the vehicle's acceleration.

2. The method for compensating lateral and longitudinal acceleration considering vehicle posture and IMU installation angle according to claim 1, characterized in that: The measurement errors caused by the drastic changes in the IMU device installation angle and the vehicle's motion posture include the following: (1) IMU installation error: The transformation matrix from the IMU device coordinate system to the vehicle coordinate system is calculated. Compensation is performed as shown in formula (1); Where: θ is the pitch angle of the IMU installation, ψ is the yaw angle of the IMU installation; Further, we can get formula (2), and the lateral and longitudinal acceleration compensation values ​​of the installation error are a vy1 、a vx1 , Among them: a by 、a bx It is the lateral and longitudinal acceleration values ​​measured by IMU; (2) Errors caused by changes in vehicle posture; When the vehicle is in emergency braking or steering, the longitudinal acceleration a measured by the IMU is bx Contains the actual longitudinal acceleration of the vehicle a vx2 The component of and the component of the reverse gravitational acceleration g are shown in formula (3): a bx =-g sinβ+a vx2 cosβ (3); Where: β is the vehicle pitch angle; The lateral acceleration α measured by the IMU by The actual lateral acceleration a vy2 The relationship between is shown in formula (4): a by =-g sinα+a vy2 cosα (4); Where: α is the vehicle roll angle; Substituting equation (2) into equation (3) and equation (4), we get the accelerometer error model based on the vehicle-mounted IMU, as shown in equation (5): Among them: the IMU installation pitch angle θ and the installation yaw angle ψ are calculated by an algorithm that estimates the installation angle between the IMU and the vehicle, and the vehicle roll angle α and the vehicle pitch angle β are collected in real time by the IMU.

3. The method for compensating lateral and longitudinal accelerations considering vehicle posture and IMU installation angle according to claim 2, characterized in that: The algorithm for estimating the installation angle between the IMU and the vehicle adopts a two-layer structure, and the output of the first layer serves as the input of the second layer.

4. The method for compensating lateral and longitudinal accelerations considering vehicle posture and IMU installation angle according to claim 3, characterized in that: The first layer obtains vehicle position measurements through GNSS / INS fusion Position increment and the IMU attitude matrix In the second layer, the attitude matrix and position increment input are imported into the dead reckoning module to obtain the position estimate The difference between the position estimate and the measured value is Kalman filtered to obtain the error state matrix And feed it back to the dead reckoning module to obtain the estimated values ​​of the pitch angle θ and yaw angle ψ.

5. The method for compensating lateral and longitudinal acceleration considering vehicle posture and IMU installation angle according to claim 1, characterized in that: The step is based on the acceleration estimation algorithm of the Kalman filter, including the following: (1) Establish the vehicle's lateral and longitudinal motion state space model; Select the vehicle's displacement x and acceleration As the observation, that is, the observation matrix Where: displacement x is calculated from the positioning data output by the integrated navigator, acceleration The output of the IMU accelerometer is calculated using the error compensation formula (5); X(k) is the displacement, velocity and acceleration of the vehicle, that is, The vehicle lateral and longitudinal motion state space model is obtained as follows: Y(k)=HX(k)+V(k) (7); Where: Φ is the state transfer matrix, W(k) is the process noise matrix, H is the observation matrix, and V(k) is the observation noise matrix; The values ​​of the state transfer matrix and the observation matrix are as follows: (2), iteration of Kalman filter; The state estimation at the current moment is optimized by continuously iterating the state at the previous moment. Each iteration process is summarized as follows: state prediction equation, Kalman gain equation and state update equation; 1) State prediction equation: including state extrapolation equation and covariance extrapolation equation; The state extrapolation equation is (6), Its meaning is based on the current state estimate Predict the state of the system at the next moment The covariance extrapolation equation is as follows: P(k+1)=ΦP(k)Φ T +Q (8); Where: P(k) is the covariance matrix of the current state estimate, Q is the covariance matrix of the process noise, and P(k+1) is the estimated value of the covariance matrix of the state estimate at the next moment; 2) Kalman gain equation: Where K(k+1) is the Kalman gain, and R is the covariance matrix of the observation noise; 3) State update equation: including state update equation and state covariance update equation; The state update equation is as follows: Where: is the a posteriori state estimate. In the next iteration, the state calculated by this formula will be As new The state covariance update equation is as follows: P′(k+1)=[I-K(k+1)H]P(k+1)[I-K(k+1)H] T +K(k+1)R (11); Where: I is the unit matrix; Where: P′(k+1) is used as the new P(k) in the next iteration; 4) Result storage: Use column vector A to store the processed acceleration; 6. A computer program, comprising a memory, a processor, and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the lateral and longitudinal acceleration compensation method considering the vehicle posture and the IMU installation angle as described in any one of claims 1-5.

Images