Door control method and system based on IMU attitude algorithm and Kalman filter

Abstract

The present invention relates to the field of door control technology, and specifically discloses a door control method based on an IMU attitude algorithm and a Kalman filter, including: obtaining the three-axis acceleration value of the door at the current moment and the three-axis angular velocity value of the door at the previous moment; filtering the three-axis acceleration value to obtain the three-axis acceleration filter value of the door at the current moment; calculating the first attitude information of the door at the current moment according to the three-axis acceleration filter value, and calculating the second attitude information of the door at the current moment according to the three-axis angular velocity value; fusing the first attitude information and the second attitude information to obtain the fused attitude information; controlling the opening and closing and hovering of the door according to the fused attitude information. The present invention also discloses a door control system based on an IMU attitude algorithm and a Kalman filter. The present invention can make the IMU attitude fitting error low, and the vehicle vibration noise can be effectively filtered out, thereby ensuring that the door attitude output is stable and reliable.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle door control, and more specifically, to a vehicle door control method based on an IMU attitude algorithm and a Kalman filter, and a vehicle door control system based on an IMU attitude algorithm and a Kalman filter. Background Art

[0002] The car needs to realize the ramp hovering function on different slopes, so that the door can achieve different opening angles and maintain them on different slopes without affecting the manual door opening and closing function. Due to the vibration interference when the vehicle engine is running, the IMU attitude fitting error is high under traditional filtering conditions, resulting in unsatisfactory control effect.

[0003] Disadvantages: Vehicle vibration noise cannot be filtered out; IMU attitude fitting error is high. Summary of the invention

[0004] In order to solve the deficiencies in the prior art, the present invention provides a door control method and system based on IMU attitude algorithm and Kalman filtering, so as to solve the problems in the prior art that the IMU attitude fitting error is high under traditional filtering conditions, the vehicle vibration noise cannot be filtered out, and the control effect is not ideal.

[0005] As a first aspect of the present invention, a door control method based on an IMU attitude algorithm and a Kalman filter is provided, comprising:

[0006] Step S1: obtaining the three-axis acceleration value of the door at the current moment through the three-axis accelerometer, and obtaining the three-axis angular velocity value of the door at the previous moment through the gyroscope;

[0007] Step S2: filtering the three-axis acceleration value of the vehicle door at the current moment according to the three-axis angular velocity value of the vehicle door at the previous moment, so as to obtain the three-axis acceleration filtering value of the vehicle door at the current moment;

[0008] Step S3: calculating the first posture information of the vehicle door at the current moment according to the three-axis acceleration filtering value of the vehicle door at the current moment, and calculating the second posture information of the vehicle door at the current moment according to the three-axis angular velocity value of the vehicle door at the previous moment; wherein the first posture information includes first roll angle information and first pitch angle information, and the second posture information includes second roll angle information, second pitch angle information and heading angle information;

[0009] Step S4: fusing the first posture information and the second posture information to obtain the fused posture information of the vehicle door at the current moment;

[0010] Step S5: Control the opening, closing and hovering of the door according to the fused posture information of the door at the current moment.

[0011] Further, filtering the three-axis acceleration value of the vehicle door at the current moment according to the three-axis angular velocity value of the vehicle door at the previous moment to obtain the three-axis acceleration filtering value of the vehicle door at the current moment also includes:

[0012] The three-axis acceleration value of the door at the current moment is filtered by the Kalman filter algorithm to obtain the three-axis acceleration filter value of the door at the current moment. The prediction and correction process is as follows:

[0013] predict:

[0014] x k =Ax k-1 +Bu k-1 (1)

[0015] P k =AP k-1 A T +Q(2)

[0016] Correction:

[0017] K k =P k H T (HP k H T +R) -1 (3)

[0018] x k =x k +K k (Z k -Hx k ) (4)

[0019] P k =(1-K k H)P k (5)

[0020] Formula (1) is the state prediction, where x k-1 is the three-axis acceleration filter value of the door at the last moment, x k is the predicted value of the three-axis acceleration of the door at the current moment, A is the state transfer matrix, the three-axis angular velocity value of the door at the previous moment is taken as the state transfer matrix A, B is the input control matrix, U k-1 is the state vector at the previous moment;

[0021] Formula (2) is the error matrix prediction, P k represents the error matrix at the current moment, P k-1 Represents the error matrix of the previous moment, A T represents the transposed matrix of the state transfer matrix A, Q is a diagonal matrix;

[0022] Formula (3) is the Kalman gain calculation, K k represents the observation matrix at the current moment, H is the measurement noise covariance matrix, and H T represents the transposed matrix of the measurement noise covariance matrix H, and R is the predicted noise covariance matrix;

[0023] Formula (4) is the state correction, and its output is the final three-axis acceleration filter value. The two x on the right side of formula (4) are k These are the predicted values ​​of the three-axis acceleration of the door at the current moment. k Refers to the three-axis acceleration filter value of the door at the current moment, Z k Refers to the Kalman gain at the current moment;

[0024] Formula (5) is the error matrix update.

[0025] Furthermore, the first posture information of the vehicle door at the current moment is calculated based on the three-axis acceleration filtering value of the vehicle door at the current moment, and further includes:

[0026] Define the rotation angles of the door around the Z, Y, and X axes as α, β, and γ respectively;

[0027] Then the rotation matrix M of the rotation angle α is z for:

[0028]

[0029] Then the rotation matrix M of the rotation angle β is y for:

[0030]

[0031] Then the rotation matrix M of the rotation angle γ is x for:

[0032]

[0033] The acceleration filter values ​​of the X, Y, and Z axes are a x 、a y 、a z , combined with the rotation matrix M z 、M y 、M x and gravitational acceleration g, we get equation (6):

[0034]

[0035] Solving equation (6), we can get the first roll angle information roll of the door at the current moment acc And the first pitch angle information pitch acc , where the first rolling angle information rollacc is the rotation angle γ, the first pitch angle information pitch acc is the rotation angle β. Since the gravity acceleration is constant when rotating around the Z axis, the three-axis accelerometer cannot calculate the heading angle information; then the first roll angle information roll acc And the first pitch angle information pitch acc The calculation formula is as follows:

[0036]

[0037] Furthermore, the method of calculating the second posture information of the vehicle door at the current moment according to the three-axis angular velocity values ​​of the vehicle door at the previous moment also includes:

[0038] Acquire the second roll angle information, second pitch angle information and heading angle information of the vehicle door at the last moment; wherein the heading angle information of the vehicle door at the last moment is the rotation angle α of the vehicle door around the Z axis; the second pitch angle information of the vehicle door at the last moment is the rotation angle β of the vehicle door around the Y axis; the second roll angle information of the vehicle door at the last moment is the rotation angle γ of the vehicle door around the X axis;

[0039] The three-axis angular velocity value of the door at the current moment is calculated according to the second roll angle information, the second pitch angle information, the heading angle information and the three-axis angular velocity value of the door at the previous moment, that is, equation (7):

[0040]

[0041] Among them, dγ / dt, dβ / dt, dα / dt are the current X-axis angular velocity value, Y-axis angular velocity value, and Z-axis angular velocity value of the door, respectively. x , g y , g z They are the angular velocity values ​​of the X, Y, and Z axes measured by the gyroscope at the last moment;

[0042] The three-axis angular velocity values ​​dγ / dt, dβ / dt, and dα / dt of the door at the current moment are integrated to obtain the three-axis angle changes Δγ, Δβ, and Δα between the current moment and the previous moment. The calculation formulas of the three-axis angle changes Δγ, Δβ, and Δα are as follows:

[0043]

[0044] Among them, Δt is the sampling time period between the current moment and the previous moment;

[0045] According to the second roll angle information γ, the second pitch angle information β and the heading angle information α of the door at the previous moment and the three-axis angle changes Δγ, Δβ and Δα of the door within the time Δt, the second roll angle information, the second pitch angle information and the heading angle information of the door at the current moment are calculated. Then, the calculation formulas of the second roll angle information roll(n+1), the second pitch angle information pitch(n+1) and the heading angle information yaw(n+1) of the door at the current moment are as follows:

[0046]

[0047] Among them, roll(n), pitch(n), and yaw(n) are respectively the second roll angle information γ, the second pitch angle information β, and the heading angle information α of the door at the last moment.

[0048] Furthermore, the step of fusing the first posture information and the second posture information to obtain the fused posture information of the vehicle door at the current moment further includes:

[0049] The first roll angle information and the second roll angle information are fused to obtain fused roll angle information of the door at the current moment;

[0050] The first pitch angle information and the second pitch angle information are fused to obtain fused pitch angle information of the door at the current moment;

[0051] Using the heading angle information of the vehicle door at the current moment as the fused heading angle information of the vehicle door at the current moment;

[0052] Then the fused roll angle information roll of the door at the current moment new , fusion pitch angle information new , fusion heading angle information yaw new The calculation formula is as follows:

[0053]

[0054] Among them, roll old is the fused roll angle information of the door at the last moment, pitch old is the fused pitch angle information of the door at the last moment, roll acc is the first roll angle information of the door at the current moment, pitch acc is the first pitch angle information of the door at the current moment, roll gyro is the second roll angle information of the door at the current moment, pitch gyro is the second pitch angle information of the door at the current moment, yaw gyro is the heading angle information of the door at the current moment, and K is the proportional coefficient;

[0055] The fused posture information of the vehicle door at the current moment includes the fused roll angle information, the fused pitch angle information and the fused heading angle information.

[0056] As a second aspect of the present invention, a door control system based on an IMU attitude algorithm and a Kalman filter is provided, comprising:

[0057] A three-axis accelerometer is used to obtain the three-axis acceleration value of the door at the current moment;

[0058] The gyroscope is used to obtain the three-axis angular velocity value of the door at the last moment;

[0059] A Kalman filter module, used for filtering the three-axis acceleration value of the vehicle door at a current moment according to the three-axis angular velocity value of the vehicle door at a previous moment, so as to obtain the three-axis acceleration filtering value of the vehicle door at the current moment;

[0060] A calculation module, used to calculate the first posture information of the vehicle door at the current moment according to the three-axis acceleration filter value of the vehicle door at the current moment, and calculate the second posture information of the vehicle door at the current moment according to the three-axis angular velocity value of the vehicle door at the previous moment; wherein the first posture information includes first roll angle information and first pitch angle information, and the second posture information includes second roll angle information, second pitch angle information and heading angle information;

[0061] A fusion module, used for fusing the first posture information and the second posture information to obtain fused posture information of the vehicle door at a current moment;

[0062] The control module is used to control the opening and closing and hovering of the door according to the fusion posture information of the door at the current moment.

[0063] Furthermore, the Kalman filter module is specifically used for:

[0064] The three-axis acceleration value of the door at the current moment is filtered by the Kalman filter algorithm to obtain the three-axis acceleration filter value of the door at the current moment. The prediction and correction process is as follows:

[0065] predict:

[0066] x k =Ax k-1 +Bu k-1 (1)

[0067] P k =AP k-1 A T +Q (2)

[0068] Correction:

[0069] K k =P kH T (HP k H T +R) -1 (3)

[0070] x k =x k +K k (Z k -Hx k ) (4)

[0071] P k =(1-K k H)P k (5)

[0072] Formula (1) is the state prediction, where x k-1 is the three-axis acceleration filter value of the door at the last moment, x k is the predicted value of the three-axis acceleration of the door at the current moment, A is the state transfer matrix, the three-axis angular velocity value of the door at the previous moment is taken as the state transfer matrix A, B is the input control matrix, U k-1 is the state vector at the previous moment;

[0073] Formula (2) is the error matrix prediction, P k represents the error matrix at the current moment, P k-1 Represents the error matrix of the previous moment, A T represents the transposed matrix of the state transfer matrix A, Q is a diagonal matrix;

[0074] Formula (3) is the Kalman gain calculation, K k represents the observation matrix at the current moment, H is the measurement noise covariance matrix, and H T represents the transposed matrix of the measurement noise covariance matrix H, and R is the predicted noise covariance matrix;

[0075] Formula (4) is the state correction, and its output is the final three-axis acceleration filter value. The two x on the right side of formula (4) are k These are the predicted values ​​of the three-axis acceleration of the door at the current moment. k Refers to the three-axis acceleration filter value of the door at the current moment, Z k Refers to the Kalman gain at the current moment;

[0076] Formula (5) is the error matrix update.

[0077] Furthermore, the calculation module is specifically used for:

[0078] Define the rotation angles of the door around the Z, Y, and X axes as α, β, and γ respectively;

[0079] Then the rotation matrix M of the rotation angle α isz for:

[0080]

[0081] Then the rotation matrix M of the rotation angle β is y for:

[0082]

[0083] Then the rotation matrix M of the rotation angle γ is x for:

[0084]

[0085] The acceleration filter values ​​of the X, Y, and Z axes are a x 、a y 、a z , combined with the rotation matrix M z 、M y 、M x and gravitational acceleration g, we get equation (6):

[0086]

[0087] Solving equation (6), we can get the first roll angle information roll of the door at the current moment acc And the first pitch angle information pitch acc , where the first rolling angle information roll acc is the rotation angle γ, the first pitch angle information pitch acc is the rotation angle β. Since the gravity acceleration is constant when rotating around the Z axis, the three-axis accelerometer cannot calculate the heading angle information; then the first roll angle information roll acc And the first pitch angle information pitch acc The calculation formula is as follows:

[0088]

[0089] Furthermore, the calculation module is specifically used for:

[0090] Acquire the second roll angle information, second pitch angle information and heading angle information of the vehicle door at the last moment; wherein the heading angle information of the vehicle door at the last moment is the rotation angle α of the vehicle door around the Z axis; the second pitch angle information of the vehicle door at the last moment is the rotation angle β of the vehicle door around the Y axis; the second roll angle information of the vehicle door at the last moment is the rotation angle γ of the vehicle door around the X axis;

[0091] The three-axis angular velocity value of the door at the current moment is calculated according to the second roll angle information, the second pitch angle information, the heading angle information and the three-axis angular velocity value of the door at the previous moment, that is, equation (7):

[0092]

[0093] Among them, dγ / dt, dβ / dt, dα / dt are the current X-axis angular velocity value, Y-axis angular velocity value, and Z-axis angular velocity value of the door, respectively. x , g y , g z They are the angular velocity values ​​of the X, Y, and Z axes measured by the gyroscope at the last moment;

[0094] The three-axis angular velocity values ​​dγ / dt, dβ / dt, and dα / dt of the door at the current moment are integrated to obtain the three-axis angle changes Δγ, Δβ, and Δα between the current moment and the previous moment. The calculation formulas of the three-axis angle changes Δγ, Δβ, and Δα are as follows:

[0095]

[0096] Among them, Δt is the sampling time period between the current moment and the previous moment;

[0097] According to the second roll angle information γ, the second pitch angle information β and the heading angle information α of the door at the previous moment and the three-axis angle changes Δγ, Δβ and Δα of the door within the time Δt, the second roll angle information, the second pitch angle information and the heading angle information of the door at the current moment are calculated. Then, the calculation formulas of the second roll angle information roll(n+1), the second pitch angle information pitch(n+1) and the heading angle information yaw(n+1) of the door at the current moment are as follows:

[0098]

[0099] Among them, roll(n), pitch(n), and yaw(n) are respectively the second roll angle information γ, the second pitch angle information β, and the heading angle information α of the door at the last moment.

[0100] Furthermore, the fusion module is specifically used for:

[0101] The first roll angle information and the second roll angle information are fused to obtain fused roll angle information of the door at the current moment;

[0102] The first pitch angle information and the second pitch angle information are fused to obtain fused pitch angle information of the door at the current moment;

[0103] Using the heading angle information of the vehicle door at the current moment as the fused heading angle information of the vehicle door at the current moment;

[0104] Then the fused roll angle information roll of the door at the current moment new , fusion pitch angle information new , fusion heading angle information yaw new The calculation formula is as follows:

[0105]

[0106] Among them, roll old is the fused roll angle information of the door at the last moment, pitch old is the fused pitch angle information of the door at the last moment, roll acc is the first roll angle information of the door at the current moment, pitch acc is the first pitch angle information of the door at the current moment, roll gyro is the second roll angle information of the door at the current moment, pitch gyro is the second pitch angle information of the door at the current moment, yaw gyro is the heading angle information of the door at the current moment, and K is the proportional coefficient;

[0107] The fused posture information of the vehicle door at the current moment includes the fused roll angle information, the fused pitch angle information and the fused heading angle information.

[0108] The door control method and system based on IMU attitude algorithm and Kalman filter provided by the present invention have the following advantages: when the vehicle is on a slope, the slope hovering function is realized when the door does not drift, and the manual door opening and closing resistance is consistent on different slopes. When there is vibration interference when the vehicle engine is running, the IMU attitude algorithm is fitted by relying on Kalman filter, so that the IMU attitude fitting error is low, the vehicle vibration noise can be effectively filtered out, and the interference can be effectively reduced, thereby ensuring that the door attitude output is stable and reliable. BRIEF DESCRIPTION OF THE DRAWINGS

[0109] The accompanying drawings are used to provide further understanding of the present invention and constitute a part of the specification. Together with the following specific embodiments, they are used to explain the present invention, but do not constitute a limitation of the present invention.

[0110] Figure 1 The present invention is a flow chart of a vehicle door control method based on an IMU attitude algorithm and a Kalman filter.

[0111] Figure 2 Schematic diagram of three rotations of the IMU coordinate system of the present invention.

[0112] Figure 3This is a diagram showing the actual meanings of the roll angle roll, pitch angle pitch, and yaw angle of the present invention.

[0113] Figure 4 It is a schematic diagram of the two-dimensional rotation of the plane of the present invention.

[0114] Figure 5A-5C It is a schematic diagram of the rotation about the Z, Y, and X axes in the three-dimensional rotation of the present invention.

[0115] Figure 6 It is a schematic diagram of the rotation of the unit matrix of the present invention.

[0116] Figure 7 The present invention corresponds to Figure 4 Schematic diagram of the reverse rotation.

[0117] Figure 8 It is a schematic diagram of the decomposition of the three rotation processes in the accelerometer of the present invention for calculating the attitude angle.

[0118] Fig. 9 The figure is a schematic diagram of the calculation process of attitude update in the attitude angle calculation of the gyroscope of the present invention.

[0119] Fig.10 The figure is a schematic diagram of the decomposition of the three rotation processes in the attitude angle calculation of the gyroscope of the present invention. DETAILED DESCRIPTION

[0120] In order to further explain the technical means and effects adopted by the present invention to achieve the predetermined invention purpose, the specific implementation method, structure, characteristics and effects of the door control method and system based on IMU attitude algorithm and Kalman filter proposed by the present invention are described in detail below in combination with the accompanying drawings and preferred embodiments. Obviously, the described embodiments are part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work belong to the protection scope of the present invention.

[0121] In this embodiment, a door control method based on IMU attitude algorithm and Kalman filter is provided. Figure 1 As shown, the door control method based on IMU attitude algorithm and Kalman filter includes:

[0122] Step S1: obtaining the three-axis acceleration value of the door at the current moment through the three-axis accelerometer, and obtaining the three-axis angular velocity value of the door at the previous moment through the gyroscope;

[0123] Step S2: filtering the three-axis acceleration value of the vehicle door at the current moment according to the three-axis angular velocity value of the vehicle door at the previous moment, so as to obtain the three-axis acceleration filtering value of the vehicle door at the current moment;

[0124] Kalman filtering is a linear filtering and prediction method, which is divided into two steps, prediction and correction. Prediction is to estimate the current state based on the state at the previous moment, while correction is to combine the estimated state at the current moment with the observed state to estimate the optimal state.

[0125] Preferably, filtering the three-axis acceleration value of the vehicle door at the current moment according to the three-axis angular velocity value of the vehicle door at the previous moment to obtain the three-axis acceleration filtering value of the vehicle door at the current moment also includes:

[0126] The three-axis acceleration value of the door at the current moment is filtered by the Kalman filter algorithm to obtain the three-axis acceleration filter value of the door at the current moment. The prediction and correction process is as follows:

[0127] predict:

[0128] x k =Ax k-1 +Bu k-1 (1)

[0129] P k =AP k-1 A T +Q (2)

[0130] Correction:

[0131] K k =P k H T (HP k H T +R) -1 (3)

[0132] x k =x k +K k (Z k -Hx k ) (4)

[0133] P k =(1-K k H)P k (5)

[0134] Formula (1) is the state prediction, where x k-1 is the three-axis acceleration filter value of the door at the last moment, x k is the predicted value of the three-axis acceleration of the door at the current moment, A is the state transfer matrix, the three-axis angular velocity value of the door at the previous moment is taken as the state transfer matrix A, B is the input control matrix, U k-1 is the state vector at the previous moment;

[0135] Formula (2) is the error matrix prediction, P k represents the error matrix at the current moment, P k-1 Represents the error matrix of the previous moment, A T represents the transposed matrix of the state transfer matrix A, Q is a diagonal matrix;

[0136] Formula (3) is the Kalman gain calculation, K k represents the observation matrix at the current moment, H is the measurement noise covariance matrix, and H T represents the transposed matrix of the measurement noise covariance matrix H, and R is the predicted noise covariance matrix;

[0137] Formula (4) is the state correction, and its output is the final three-axis acceleration filter value. The two x on the right side of formula (4) are k These are the predicted values ​​of the three-axis acceleration of the door at the current moment. k Refers to the three-axis acceleration filter value of the door at the current moment, Z k Refers to the Kalman gain at the current moment;

[0138] Formula (5) is the error matrix update.

[0139] Kalman filter formula is imported into IMU attitude calculation:

[0140] IMU model

[0141] The principle of using Kalman filtering to fit acceleration and angular velocity values ​​as IMU attitude calculation:

[0142] The accelerometer outputs x, y, and z axis acceleration values ​​as observation values.

[0143] The gyroscope outputs the angular velocity values ​​of the three axes x, y, and z as the state transfer matrix.

[0144] The output values ​​of the accelerometer and gyroscope are the projections of the inertial force vector on the x, y, and z axes, reflecting the attitude rotation (roll / pitch).

[0145] The IMU attitude is a rotation matrix, which can also represent a coordinate system and a set of orthogonal bases.

[0146] Step 1 (first equation):

[0147] The gyroscope output angular velocity is used as the control variable input, the accelerometer output value is used as the observation value, and the system is represented by a differential equation to model the state vector:

[0148] x k =Ax k-1 +Bu k +w k

[0149] Where x is the state vector of the system, with size n*1 columns. A is the transition matrix, with size n*n. u is the system input, with size k*1. B is the matrix that converts input to state, with size n*k. The random variable w is the system noise. The size of the matrix is ​​closely related to the actual programming.

[0150] Convert to a matrix:

[0151]

[0152] Note: Modeling should be accurate and model predictions should be as close to the true value as possible. If modeling is inaccurate, the overall filtering result will lag behind the true value.

[0153] Step 2 (second equation):

[0154] The prediction equation of the error covariance matrix P is:

[0155] P k|k-1 =AP k-1|k-1 A T +Q k

[0156] Derivation process: Assuming the true value is x, expressed by the model, the 0 input is:

[0157] x k =[θ,0]T

[0158] x k =Ax k-1 +w

[0159]

[0160]

[0161] Considering the effect of system noise on the true value, P, by definition, the covariance of the error between the true value and the estimated value, P k It is derived as:

[0162]

[0163] Omega follows a normal distribution. According to the properties of the covariance matrix, P k It is derived as:

[0164] P k =E[(Ae k-1 )(Ae k-1 ) T ]+E[w k w k T ]=AP k-1 AT +Q

[0165] Note: The initial value of P has no effect on the result and will converge with iteration. In the current example, Q should be a diagonal matrix. The angle error and the angular velocity deviation error are not related, and the value on the secondary diagonal is 0. The main diagonal of Q is the variance of the two variables. If the predicted value is less reliable (the model noise is large), the P value will be larger (the actual value of P does not necessarily increase. Here, the larger the value is, the more reliable the model iteration is). P needs a true value, which is unknown and cannot be calculated from the definition.

[0166] Step 3 (third-party program):

[0167]

[0168]

[0169]

[0170] K k =P k ′H T (HP k ′H T +R) -1

[0171] Update K according to P k , it is derived as:

[0172]

[0173] Step 4 (fourth equation):

[0174]

[0175] The difference equation uses the drift value:

[0176]

[0177] Note: The drift value follows a normal distribution. The angular velocity of the gyroscope read in a stationary state is the drift value. When the differential equation is used, the drift value should be stable when the input is zero, and the residual should continue to converge to 0.

[0178] Step 5 (fifth equation):

[0179]

[0180] K k =P k H T (HP k H T +R) -1 The import is derived as:

[0181] P k|k =(1-K k H)P k|k-1

[0182] The meanings of the symbols in the above Kalman filter formula are as follows:

[0183] Table 1

[0184]

[0185]

[0186] Step S3: calculating the first posture information of the vehicle door at the current moment according to the three-axis acceleration filtering value of the vehicle door at the current moment, and calculating the second posture information of the vehicle door at the current moment according to the three-axis angular velocity value of the vehicle door at the previous moment; wherein the first posture information includes first roll angle information and first pitch angle information, and the second posture information includes second roll angle information, second pitch angle information and heading angle information;

[0187] IMU attitude introduction:

[0188] It should be noted that the inertial measurement unit IMU, also known as a 6D sensor, generally includes a three-axis gyroscope and a three-axis accelerometer. Through software calculation, the Euler angle and rotation matrix are used to solve the attitude of the raw data of the gyroscope and accelerometer, and the two attitudes are complementarily fused to finally obtain the real-time attitude of the IMU.

[0189] The rotation order used for attitude solution is ZYX, that is, the IMU coordinate system coincides with the earth coordinate system at the initial moment, and then rotates around its own Z, Y, and X axes in turn. The schematic diagram of the three rotations is as follows Figure 2 As shown, the actual meanings of roll angle roll, pitch angle pitch, and heading angle yaw are as follows Figure 3 Here we first customize the name and symbol of each rotation:

[0190] Rotation around the IMU’s Z axis: heading angle yaw, rotation angle α.

[0191] Rotation around the Y axis of the IMU: pitch angle, rotation angle β.

[0192] Rotation around the IMU’s X-axis: roll angle, rotation angle γ.

[0193] In this embodiment, the 6D sensor may be LSM6DSL produced by STMicroelectronics.

[0194] In this embodiment, the posture solution adopts software methods: axis-angle method and quaternion solution.

[0195] Three-axis gyroscope:

[0196] Sensitivity: The three-axis gyroscope measures angular velocity and has high dynamic characteristics. The key parameter is gyrosensitivity (its unit is millivolts per degree per second, which converts the rotation speed into voltage value). The smaller the measurement range, the higher the sensitivity. The gyroscope measures the derivative of the angle, that is, the angular velocity, and the angle is obtained by integrating the angular velocity over time. Each channel of the gyroscope detects the rotation of one axis and calculates the rotation direction and angle through the deviation from the initial direction.

[0197] Zero offset: The gyroscope will have an output value when it is not rotating. The size of this output value is related to the supply voltage and temperature. This offset can be corrected by measuring and calibrating for a short period of time when the gyroscope is powered on.

[0198] Drift: It is the result of the interaction between offset and noise over time. For example, if the offset is 0.1 dps, the output value is 1 dps. After integrating for one second, the angle is 1°, and after one minute it is 60°. In other words, the gyroscope has great reference value in a short time, but long-term observation will cause large errors, so software compensation calibration correction is required in attitude solution.

[0199] Triaxial accelerometer:

[0200] The accelerometer has good low-frequency characteristics, so the instantaneous acceleration can be ignored, and the low-speed static acceleration can be measured more accurately. The acceleration measured by the accelerometer needs to be measured by specific force (specific force equation, "Inertial Navigation" Qin Yongyuan), rather than directly measuring the acceleration. The accelerometer only measures the components of gravity acceleration on the three axes under the airborne coordinates, and the gravity acceleration is fixed to the R coordinate system (EarthFrame). Through this relationship, the angle relationship between the plane where the accelerometer is located and the ground can be obtained.

[0201] Note: If the accelerometer rotates around the axis of gravity acceleration, the measured value will not change, but the accelerometer cannot sense horizontal rotation, that is, the accelerometer's imported absolute heading angle yaw (geo-coordinate axis) needs to be solved with an electronic compass (magnetometer).

[0202] Rotation matrix:

[0203] Planar 2D rotation

[0204] like Figure 4 As shown, in the XY coordinate system, the vector OP rotates by an angle of β to the position of OP'. According to the relationship between trigonometric functions, the coordinate representation of the vectors OP and OP' can be listed:

[0205]

[0206]

[0207] Compare the above two formulas and expand the second formula:

[0208]

[0209] Re-expressed in matrix form:

[0210]

[0211] This is the basic form of two-dimensional rotation. The matrix in the middle is the rotation matrix of the two-dimensional rotation. After a vector in the coordinates is multiplied by the matrix on the left, the coordinates of the vector after being rotated by an angle of β are obtained.

[0212] ·3D rotation

[0213] Three-dimensional rotation can be understood with the help of two-dimensional rotation. Since any axis can be rotated in three-dimensional space, for the convenience of analysis and use, only rotation around the X, Y, and Z axes is considered.

[0214] ①Around the Z axis

[0215] Refer to the above Figure 4 , add a Z axis, then the above two-dimensional rotation is actually a three-dimensional rotation around the Z axis, such as Figure 5A shown.

[0216] According to the derivation formula of the two-dimensional rotation matrix, and adding the transformation relationship of the Z coordinate (actually there is no change), it is rewritten into a matrix form to obtain the rotation matrix around the Z axis.

[0217]

[0218]

[0219] ②Around the Y axis

[0220] The same is true for rotation around the Y axis, such as Figure 5B As shown, the sign of the coordinate axis is changed directly here, and the coordinate order must conform to the right-hand system. The final matrix form should be further rewritten in the order of XYZ to obtain the rotation matrix around the Y axis as follows:

[0221]

[0222]

[0223] ③Around the X axis

[0224] Refer to the derivation around the Y axis, such as Figure 5CAs shown, the rotation matrix around the X axis can be obtained.

[0225]

[0226]

[0227] It should be noted that for the unit matrix, which column does not need to be changed when rotating around which axis, and then replace the corresponding 4 positions of the two-dimensional rotation matrix. Note that the rotation matrix around Y is different from the other two, and its -sinβ is in the lower left, such as Figure 6 shown.

[0228] Reverse rotation

[0229] If it is rotated in the opposite direction by an angle, such as Figure 7 As shown, the derivation process is similar:

[0230] The final rotation matrix is ​​actually the inverse matrix of the forward rotation matrix. Since this matrix is ​​an orthogonal matrix, the inverse matrix is ​​the transposed matrix.

[0231]

[0232]

[0233]

[0234] The coordinates of the vectors above are written in column form. If they are expressed in row form, the rotation matrix is ​​transposed and the matrix is ​​multiplied to the right of the row vector.

[0235]

[0236] Specifically, the first posture information of the vehicle door at the current moment is calculated based on the three-axis acceleration filtering value of the vehicle door at the current moment, and further includes:

[0237] According to the previously defined rotation angles α, β, and γ of the door around the Z, Y, and X axes, its rotation matrix M z 、M y 、M x They are:

[0238] α: heading angle yaw:

[0239] β: Pitch angle

[0240] γ: Roll angle roll:

[0241] Euler angle rotation:

[0242] Euler angles are a method of describing three-dimensional rotations. The calculation of Euler angles requires the use of a rotation matrix.

[0243] Static definition

[0244] For a reference system in three-dimensional space, the orientation of any coordinate system can be expressed by three Euler angles.

[0245] ①The reference system is also called the laboratory reference system. It is stationary and can be simply understood as the geodetic coordinate system, also called the inertial coordinate system.

[0246] ②The coordinate system is fixed to the rigid body and rotates with the rotation of the rigid body, such as the coordinate system of the aircraft itself, also called the carrier coordinate system.

[0247] Dynamic definition

[0248] Two different dynamic definitions of Euler angles.

[0249] ① A compound of three rotations around the coordinate axes fixed to the carrier;

[0250] ②The combination of three rotations around the reference axis of the geodetic coordinate system.

[0251] Using the dynamic definition, it is easy to understand the physical meaning and application of Euler angles.

[0252] In the following description, the uppercase XYZ coordinate axes are rotating carrier coordinate axes; the lowercase xyz coordinate axes are stationary earth reference axes.

[0253] Now let's describe the two dynamic definitions of Euler angles in the order of rotation: Z, Y, X.

[0254] Definition A: Rotation around the XYZ coordinate axis (carrier coordinate axis):

[0255] Initially, the axes of both coordinate systems xyz and XYZ overlap.

[0256] Start by rotating by an angle α around the Z-axis.

[0257] Then, rotate by an angle β around the Y-axis.

[0258] Finally, rotate by angle γ around the X-axis.

[0259] Suppose the coordinates of any point P1 in the xyz and XYZ coordinate systems are r1 and R1 respectively. Define Z(α) as a rotation of α around the Z-axis, Y(β) as a rotation of β around the Y-axis, and X(γ) as a rotation of γ around the X-axis. The rotation matrix is ​​derived and expressed as follows:

[0260]

[0261] Note: There is a concept of matrix left multiplication and right multiplication here. Rotation around the carrier coordinate system is the matrix left multiplication in sequence, that is, X<-Y<-Z.

[0262] Definition B: Rotation around the xyz coordinate axis (geodetic coordinate axis):

[0263] Initially, the axes of both coordinate systems xyz and XYZ overlap.

[0264] Start by rotating by an angle α around the z-axis.

[0265] Then, rotate by an angle β around the y-axis.

[0266] Finally, rotate by an angle γ around the x-axis.

[0267] Suppose the coordinates of any point P2 in the xyz and XYZ coordinate systems are r2 and R2 respectively. Define z(α) as a rotation of α around the z-axis, y(β) as a rotation of β around the y-axis, and x(γ) as a rotation of γ around the x-axis. Then Definition B can be expressed as follows:

[0268]

[0269] Note: Rotation around the geodetic coordinate system is to right-multiply the matrix in sequence, i.e. z->y->x.

[0270] The equality of definition A and definition B can be proved using the rotation matrix:

[0271] Assuming r1 and r2 are equal, we have:

[0272] r 1 =z(α)·y(β)·x(γ)·R 2 With the help of the inverse matrix, it can be deduced as:

[0273] x -1 (γ)·y -1 (β)·z -1 (α)·r 1 =x -1 (γ)·y -1 (β)·z -1 (α)·z(α)·y(β)·x(γ)·R 2

[0274] Both: x -1 (γ)·y -1 (β)·z -1 (α)·r 1 =R 2

[0275] because: So: X(γ)·Y(β)·Z(α)·r 1 =R 2

[0276] It has been proved that R 1 =R 2 , that is, Definition A and Definition B are equivalent.

[0277] Accelerometer calculates attitude angle:

[0278] The accelerometer measures the acceleration it feels. When it is stationary, it does not accelerate. However, due to the acceleration of gravity, according to the theory of relative motion, the acceleration it feels is exactly opposite to the acceleration of gravity, that is, the data read is vertically upward. The initial letter a is used below to represent accelerometer data.

[0279] Acceleration uses the gravity acceleration felt at rest to calculate the posture:

[0280] ① When the accelerometer is placed horizontally, that is, the Z axis is vertically upward, the Z axis can read a value of 1g (g is the acceleration of gravity), and the X-axis and Y-axis directions read 0, which can be recorded as (0, 0, g).

[0281] ②When the accelerometer rotates to a certain posture, the gravitational acceleration will produce corresponding components on the three axes of acceleration. Its essence is the coordinates of (0, 0, g) in the earth coordinate system in the new accelerometer's own coordinate system. The three values ​​read by the accelerometer are the new coordinates of the (0, 0, g) vector.

[0282] The rotation of the posture adopts the three-rotation method in the order of ZYX. The decomposition process of the three-rotation process is as follows Figure 8 As shown, the above description can be expressed as:

[0283] The acceleration filter values ​​of the X, Y, and Z axes are a x 、a y 、a z , combined with the rotation matrix M z 、M y 、M x and gravitational acceleration g, we get equation (6):

[0284]

[0285] Solving equation (6), we can get the first roll angle information roll of the door at the current moment acc And the first pitch angle information pitch acc , where the first rolling angle information roll acc is the rotation angle γ, the first pitch angle information pitch accis the rotation angle β. Since the gravity acceleration is constant when rotating around the Z axis, the three-axis accelerometer cannot calculate the heading angle information; then the first roll angle information roll acc And the first pitch angle information pitch acc The calculation formula is as follows:

[0286]

[0287] Gyroscope calculates attitude angle:

[0288] The gyroscope measures the angular velocity of rotation around three axes. Therefore, by integrating the angular velocity, the angle can be obtained. The English abbreviation of gyroscope is gyro, and the initials g are used below to represent gyroscope data.

[0289] like Fig. 9 As shown in the figure, the attitude angles of the IMU at the nth moment are γ, β, and α, which means that the IMU coordinate system is rotated from the initial position by α angle around Z, β angle around Y, and γ angle around X to obtain the final attitude. At this time, it is necessary to calculate the attitude of the next moment (n+1). Suppose the attitude angles at the n+1 moment are γ+Δγ, β+Δβ, and α+Δα. This attitude also undergoes 3 rotations. To calculate the attitude at the n+1 moment, just add the corresponding attitude angle change based on the attitude at the n moment. The change in attitude angle can be obtained by integrating the angular velocity and the sampling time period.

[0290] Here, the angular velocities such as dγ / dt are actually imaginary angular velocities used for attitude update. The attitude update is based on the earth coordinate system, while the angular velocity read by the gyroscope in the nth state is based on its own coordinate system. The gyro data read needs to be transformed before it can be used to calculate and update the attitude for the n+1th time.

[0291] Specifically, the calculating the second posture information of the door at the current moment according to the three-axis angular velocity values ​​of the door at the previous moment also includes:

[0292] Acquire the second roll angle information, second pitch angle information and heading angle information of the vehicle door at the last moment; wherein the heading angle information of the vehicle door at the last moment is the rotation angle α of the vehicle door around the Z axis; the second pitch angle information of the vehicle door at the last moment is the rotation angle β of the vehicle door around the Y axis; the second roll angle information of the vehicle door at the last moment is the rotation angle γ of the vehicle door around the X axis;

[0293] The three-axis angular velocity value of the door at the current moment is calculated according to the second roll angle information, the second pitch angle information, the heading angle information and the three-axis angular velocity value of the door at the previous moment, that is, equation (7):

[0294]

[0295] Among them, dγ / dt, dβ / dt, and dα / dt are the current X-axis angular velocity value, Y-axis angular velocity value, and Z-axis angular velocity value of the door, that is, the angular velocity required for attitude update, g x , g y , g z They are the angular velocity values ​​of the X, Y, and Z axes measured by the gyroscope at the last moment, that is, the gyroscope output values ​​read;

[0296] The three-axis angular velocity values ​​dγ / dt, dβ / dt, and dα / dt of the door at the current moment are integrated to obtain the three-axis angle changes Δγ, Δβ, and Δα between the current moment and the previous moment. The calculation formulas of the three-axis angle changes Δγ, Δβ, and Δα are as follows:

[0297]

[0298] Among them, Δt is the sampling time period between the current moment and the previous moment;

[0299] According to the second roll angle information γ, second pitch angle information β and heading angle information α of the door at the previous moment and the three-axis angle changes Δγ, Δβ, Δα of the door within time Δt, the second roll angle information, second pitch angle information and heading angle information of the door at the current moment are calculated. Then, the calculation formula of the second roll angle information roll(n+1), the second pitch angle information pitch(n+1) and the heading angle information yaw(n+1) of the door at the current moment, that is, the attitude update formula is as follows:

[0300]

[0301] Among them, roll(n), pitch(n), and yaw(n) are respectively the second roll angle information γ, the second pitch angle information β, and the heading angle information α of the door at the last moment.

[0302] like Fig.10 As shown, we first analyze dα / dt, which is the angular velocity of the yaw angle around the Z axis during the three rotations. The first of the three rotations is a rotation around the Z axis. The unit vector in the Z axis direction can be expressed as

[001] T, where T represents vector transpose. Therefore, [00dα / dt]T represents the angular velocity around Z in the coordinates of state ① in the figure.

[0303] Since the coordinate system will undergo two rotations around Y and X, the angular velocity [00dα / dt]T will also undergo two transformations in the final coordinate system (state ③) after undergoing two rotations. T represents the equivalent angular velocity of the yaw angle around the Z axis in the final posture during the three rotations. It is actually the new coordinate of the angular velocity around the Z axis in the state ① coordinate system in the state ③ coordinate system.

[0304] Similarly, dβ / dt needs to undergo one rotation transformation, while dγ / dt does not need to undergo rotation.

[0305] Transform dα / dt, dβ / dt, and dγ / dt to new coordinates in the state ③ coordinate system and add them together, which is actually the gyro data read by the gyroscope itself at the moment of state ③.

[0306] The conversion relationship from angular velocity dγ / dt to angular velocity gx read by the gyroscope is derived as follows:

[0307]

[0308] Furthermore, if we understand state ③ as state n, then we can inverse the angular velocity data such as dy / dt according to the gyroscope data read at state n to update the attitude of state n+1. The inverse solution is to find the inverse matrix, that is:

[0309]

[0310] Posture Fusion:

[0311] Step S4: fusing the first posture information and the second posture information to obtain the fused posture information of the vehicle door at the current moment;

[0312] Preferably, the fusing the first posture information and the second posture information to obtain the fused posture information of the vehicle door at the current moment also includes:

[0313] The first roll angle information and the second roll angle information are fused to obtain fused roll angle information of the door at the current moment;

[0314] The first pitch angle information and the second pitch angle information are fused to obtain fused pitch angle information of the door at the current moment;

[0315] Using the heading angle information of the vehicle door at the current moment as the fused heading angle information of the vehicle door at the current moment;

[0316] From the above analysis, we can know that the accelerometer can calculate the roll and pitch angles based on the gravity acceleration it feels when it is stationary, and the angle calculation is only related to the current attitude. The gyroscope integrates the angular velocity within the time interval, obtains the angle change each time, and adds it to the previous attitude angle to obtain the new attitude angle. The gyroscope can calculate the three angles of roll, pitch, and yaw.

[0317] In fact, the acceleration can only obtain a relatively accurate attitude at the stationary moment, while the gyroscope is only sensitive to the attitude change during rotation. If the gyroscope itself has errors, the errors will continue to increase after continuous time integration. Therefore, it is necessary to combine the attitudes calculated by the two and perform complementary fusion. Of course, only roll and pitch can be fused because the accelerometer does not obtain yaw.

[0318] Then the fused roll angle information roll of the door at the current moment new , fusion pitch angle information new , fusion heading angle information yaw new The calculation formula is as follows:

[0319]

[0320] Among them, roll old is the fused roll angle information of the door at the last moment, pitch old is the fused pitch angle information of the door at the last moment, roll acc is the first roll angle information of the door at the current moment, pitch acc is the first pitch angle information of the door at the current moment, roll gyro is the second roll angle information of the door at the current moment, pitch gyro is the second pitch angle information of the door at the current moment, yaw gyro is the current heading angle information of the door, K is the proportional coefficient, which needs to be adjusted according to the actual situation, such as 1.5;

[0321] The fused posture information of the vehicle door at the current moment includes the fused roll angle information, the fused pitch angle information and the fused heading angle information.

[0322] Quaternion to Euler angle conversion: This is a method used in actual products to express algorithms through code.

[0323] Among the sensors, the gyroscope is the protagonist, and the accelerometer and magnetic sensor only play an auxiliary correction role. Among them, the accelerometer cannot correct the heading angle, and a magnetometer is needed to correct the heading angle. In the commonly used coordinate system, drones generally use the following two coordinate systems: the reference coordinate system is the n system (geographic coordinate system), and the carrier coordinate system is the b system (airborne coordinate system). In the attitude solution process, there are many ways to represent the attitude, such as Euler angles, quaternions, and DCM (direction cosines), each with its own advantages, and the more commonly used is quaternions.

[0324] Comparison of attitude solution methods:

[0325]

[0326] Quaternions can conveniently represent rotations in 3D space, but their concept is not easy to understand. We can first compare them to complex numbers, which actually represent rotations in a 2D plane.

[0327] The basic representation of quaternion is: q0+q1*i+q2*j+q3*k, that is, 1 real part and 3 imaginary parts. For details, see the introduction to the Runge-Kutta method.

[0328] Although quaternions are convenient for representing rotations, their form is not very intuitive and needs to be converted into pitch, roll, and yaw representations to facilitate observation of posture.

[0329] The conversion formula is:

[0330]

[0331] Step S5: Control the opening, closing and hovering of the door according to the fused posture information of the door at the current moment.

[0332] The door control method based on IMU attitude algorithm and Kalman filtering provided by the present invention is applied to the automobile electric sliding door system or electric side door system, and adopts a 6D sensor (inertial measurement unit IMU) to obtain the current attitude information of the door through an algorithm, mainly obtaining the roll angle roll, pitch angle pitch and heading angle yaw of the door; according to the current attitude information of the door (carrier coordinate axis), the door opening value and the opening value change are sensed to restart the electric power assistance when the door is manually opened and closed; according to the current attitude information of the door (geodetic coordinate axis) and the calibrated value, the door is controlled to hover according to the comparison result to prevent the door from accidentally opening or closing due to its own gravity when it is unlocked at different slopes and different door openings (both the vehicle driving state and the stationary state can be detected).

[0333] As a second embodiment of the present invention, a door control system based on an IMU attitude algorithm and a Kalman filter is provided, comprising:

[0334] A three-axis accelerometer is used to obtain the three-axis acceleration value of the door at the current moment;

[0335] The gyroscope is used to obtain the three-axis angular velocity value of the door at the last moment;

[0336] A Kalman filter module, used for filtering the three-axis acceleration value of the vehicle door at a current moment according to the three-axis angular velocity value of the vehicle door at a previous moment, so as to obtain the three-axis acceleration filtering value of the vehicle door at the current moment;

[0337] A calculation module, used to calculate the first posture information of the vehicle door at the current moment according to the three-axis acceleration filter value of the vehicle door at the current moment, and calculate the second posture information of the vehicle door at the current moment according to the three-axis angular velocity value of the vehicle door at the previous moment; wherein the first posture information includes first roll angle information and first pitch angle information, and the second posture information includes second roll angle information, second pitch angle information and heading angle information;

[0338] A fusion module, used for fusing the first posture information and the second posture information to obtain fused posture information of the vehicle door at a current moment;

[0339] The control module is used to control the opening and closing and hovering of the door according to the fusion posture information of the door at the current moment.

[0340] Preferably, the Kalman filter module is specifically used for:

[0341] The three-axis acceleration value of the door at the current moment is filtered by the Kalman filter algorithm to obtain the three-axis acceleration filter value of the door at the current moment. The prediction and correction process is as follows:

[0342] predict:

[0343] x k =Ax k-1 +Bu k-1 (1)

[0344] P k =AP k-1 A T +Q (2)

[0345] Correction:

[0346] K k =P k H T (HP k H T +R) -1 (3)

[0347] x k =x k +K k (Z k -Hx k ) (4)

[0348] P k =(1-K k H)P k (5)

[0349] Formula (1) is the state prediction, where x k-1 is the three-axis acceleration filter value of the door at the last moment, x kis the predicted value of the three-axis acceleration of the door at the current moment, A is the state transfer matrix, the three-axis angular velocity value of the door at the previous moment is taken as the state transfer matrix A, B is the input control matrix, U k-1 is the state vector at the previous moment;

[0350] Formula (2) is the error matrix prediction, P k represents the error matrix at the current moment, P k-1 Represents the error matrix of the previous moment, A T represents the transposed matrix of the state transfer matrix A, Q is a diagonal matrix;

[0351] Formula (3) is the Kalman gain calculation, K k represents the observation matrix at the current moment, H is the measurement noise covariance matrix, and H T represents the transposed matrix of the measurement noise covariance matrix H, and R is the predicted noise covariance matrix;

[0352] Formula (4) is the state correction, and its output is the final three-axis acceleration filter value. The two x on the right side of formula (4) are k These are the predicted values ​​of the three-axis acceleration of the door at the current moment. k Refers to the three-axis acceleration filter value of the door at the current moment, Z k Refers to the Kalman gain at the current moment;

[0353] Formula (5) is the error matrix update.

[0354] Preferably, the calculation module is specifically used for:

[0355] Define the rotation angles of the door around the Z, Y, and X axes as α, β, and γ respectively;

[0356] Then the rotation matrix M of the rotation angle α is z for:

[0357]

[0358] Then the rotation matrix M of the rotation angle β is y for:

[0359]

[0360] Then the rotation matrix M of the rotation angle γ is x for:

[0361]

[0362] The acceleration filter values ​​of the X, Y, and Z axes are a x 、a y 、a z , combined with the rotation matrix Mz 、M y 、M x and gravitational acceleration g, we get equation (6):

[0363]

[0364] Solving equation (6), we can get the first roll angle information roll of the door at the current moment acc And the first pitch angle information pitch acc , where the first rolling angle information roll acc is the rotation angle γ, the first pitch angle information pitch acc is the rotation angle β. Since the gravity acceleration is constant when rotating around the Z axis, the three-axis accelerometer cannot calculate the heading angle information; then the first roll angle information roll acc And the first pitch angle information pitch acc The calculation formula is as follows:

[0365]

[0366] Preferably, the calculation module is specifically used for:

[0367] Acquire the second roll angle information, second pitch angle information and heading angle information of the vehicle door at the last moment; wherein the heading angle information of the vehicle door at the last moment is the rotation angle α of the vehicle door around the Z axis; the second pitch angle information of the vehicle door at the last moment is the rotation angle β of the vehicle door around the Y axis; the second roll angle information of the vehicle door at the last moment is the rotation angle γ of the vehicle door around the X axis;

[0368] The three-axis angular velocity value of the door at the current moment is calculated according to the second roll angle information, the second pitch angle information, the heading angle information and the three-axis angular velocity value of the door at the previous moment, that is, equation (7):

[0369]

[0370] Among them, dγ / dt, dβ / dt, dα / dt are the current X-axis angular velocity value, Y-axis angular velocity value, and Z-axis angular velocity value of the door, respectively. x , g y , g z They are the angular velocity values ​​of the X, Y, and Z axes measured by the gyroscope at the last moment;

[0371] The three-axis angular velocity values ​​dγ / dt, dβ / dt, and dα / dt of the door at the current moment are integrated to obtain the three-axis angle changes Δγ, Δβ, and Δα between the current moment and the previous moment. The calculation formulas of the three-axis angle changes Δγ, Δβ, and Δα are as follows:

[0372]

[0373] Among them, Δt is the sampling time period between the current moment and the previous moment;

[0374] According to the second roll angle information γ, the second pitch angle information β and the heading angle information α of the door at the previous moment and the three-axis angle changes Δγ, Δβ and Δα of the door within the time Δt, the second roll angle information, the second pitch angle information and the heading angle information of the door at the current moment are calculated. Then, the calculation formulas of the second roll angle information roll(n+1), the second pitch angle information pitch(n+1) and the heading angle information yaw(n+1) of the door at the current moment are as follows:

[0375]

[0376] Among them, roll(n), pitch(n), and yaw(n) are respectively the second roll angle information γ, the second pitch angle information β, and the heading angle information α of the door at the last moment.

[0377] Preferably, the fusion module is specifically used for:

[0378] The first roll angle information and the second roll angle information are fused to obtain fused roll angle information of the door at the current moment;

[0379] The first pitch angle information and the second pitch angle information are fused to obtain fused pitch angle information of the door at the current moment;

[0380] Using the heading angle information of the vehicle door at the current moment as the fused heading angle information of the vehicle door at the current moment;

[0381] Then the fused roll angle information roll of the door at the current moment new , fusion pitch angle information new , fusion heading angle information yaw new The calculation formula is as follows:

[0382]

[0383] Among them, roll old is the fused roll angle information of the door at the last moment, pitch old is the fused pitch angle information of the door at the last moment, roll acc is the first roll angle information of the door at the current moment, pitch acc is the first pitch angle information of the door at the current moment, roll gyro is the second roll angle information of the door at the current moment, pitch gyro is the second pitch angle information of the door at the current moment, yawgyro is the heading angle information of the door at the current moment, and K is the proportional coefficient;

[0384] The fused posture information of the vehicle door at the current moment includes the fused roll angle information, the fused pitch angle information and the fused heading angle information.

[0385] Specifically, door hovering: when the side door is in a stopped and released state, the door's 6D sensor is used in conjunction with the IMU attitude algorithm to detect the door's attitude in real time, and perform hovering control in real time, thereby realizing the door hovering function under different slope conditions.

[0386] The door control method based on IMU attitude algorithm and Kalman filter provided by the present invention can realize the ramp hovering function when the vehicle is on a slope and the door does not drift, and realizes consistent resistance of manual door opening and closing on different slopes. When there is vibration interference when the vehicle engine is running, the IMU attitude algorithm is fitted by relying on Kalman filter, so that the IMU attitude fitting error is low, the vehicle vibration noise can be effectively filtered out, and the interference can be effectively reduced, thereby ensuring that the door attitude output is stable and reliable.

[0387] The above description is only a preferred embodiment of the present invention and does not limit the present invention in any form. Although the present invention has been disclosed as a preferred embodiment as above, it is not used to limit the present invention. Any technician familiar with this profession can make some changes or modifications to equivalent embodiments of equivalent changes using the technical contents disclosed above without departing from the scope of the technical solution of the present invention. However, any simple modification, equivalent change and modification made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solution of the present invention still falls within the scope of the technical solution of the present invention.

Claims

1. A door control method based on IMU attitude algorithm and Kalman filter, characterized in that: include: Step S1: obtaining the three-axis acceleration value of the door at the current moment through the three-axis accelerometer, and obtaining the three-axis angular velocity value of the door at the previous moment through the gyroscope; Step S2: filtering the three-axis acceleration value of the vehicle door at the current moment according to the three-axis angular velocity value of the vehicle door at the previous moment, so as to obtain the three-axis acceleration filtering value of the vehicle door at the current moment; Step S3: calculating the first posture information of the vehicle door at the current moment according to the three-axis acceleration filtering value of the vehicle door at the current moment, and calculating the second posture information of the vehicle door at the current moment according to the three-axis angular velocity value of the vehicle door at the previous moment; wherein the first posture information includes first roll angle information and first pitch angle information, and the second posture information includes second roll angle information, second pitch angle information and heading angle information; Step S4: fusing the first posture information and the second posture information to obtain the fused posture information of the vehicle door at the current moment; Step S5: controlling the opening, closing and hovering of the door according to the fused posture information of the door at the current moment; The filtering of the three-axis acceleration value of the vehicle door at the current moment according to the three-axis angular velocity value of the vehicle door at the previous moment to obtain the three-axis acceleration filtering value of the vehicle door at the current moment also includes: The three-axis acceleration value of the door at the current moment is filtered by the Kalman filter algorithm to obtain the three-axis acceleration filter value of the door at the current moment. The prediction and correction process is as follows: predict: x k =Ax k-1 +Bu k-1 (1) P k =AP k-1 From T +Q (2) Correction: K k =P k H T (HP k H T +R) -1 (3) x k =x k +K k (Z k -Hx k ) (4) P k =(1-K k H)P k (5) Formula (1) is the state prediction, where x k-1 is the three-axis acceleration filter value of the door at the last moment, x k is the predicted value of the three-axis acceleration of the door at the current moment, A is the state transfer matrix, the three-axis angular velocity value of the door at the previous moment is taken as the state transfer matrix A, B is the input control matrix, U k-1 is the state vector at the previous moment; Formula (2) is the error matrix prediction, P k represents the error matrix at the current moment, P k-1 Represents the error matrix of the previous moment, A T represents the transposed matrix of the state transfer matrix A, Q is a diagonal matrix; Formula (3) is the Kalman gain calculation, K k represents the observation matrix at the current moment, H is the measurement noise covariance matrix, H T represents the transposed matrix of the measurement noise covariance matrix H, and R is the predicted noise covariance matrix; Formula (4) is the state correction, and its output is the final three-axis acceleration filter value. The two x on the right side of formula (4) are k These are the predicted values ​​of the three-axis acceleration of the door at the current moment. k Refers to the three-axis acceleration filter value of the door at the current moment, Z k Refers to the Kalman gain at the current moment; Formula (5) is the error matrix update.

2. The door control method based on IMU attitude algorithm and Kalman filtering according to claim 1 is characterized in that: The first posture information of the vehicle door at the current moment is calculated based on the three-axis acceleration filtering value of the vehicle door at the current moment, and further includes: Define the rotation angles of the door around the Z, Y, and X axes as α, β, and γ respectively; Then the rotation matrix M of the rotation angle α is z for: Then the rotation matrix M of the rotation angle β is y for: Then the rotation matrix M of the rotation angle γ is x for: The acceleration filter values ​​of the X, Y, and Z axes are a x 、a y 、a z , combined with the rotation matrix M z 、M y 、M x and gravitational acceleration g, we get equation (6): Solving equation (6), we can get the first roll angle information roll of the door at the current moment acc And the first pitch angle information pitch acc , where the first rolling angle information roll acc is the rotation angle γ, the first pitch angle information pitch acc is the rotation angle β. Since the gravity acceleration is constant when rotating around the Z axis, the three-axis accelerometer cannot calculate the heading angle information; then the first roll angle information roll acc And the first pitch angle information pitch acc The calculation formula is as follows:

3. The door control method based on IMU attitude algorithm and Kalman filtering according to claim 1 is characterized in that: The step of calculating the second posture information of the vehicle door at the current moment according to the three-axis angular velocity values ​​of the vehicle door at the previous moment also includes: Acquire the second roll angle information, second pitch angle information and heading angle information of the vehicle door at the last moment; wherein the heading angle information of the vehicle door at the last moment is the rotation angle α of the vehicle door around the Z axis; the second pitch angle information of the vehicle door at the last moment is the rotation angle β of the vehicle door around the Y axis; the second roll angle information of the vehicle door at the last moment is the rotation angle γ of the vehicle door around the X axis; The three-axis angular velocity value of the door at the current moment is calculated according to the second roll angle information, the second pitch angle information, the heading angle information and the three-axis angular velocity value of the door at the previous moment, that is, equation (7): Among them, dγ / dt, dβ / dt, dα / dt are the angular velocity values ​​of the X-axis, Y-axis, and Z-axis of the door at the current moment, respectively. x , g y , g z They are the angular velocity values ​​of the X, Y, and Z axes measured by the gyroscope at the last moment; The three-axis angular velocity values ​​dγ / dt, dβ / dt, and dα / dt of the door at the current moment are integrated to obtain the three-axis angle changes Δγ, Δβ, and Δα between the current moment and the previous moment. The calculation formulas of the three-axis angle changes Δγ, Δβ, and Δα are as follows: Among them, Δt is the sampling time period between the current moment and the previous moment; According to the second roll angle information γ, the second pitch angle information β and the heading angle information α of the door at the previous moment and the three-axis angle changes Δγ, Δβ and Δα of the door within the time Δt, the second roll angle information, the second pitch angle information and the heading angle information of the door at the current moment are calculated. Then, the calculation formulas of the second roll angle information roll(n+1), the second pitch angle information pitch(n+1) and the heading angle information yaw(n+1) of the door at the current moment are as follows: Among them, roll(n), pitch(n), and yaw(n) are respectively the second roll angle information γ, the second pitch angle information β, and the heading angle information α of the door at the last moment.

4. The door control method based on IMU attitude algorithm and Kalman filtering according to claim 1 is characterized in that: The fusing the first posture information and the second posture information to obtain the fused posture information of the vehicle door at the current moment also includes: The first roll angle information and the second roll angle information are fused to obtain fused roll angle information of the door at the current moment; The first pitch angle information and the second pitch angle information are fused to obtain fused pitch angle information of the door at the current moment; Using the heading angle information of the vehicle door at the current moment as the fused heading angle information of the vehicle door at the current moment; Then the fused roll angle information roll of the door at the current moment new , fusion pitch angle information new , fusion heading angle information yaw new The calculation formula is as follows: Among them, roll old is the fused roll angle information of the door at the last moment, pitch old is the fused pitch angle information of the door at the last moment, roll acc is the first roll angle information of the door at the current moment, pitch acc is the first pitch angle information of the door at the current moment, roll gyro is the second roll angle information of the door at the current moment, pitch gyro is the second pitch angle information of the door at the current moment, yaw gyro is the heading angle information of the door at the current moment, and K is the proportional coefficient; The fused posture information of the vehicle door at the current moment includes the fused roll angle information, the fused pitch angle information and the fused heading angle information.

5. A door control system based on IMU attitude algorithm and Kalman filter, characterized in that: include: A three-axis accelerometer is used to obtain the three-axis acceleration value of the door at the current moment; The gyroscope is used to obtain the three-axis angular velocity value of the door at the last moment; A Kalman filter module, used for filtering the three-axis acceleration value of the vehicle door at a current moment according to the three-axis angular velocity value of the vehicle door at a previous moment, so as to obtain the three-axis acceleration filtering value of the vehicle door at the current moment; A calculation module, used to calculate the first posture information of the vehicle door at the current moment according to the three-axis acceleration filter value of the vehicle door at the current moment, and calculate the second posture information of the vehicle door at the current moment according to the three-axis angular velocity value of the vehicle door at the previous moment; wherein the first posture information includes first roll angle information and first pitch angle information, and the second posture information includes second roll angle information, second pitch angle information and heading angle information; A fusion module, used for fusing the first posture information and the second posture information to obtain fused posture information of the vehicle door at a current moment; A control module, used to control the opening and closing and hovering of the door according to the fusion posture information of the door at the current moment; Wherein, the Kalman filter module is specifically used for: The three-axis acceleration value of the door at the current moment is filtered by the Kalman filter algorithm to obtain the three-axis acceleration filter value of the door at the current moment. The prediction and correction process is as follows: predict: x k =Ax k-1 +Bu k-1 (1) P k =AP k-1 From T +Q (2) Correction: K k =P k H T (HP k H T +R) -1 (3) x k =x k +K k (Z k -Hx k ) (4) P k =(1-K k H)P k (5) Formula (1) is the state prediction, where x k-1 is the three-axis acceleration filter value of the door at the last moment, x k is the predicted value of the three-axis acceleration of the door at the current moment, A is the state transfer matrix, the three-axis angular velocity value of the door at the previous moment is taken as the state transfer matrix A, B is the input control matrix, U k-1 is the state vector at the previous moment; Formula (2) is the error matrix prediction, P k represents the error matrix at the current moment, P k-1 Represents the error matrix of the previous moment, A T represents the transposed matrix of the state transfer matrix A, Q is a diagonal matrix; Formula (3) is the Kalman gain calculation, K k represents the observation matrix at the current moment, H is the measurement noise covariance matrix, H T represents the transposed matrix of the measurement noise covariance matrix H, and R is the predicted noise covariance matrix; Formula (4) is the state correction, and its output is the final three-axis acceleration filter value. The two x on the right side of formula (4) are k These are the predicted values ​​of the three-axis acceleration of the door at the current moment. k Refers to the three-axis acceleration filter value of the door at the current moment, Z k Refers to the Kalman gain at the current moment; Formula (5) is the error matrix update.

6. The door control system based on IMU attitude algorithm and Kalman filter according to claim 5, characterized in that: The calculation module is specifically used for: Define the rotation angles of the door around the Z, Y, and X axes as α, β, and γ respectively; Then the rotation matrix M of the rotation angle α is z for: Then the rotation matrix M of the rotation angle β is y for: Then the rotation matrix M of the rotation angle γ is x for: The acceleration filter values ​​of the X, Y, and Z axes are a x 、a y 、a z , combined with the rotation matrix M z 、M y 、M x and gravitational acceleration g, we get equation (6): Solving equation (6), we can get the first roll angle information roll of the door at the current moment acc And the first pitch angle information pitch acc , where the first rolling angle information roll acc is the rotation angle γ, the first pitch angle information pitch acc is the rotation angle β. Since the gravity acceleration is constant when rotating around the Z axis, the three-axis accelerometer cannot calculate the heading angle information; then the first roll angle information roll acc And the first pitch angle information pitch acc The calculation formula is as follows:

7. The door control system based on IMU attitude algorithm and Kalman filter according to claim 5, characterized in that: The calculation module is specifically used for: Acquire the second roll angle information, second pitch angle information and heading angle information of the vehicle door at the last moment; wherein the heading angle information of the vehicle door at the last moment is the rotation angle α of the vehicle door around the Z axis; the second pitch angle information of the vehicle door at the last moment is the rotation angle β of the vehicle door around the Y axis; the second roll angle information of the vehicle door at the last moment is the rotation angle γ of the vehicle door around the X axis; The three-axis angular velocity value of the door at the current moment is calculated according to the second roll angle information, the second pitch angle information, the heading angle information and the three-axis angular velocity value of the door at the previous moment, that is, equation (7): Among them, dγ / dt, dβ / dt, dα / dt are the angular velocity values ​​of the X-axis, Y-axis, and Z-axis of the door at the current moment, respectively. x , g y , g z They are the angular velocity values ​​of the X, Y, and Z axes measured by the gyroscope at the last moment; The three-axis angular velocity values ​​dγ / dt, dβ / dt, and dα / dt of the door at the current moment are integrated to obtain the three-axis angle changes Δγ, Δβ, and Δα between the current moment and the previous moment. The calculation formulas of the three-axis angle changes Δγ, Δβ, and Δα are as follows: Among them, Δt is the sampling time period between the current moment and the previous moment; According to the second roll angle information γ, the second pitch angle information β and the heading angle information α of the door at the previous moment and the three-axis angle changes Δγ, Δβ and Δα of the door within the time Δt, the second roll angle information, the second pitch angle information and the heading angle information of the door at the current moment are calculated. Then, the calculation formulas of the second roll angle information roll(n+1), the second pitch angle information pitch(n+1) and the heading angle information yaw(n+1) of the door at the current moment are as follows: Among them, roll(n), pitch(n), and yaw(n) are respectively the second roll angle information γ, the second pitch angle information β, and the heading angle information α of the door at the last moment.

8. The door control system based on IMU attitude algorithm and Kalman filter according to claim 5, characterized in that: The fusion module is specifically used for: The first roll angle information and the second roll angle information are fused to obtain fused roll angle information of the door at the current moment; The first pitch angle information and the second pitch angle information are fused to obtain fused pitch angle information of the door at the current moment; Using the heading angle information of the vehicle door at the current moment as the fused heading angle information of the vehicle door at the current moment; Then the fused roll angle information roll of the door at the current moment new , fusion pitch angle information new , fusion heading angle information yaw new The calculation formula is as follows: Among them, roll old is the fused roll angle information of the door at the last moment, pitch old is the fused pitch angle information of the door at the last moment, roll acc is the first roll angle information of the door at the current moment, pitch acc is the first pitch angle information of the door at the current moment, roll gyro is the second roll angle information of the door at the current moment, pitch gyro is the second pitch angle information of the door at the current moment, yaw gyro is the heading angle information of the door at the current moment, and K is the proportional coefficient; The fused posture information of the vehicle door at the current moment includes the fused roll angle information, the fused pitch angle information and the fused heading angle information.

Images