EKF attitude determination method and system based on rotation matrix and gravitational acceleration

Abstract

The application provides an EKF attitude solution method and system based on a rotation matrix and gravitational acceleration, and the method comprises the following steps: reading data of an inertial measurement element, calculating initial Euler angles and converting the initial Euler angles into quaternions; performing prediction update on the quaternions through a quaternion differential equation according to gyroscope data and the latest quaternions, calculating the assignment of the prior state vector through a quaternion gravitational acceleration extraction formula based on the prediction updated quaternions, and assigning accelerometer data to a measurement matrix; calculating a Jacobian matrix A and W of a state transition equation according to a gravitational acceleration differential equation derived from a differential equation of a rotation matrix based on gyroscope data, calculating a Jacobian matrix H and V according to a measurement equation, and then calculating a posterior state vector and a posterior covariance matrix; calculating a roll angle and a pitch angle according to the posterior state vector, and calculating a heading angle according to the prediction updated quaternions. The application can avoid EKF divergence and has strong anti-interference capability, so as to ensure the attitude solution precision.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of attitude solution of inertial measurement unit (IMU), in particular to an EKF attitude solution method and system based on rotation matrix and gravity acceleration. BACKGROUND

[0002] In the attitude solution calculation of inertial measurement unit (IMU) only, the inertial measurement unit includes an accelerometer and a gyroscope, the acceleration and angular velocity data are measured by the inertial measurement unit, and then the attitude angle can be obtained by some attitude solution algorithm, and the robustness and adaptability of the system can be further improved by detecting the motion state and dynamically adjusting the filter parameters.

[0003] In the prior art, common attitude solution algorithms mainly include quaternion Mahony attitude solution, gyroscope + accelerometer gradient descent attitude fusion algorithm, and attitude solution based on quaternion EKF (extended Kalman filter). However, the quaternion Mahony attitude solution and the gyroscope + accelerometer gradient descent attitude fusion algorithm have poor anti-interference ability and poor effect, and it is not convenient to dynamically adjust the weights of the gyroscope and the accelerometer. The attitude solution based on quaternion EKF will cause the covariance matrix corresponding to the heading axis to diverge and result in poor solution accuracy because the measured value lacks heading information. SUMMARY

[0004] In order to solve the technical problems of poor anti-interference ability and poor solution accuracy of the existing attitude solution algorithm, the present application proposes an EKF attitude solution method and system based on rotation matrix and gravity acceleration, and combines a measurement fault detection mechanism to realize dynamic covariance adjustment, which can improve the adaptability of the system to dynamic environment while ensuring the attitude solution accuracy.

[0005] According to a first aspect of the present application, an EKF attitude solution method based on rotation matrix and gravity acceleration is provided, comprising:

[0006] reading data of an inertial measurement unit, calculating initial Euler angles and converting them into initial quaternions, the inertial measurement unit including a gyroscope and an accelerometer;

[0007] initializing a state transition equation f process noise covariance matrix Q, measurement noise covariance matrix R of the accelerometer, and covariance matrix P of the state vector X of the EKF;

[0008] performing prediction update on the basis of the collected gyroscope data and the latest quaternion through a quaternion differential equation, and calculating the prior state vector of the EKF on the basis of the prediction updated quaternion through a quaternion gravity acceleration extraction formula The accelerometer data is collected, normalized, and then assigned to the measurement matrix Z of EKF.

[0009] Based on the collected gyroscope data, the state transition equation is calculated using the differential equation of gravitational acceleration derived from the differential equation of the rotation matrix. f The Jacobian matrices A and W, according to the measurement equation h Calculate its Jacobian matrices H and V;

[0010] Based on the posterior covariance matrix of the previous time step P k-1 Calculate the prior covariance matrix at the current time step using the Jacobian matrices A and W. Based on the prior covariance matrix at the current time Calculate the EKF gain matrix K using the Jacobian matrices H and V, where k For a specific moment;

[0011] Based on prior state vector The EKF gain matrix K and the measurement matrix Z are used to calculate the posterior state vector. Based on the EKF gain matrix K, Jacobi matrix H, and prior covariance matrix Calculate the posterior covariance matrix P k ;

[0012] Based on the posterior state vector Calculate the roll angle and pitch angle, and calculate the heading angle based on the predicted updated quaternion;

[0013] The calculated Euler angles are converted and the quaternions are updated, and the data of the inertial measurement element at the next moment are solved.

[0014] Preferably, the posterior state vector and posterior covariance matrix P k The calculation process specifically includes:

[0015] Determine whether the absolute value of the modulus minus 1 of the collected accelerometer measurements is greater than a preset threshold;

[0016] If not, then based on the prior state vector The measurement noise covariance matrix R is updated based on the difference between the measurement matrix Z and the measurement matrix Z, and then based on the prior state vector. The EKF gain matrix K and the measurement matrix Z are used to calculate the posterior state vector. Based on the EKF gain matrix K, Jacobi matrix H, and prior covariance matrix The posterior covariance matrix is ​​calculated using the updated measurement noise covariance matrix R. P k ;

[0017] If yes, the prior state vector is assigned to the posterior state vector The prior covariance matrix is assigned to the posterior covariance matrix P k .

[0018] Preferably, the reading data of the inertial measurement element, calculating initial Euler angles and converting into initial quaternion, specifically includes:

[0019] Reading the accelerometer data, calculating the initial Euler angles according to the following formula:

[0020]

[0021] Wherein, a x , a y , a z are the data of the x, y, z three axes of the accelerometer, roll 0, pintch 0, yaw 0 are the initial roll angle, initial pitch angle and initial heading angle respectively;

[0022] The calculated initial Euler angles are converted into initial quaternion by the following formula:

[0023]

[0024] Wherein, q 0, q 1, q 2, q 3 are the four coefficients of the initial quaternion.

[0025] Preferably, the prediction update is performed through the quaternion differential equation according to the collected gyroscope data and the latest quaternion, specifically including:

[0026] Reading the gyroscope data, predicting and updating the quaternion according to the following formula:

[0027]

[0028] Wherein, , , , are the four coefficients of the predicted and updated quaternion, g x , g y , gz These are the data for the x, y, and z axes of the gyroscope, respectively, with T being the update period;

[0029] The quaternion-based prediction update is used to calculate the prior state vector assigned to EKF using the quaternion gravity acceleration extraction formula. Specifically, it includes:

[0030] Initialize the prior state vector :

[0031]

[0032] The prior state vector is extracted using the quaternion gravitational acceleration extraction formula. Assign a value:

[0033]

[0034] in, x 1. x 2. x 3 represent the prior state vectors. The three elements.

[0035] Preferably, the state transition equation is calculated based on the gravitational acceleration differential equation derived from the differential equation of the rotation matrix using the collected gyroscope data. f The Jacobian matrices A and W specifically include:

[0036] State transition equation f as follows:

[0037]

[0038] in, x k , x k-1 They are respectively k , k The state vector at time -1 u k-1 for k Gyroscope data at time -1 for k The process noise predicted at time -1 T represents the update period;

[0039] The differential equation for gravitational acceleration, derived from the differential equation based on the rotation matrix, is as follows:

[0040]

[0041] in, C 31 , C32 、 C 33 are three elements of a rotation matrix respectively, g x 、 g y 、 g z are data of x, y, z three axes of the gyroscope respectively;

[0042] Thus, the Jacobian matrix A and W can be calculated as follows:

[0043]

[0044] wherein, is the prior state vector estimated at the moment; k

[0045] The Jacobian matrix H and V of the measurement equation h are calculated, and specifically comprising:

[0046] The measurement equation h is as follows:

[0047]

[0048] wherein, v k is the measurement noise of the accelerometer at the moment; k Thus, the Jacobian matrix H and V can be calculated as follows:

[0049]

[0050] .

[0051] Preferably, the posterior covariance matrix P k-1 at the last moment and the Jacobian matrix A, W are used to calculate the prior covariance matrix at the current moment, and specifically comprising:

[0052] The prior covariance matrix at the current moment is calculated according to the following formula:

[0053]

[0054] wherein, A T , W T are the transposition of the Jacobian matrix A, W respectively, Q is the predicted process noise;

[0055] The prior covariance matrix ​​The EKF gain matrix K is calculated using the Jacobian matrices H and V, specifically including:

[0056] Calculate the EKF gain matrix K using the following formula:

[0057]

[0058] Among them, H T V T are the transposes of the Jacobian matrices H and V, respectively, and R is the measurement noise covariance matrix.

[0059] Preferably, the step of basing the prior state vector The measurement noise covariance matrix R is updated based on the difference between the measurement matrix Z and the measurement matrix Z, and then based on the prior state vector. The EKF gain matrix K and the measurement matrix Z are used to calculate the posterior state vector. Based on the EKF gain matrix K, Jacobi matrix H, and prior covariance matrix The posterior covariance matrix is ​​calculated using the updated measurement noise covariance matrix R. P k Specifically, it includes:

[0060] The measurement noise covariance matrix R is updated using the following formula:

[0061]

[0062] in, R 1. R 2. R 3 are the three elements of the measurement noise covariance matrix R, and J is the scaling factor;

[0063] Then, the posterior state vector is predicted and calculated using the following formula. :

[0064]

[0065] in, for k The prior state vector estimated at time step;

[0066] The posterior covariance matrix is ​​calculated using the following formula. P k :

[0067]

[0068] in, I It is an identity matrix.

[0069] Preferably, the step of basing the posterior state vector The roll angle and the pitch angle are calculated, and the heading angle is calculated according to the predicted updated quaternion, specifically comprising:

[0070] The posterior state vector is calculated according to the measurement equation The roll angle and the pitch angle are calculated by the following formula:

[0071]

[0072] The heading angle is calculated according to the predicted updated quaternion by the following formula:

[0073]

[0074] Wherein, roll , pintch , yaw The roll angle, the pitch angle and the heading angle are respectively.

[0075] According to the second aspect of the application, an EKF attitude solution system based on a rotation matrix and a gravity acceleration is provided, comprising:

[0076] A reading unit configured to read data of an inertial measurement element, calculate initial Euler angles and convert them into initial quaternions, the inertial measurement element including a gyroscope and an accelerometer;

[0077] An initialization unit configured to initialize a process noise covariance matrix Q of a state transition equation f , a measurement noise covariance matrix R of the accelerometer, and a covariance matrix P of a state vector X of the EKF;

[0078] A prediction unit configured to perform a prediction update through a quaternion differential equation according to collected gyroscope data and the latest quaternion, calculate an assignment of a prior state vector of the EKF through a quaternion gravity acceleration extraction formula based on the predicted updated quaternion, and collect accelerometer data and assign them to a measurement matrix Z of the EKF after normalization processing;

[0079] A calculation unit configured to calculate a Jacobian matrix A and W of the state transition equation f based on a gravity acceleration differential equation derived from a differential equation of a rotation matrix according to collected gyroscope data, calculate a Jacobian matrix H and V thereof according to a measurement equation h , calculate a prior covariance matrix P k-1 of the current time based on the posterior covariance matrix of the last time and the Jacobian matrices A, W, calculate an EKF gain matrix K based on the prior covariance matrix of the current time and the Jacobian matrices H, V, wherein ktime; posterior state vector based on prior state vector , EKF gain matrix K and measurement matrix Z to calculate posterior state vector , based on EKF gain matrix K, Jacobian matrix H and prior covariance matrix to calculate posterior covariance matrix P k ; according to the posterior state vector roll angle and pitch angle are calculated, and the heading angle is calculated according to the predicted updated quaternion;

[0080] The loop unit is configured to convert the calculated Euler angle to update the quaternion, and to solve the data of the next time of the inertial measurement element.

[0081] According to a third aspect of the present application, a computer readable storage medium is provided, and a computer program is stored on the computer readable storage medium, and the program is executed by a processor to implement the EKF attitude solving method based on the rotation matrix and the gravity acceleration provided by any one of the above first aspect.

[0082] The present application provides an EKF attitude solving method and system based on a rotation matrix and gravity acceleration, the state vector X of the EKF is the data of the accelerometer, which directly corresponds to the measurement value, and there is no problem of divergence of the heading angle affecting the attitude solving accuracy caused by the fact that the quaternion is used as the attitude angle vector and the quaternion contains all Euler angles while the measurement value does not contain the heading angle. In addition, the data of the accelerometer is collected for measurement fault detection. When the measurement value interference is large, the step of fusing the accelerometer measurement value is directly skipped, and the prior value is assigned to the posterior value. When the measurement value interference is small, the measurement noise covariance matrix R is adjusted, so as to reduce the influence of the measurement interference and ensure the accuracy of the attitude solving. BRIEF DESCRIPTION OF DRAWINGS

[0083] The accompanying drawings are included to provide a further understanding of embodiments and are incorporated in and constitute a part of this specification. The drawings illustrate embodiments and, together with the description, serve to explain the principles of the application. Other embodiments and many of the intended advantages of the present application will be readily appreciated as the same becomes better understood by reference to the following detailed description. The elements of the drawings are not necessarily to scale relative to each other. Like reference numerals designate corresponding similar parts.

[0084] Figure 1 is a flowchart of an EKF attitude solving method based on a rotation matrix and gravity acceleration according to an embodiment of the present application;

[0085] Figure 2 is a schematic diagram of an EKF attitude solving system based on a rotation matrix and gravity acceleration according to an embodiment of the present application. DETAILED DESCRIPTION

[0086] The features and exemplary embodiments of the various aspects of the present application will be described in detail below with reference to the figures and examples. It should be understood that the specific embodiments described herein are merely exemplary and are not intended to limit the present application. The present application can be implemented in ways other than those specifically described herein without departing from the spirit of the present application. The following description of the examples is merely provided to give a better understanding of the present application by showing examples of the present application.

[0087] It should be noted that the terms such as first and second, etc., are merely used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Also, the terms "include", "contain" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements not only includes those elements, but also includes other elements not explicitly listed or inherent to such a process, method, article or device. Without more limitations, the elements defined by the statement "include" do not exclude the presence of other identical elements in the process, method, article or device including the elements.

[0088] The present application proposes an EKF attitude solution method based on rotation matrix and gravity acceleration. Figure 1 A flow chart of the EKF attitude solution method based on rotation matrix and gravity acceleration according to one specific embodiment of the present application is shown, as shown in FIG. 1, the method comprises the following steps: Figure 1

[0089] Step S1: read the data of the inertial measurement element, calculate the initial Euler angle and convert it into the initial quaternion, the inertial measurement element includes a gyroscope and an accelerometer.

[0090] In one specific embodiment, the accelerometer data is read, and then the initial Euler angle is calculated by the following formula:

[0091]

[0092] wherein, a x , a y , a z are the data of the x, y, z three axes of the accelerometer, roll 0, pintch 0, yaw ​0, 0, 0 are initial roll angle, initial pitch angle and initial heading angle respectively.

[0093] The calculated initial Euler angle is converted into initial quaternion by the following formula:

[0094]

[0095] wherein, q 0, q 1, q 2, q 3 are four coefficients of the initial quaternion respectively.

[0096] It should be noted that in the embodiment, since the inertial measurement element (IMU) only includes a gyroscope and an accelerometer, but does not include a magnetometer, the initial heading angle is 0.

[0097] Step S2: initialization of the process noise covariance matrix Q of the state transition equation f , the measurement noise covariance matrix R of the accelerometer, and the covariance matrix P of the state vector X of the EKF.

[0098] In a specific embodiment, the initialization process is specifically as follows:

[0099]

[0100] It should be noted that in the prior art, a common attitude representation method is a direction cosine matrix. For the direction cosine matrix, 9 elements of the direction cosine matrix are required at each time of attitude update, and the calculation amount is large. In the embodiment, the state vector X of the EKF only includes 3 elements, and the calculation amount is small at each time of attitude solving.

[0101] Step S3: according to the collected gyroscope data and the latest quaternion, the quaternion is predicted and updated through a quaternion differential equation, and based on the predicted and updated quaternion, the prior state vector X of the EKF is calculated and assigned by a quaternion gravity acceleration extraction formula , and the accelerometer data is collected and assigned to the measurement matrix Z of the EKF after normalization processing.

[0102] In a specific embodiment, according to the read gyroscope data and the latest quaternion, the quaternion is predicted and updated according to the following formula:

[0103]

[0104] wherein, , , , are four coefficients of the predicted and updated quaternion respectively, g x, g y , g z are the data of x, y, z three axes of the gyroscope respectively, T is the update period.

[0105] According to the predicted update quaternion, the prior state vector is assigned by a quaternion gravity acceleration extraction formula, and specifically includes:

[0106] The prior state vector is initialized:

[0107]

[0108] The prior state vector is assigned by a quaternion gravity acceleration extraction formula:

[0109]

[0110] wherein, x 1, x 2, x 3 are three elements of the prior state vector .

[0111] Step S4: According to the collected gyroscope data, the Jacobian matrix A and W of the state transition equation f are calculated based on the gravity acceleration differential equation derived from the differential equation of the rotation matrix, and the Jacobian matrix H and V thereof are calculated according to the measurement equation h .

[0112] In one specific embodiment, the state transition equation f is as follows:

[0113]

[0114] wherein, x k , x k-1 are the state vectors at time k , k -1, u k-1 is the gyroscope data at time k -1, is the predicted process noise at time k -1, , and T is the update period.

[0115] The gravity acceleration differential equation derived based on the differential equation of the rotation matrix is as follows:

[0116]

[0117] in, C 31 , C 32 , C 33 These are the three elements of the rotation matrix. g x , g y , g z These are the data for the x, y, and z axes of the gyroscope, respectively.

[0118] Therefore, the Jacobian matrices A and W can be calculated as follows:

[0119]

[0120] Measurement equation h as follows:

[0121]

[0122] in, v k for k The measurement noise of the accelerometer at any given time.

[0123] Therefore, the Jacobian matrices H and V can be calculated as follows:

[0124]

[0125] in, for k The prior state vector estimated at time step.

[0126] Step S5: Based on the posterior covariance matrix of the previous time step P k-1 Calculate the prior covariance matrix at the current time step using the Jacobian matrices A and W. Based on the prior covariance matrix at the current time Calculate the EKF gain matrix K using the Jacobian matrices H and V, where k For a moment.

[0127] In a specific embodiment, based on the posterior covariance matrix of the previous time step... P k-1 Given the Jacobian matrices A and W, calculate the prior covariance matrix at the current time using the following formula. :

[0128]

[0129] Among them, A T WT Let A and W be the transposes of the Jacobian matrices A and W, respectively. Q This refers to the process noise during prediction.

[0130] Based on the prior covariance matrix at the current moment Given the Jacobian matrices H and V, the EKF gain matrix K is calculated using the following formula:

[0131]

[0132] Among them, H T V T are the transposes of the Jacobian matrices H and V, respectively, and R is the measurement noise covariance matrix.

[0133] Step S6: Determine whether the absolute value of the modulus minus 1 of the collected accelerometer measurement is greater than the preset threshold. If not, proceed to step S7a; if yes, proceed to step S7b.

[0134] Step S7a: Based on the prior state vector The measurement noise covariance matrix R is updated based on the difference between the measurement matrix Z and the measurement matrix Z, and then based on the prior state vector. The EKF gain matrix K and the measurement matrix Z are used to calculate the posterior state vector. Based on the EKF gain matrix K, Jacobi matrix H, and prior covariance matrix The posterior covariance matrix is ​​calculated using the updated measurement noise covariance matrix R. P k .

[0135] Specifically, when the interference of the measurement value is small, that is, when the absolute value of the modulus of the collected accelerometer measurement value minus 1 is less than the preset threshold, the influence of the measurement interference can be reduced by adjusting the measurement noise covariance matrix R, thus ensuring the accuracy of attitude calculation.

[0136] In a specific embodiment, the measurement noise covariance matrix R is updated using the following formula:

[0137]

[0138] in, R 1. R 2. R 3 represents the three elements of the measurement noise covariance matrix R, Z k for k The measurement matrix at time, where J is the scaling factor.

[0139] Then, the posterior state vector is predicted and calculated using the following formula. :

[0140]

[0141] in, for k The prior state vector estimated at time step, K k for k The EKF gain matrix at time t.

[0142] The posterior covariance matrix is ​​calculated using the following formula. P k :

[0143]

[0144] in, I It is the identity matrix. R k for k The measurement noise covariance matrix at time.

[0145] Step S7b: Convert the prior state vector Assigned to the posterior state vector The prior covariance matrix Assigned to the posterior covariance matrix P k .

[0146] Specifically, when the absolute value of the modulus minus 1 of the collected accelerometer measurement is greater than the preset threshold, it can be considered that the measurement interference is large. Therefore, the step of fusing accelerometer measurement values ​​is skipped, and the prior value is directly assigned to the subsequent value to avoid the large impact of measurement interference and ensure the accuracy of attitude calculation.

[0147] It should be noted that the preset thresholds mentioned above can be freely set according to the system's allowable error range, and there are no restrictions here.

[0148] Step S8: Based on the posterior state vector The roll and pitch angles are calculated, and the heading angle is calculated based on the predicted updated quaternions.

[0149] In one specific embodiment, based on the posterior state vector The roll angle and pitch angle are calculated using the following formulas:

[0150]

[0151] in, , , They represent k The first, second, and third elements of the posterior state vector at time t.

[0152] The heading angle is calculated based on the predicted updated quaternion using the following formula:

[0153]

[0154] wherein, roll 、 pintch 、 yaw are the roll angle, the pitch angle and the heading angle, respectively.

[0155] Step S9: updating the quaternion by converting the calculated Euler angle, and calculating the next time data of the inertial measurement element.

[0156] This step is a loop step. After each attitude calculation, the calculated Euler angle is converted into a quaternion by the formula of step S1 above to update, and then proceeds to step S3 to calculate the next time data.

[0157] In summary, the EKF attitude calculation method based on rotation matrix and gravity acceleration provided by the application has the state vector X of the EKF as the data of the accelerometer, which directly corresponds to the measurement value, and does not have the problem of divergence of the heading angle affecting the accuracy of the attitude calculation caused by the fact that the quaternion is used as the attitude angle vector and the quaternion contains all Euler angles while the measurement value does not contain the heading angle. In addition, the data of the accelerometer is collected for measurement fault detection. When the measurement value is greatly disturbed, the step of fusing the accelerometer measurement value is directly skipped, and the prior value is assigned to the posterior value. When the measurement value is slightly disturbed, the measurement noise covariance matrix R is adjusted, thereby reducing the influence of the measurement disturbance and ensuring the accuracy of the attitude calculation.

[0158] According to the above-mentioned EKF attitude calculation method based on rotation matrix and gravity acceleration, based on the same inventive concept, the application further provides an EKF attitude calculation system based on rotation matrix and gravity acceleration. Figure 2 A schematic diagram of the EKF attitude calculation system based on rotation matrix and gravity acceleration according to one specific embodiment of the application is shown, as shown in Figure 2 The system comprises:

[0159] A reading unit 10 configured to read the data of the inertial measurement element, calculate the initial Euler angle and convert it into the initial quaternion, the inertial measurement element comprising a gyroscope and an accelerometer.

[0160] An initialization unit 20 configured to initialize the process noise covariance matrix Q of the state transition equation f , the measurement noise covariance matrix R of the accelerometer, and the covariance matrix P of the state vector X of the EKF.

[0161] The prediction unit 30 is configured to predict and update according to the collected gyroscope data and the latest quaternion through a quaternion differential equation, calculate the prior state vector of the EKF based on the predicted and updated quaternion through a quaternion gravity acceleration extraction formula , and collect the accelerometer data to assign the measurement matrix Z of the EKF after normalization processing.

[0162] The calculation unit 40 is configured to calculate the state transition equation of the gravity acceleration differential equation based on the differential equation of the rotation matrix according to the collected gyroscope data f , calculate the Jacobian matrix A and W of the measurement equation h , calculate the Jacobian matrix H and V thereof based on the posterior covariance matrix P k-1 of the last moment and the Jacobian matrix A, W to calculate the prior covariance matrix of the current moment, calculate the EKF gain matrix K based on the prior covariance matrix of the current moment and the Jacobian matrix H, V, wherein k is the moment; calculate the posterior state vector based on the prior state vector , the EKF gain matrix K, and the measurement matrix Z, calculate the posterior covariance matrix based on the EKF gain matrix K, the Jacobian matrix H, and the prior covariance matrix P k ; calculate the roll angle and the pitch angle according to the posterior state vector , and calculate the heading angle according to the predicted and updated quaternion.

[0163] The loop unit 50 is configured to convert and update the quaternion according to the calculated Euler angle, and solve the data of the next moment of the inertial measurement element.

[0164] In a specific embodiment, the calculation unit 40 specifically further comprises: judging whether the absolute value of the modulus of the measurement matrix Z collected by the accelerometer minus 1 is greater than a preset threshold value; if not, updating the measurement noise covariance matrix R according to the difference between the prior state vector and the measurement matrix Z, then calculating the posterior state vector based on the prior state vector , the EKF gain matrix K, and the measurement matrix Z, calculating the posterior covariance matrix based on the EKF gain matrix K, the Jacobian matrix H, the prior covariance matrix P k R updated; if yes, assigning the prior state vector to the posterior state vector , and assigning the prior covariance matrix assigning a posterior covariance matrix P k .

[0165] According to the EKF attitude resolving method based on the rotation matrix and the gravity acceleration described above, based on the same inventive concept, the present application further provides a computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to implement the EKF attitude resolving method based on the rotation matrix and the gravity acceleration provided by any of the above embodiments.

[0166] In the embodiments of the present application, it should be understood that the disclosed technical contents can be implemented by other ways. Among them, the above-described device / system / method embodiment is only schematic, for example, the division of the units can be a logical function division, and actual implementation can have another division mode, for example, a plurality of units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the coupling or direct coupling or communication connection between the units shown or discussed can be indirect coupling or communication connection through some interfaces, units or modules, which can be electrical or other forms.

[0167] The units described as separate components can or can not be physically separate, and the components shown as units can or can not be physical units, that is, they can be located in one place, or they can be distributed on multiple units. Part or all of the units can be selected according to actual needs to achieve the purpose of the present embodiment.

[0168] In addition, each functional unit in each embodiment of the present application can be integrated in one processing unit, or each unit can exist physically, or two or more units can be integrated in one unit. The integrated unit can be realized in the form of hardware or in the form of a software functional unit.

[0169] The integrated unit, if implemented in the form of a software function unit and sold or used as an independent product, can be stored in a computer readable storage medium. Based on such understanding, the technical solutions of the present application essentially or the part that contributes to the prior art or the whole or part of the technical solutions can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes a plurality of instructions for causing a computer device (which can be a personal computer, a server or a network device, etc.) to execute all or part of the steps of the method described in the various embodiments of the present application. The aforementioned storage medium includes: a U disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a mobile hard disk, a magnetic disk or an optical disk, and various media that can store program codes.

[0170] Obviously, those skilled in the art can make various modifications and changes to the embodiments of the present application without departing from the spirit and scope of the present application. In this way, if these modifications and changes are within the scope of the claims of the present application and their equivalents, the present application also intends to cover these modifications and changes. The word "comprises" does not exclude the presence of other elements or steps not listed in the claims. The simple fact that certain measures are described in mutually different dependent claims does not mean that the combination of these measures cannot be used to advantage. Any reference signs in the claims should not be considered as limiting the scope.

Claims

1. An EKF attitude determination method based on rotation matrix and gravitational acceleration, characterized in that, The method comprises: reading data of an inertial measurement element, calculating initial Euler angles and converting the initial Euler angles into initial quaternions, the inertial measurement element comprising a gyroscope and an accelerometer; Initialization of the state transition equation f the process noise covariance matrix Q, the measurement noise covariance matrix R of the accelerometer, and the covariance matrix P of the state vector X of the EKF; According to the collected gyroscope data and the latest quaternion, the prediction update is carried out through a quaternion differential equation, and based on the prediction update quaternion, the prior state vector assigned to the EKF is calculated through a quaternion gravity acceleration extraction formula , and the accelerometer data is collected and assigned to the measurement matrix Z of the EKF after normalization processing; Based on the collected gyroscope data, the state transition equation is calculated based on the differential equation of the gravity acceleration derived from the rotation matrix f The Jacobian matrix A and W of the measurement equation h Calculate its Jacobian matrix H and V; based on the posterior covariance matrix of the previous time instant P k-1 and the Jacobian matrix A, W to compute the prior covariance matrix of the current time instant based on the prior covariance matrix of the current time instant and the Jacobian matrix H, V to compute the EKF gain matrix K, where k is the time instant; based on the prior state vector , the EKF gain matrix K and the measurement matrix Z to compute the posterior state vector , based on the EKF gain matrix K, the Jacobian matrix H and the prior covariance matrix to compute the posterior covariance matrix P k ; According to the posterior state vector A roll angle and a pitch angle are calculated, and a heading angle is calculated from the predicted updated quaternion. updating the quaternions by converting the calculated Euler angles, and calculating data of the inertial measurement element at a next time.

2. The method of claim 1, wherein, posterior state vector and posterior covariance matrix P k the calculation process specifically includes: determining whether an absolute value of a modulus of the collected accelerometer measurement value minus 1 is greater than a preset threshold value; If not, then based on the prior state vector The difference between the measurement matrix Z and the measurement noise covariance matrix R is used to update the measurement noise covariance matrix, and then based on the prior state vector... The EKF gain matrix K and the measurement matrix Z are used to calculate the posterior state vector. Based on the EKF gain matrix K, Jacobi matrix H, and prior covariance matrix The posterior covariance matrix is ​​calculated using the updated measurement noise covariance matrix R. P k ; If so, the prior state vector is assigned to the posterior state vector and the prior covariance matrix is assigned to the posterior covariance matrix P k .

3. The method of claim 1, wherein, The reading data of the inertial measurement element, calculating initial Euler angles and converting the initial Euler angles into initial quaternions specifically comprises: reading the accelerometer data, and calculating the initial Euler angles according to the following formula: wherein, a x , a y , a z are data of x, y, z three axes of the accelerometer respectively, roll 0, pintch 0, yaw 0 are initial roll angle, initial pitch angle and initial heading angle respectively; converting the calculated initial Euler angles into initial quaternions according to the following formula: wherein q 0, q 1, q 2, q 3 are the four coefficients of the initial quaternion, respectively.

4. The method of claim 3, wherein, The predicting updating according to the collected gyroscope data and the latest quaternions through a quaternion differential equation specifically comprises: reading the gyroscope data, and predicting updating the quaternions according to the following formula: wherein, , , , are respectively four coefficients of the predicted updated quaternion, g x , g y , g z are respectively data of x, y, z three axes of the gyroscope, and T is an update period. The quaternion based on the prediction update is assigned to the prior state vector of the EKF through a quaternion gravity acceleration extraction formula , and specifically comprises: initializing the prior state vector : The prior state vector is assigned by the quaternion gravity acceleration extraction formula is assigned: wherein x 1、 x 2、 x 3are three elements of the a priori state vector respectively.

5. The method of claim 1, wherein, The state transition equation is calculated according to the collected gyro data, based on a gravity acceleration differential equation derived from a differential equation of a rotation matrix f The Jacobian matrix A and W, specifically comprising: State transition equation f As follows: wherein, x k , x k-1 are respectively k , k state vector at time -1, u k-1 is k gyroscope data at time -1, is k predicted process noise at time -1, T is the update period; The gravity acceleration differential equation derived based on a differential equation of a rotation matrix is as follows: wherein, C 31 , C 32 , C 33 are respectively three elements of a rotation matrix, g x , g y , g z are respectively data of x, y, z three axes of a gyroscope; Thus, the Jacobian matrix A and W can be calculated as follows: wherein is k the a priori state vector estimated at time The measurement equation is h Calculate its Jacobian matrix H and V, specifically including: Measurement equation h As follows: wherein v k to k measurement noise of the accelerometer at the moment Thus, the Jacobian matrix H and V can be calculated as follows: 。 6. The method of claim 5, wherein, the posterior covariance matrix of the last time P k-1 and the Jacobian matrix A, W to calculate the prior covariance matrix of the current time and the Jacobian matrix A, W to calculate the prior covariance matrix of the current time The a priori covariance matrix at the current time instant is calculated according to the following equation : where A T , W T are the transpose of the Jacobian matrices A, W, respectively, Q is the predicted process noise; the prior covariance matrix based on the current time and the Jacobian matrix H, V to calculate the EKF gain matrix K, specifically comprising: calculating an EKF gain matrix K according to the following formula: where H T , V T are the transpose of the Jacobian matrices H, V, respectively, and R is the measurement noise covariance matrix.

7. The method of claim 2, wherein, The prior state vector The difference between the measurement matrix Z and the measurement noise covariance matrix R is used to update the measurement noise covariance matrix, and then based on the prior state vector... The EKF gain matrix K and the measurement matrix Z are used to calculate the posterior state vector. Based on the EKF gain matrix K, Jacobi matrix H, and prior covariance matrix The posterior covariance matrix is ​​calculated using the updated measurement noise covariance matrix R. P k Specifically, it includes: updating a measurement noise covariance matrix R through the following formula: wherein R 1、 R 2、 R 3are three elements of the measurement noise covariance matrix R, respectively, and J is a scaling factor. The posterior state vector is then predicted by the following equation : wherein is k the a priori state vector estimated at time The posterior covariance matrix is calculated by the following equation P k : wherein I is the identity matrix.

8. The method of claim 4, wherein, the predicted update quaternion The roll angle and the pitch angle are calculated, and the heading angle is calculated according to the predicted update quaternion. According to the posterior state vector The roll and pitch angles are calculated by the following equations: calculating a heading angle according to the predicted updated quaternions through the following formula: wherein roll , pintch , yaw are the roll, pitch and yaw angles, respectively.

9. An EKF attitude determination system based on rotation matrices and gravitational acceleration, characterized in that, The method comprises: a reading unit configured to read data of an inertial measurement element, calculate initial Euler angles and convert the initial Euler angles into initial quaternions, the inertial measurement element comprising a gyroscope and an accelerometer; An initialization unit configured to initialize a process noise covariance matrix Q of the state transition equation, a measurement noise covariance matrix R of the accelerometer, and a covariance matrix P of a state vector X of the EKF f ; a prediction unit configured to predict an update according to the collected gyroscope data and the latest quaternion through a quaternion differential equation, calculate an a priori state vector assigned to the EKF through a quaternion gravity acceleration extraction formula based on the predicted updated quaternion and collect accelerometer data to assign a measurement matrix Z to the EKF after normalization processing; a computing unit configured to calculate a state transition equation based on a differential equation of gravitational acceleration derived from a differential equation of a rotation matrix according to the collected gyroscope data f the Jacobian matrix A and W of the measurement equation h calculate its Jacobian matrix H and V; based on the posterior covariance matrix P k-1 and the Jacobian matrix A, W calculate the prior covariance matrix at the current time, based on the prior covariance matrix and the Jacobian matrix H, V calculate the EKF gain matrix K, wherein k is the time; based on the prior state vector , the EKF gain matrix K and the measurement matrix Z calculate the posterior state vector , based on the EKF gain matrix K, the Jacobian matrix H and the prior covariance matrix calculate the posterior covariance matrix P k ; according to the posterior state vector calculate the roll angle and the pitch angle, and calculate the heading angle according to the predicted updated quaternion; a loop unit configured to update the quaternions by converting the calculated Euler angles, and calculate data of the inertial measurement element at a next time.

10. A computer-readable storage medium having stored thereon a computer program, characterized in that, The program is executed by the processor to implement the method of any one of claims 1 to 8.

Images