A lateral and longitudinal acceleration compensation method and computer program considering vehicle attitude and imu installation angle

Abstract

The application discloses a lateral and longitudinal acceleration compensation method considering vehicle posture and IMU installation angle, comprising: establishing an accelerometer error model based on a vehicle-mounted IMU, measurement errors caused by IMU device installation angle and vehicle motion posture drastic change; establishing a vehicle lateral and longitudinal motion state space model, and deducing an accelerometer error correction model considering vehicle posture and IMU installation angle based on coordinate conversion and acceleration synthesis, and inputting the calculated vehicle displacement and compensated acceleration data into a Kalman observer to accurately estimate the acceleration of the vehicle. The application has the advantages that more accurate acceleration estimation results are obtained, so that the state sensing capability of the vehicle under emergency working conditions is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of intelligent driving, and in particular to a lateral and longitudinal acceleration compensation method considering vehicle attitude and IMU installation angle and a computer program. BACKGROUND

[0002] Intelligent driving has become a research focus in the automotive industry. With the progress of intelligent driving functions, it is particularly important to obtain the speed, acceleration, and attitude of the vehicle in real time and accurately. Among the many vehicle state information, acceleration is an important and basic data. At present, most vehicles obtain lateral, longitudinal, and vertical acceleration through the accelerometer in the inertial measurement unit (IMU). Since the IMU is usually composed of microelectromechanical systems (MEMS), it has the advantages of small size and low cost, and is widely used in automobiles, making acceleration information one of the more easily obtained vehicle state information. Acceleration information is not only the basis for estimating other state quantities that are not easily directly measured, but also the direct input of many vehicle electronic control algorithms, and its accuracy has an important influence on the safety, intelligence, and comfort of the vehicle.

[0003] However, the complexity of vehicle operating conditions leads to errors in IMU accelerometer measurements, mainly from two aspects. First, changes in vehicle attitude (such as pitch and roll movements) will cause part of the gravitational acceleration component to be included in the measurement results, especially when the attitude changes dramatically, the error is more significant. Second, the angle of the IMU installation is difficult to completely align with the vehicle coordinate system, and there is a slight angle deviation between the three measurement axes and the vehicle coordinate axes, thereby introducing errors.

[0004] Therefore, in order to meet the requirements of intelligent vehicles in safety, intelligence, and comfort, it is particularly necessary to analyze the error sources of vehicle acceleration measurement and perform compensation. SUMMARY

[0005] The technical problem to be solved by the present application is to provide a lateral and longitudinal acceleration compensation method considering vehicle attitude and IMU installation angle and a computer program, to obtain more accurate acceleration estimation results, and to improve the state perception ability of the vehicle in emergency conditions.

[0006] To solve the above technical problems, the present application provides a lateral and longitudinal acceleration compensation method considering vehicle attitude and IMU installation angle, comprising the following specific steps:

[0007] Step S1, establishing an accelerometer error model based on the vehicle-mounted IMU, mainly considering the measurement error caused by the IMU device installation angle and the dramatic change of the vehicle motion posture;

[0008] Step S2, establishing a vehicle lateral and longitudinal motion state space model, and based on the coordinate transformation and acceleration synthesis, deducing an accelerometer error correction model considering the vehicle posture and the IMU installation angle, and inputting the calculated vehicle displacement and the compensated acceleration data into the Kalman observer to accurately estimate the vehicle acceleration.

[0009] The measurement error caused by the IMU device installation angle and the dramatic change of the vehicle motion posture includes 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, and this angle difference will cause acceleration measurement error. Through the transformation matrix from the IMU device coordinate system to the vehicle coordinate system compensation, as shown in equation (1);

[0011]

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

[0013] Further, equation (2) is obtained, and the lateral and longitudinal acceleration compensation values of the installation error are a vy1 、a vx1 ,

[0014]

[0015] Where: a by 、a bx are the lateral and longitudinal acceleration values measured by the IMU;

[0016] (2) error caused by vehicle posture change;

[0017] During the implementation of emergency braking or steering, the posture of the vehicle will change dramatically, which will affect the measurement accuracy of the IMU. The longitudinal acceleration a bx measured by the IMU contains the component of the actual longitudinal acceleration a vx2 of the vehicle and the component of the reverse gravity acceleration g, as shown in equation (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 relationship between the actual lateral acceleration a vy2 is shown in equation (4):

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

[0022] Wherein: a is the vehicle roll angle;

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

[0024]

[0025] Wherein: the IMU installation pitch angle θ and the installation yaw angle ψ are calculated by an algorithm for estimating the IMU and vehicle installation angles, and the vehicle roll angle a and the vehicle pitch angle β are collected in real time by the IMU.

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

[0027] The first layer obtains the vehicle position measurement value Position increment and the IMU attitude matrix The second layer inputs the attitude matrix and the position increment into the dead reckoning module to obtain the position estimate value The difference between the position estimate value and the measurement value reflects the influence caused by the IMU installation angle, and the position residual is subjected to Kalman filtering to obtain the error state matrix And feedback to the dead reckoning module, so as to obtain the estimated values of the pitch angle θ and the yaw angle ψ.

[0028] The acceleration estimation algorithm based on the Kalman filter comprises the following steps:

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

[0030] The displacement x of the vehicle and the acceleration are selected as the observation quantity, i.e. the observation matrix

[0031] Wherein: the displacement x is calculated from the positioning data output by the integrated navigation instrument, and the acceleration is calculated from the output of the IMU accelerometer through the error compensation formula (5);

[0032] X(k) is the displacement, speed and acceleration of the vehicle, i.e.

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

[0034]

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

[0036] In the formula, Φ is a state transition matrix, W(k) is a process noise matrix, H is an observation matrix, and V(k) is an observation noise matrix;

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

[0038]

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

[0040] The state estimation at the current time is optimized by continuously iterating the state at the previous time, so that the estimation is closer to the actual state. The 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 formula (6), which means that the state estimation value based on the current state predicts the state of the next time of the system

[0043] The covariance extrapolation equation is formula (8):

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

[0045] In the formula, P(k) is the covariance matrix of the current state estimation, Q is the covariance matrix of the process noise, and P(k+1) is the covariance matrix estimation value of the next time state estimation;

[0046] 2) Kalman gain equation:

[0047]

[0048] In the formula, 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 formula (10):

[0051]

[0052] In the formula: is the posterior state estimation, in the next iteration process, the acceleration data measured by the IMU is calculated by the formula as the new

[0053] The state covariance update equation is as formula (11):

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

[0055] In the formula: I is a unit matrix;

[0056] In the formula: P'(k+1) is used as the new P(k) in the next iteration process;

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

[0058]

[0059] The application also provides a computer program, comprising a memory, a processor and a computer program stored in the memory, and the processor executes the computer program to realize the steps of the vehicle attitude and IMU installation angle considered lateral and longitudinal acceleration compensation method.

[0060] The application has the beneficial effects that: by analyzing the error characteristics of the acceleration data measured by the IMU of the vehicle in the emergency driving condition, the error model considering the vehicle attitude and the IMU installation angle is established, and the vehicle lateral and longitudinal acceleration is compensated by introducing the Kalman observer, so that the acceleration measurement accuracy is further improved. BRIEF DESCRIPTION OF DRAWINGS

[0061] The drawings accompanying the specification of this application form a part of the application and serve to further provide a further understanding of the application, the illustrative embodiments of the application and the explanations thereof serve to explain the application and do not constitute an improper limitation of the application. In the drawings:

[0062] Figure 1 The schematic diagram of the vehicle IMU installation error of the embodiment of the application;

[0063] Figure 2 The relationship between the accelerations of the vehicle in the emergency braking of the embodiment of the application;

[0064] Figure 3 The schematic diagram of the vehicle IMU installation angle estimation algorithm of the embodiment of the application;

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

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

[0067] This invention provides a method for lateral and longitudinal acceleration compensation that takes into account vehicle attitude and IMU mounting angle, including the following specific steps:

[0068] Step S1, IMU installation error analysis;

[0069] (1) IMU installation error: such as Figure 1 As shown, during equipment installation, the three axes of the IMU equipment coordinate system (referred to as the b-frame) often do not coincide with the three axes of the vehicle coordinate system (v-frame). This angular difference will cause acceleration measurement errors (i.e., pitch angle θ and yaw angle ψ). This can be addressed by adjusting the transformation matrix from the b-frame to the v-frame. 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 These are the lateral and longitudinal acceleration values ​​measured by the IMU;

[0070]

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

[0072] During emergency braking or steering, a vehicle's attitude changes drastically, which can affect the measurement accuracy of the IMU. For example... Figure 2 The image shows a vehicle emergency braking scenario where the vehicle's pitch angle is β, and the longitudinal acceleration a measured by the IMU during this process. bx Includes the vehicle's actual longitudinal acceleration a vx2 The component of g is the same as the component of the reverse gravitational acceleration g, that is:

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

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

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

[0076] Substitute formula (2) into (3) and (4), and rearrange to obtain formula (5):

[0077]

[0078] Where: the IMU installation pitch angle θ and the installation yaw angle ψ are calculated by a 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] As shown in Figure 3 An algorithm for estimating the installation angle of the IMU and the vehicle, i.e., the algorithm for estimating the pitch angle θ and the yaw angle ψ, adopts a two-layer structure, wherein the output of the first layer is taken as the input of the second layer. Specifically, the first layer obtains vehicle position measurement values Position increment and an IMU attitude matrix The second layer inputs the attitude matrix and the position increment into a dead reckoning module, and obtains a position estimate value by means of a kinematic model The difference between the position estimate value and the measurement value reflects the influence caused by the installation angle of the IMU, and the position residual is subjected to Kalman filtering to obtain an error state matrix and is fed back to the dead reckoning module, so as to obtain the estimated values of the pitch angle θ and the yaw angle ψ.

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

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

[0083] The displacement x and the acceleration of the vehicle are selected as observation quantities, i.e., an observation matrix

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

[0085] X(k) is the displacement, speed and acceleration of the vehicle, i.e.,

[0086] The state space model is finally obtained as:

[0087]

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

[0089] wherein: Φ is the state transition 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 transition matrix and the observation matrix are as follows:

[0091]

[0092] Step S32, iteration of the Kalman filter;

[0093] As shown in the flowchart for predicting acceleration using the Kalman filtering algorithm, the state estimation at the current time is optimized by continuously iterating the state at the previous time, so that the estimation is closer to the actual state. The iteration process is summarized as follows: state prediction equation, Kalman gain equation, and state update equation. Figure 4

[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 estimation value the state at the next time of the system is predicted

[0096] The form of the covariance extrapolation equation is as follows:

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

[0098] wherein: P(k) is the covariance matrix of the current state estimation, Q is the covariance matrix of the process noise, and P(k+1) is the covariance matrix estimation value of the state estimation at the next time;

[0099] ② Kalman gain equation:

[0100]

[0101] wherein: 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] wherein: is the posterior state estimation, and in the next iteration process, the calculated by this formula will be used as the new ​

[0106] Obtain the posterior state estimate Then, the state covariance update equation needs to be used to calculate The uncertainty of P'(k+1):

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

[0108] In the formula, I is a unit matrix;

[0109] Similarly, P'(k+1) calculated by formula (11) will be used as the new P(k) in the next iteration to participate in the calculation;

[0110] 4. The result is stored: store the processed acceleration with column vector A;

[0111]

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

[0113] The application also provides a computer program, including a memory, a processor and a computer program stored on the memory, and the processor executes the computer program to realize the steps of the method for compensating lateral and longitudinal accelerations considering the vehicle attitude and the IMU installation angle.

[0114] The above only describes the preferred embodiments of the present application and is not used to limit the present application, and the present application can have various changes and variations for those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1.A method for lateral and longitudinal acceleration compensation considering vehicle attitude and IMU installation angle, comprising the following steps: Step S1, establishing an accelerometer error model based on vehicle-mounted IMU, wherein the measurement error caused by IMU device installation angle and vehicle motion attitude change; The measurement error caused by IMU device installation angle and vehicle motion attitude change includes the following: (1) IMU installation error: The transformation matrix from the IMU device coordinate system to the vehicle coordinate system is compensated by as shown in equation (1); （1）； wherein: θ is the IMU installation pitch angle, and ψ is the IMU installation yaw angle; Further, the lateral and longitudinal acceleration compensation values of the mounting error of formula (2) are obtained as , , （2）； wherein: , are the lateral and longitudinal acceleration values measured by the IMU; (2) error caused by vehicle attitude change; when the vehicle is in the process of performing an emergency braking or a steering maneuver, at which time the longitudinal acceleration measured by the IMU comprises a component of the actual longitudinal acceleration of the vehicle and a component of the reversed gravitational acceleration g, as shown in equation (3): ​ （3）； wherein: is the vehicle pitch angle; The relationship between the lateral acceleration measured by the IMU and the actual lateral acceleration is given by equation (4): （4）； wherein: is the vehicle roll angle; Substitute formula (2) into formula (3) and formula (4) to obtain the accelerometer error model based on vehicle-mounted IMU, as formula (5): （5）； Wherein: the IMU installation pitch angle θ and the installation yaw angle ψ are calculated by an algorithm for estimating the IMU and vehicle installation angles, and the vehicle roll angle and the vehicle pitch angle are collected in real time by the IMU The algorithm for estimating the IMU and vehicle installation angle adopts a two-layer structure, and the output of the first layer is used as the input of the second layer; said first layer, obtaining vehicle position measurements by GNSS / INS fusion , position increments and IMU attitude matrix ; a second layer, inputting the attitude matrix and the position increments into a dead reckoning module to obtain position estimates , the difference between the position estimates and the measurements being Kalman filtered to obtain an error state matrix and fed back to the dead reckoning module, thus obtaining estimates of the pitch angle θ and the yaw angle ψ; Step S2, establishing a vehicle lateral and longitudinal motion state space model, and based on coordinate transformation and acceleration synthesis, deducing an accelerometer error correction model considering vehicle attitude and IMU installation angle, and inputting the calculated vehicle displacement and compensated acceleration data into a Kalman observer to accurately estimate the vehicle acceleration; The acceleration estimation algorithm based on Kalman filter includes the following: (1) establishing a vehicle lateral and longitudinal motion state space model; Selecting displacement of the vehicle with acceleration as the observation, i.e. the observation matrix ; where: displacement Positioning data calculated from the output of the integrated navigator, acceleration calculated from the output of the IMU accelerometer via the error compensation formula (5); For displacement, velocity and acceleration of the vehicle, i.e. ; The vehicle lateral and longitudinal motion state space model is obtained as follows: （6）； （7）； wherein: is a state transition matrix, is a process noise matrix, is an observation matrix, is an observation noise matrix; The values of the state transition matrix and the observation matrix are as follows: ； (2) Kalman filter iteration; The state estimation at the current time is optimized by continuously iterating the state at the last time, and 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 equation (6), ; which means the current state estimate predicts the state of the system at the next time instant ; The covariance extrapolation equation is as formula (8): （8）； where: is the covariance matrix of the current state estimate, is the covariance matrix of the process noise, is the covariance matrix estimate of the next time state estimate; 2) Kalman gain equation: （9）； wherein is the Kalman gain, 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 formula (10): （10） where: is the a posteriori state estimate, which will be used in the next iteration process as the new ; The state covariance update equation is as formula (11): （11）； In the formulae: is the identity matrix; In the formula: In the next iteration as new Participate in the calculation; 4) Result storage: store the processed acceleration in column vector A; A= (12)。 2. A computer program comprising a memory, a processor and a computer program stored on the memory, characterized in that: The processor executes the computer program to realize the steps of the lateral and longitudinal acceleration compensation method considering vehicle attitude and IMU installation angle according to claim 1.

Images