Attitude correction method and device for heading attitude reference system

Abstract

The invention provides an attitude correction method and device for a heading attitude reference system, and the method comprises the steps: obtaining the historical attitude data of a target carrier at a previous moment, and the acceleration data and angular velocity data at a current moment, and carrying out the correction of the attitude of the target carrier based on the historical attitude data and angular velocity data; determining the attitude quaternion of the target carrier at the current moment; the acceleration output module value and the angular speed output module value are compared with corresponding threshold values respectively, and whether the target carrier is in a static state at the current moment or not is judged; based on the judgment result, determining a noise covariance matrix of an accelerometer in the attitude and heading reference system at the current moment; determining the attitude correction of the target carrier at the current moment based on the noise covariance matrix, the historical attitude data and the observation vector; and correcting the attitude quaternion by using the attitude correction number to obtain a target attitude quaternion of the target carrier at the current moment. According to the method, the accuracy of detecting the carrier attitude by the attitude and heading reference system is improved.

Description

Technical Field

[0001] The present application relates to the technical field of attitude and heading reference systems, and in particular, to an attitude correction method and device for an attitude and heading reference system. Background Art

[0002] An Attitude and Heading Reference System (AHRS) consists of three-axis accelerometers, three-axis gyroscopes, and three-axis magnetometers. The attitude of the carrier is determined by integrating the angular velocity data output by the gyroscopes. However, the integration calculation will introduce additional error terms, and it is necessary to use the accelerometers and magnetometers to sense the earth's gravity and magnetic field to correct the gyroscopes, so as to improve the accuracy and dynamic performance of the system for detecting the attitude. For a moving carrier, adjusting the trust level of the system for the accelerometers according to different motion scenarios will directly affect the accuracy of the carrier attitude detection.

[0003] The trust level of the accelerometers can be expressed as the weight of the accelerometer observations, that is, a Kalman filter for attitude detection is performed using the noise covariance matrix of the accelerometer-observed acceleration. When the carrier is stationary, the specific force modulus value output by the accelerometers is equal to the local gravity. When the carrier is in a dynamic state, the traditional method defines the noise covariance matrix as a fixed value, which will reduce the accuracy of the carrier state variable estimation, and thus reduce the accuracy of the attitude and heading reference system for detecting the carrier attitude. Summary of the Invention

[0004] In view of this, the purpose of the present application is to provide an attitude correction method and device for an attitude and heading reference system. During the process of using the attitude and heading reference system to detect the attitude of a carrier in different motion scenarios, the noise covariance matrix of the accelerometers of the attitude and heading reference system is adaptively adjusted according to the judgment results of different motion states of the carrier, and further the attitude correction number of the carrier is determined, and the detected attitude quaternion of the carrier at the current moment is corrected using the attitude correction number, realizing reliable and accurate three-dimensional attitude calculation of the carrier under various working conditions and improving the accuracy of the attitude and heading reference system for detecting the carrier attitude.

[0005] The embodiment of the present application provides an attitude correction method for an attitude and heading reference system, and the attitude correction method includes:

[0006] During the process of using the attitude and heading reference system set on the target carrier to detect the carrier attitude, obtain the historical attitude data of the target carrier at the previous moment and the acceleration data and angular velocity data of the target carrier at the current moment, and based on the historical attitude data and the angular velocity data, determine the attitude quaternion of the target carrier at the current moment;

[0007] Compare the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with a preset acceleration threshold and a preset angular velocity threshold respectively to determine whether the target carrier is in a stationary state at the current moment, and obtain a judgment result;

[0008] Based on the judgment result, determine the noise covariance matrix of the accelerometer in the attitude reference system at the current moment;

[0009] Based on the noise covariance matrix, the historical attitude data, and the observation vector of the target carrier at the current moment, determine the attitude correction number of the target carrier at the current moment;

[0010] Use the attitude correction number to correct the attitude quaternion to obtain the target attitude quaternion of the target carrier at the current moment, so as to correct the carrier attitude detection of the attitude reference system.

[0011] Further, determining the attitude quaternion of the target carrier at the current moment based on the historical attitude data and the angular velocity data includes:

[0012] Determine the historical attitude quaternion and historical angular velocity data of the target carrier at the previous moment in the historical attitude data;

[0013] Based on the angular velocity data and the historical angular velocity data, determine the gyro angle increment of the target carrier at the current moment, and calculate the equivalent rotation vector of the target carrier at the current moment based on the gyro angle increment and a preset coning compensation coefficient;

[0014] Based on the historical attitude quaternion and the equivalent rotation vector, determine the attitude quaternion of the target carrier at the current moment.

[0015] Further, the comparing the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with a preset acceleration threshold and a preset angular velocity threshold respectively to determine whether the target carrier is in a stationary state at the current moment and obtaining a judgment result includes:

[0016] Compare the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with a preset acceleration threshold and a preset angular velocity threshold respectively, determine whether the acceleration output modulus is less than or equal to the preset acceleration threshold, and determine whether the angular velocity output modulus is equal to the preset angular velocity threshold;

[0017] When the magnitude of the acceleration output is less than or equal to a preset acceleration threshold and the magnitude of the angular velocity output is equal to the preset angular velocity threshold, it is determined that the determination result is that the target carrier is in a stationary state at the current moment;

[0018] When the magnitude of the acceleration output is greater than the preset acceleration threshold and / or the magnitude of the angular velocity output is not equal to the preset angular velocity threshold, it is determined that the determination result is that the target carrier is in a non-stationary state at the current moment.

[0019] Further, determining the noise covariance matrix of the accelerometer in the attitude reference system at the current moment based on the determination result includes:

[0020] When the determination result is that the target carrier is in a stationary state at the current moment, a preset first covariance matrix is determined as the noise covariance matrix of the accelerometer in the attitude reference system at the current moment;

[0021] When the determination result is that the target carrier is in a non-stationary state at the current moment, a numerical comparison is made on the carrier vibration values, specific force estimation error values, and the motion acceleration of the target carrier corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and based on the numerical comparison result, the noise covariance matrix of the accelerometer in the attitude reference system at the current moment is determined.

[0022] Further, the numerical comparison of the carrier vibration values, specific force estimation error values, and the motion acceleration of the target carrier corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and determining the noise covariance matrix of the accelerometer in the attitude reference system at the current moment based on the numerical comparison result includes:

[0023] Obtain the carrier vibration values of the target carrier corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and make a numerical comparison between the carrier vibration values and the preset random walk values of the horizontal axis, vertical axis, and vertical axis respectively to obtain a first comparison result; wherein, the carrier vibration value represents the standard deviation of the acceleration data of the target carrier within a preset time window before the current moment;

[0024] Select the first numerical values with the first comparison result being greater from the carrier vibration values or the random walk values corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and construct the first numerical values into a second covariance matrix;

[0025] Obtain the specific force estimation error values of the target carrier corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and make a numerical comparison between the specific force estimation error values and the first numerical values corresponding to the horizontal axis, vertical axis, and vertical axis respectively to obtain a second comparison result;

[0026] Select the second numerical values with the second comparison result being greater from the specific force estimation error values or the first numerical values corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and construct the second numerical values into a third covariance matrix;

[0027] Obtain the motion acceleration of the target carrier at the current moment, and compare the motion acceleration with the sum of the second numerical values corresponding to the horizontal axis, vertical axis, and vertical axis respectively to obtain a third comparison result;

[0028] When the third comparison result is that the motion acceleration is greater than the sum of the second numerical values, determine that the noise covariance matrix is infinite;

[0029] When the third comparison result is that the motion acceleration is less than or equal to the sum of the second numerical values, determine the third covariance matrix as the noise covariance matrix.

[0030] Further, determining the attitude correction number of the target carrier at the current moment based on the noise covariance matrix, the historical attitude data, and the observation vector of the target carrier at the current moment includes:

[0031] Determine the state estimation vector and the estimation error covariance matrix of the target carrier at the previous moment in the historical attitude data, and determine the state prediction vector of the target carrier from the previous moment to the current moment based on the state estimation vector of the target carrier at the previous moment and a preset prediction matrix;

[0032] Determine the filtering gain matrix of the target carrier at the current moment based on the noise covariance matrix, the estimation error covariance matrix, the preset observation matrix, and the preset system noise covariance matrix;

[0033] Determine the state estimation vector of the target carrier at the current moment based on the observation vector of the target carrier at the current moment, the filtering gain matrix, the preset observation matrix, and the state prediction vector;

[0034] Determine the attitude correction numbers corresponding to the roll angle, pitch angle, and heading angle of the target carrier at the current moment in the state estimation vector at the current moment.

[0035] Further, correcting the attitude quaternion using the attitude correction number to obtain the target attitude quaternion of the target carrier at the current moment for correcting the carrier attitude detection of the attitude reference system includes:

[0036] Based on the attitude correction number, construct a three-dimensional attitude error matrix of the target carrier at the current moment to determine the three-dimensional attitude error value of the target carrier at the current moment;

[0037] Based on the three-dimensional attitude error value, determine the attitude error quaternion of the target carrier at the current moment;

[0038] Perform quaternion multiplication on the attitude error quaternion and the attitude quaternion to obtain the target attitude quaternion of the target carrier at the current moment, so as to correct the carrier attitude detection of the attitude reference system.

[0039] The embodiment of the present application also provides an attitude correction device for an attitude reference system. The attitude correction device includes:

[0040] An attitude determination module, configured to obtain the historical attitude data of the target carrier at the previous moment and the acceleration data and angular velocity data of the target carrier at the current moment during the process of using the attitude reference system set by the target carrier for carrier attitude detection, and determine the attitude quaternion of the target carrier at the current moment based on the historical attitude data and the angular velocity data;

[0041] A static judgment module, configured to respectively compare the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with a preset acceleration threshold and a preset angular velocity threshold, and judge whether the target carrier is in a static state at the current moment to obtain a judgment result;

[0042] A matrix determination module, configured to determine the noise covariance matrix of the accelerometer in the attitude reference system at the current moment based on the judgment result;

[0043] An attitude filtering module, configured to determine the attitude correction number of the target carrier at the current moment based on the noise covariance matrix, the historical attitude data, and the observation vector of the target carrier at the current moment;

[0044] An attitude correction module, configured to correct the attitude quaternion by using the attitude correction number to obtain the target attitude quaternion of the target carrier at the current moment, so as to correct the carrier attitude detection of the attitude reference system.

[0045] Further, when the attitude determination module is used to determine the attitude quaternion of the target carrier at the current moment based on the historical attitude data and the angular velocity data, the attitude determination module is used to:

[0046] Determine the historical attitude quaternion and historical angular velocity data of the target carrier at the previous moment from the historical attitude data;

[0047] Based on the angular velocity data and the historical angular velocity data, determine the gyro angle increment of the target carrier at the current moment, and calculate the equivalent rotation vector of the target carrier at the current moment based on the gyro angle increment and a preset conical compensation coefficient;

[0048] Based on the historical attitude quaternion and the equivalent rotation vector, determine the attitude quaternion of the target carrier at the current moment.

[0049] Further, when the stationary determination module is used to respectively compare the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with a preset acceleration threshold and a preset angular velocity threshold to determine whether the target carrier is in a stationary state at the current moment and obtain a determination result, the stationary determination module is used for:

[0050] Respectively compare the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with the preset acceleration threshold and the preset angular velocity threshold, determine whether the acceleration output modulus is less than or equal to the preset acceleration threshold, and determine whether the angular velocity output modulus is equal to the preset angular velocity threshold;

[0051] When the acceleration output modulus is less than or equal to the preset acceleration threshold and the angular velocity output modulus is equal to the preset angular velocity threshold, determine that the determination result is that the target carrier is in a stationary state at the current moment;

[0052] When the acceleration output modulus is greater than the preset acceleration threshold and / or the angular velocity output modulus is not equal to the preset angular velocity threshold, determine that the determination result is that the target carrier is in a non-stationary state at the current moment.

[0053] Further, when the matrix determination module is used to determine the noise covariance matrix of the accelerometer in the attitude and heading reference system at the current moment based on the determination result, the matrix determination module is used for:

[0054] When the determination result is that the target carrier is in a stationary state at the current moment, determine a preset first covariance matrix as the noise covariance matrix of the accelerometer in the attitude and heading reference system at the current moment;

[0055] When the determination result is that the target carrier is in a non-stationary state at the current moment, numerically compare the carrier vibration values and specific force estimation error values corresponding to the target carrier on the horizontal axis, vertical axis, and vertical axis respectively and the motion acceleration of the target carrier, and determine the noise covariance matrix of the accelerometer in the attitude and heading reference system at the current moment based on the numerical comparison result.

[0056] Further, when the matrix determination module is used to numerically compare the carrier vibration values, specific force estimation error values, and the motion acceleration of the target carrier corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and determine the noise covariance matrix of the accelerometer in the attitude reference system at the current moment based on the numerical comparison results, the matrix determination module is used for:

[0057] Obtain the carrier vibration values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier respectively, and numerically compare the carrier vibration values with the preset random walk values corresponding to the horizontal axis, vertical axis, and vertical axis respectively to obtain a first comparison result; wherein, the carrier vibration value represents the standard deviation of the acceleration data of the target carrier within a preset time window before the current moment.

[0058] Select the first numerical values for which the first comparison result is greater from the carrier vibration values or the random walk values corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and construct the first numerical values into a second covariance matrix.

[0059] Obtain the specific force estimation error values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier respectively, and numerically compare the specific force estimation error values with the first numerical values corresponding to the horizontal axis, vertical axis, and vertical axis respectively to obtain a second comparison result.

[0060] Select the second numerical values for which the second comparison result is greater from the specific force estimation error values or the first numerical values corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and construct the second numerical values into a third covariance matrix.

[0061] Obtain the motion acceleration of the target carrier at the current moment, and compare the motion acceleration with the sum of the second numerical values corresponding to the horizontal axis, vertical axis, and vertical axis respectively to obtain a third comparison result.

[0062] When the third comparison result is that the motion acceleration is greater than the sum of the second numerical values, determine that the noise covariance matrix is infinite.

[0063] When the third comparison result is that the motion acceleration is less than or equal to the sum of the second numerical values, determine the third covariance matrix as the noise covariance matrix.

[0064] Further, when the attitude filtering module is used to determine the attitude correction number of the target carrier at the current moment based on the noise covariance matrix, the historical attitude data, and the observation vector of the target carrier at the current moment, the attitude filtering module is used for:

[0065] Determine the state estimation vector and the estimation error covariance matrix of the target carrier at the previous moment in the historical attitude data, and determine the state prediction vector of the target carrier from the previous moment to the current moment based on the state estimation vector of the target carrier at the previous moment and a preset prediction matrix;

[0066] Determine the filtering gain matrix of the target carrier at the current moment based on the noise covariance matrix, the estimation error covariance matrix, the preset observation matrix, and the preset system noise covariance matrix;

[0067] Determine the state estimation vector of the target carrier at the current moment based on the observation vector of the target carrier at the current moment, the filtering gain matrix, the preset observation matrix, and the state prediction vector;

[0068] Determine the attitude correction numbers corresponding to the roll angle, pitch angle, and heading angle of the target carrier at the current moment in the state estimation vector at the current moment.

[0069] Further, when the attitude correction module is used to correct the attitude quaternion by using the attitude correction numbers to obtain the target attitude quaternion of the target carrier at the current moment for correcting the carrier attitude detection of the attitude reference system, the attitude correction module is used for:

[0070] Based on the attitude correction numbers, construct a three-dimensional attitude error matrix of the target carrier at the current moment to determine the three-dimensional attitude error value of the target carrier at the current moment;

[0071] Determine the attitude error quaternion of the target carrier at the current moment based on the three-dimensional attitude error value;

[0072] Perform quaternion multiplication calculation on the attitude error quaternion and the attitude quaternion to obtain the target attitude quaternion of the target carrier at the current moment for correcting the carrier attitude detection of the attitude reference system.

[0073] An embodiment of the present application further provides an electronic device, including: a processor, a memory, and a bus. The memory stores machine-readable instructions executable by the processor. When the electronic device runs, the processor communicates with the memory through the bus. When the machine-readable instructions are executed by the processor, the steps of the attitude correction method of the attitude reference system as described above are executed.

[0074] An embodiment of the present application further provides a computer-readable storage medium. A computer program is stored on the computer-readable storage medium. When the computer program is run by a processor, the steps of the attitude correction method of the attitude reference system as described above are executed.

[0075] The attitude correction method and device for an attitude and heading reference system provided by an embodiment of the present application. The attitude correction method includes: during the process of detecting the carrier attitude using an attitude and heading reference system set on a target carrier, obtaining the historical attitude data of the target carrier at the previous moment, as well as the acceleration data and angular velocity data of the target carrier at the current moment, and determining the attitude quaternion of the target carrier at the current moment based on the historical attitude data and the angular velocity data; respectively comparing the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with a preset acceleration threshold and a preset angular velocity threshold to determine whether the target carrier is in a stationary state at the current moment, and obtaining a judgment result; determining the noise covariance matrix of the accelerometer in the attitude and heading reference system at the current moment based on the judgment result; determining the attitude correction number of the target carrier at the current moment based on the noise covariance matrix, the historical attitude data, and the observation vector of the target carrier at the current moment; and correcting the attitude quaternion using the attitude correction number to obtain the target attitude quaternion of the target carrier at the current moment, so as to correct the carrier attitude detection of the attitude and heading reference system.

[0076] Compared with the method in the prior art of defining the noise covariance matrix as a fixed value for carrier attitude detection when the carrier is in a dynamic state, by adaptively adjusting the noise covariance matrix of the accelerometer in the attitude and heading reference system according to the judgment result of different motion states of the carrier during the process of detecting the attitude of the carrier in different motion scenarios using the attitude and heading reference system, further determining the attitude correction number of the carrier, and using the attitude correction number to correct the attitude quaternion of the detected carrier at the current moment, reliable and accurate three-dimensional attitude solution of the carrier under various working conditions is realized, and the accuracy of the attitude and heading reference system for detecting the carrier attitude is improved.

[0077] To make the above objects, features, and advantages of the present application more obvious and understandable, the following specific preferred embodiments are given, and in conjunction with the accompanying drawings, the detailed description is as follows. Description of the Drawings

[0078] To more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for use in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the present application, and therefore should not be regarded as limiting the scope. For those of ordinary skill in the art, other related drawings can be obtained based on these drawings without creative efforts.

[0079] Figure 1 It is a flowchart of an attitude correction method for an attitude and heading reference system provided by an embodiment of the present application;

[0080] Figure 2Schematic structural diagram of an attitude correction device for an attitude reference system provided by an embodiment of the present application;

[0081] Figure 3 Schematic structural diagram of an electronic device provided by an embodiment of the present application. Detailed implementation manners

[0082] To make the objectives, technical solutions, and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present application. Apparently, the described embodiments are only some, but not all, of the embodiments of the present application. Components of the embodiments of the present application described and illustrated herein generally may be arranged and designed in a variety of different configurations. Therefore, the detailed description of the embodiments of the present application provided herein is not intended to limit the scope of the claimed present application, but is merely representative of selected embodiments of the present application. Based on the embodiments of the present application, every other embodiment obtained by a person of ordinary skill in the art without creative efforts shall fall within the protection scope of the present application.

[0083] Through research, it is found that for a moving vehicle, the accelerometer will simultaneously sense the earth's gravity and the vehicle's motion acceleration. How to enable the attitude reference system to accurately distinguish the earth's gravity and the vehicle's motion acceleration, and how to adjust the system's trust in the accelerometer according to different motion scenarios are crucial issues, which will directly affect the accuracy of vehicle attitude detection.

[0084] Among them, the trust in the accelerometer can be expressed as the weight of the accelerometer observation. That is, Kalman filtering for attitude detection is performed using the noise covariance matrix of the accelerometer-observed acceleration. When the vehicle is stationary, the specific force modulus value output by the accelerometer is equal to the local gravity. When the vehicle is in a dynamic state, the traditional method defines the noise covariance matrix as a fixed value, which will reduce the accuracy of the estimation of the vehicle's state variables, and further reduce the accuracy of the attitude reference system in detecting the vehicle's attitude.

[0085] Therefore, to enable the observed values to act correctly in the attitude reference system, the setting of the noise covariance matrix for the accelerometer observation data needs to be adaptively adjusted according to different motion scenarios of the vehicle, so as to better assist the gyroscope to obtain the accurate vehicle attitude.

[0086] Based on this, the embodiment of the present application provides an attitude correction method for an attitude and heading reference system. During the process of using the attitude and heading reference system to detect the attitude of a carrier in different motion scenarios, according to the judgment results of different motion states of the carrier, the noise covariance matrix of the accelerometer of the attitude and heading reference system is adaptively adjusted, and further the attitude correction number of the carrier is determined. Then, the detected attitude quaternion of the carrier at the current moment is corrected by using the attitude correction number, realizing reliable and accurate three-dimensional attitude calculation of the carrier under various working conditions and improving the accuracy of the attitude and heading reference system in detecting the attitude of the carrier.

[0087] Please refer to Figure 1 , Figure 1 which is a flowchart of an attitude correction method for an attitude and heading reference system provided by an embodiment of the present application. As Figure 1 shown in

[0088] S101. During the process of using the attitude and heading reference system set on the target carrier to detect the carrier attitude, obtain the historical attitude data of the target carrier at the previous moment, the acceleration data and angular velocity data of the target carrier at the current moment, and determine the attitude quaternion of the target carrier at the current moment based on the historical attitude data and the angular velocity data.

[0089] It should be noted that an attitude and heading reference system (AHRS) is an electronic device used to measure the attitude angles (such as pitch angle, roll angle, and yaw angle, etc.) and heading of an aircraft, and is usually composed of a three-axis accelerometer, a three-axis gyroscope, a three-axis magnetic sensor, and related processing circuits. The main function of the AHRS is to provide accurate data for flight control systems, navigation systems, and other systems that rely on accurate attitude and heading information.

[0090] Here, due to the advantages of miniaturization, low cost, and high precision of the micro electro mechanical system (MEMS), the application scenarios of the attitude and heading reference system are no longer limited to the attitude control systems of various aircraft such as aircraft, rockets, and navigators, but turn to various intelligent civilian fields such as unmanned vehicles, robots, virtual reality devices, and wearable intelligent devices.

[0091] In the embodiment of the present application, the target carrier provided with the attitude and heading reference system may include, but is not limited to, aircraft, vehicles, robots, and intelligent devices, such as aircraft, rockets, navigators, unmanned vehicles, robots, virtual reality devices, and wearable intelligent devices, etc.

[0092] In this step, when detecting the attitude of the target vehicle using the attitude reference system set on the target vehicle, initial alignment between the target vehicle and the attitude reference system is required. After completing the initial alignment, the initial attitude quaternion of the target vehicle at the initial moment is obtained using the attitude reference system, that is, where n is the navigation coordinate system; b is the vehicle coordinate system; and t0 is the initial moment.

[0093] Specifically, when performing the initial alignment between the target vehicle and the attitude reference system, first, control and ensure that the target vehicle is in a stationary state. When the attitude reference system is powered on and the power-on duration is greater than the first preset time, it is determined that the output of the attitude reference system is in a stable state. At this time, the initial alignment between the target vehicle and the attitude reference system is started. During the initial alignment process, the stationary state of the target vehicle must be ensured, and the initial alignment is completed when the duration of the initial alignment process is greater than or equal to the second preset time.

[0094] Furthermore, the acceleration data and angular velocity data of the target vehicle at the current moment are respectively obtained using the three-axis accelerometer and three-axis gyroscope set in the attitude reference system, and the historical attitude data of the target vehicle at the previous moment is obtained from the database set in the attitude reference system.

[0095] Here, the target vehicle can also respectively obtain the acceleration data and angular velocity data of the target vehicle at the current moment using the three-axis accelerometer and three-axis gyroscope in the installed inertial measurement unit.

[0096] Among them, the acceleration data may include, but is not limited to, the accelerations corresponding to the horizontal axis, vertical axis, and vertical axis of the target vehicle respectively; the angular velocity data may include, but is not limited to, the angular velocities and angle values corresponding to the horizontal axis, vertical axis, and vertical axis of the target vehicle respectively.

[0097] The historical attitude data may include, but is not limited to, the historical attitude quaternion, historical angular velocity data, specific force measured value, state estimation vector, and estimation error covariance matrix of the target vehicle at the previous moment, etc.

[0098] In an implementation manner of the present application, in specific implementation, the step of determining the attitude quaternion of the target vehicle at the current moment based on the historical attitude data and the angular velocity data in step S101 may include:

[0099] S1011. Determine the historical attitude quaternion and historical angular velocity data of the target vehicle at the previous moment from the historical attitude data.

[0100] In this step, the historical attitude quaternion and historical angular velocity data of the target carrier at the previous moment are respectively determined from the obtained historical attitude data; wherein, the historical angular velocity data may include the angular velocity and angle values corresponding to the transverse axis, longitudinal axis, and vertical axis of the target carrier at the previous moment.

[0101] Here, in the process of detecting the carrier attitude using the attitude reference system set on the target carrier, the form of determining the target carrier attitude adopts the attitude quaternion. For example, the historical attitude quaternion can be expressed as where n is the navigation coordinate system; b is the carrier coordinate system; t k-1 is the previous moment.

[0102] S1012. Based on the angular velocity data and the historical angular velocity data, determine the gyro angle increment of the target carrier at the current moment, and calculate the equivalent rotation vector of the target carrier at the current moment based on the gyro angle increment and the preset coning compensation coefficient.

[0103] It should be noted that in actual situations, the rotation of the axis vector of the target carrier is non-fixed-axis rotation, and the axis of rotation and rotation speed of the target carrier change with time. When two orthogonal axes of the attitude reference system simultaneously perform angular vibrations with different phases, the target carrier will exhibit coning motion, thereby exciting non-commutable errors. Coning motion is the worst motion for attitude drift and can induce the largest drift angular velocity. When the update period of the target carrier is shorter, the non-commutable error is smaller.

[0104] In this step, in specific implementation, the angle values in the angular velocity data of the target carrier at the current moment are subtracted from the angle values in the historical angular velocity data of the previous moment to determine the gyro angle increment of the target carrier at the current moment; then, based on the gyro angle increment and the preset coning compensation coefficient, the equivalent rotation vector of the target carrier at the current moment is calculated using the preset equivalent rotation vector expression.

[0105] Here, when the angular velocity data does not include angle values, the angular velocities at the current moment and the previous moment can be multiplied by the window time from the previous moment to the current moment and then subtracted to obtain the gyro angle increment of the target carrier at the current moment.

[0106] In the embodiments of the present application, the equivalent rotation vector expression is as follows.

[0107]

[0108] where σ represents the equivalent rotation vector of the target carrier at the current moment; Δθ represents the gyro angle increment of the target carrier at the current moment; i represents the number of sub-samples. Generally, two sub-samples are selected in the attitude reference system, that is, N is 2; k N-iRepresents the coning compensation coefficient.

[0109] Here, the equivalent rotation vector can be represented by the sum of the direct integration term and the coning compensation term of the target carrier from the previous moment to the current moment. Another representation of the equivalent rotation vector is as follows.

[0110] σ = α k +δα k ;

[0111] Among them, α k Represents the direct integration term of the target carrier from the previous moment to the current moment; δα k Represents the coning compensation term of the target carrier from the previous moment to the current moment.

[0112] S1013. Based on the historical attitude quaternion and the equivalent rotation vector, determine the attitude quaternion of the target carrier at the current moment.

[0113] In this step, substitute the historical attitude quaternion and the equivalent rotation vector into the attitude update quaternion expression to calculate the attitude quaternion of the target carrier at the current moment.

[0114] In the embodiment of the present application, the attitude update quaternion expression is as follows.

[0115]

[0116] Among them, Represents the attitude quaternion of the target carrier at the current moment; Represents the historical attitude quaternion of the target carrier at the previous moment; σ represents the equivalent rotation vector of the target carrier at the current moment.

[0117] S102. Respectively compare the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with a preset acceleration threshold and a preset angular velocity threshold to determine whether the target carrier is in a stationary state at the current moment, and obtain a judgment result.

[0118] In the embodiment of the present application, when using an attitude and heading reference system to perform attitude calculation on a target carrier, since the attitude and heading reference system uses an accelerometer for detection, it is necessary to clarify whether the target carrier is in a stationary state to determine whether the attitude and heading reference system depends on the accelerometer, and the degree of dependence of the attitude and heading reference system on the accelerometer when it depends on the accelerometer, and then further determine the noise covariance matrix when the accelerometer observes the attitude.

[0119] In an implementation manner of the present application, in specific implementation, step S102 may include:

[0120] S1021. Compare the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with a preset acceleration threshold and a preset angular velocity threshold respectively, to determine whether the acceleration output modulus is less than or equal to the preset acceleration threshold, and to determine whether the angular velocity output modulus is equal to the preset angular velocity threshold.

[0121] In an embodiment of the present application, when the target carrier is in a stationary state, the theoretical acceleration output modulus of the target carrier should be equal to the gravitational acceleration at the location of the target carrier, and the theoretical angular velocity output modulus of the target carrier should be equal to 0. Therefore, the preset acceleration threshold is generally set to the gravitational acceleration at the location of the target carrier, and the preset angular velocity threshold is generally set to 0.

[0122] In this step, in specific implementation, first, determine the acceleration output modulus in the acceleration data and determine the angular velocity output modulus in the angular velocity data; then, compare the acceleration output modulus with the preset acceleration threshold to determine whether the acceleration output modulus is less than or equal to the preset acceleration threshold; finally, compare the angular velocity output modulus with the preset angular velocity threshold to determine whether the angular velocity output modulus is equal to the preset angular velocity threshold.

[0123] S1022. When the acceleration output modulus is less than or equal to the preset acceleration threshold and the angular velocity output modulus is equal to the preset angular velocity threshold, determine that the judgment result is that the target carrier is in a stationary state at the current moment.

[0124] In this step, when the acceleration output modulus is less than or equal to the preset acceleration threshold and the angular velocity output modulus is equal to the preset angular velocity threshold, determine that the judgment result of whether the target carrier is in a stationary state at the current moment is that the target carrier is in a stationary state at the current moment.

[0125] S1023. When the acceleration output modulus is greater than the preset acceleration threshold and / or the angular velocity output modulus is not equal to the preset angular velocity threshold, determine that the judgment result is that the target carrier is in a non-stationary state at the current moment.

[0126] In this step, when the acceleration output modulus is greater than the preset acceleration threshold and / or the angular velocity output modulus is not equal to the preset angular velocity threshold, determine that the judgment result of whether the target carrier is in a stationary state at the current moment is that the target carrier is in a non-stationary state at the current moment.

[0127] S103. Based on the judgment result, determine the noise covariance matrix of the accelerometer in the attitude reference system at the current moment.

[0128] In the embodiments of the present application, for the two cases where the judgment result is that the target carrier is in a stationary state and a non-stationary state at the current moment, when the attitude reference system calculates the attitude of the target carrier, the noise covariance matrix of the accelerometer in the attitude reference system at the current moment is determined respectively.

[0129] Among them, the noise covariance matrix plays a key role in the Kalman filter and its variants (such as the Extended Kalman Filter EKF and the Unscented Kalman Filter UKF, etc.). It quantifies the uncertainty or noise characteristics in the detected attitude, that is, the deviation between the sensor reading and the true value. In the attitude reference system, the noise covariance matrix reflects the noise level of the measurement data of sensors such as accelerometers, gyroscopes, and magnetometers. The noise covariance matrix is a symmetric positive definite matrix, and its dimension is the same as that of the observation vector. In the attitude reference system, the observation vector may include but is not limited to attitude angles (pitch angle, roll angle, yaw angle), angular velocity, and geomagnetic field intensity, etc.

[0130] In the embodiments of the present application, when the observation vector is the attitude of the target carrier, the observation vector may include but is not limited to attitude angles (pitch angle, roll angle, yaw angle), and the noise covariance matrix of the accelerometer may include the noise variances of the accelerometer on the horizontal axis, vertical axis, and vertical axis.

[0131] Exemplarily, the representation form of the noise covariance matrix of the accelerometer is as follows.

[0132]

[0133] Among them, R represents the noise covariance matrix of the accelerometer in the attitude reference system at the current moment; r1, r2, and r3 respectively represent the noise variances of the accelerometer on the horizontal axis, vertical axis, and vertical axis.

[0134] In an implementation manner of the present application, in specific implementation, step S103 may include:

[0135] S1031. When the judgment result is that the target carrier is in a stationary state at the current moment, a preset first covariance matrix is determined as the noise covariance matrix of the accelerometer in the attitude reference system at the current moment.

[0136] In this step, when the judgment result is that the target carrier is in a stationary state at the current moment, it indicates that the measured value of the acceleration data collected by the accelerometer in the attitude reference system is relatively accurate. When the attitude reference system calculates the attitude of the target carrier, it is selected to approximately believe the acceleration data observed by the accelerometer, and a preset first covariance matrix is determined as the noise covariance matrix of the accelerometer in the attitude reference system at the current moment.

[0137] Among them, the noise variances of the accelerometer in the first covariance matrix on the horizontal axis, vertical axis, and vertical axis are all preset empirical values, and the set noise variance values are relatively small, that is, the value of the first covariance matrix is set relatively small.

[0138] S1032. When the judgment result is that the target carrier is in a non-stationary state at the current moment, numerically compare the carrier vibration values, specific force estimation error values, and the motion acceleration of the target carrier corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and determine the noise covariance matrix of the accelerometer in the attitude reference system at the current moment based on the numerical comparison result.

[0139] In the embodiment of the present application, when the target carrier is in a non-stationary state, there are various different motion states. For example, the target carrier is affected by vibration interference from the external environment, vibration caused by the target carrier itself (such as an automobile engine), and the carrier is moving. At this time, the measured values of the acceleration data will be interfered, and it is necessary to increase the noise covariance matrix of the accelerometer at the current moment (that is, increase the noise variances of the accelerometer on the horizontal axis, vertical axis, and vertical axis) to reduce the degree of trust in the acceleration data.

[0140] In an implementation manner of the present application, in specific implementation, step S1032 may include:

[0141] S10321. Obtain the carrier vibration values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier respectively, and numerically compare the carrier vibration values with the randomly walked values preset for the horizontal axis, vertical axis, and vertical axis respectively to obtain a first comparison result.

[0142] In this step, obtain the carrier vibration values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier respectively; among them, the carrier vibration value represents the standard deviation of the acceleration data of the target carrier within a preset time window before the current moment.

[0143] Specifically, within a preset time window before the current moment, obtain the acceleration data corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier in this time window, and calculate the standard deviation corresponding to the acceleration data to determine the carrier vibration values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier.

[0144] Further, numerically compare the carrier vibration values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier with the randomly walked values preset for the horizontal axis, vertical axis, and vertical axis respectively to obtain a first comparison result.

[0145] Among them, the randomly walked value can be determined based on the factory indicators of the sensors for detecting acceleration and angular velocity. For example, determine the randomly walked values of the horizontal axis, vertical axis, and vertical axis in the factory indicators of the inertial measurement unit.

[0146] S10322. Select the first numerical value where the first comparison result is greater from the carrier vibration values or the random walk values corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and construct the first numerical value into a second covariance matrix.

[0147] In this step, based on the first comparison result of the numerical comparison between the carrier vibration values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier and the randomly preset walk values corresponding to the horizontal axis, vertical axis, and vertical axis respectively, select the numerical value with the larger value among the carrier vibration values and the random walk values in the horizontal axis, vertical axis, and vertical axis, and determine the numerical value with the larger value among the carrier vibration values and the random walk values as the first numerical value.

[0148] For example, when numerically comparing the carrier vibration values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier with the randomly preset walk values corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and the first comparison result is that the carrier vibration values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier are greater than the randomly preset walk values corresponding to the horizontal axis, vertical axis, and vertical axis respectively, then determine the carrier vibration values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier as the first numerical value.

[0149] Furthermore, determine the first numerical values corresponding to the horizontal axis, vertical axis, and vertical axis as the noise variances of the accelerometer on the horizontal axis, vertical axis, and vertical axis respectively, so as to construct a second covariance matrix composed of the first numerical values.

[0150] S10323. Obtain the specific force estimation error values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier respectively, and numerically compare the specific force estimation error values with the first numerical values corresponding to the horizontal axis, vertical axis, and vertical axis respectively to obtain a second comparison result.

[0151] In this step, after determining the second covariance matrix composed of the first numerical values, obtain the specific force estimation error values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier respectively.

[0152] Specifically, when determining the specific force estimation error values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier respectively, for each spatial axis, first, obtain the measured specific force value of the target carrier at the previous moment; then, based on the measured specific force value at the previous moment, the angular velocity data of the target carrier at the current moment, and the time interval from the previous moment to the current moment, determine the predicted specific force value of the target carrier at the current moment; then, based on the predicted specific force value and the measured specific force value of the target carrier at the current moment, determine the specific force estimation error value of the target carrier at the current moment; finally, through the calculation of each spatial axis, determine the specific force estimation error values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier respectively.

[0153] In the embodiment of the present application, the formula expression for calculating the predicted specific force value is as follows.

[0154]

[0155] Among them, f b (t k ) represents the predicted specific force value of the target vehicle at the current moment; represents the measured specific force value of the target vehicle at the previous moment; ω(t k ) represents the angular velocity data of the target vehicle at the current moment; dt represents the time interval from the previous moment to the current moment.

[0156] In the embodiments of the present application, the formula expression for calculating the specific force estimation error value is as follows.

[0157]

[0158] Among them, Δf b (t k ) represents the specific force estimation error value of the target vehicle at the current moment, that is, and respectively represent the specific force estimation error values corresponding to the horizontal axis, vertical axis, and vertical axis of the target vehicle at the current moment; represents the measured specific force value of the target vehicle at the current moment; f b (t k ) represents the predicted specific force value of the target vehicle at the current moment.

[0159] Furthermore, the specific force estimation error values corresponding to the horizontal axis, vertical axis, and vertical axis are respectively numerically compared with the first numerical values corresponding to the horizontal axis, vertical axis, and vertical axis to obtain a second comparison result including the comparison results of each spatial axis.

[0160] S10324. Respectively screen out the second numerical values for which the second comparison result is greater among the specific force estimation error values or the first numerical values corresponding to the horizontal axis, vertical axis, and vertical axis, and construct the second numerical values into a third covariance matrix.

[0161] In this step, based on the second comparison result of numerically comparing the specific force estimation error values corresponding to the horizontal axis, vertical axis, and vertical axis with the first numerical values corresponding to the horizontal axis, vertical axis, and vertical axis, screen out the numerically larger numerical values among the specific force estimation error values and the first numerical values in the horizontal axis, vertical axis, and vertical axis, and determine the numerically larger numerical values among the specific force estimation error values and the first numerical values as the second numerical values. For example, when numerically comparing the specific force estimation error values corresponding to the horizontal axis, vertical axis, and vertical axis with the first numerical values corresponding to the horizontal axis, vertical axis, and vertical axis, and the second comparison result is that the specific force estimation error values corresponding to the horizontal axis, vertical axis, and vertical axis are greater than the first numerical values corresponding to the horizontal axis, vertical axis, and vertical axis, then determine the specific force estimation error values corresponding to the horizontal axis, vertical axis, and vertical axis as the second numerical values.

[0162] Further, the second numerical values corresponding to the horizontal axis, the vertical axis, and the vertical axis are respectively determined as the noise variances of the accelerometer on the horizontal axis, the vertical axis, and the vertical axis, so as to construct a third covariance matrix composed of the second numerical values.

[0163] S10325. Obtain the motion acceleration of the target carrier at the current moment, and compare the motion acceleration with the sum of the second numerical values corresponding to the horizontal axis, the vertical axis, and the vertical axis respectively, to obtain a third comparison result.

[0164] In this step, in specific implementation, first, subtract the output modulus of the acceleration of the target carrier at the current moment from the gravitational acceleration at the location of the target carrier, and determine the result of the subtraction calculation as the motion acceleration of the target carrier; then, calculate the sum of the second numerical values corresponding to the horizontal axis, the vertical axis, and the vertical axis respectively; finally, perform a numerical comparison between the motion acceleration and the calculated sum to obtain a third comparison result.

[0165] Here, the motion acceleration of the target carrier at the current moment is a numerical value, not a three-dimensional vector.

[0166] S10326. When the third comparison result is that the motion acceleration is greater than the sum of the second numerical values, then determine that the noise covariance matrix is infinite.

[0167] In this step, when the third comparison result is that the motion acceleration is greater than the sum of the second numerical values, at this time, when the attitude of the target carrier is solved in the attitude and heading reference system, the acceleration data observed by the accelerometer is completely not trusted, that is, the noise covariance matrix of the accelerometer is determined to be infinite.

[0168] S10327. When the third comparison result is that the motion acceleration is less than or equal to the sum of the second numerical values, determine the third covariance matrix as the noise covariance matrix.

[0169] In this step, when the third comparison result is that the motion acceleration is less than or equal to the sum of the second numerical values, determine the third covariance matrix constructed by the second numerical values as the noise covariance matrix of the accelerometer.

[0170] S104. Based on the noise covariance matrix, the historical attitude data, and the observation vector of the target carrier at the current moment, determine the attitude correction number of the target carrier at the current moment.

[0171] In the embodiment of the present application, by performing Kalman filter calculation on the noise covariance matrix, the historical attitude data, and the observation vector of the target carrier at the current moment, the attitude correction number of the target carrier at the current moment is determined.

[0172] Among them, the Kalman filter calculation includes state prediction calculation and state estimation calculation. The state prediction calculation is used to determine the state prediction vector of the target carrier from the previous moment to the current moment, and the state estimation calculation is used to determine the state estimation vector of the target carrier at the current moment.

[0173] In an implementation manner of the present application, in specific implementation, step S104 may include:

[0174] S1041. Determine the state estimation vector and the estimation error covariance matrix of the target carrier at the previous moment in the historical attitude data, and based on the state estimation vector of the target carrier at the previous moment and a preset prediction matrix, determine the state prediction vector of the target carrier from the previous moment to the current moment.

[0175] In this step, determine the state estimation vector and the estimation error covariance matrix of the target carrier at the previous moment in the historical attitude data; then, input the state estimation vector of the target carrier at the previous moment and the preset prediction matrix into the expression of the one-step state prediction equation, and calculate and determine the state prediction vector of the target carrier from the previous moment to the current moment.

[0176] In the embodiment of the present application, the expression of the one-step state prediction equation is as follows.

[0177]

[0178] Among them, represents the state prediction vector of the target carrier from the previous moment to the current moment; represents the state estimation vector of the target carrier at the previous moment; Φ k / k-1 represents the preset prediction matrix.

[0179] S1042. Based on the noise covariance matrix, the estimation error covariance matrix, the preset observation matrix, and the preset system noise covariance matrix, determine the filtering gain matrix of the target carrier at the current moment.

[0180] In this step, in specific implementation, first, based on the estimation error covariance matrix of the target carrier at the previous moment, the preset observation matrix, and the preset system noise covariance matrix, use the one-step mean square error prediction equation to determine the estimation error covariance matrix of the target carrier from the previous moment to the current moment; then, input the noise covariance matrix, the estimation error covariance matrix, and the preset observation matrix into the expression of the filtering gain equation; finally, determine the filtering gain matrix of the target carrier at the current moment through the calculation of the filtering gain equation.

[0181] In the embodiment of the present application, the expression of the one-step mean square error prediction equation is as follows.

[0182]

[0183] Among them, P k / k-1 represents the estimated error covariance matrix of the target carrier from the previous moment to the current moment; P k-1 represents the estimated error covariance matrix of the target carrier at the previous moment; Q k-1 represents the system noise covariance matrix preset by the attitude and heading reference system; Φ k / k-1 represents the preset prediction matrix.

[0184] In the embodiment of the present application, the expression of the filtering gain equation is as follows.

[0185]

[0186] Among them, K k represents the filtering gain matrix of the target carrier at the current moment; P k / k-1 represents the estimated error covariance matrix of the target carrier from the previous moment to the current moment; R k represents the noise covariance matrix of the accelerometer; H k represents the preset observation matrix.

[0187] Here, based on the filtering gain equation, it can be known that when the value of the noise covariance matrix of the accelerometer is larger, the filtering gain matrix is smaller, that is, the weight of the accelerometer-observed acceleration data in the attitude state update is smaller, indicating that the constraint effect of the accelerometer on the state quantity estimation in the attitude solution is smaller, which is equivalent to reducing the trust level in the accelerometer. And when the value of the noise covariance matrix of the accelerometer is smaller, the filtering gain matrix is larger, that is, the trust level in the accelerometer is increased.

[0188] S1043. Determine the state estimation vector of the target carrier at the current moment based on the observation vector of the target carrier at the current moment, the filtering gain matrix, the preset observation matrix, and the state prediction vector.

[0189] In this step, in specific implementation, first, obtain the prediction vector and the measured vector of the target carrier at the current moment, and subtract the prediction vector from the measured vector to determine the observation vector of the target carrier at the current moment; then, input the observation vector of the target carrier at the current moment, the filtering gain matrix, the preset observation matrix, and the state prediction vector of the target carrier from the previous moment to the current moment into the expression of the state one-step estimation equation; finally, calculate and determine the state estimation vector of the target carrier at the current moment through the state one-step estimation equation.

[0190] In the embodiment of the present application, the expression of the state one-step estimation equation is as follows.

[0191]

[0192] Among them, represents the state estimation vector of the target carrier at the current moment; K k represents the filtering gain matrix of the target carrier at the current moment; H k represents the preset observation matrix; Z k represents the observation vector of the target carrier at the current moment; represents the state prediction vector of the target carrier from the previous moment to the current moment.

[0193] Furthermore, the estimated error covariance matrix of the target carrier from the previous moment to the current moment, the preset observation matrix, the filtering gain matrix of the target carrier at the current moment, and the identity matrix are input into the expression of the one-step mean square error estimation equation of the state to determine the estimated error covariance matrix of the target carrier at the current moment.

[0194] In the embodiment of the present application, the expression of the one-step mean square error estimation equation of the state is as follows.

[0195] P k =(I - K k H k )P k / k-1 .

[0196] Among them, P k represents the estimated error covariance matrix of the target carrier at the current moment; I represents the identity matrix; H k represents the preset observation matrix; Z k represents the observation vector of the target carrier at the current moment; P k / k-1 represents the estimated error covariance matrix of the target carrier from the previous moment to the current moment.

[0197] S1044. Determine the attitude correction numbers corresponding to the roll angle, pitch angle, and heading angle of the target carrier at the current moment in the state estimation vector at the current moment.

[0198] In this step, the calculated state estimation vector of the target carrier at the current moment may include various state quantities. The state estimation vector may include but is not limited to three-dimensional attitude angle errors and gyro zero bias errors, etc. That is, the attitude correction numbers corresponding to the roll angle, pitch angle, and heading angle of the target carrier at the current moment can be determined in the state estimation vector of the target carrier at the current moment.

[0199] In the embodiment of the present application, the matrix expression of the state estimation vector of the target carrier at the current moment is as follows.

[0200]

[0201] Among them, δθ, δφ, and δψ respectively represent the attitude correction numbers corresponding to the roll angle, pitch angle, and heading angle of the target carrier at the current moment; represents the state estimation vector of the target carrier at the current moment.

[0202] S105. Use the attitude correction number to correct the attitude quaternion to obtain the target attitude quaternion of the target carrier at the current moment, so as to correct the carrier attitude detection of the attitude reference system.

[0203] In the embodiment of the present application, the attitude correction number can be determined through Kalman filtering calculation, and the attitude quaternion of the target carrier at the current moment is corrected by using the attitude correction number to obtain the target attitude quaternion of the corrected target carrier at the current moment.

[0204] In an implementation manner of the present application, in specific implementation, step S105 may include:

[0205] S1051. Based on the attitude correction number, construct a three-dimensional attitude error matrix of the target carrier at the current moment to determine the three-dimensional attitude error value of the target carrier at the current moment.

[0206] In the embodiment of the present application, the expression of the three-dimensional attitude error matrix of the target carrier at the current moment is as follows.

[0207] σ′ = [δφ δθ δψ] T .

[0208] Among them, σ′ represents the three-dimensional attitude error matrix of the target carrier at the current moment; δθ, δφ, and δψ respectively represent the attitude correction numbers corresponding to the roll angle, pitch angle, and heading angle of the target carrier at the current moment.

[0209] Further, by performing numerical calculation on the three-dimensional attitude error matrix of the target carrier at the current moment, the three-dimensional attitude error value of the target carrier at the current moment is determined.

[0210] In the embodiment of the present application, the expression for calculating the three-dimensional attitude error value of the target carrier at the current moment is as follows.

[0211]

[0212] Among them, |σ′| represents the three-dimensional attitude error value of the target carrier at the current moment; δθ, δφ, and δψ respectively represent the attitude correction numbers corresponding to the roll angle, pitch angle, and heading angle of the target carrier at the current moment.

[0213] S1052. Based on the three-dimensional attitude error value, determine the attitude error quaternion of the target carrier at the current moment.

[0214] In this step, the three-dimensional attitude error value is input into a preset attitude error quaternion formula to calculate and determine the attitude error quaternion of the target carrier at the current moment.

[0215] In the embodiment of the present application, the expression of the attitude error quaternion formula is as follows.

[0216]

[0217] Wherein, represents the attitude error quaternion of the target carrier at the current moment (t k ); |σ′| represents the three-dimensional attitude error value of the target carrier at the current moment.

[0218] S1053. Perform quaternion multiplication calculation on the attitude error quaternion and the attitude quaternion to obtain the target attitude quaternion of the target carrier at the current moment, so as to correct the carrier attitude detection of the attitude reference system.

[0219] In this step, perform quaternion multiplication calculation on the attitude error quaternion and the attitude quaternion of the target carrier at the current moment. By calculating, the target attitude quaternion after correcting the attitude quaternion of the target carrier at the current moment is obtained, realizing the correction of the attitude detection of the attitude reference system for the target carrier.

[0220] In the embodiment of the present application, the expression for calculating the target attitude quaternion of the target carrier at the current moment is as follows.

[0221]

[0222] Wherein, represents the target attitude quaternion of the target carrier at the current moment; represents the attitude error quaternion of the target carrier at the current moment; represents the attitude quaternion of the target carrier at the current moment.

[0223] The attitude correction method of the attitude reference system provided by the embodiment of the present application, in the process of using the attitude reference system to detect the attitude of the carrier in different motion scenarios, adaptively adjusts the noise covariance matrix of the accelerometer of the attitude reference system according to the judgment result of different motion states of the carrier, further determines the attitude correction number of the carrier, and uses the attitude correction number to correct the attitude quaternion of the detected carrier at the current moment, realizing reliable and accurate three-dimensional attitude calculation of the carrier under various working conditions and improving the accuracy of the attitude reference system for detecting the attitude of the carrier.

[0224] Please refer to Figure 2 , Figure 2The structural schematic diagram of an attitude correction device for an attitude and heading reference system provided by an embodiment of the present application. As Figure 2 shown in, the attitude correction device 200 includes:

[0225] An attitude determination module 210, configured to obtain historical attitude data of the target carrier at the previous moment and acceleration data and angular velocity data of the target carrier at the current moment during the process of detecting the carrier attitude by using an attitude and heading reference system set by the target carrier, and determine the attitude quaternion of the target carrier at the current moment based on the historical attitude data and the angular velocity data;

[0226] A static determination module 220, configured to respectively compare the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with a preset acceleration threshold and a preset angular velocity threshold, and determine whether the target carrier is in a static state at the current moment to obtain a determination result;

[0227] A matrix determination module 230, configured to determine the noise covariance matrix of the accelerometer in the attitude and heading reference system at the current moment based on the determination result;

[0228] An attitude filtering module 240, configured to determine the attitude correction number of the target carrier at the current moment based on the noise covariance matrix, the historical attitude data, and the observation vector of the target carrier at the current moment;

[0229] An attitude correction module 250, configured to correct the attitude quaternion by using the attitude correction number to obtain the target attitude quaternion of the target carrier at the current moment, so as to correct the carrier attitude detection of the attitude and heading reference system.

[0230] Further, when the attitude determination module 210 is configured to determine the attitude quaternion of the target carrier at the current moment based on the historical attitude data and the angular velocity data, the attitude determination module 210 is configured to:

[0231] Determine the historical attitude quaternion and historical angular velocity data of the target carrier at the previous moment in the historical attitude data;

[0232] Determine the gyro angle increment of the target carrier at the current moment based on the angular velocity data and the historical angular velocity data, and calculate the equivalent rotation vector of the target carrier at the current moment based on the gyro angle increment and a preset coning compensation coefficient;

[0233] Determine the attitude quaternion of the target carrier at the current moment based on the historical attitude quaternion and the equivalent rotation vector.

[0234] Further, when the stationary determination module 220 is used to respectively compare the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with a preset acceleration threshold and a preset angular velocity threshold to determine whether the target carrier is in a stationary state at the current moment and obtain a determination result, the stationary determination module 220 is used for:

[0235] Respectively compare the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with a preset acceleration threshold and a preset angular velocity threshold, determine whether the acceleration output modulus is less than or equal to the preset acceleration threshold, and determine whether the angular velocity output modulus is equal to the preset angular velocity threshold;

[0236] When the acceleration output modulus is less than or equal to the preset acceleration threshold and the angular velocity output modulus is equal to the preset angular velocity threshold, determine that the determination result is that the target carrier is in a stationary state at the current moment;

[0237] When the acceleration output modulus is greater than the preset acceleration threshold and / or the angular velocity output modulus is not equal to the preset angular velocity threshold, determine that the determination result is that the target carrier is in a non-stationary state at the current moment.

[0238] Further, when the matrix determination module 230 is used to determine the noise covariance matrix of the accelerometer in the attitude reference system at the current moment based on the determination result, the matrix determination module 230 is used for:

[0239] When the determination result is that the target carrier is in a stationary state at the current moment, determine the preset first covariance matrix as the noise covariance matrix of the accelerometer in the attitude reference system at the current moment;

[0240] When the determination result is that the target carrier is in a non-stationary state at the current moment, perform a numerical comparison on the carrier vibration values, specific force estimation error values, and the motion acceleration of the target carrier corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and determine the noise covariance matrix of the accelerometer in the attitude reference system at the current moment based on the numerical comparison result.

[0241] Further, when the matrix determination module 230 is used to perform a numerical comparison on the carrier vibration values, specific force estimation error values, and the motion acceleration of the target carrier corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and determine the noise covariance matrix of the accelerometer in the attitude reference system at the current moment based on the numerical comparison result, the matrix determination module 230 is used for:

[0242] Obtain the carrier vibration values corresponding to the target carrier on the horizontal axis, vertical axis, and vertical axis respectively, and numerically compare the carrier vibration values with the randomly walked values preset on the horizontal axis, vertical axis, and vertical axis respectively to obtain a first comparison result; wherein, the carrier vibration value represents the standard deviation of the acceleration data of the target carrier within a preset time window forward from the current moment;

[0243] Among the carrier vibration values or the randomly walked values corresponding to the horizontal axis, vertical axis, and vertical axis respectively, respectively screen out the first numerical values for which the first comparison result is greater than, and construct the first numerical values into a second covariance matrix;

[0244] Obtain the specific force estimation error values corresponding to the target carrier on the horizontal axis, vertical axis, and vertical axis respectively, and numerically compare the specific force estimation error values with the first numerical values corresponding to the horizontal axis, vertical axis, and vertical axis respectively to obtain a second comparison result;

[0245] Among the specific force estimation error values or the first numerical values corresponding to the horizontal axis, vertical axis, and vertical axis respectively, respectively screen out the second numerical values for which the second comparison result is greater than, and construct the second numerical values into a third covariance matrix;

[0246] Obtain the motion acceleration of the target carrier at the current moment, and compare the motion acceleration with the sum of the second numerical values corresponding to the horizontal axis, vertical axis, and vertical axis respectively to obtain a third comparison result;

[0247] When the third comparison result is that the motion acceleration is greater than the sum of the second numerical values, then determine that the noise covariance matrix is infinite;

[0248] When the third comparison result is that the motion acceleration is less than or equal to the sum of the second numerical values, determine the third covariance matrix as the noise covariance matrix.

[0249] Further, when the attitude filtering module 240 is used to determine the attitude correction number of the target carrier at the current moment based on the noise covariance matrix, the historical attitude data, and the observation vector of the target carrier at the current moment, the attitude filtering module 240 is used to:

[0250] Determine the state estimation vector and the estimation error covariance matrix of the target carrier at the previous moment in the historical attitude data, and based on the state estimation vector of the target carrier at the previous moment and a preset prediction matrix, determine the state prediction vector of the target carrier from the previous moment to the current moment;

[0251] Based on the noise covariance matrix, the estimation error covariance matrix, the preset observation matrix, and the preset system noise covariance matrix, determine the filtering gain matrix of the target carrier at the current moment;

[0252] Determine the state estimation vector of the target vehicle at the current moment based on the observation vector of the target vehicle at the current moment, the filtering gain matrix, the preset observation matrix, and the state prediction vector;

[0253] Determine the attitude correction numbers corresponding to the roll angle, pitch angle, and heading angle of the target vehicle at the current moment in the state estimation vector at the current moment.

[0254] Further, when the attitude correction module 250 is used to correct the attitude quaternion by using the attitude correction numbers to obtain the target attitude quaternion of the target vehicle at the current moment for correcting the vehicle attitude detection of the attitude and heading reference system, the attitude correction module 250 is used for:

[0255] Based on the attitude correction numbers, construct a three-dimensional attitude error matrix of the target vehicle at the current moment to determine the three-dimensional attitude error value of the target vehicle at the current moment;

[0256] Based on the three-dimensional attitude error value, determine the attitude error quaternion of the target vehicle at the current moment;

[0257] Perform quaternion multiplication calculation on the attitude error quaternion and the attitude quaternion to obtain the target attitude quaternion of the target vehicle at the current moment for correcting the vehicle attitude detection of the attitude and heading reference system.

[0258] The attitude correction device of the attitude and heading reference system provided by the embodiments of the present application, during the process of using the attitude and heading reference system to detect the attitude of the vehicle in different motion scenarios, adaptively adjusts the noise covariance matrix of the accelerometer of the attitude and heading reference system according to the judgment result of different motion states of the vehicle, further determines the attitude correction numbers of the vehicle, and uses the attitude correction numbers to correct the attitude quaternion of the detected vehicle at the current moment, realizing reliable and accurate three-dimensional attitude calculation of the vehicle under various working conditions and improving the accuracy of the attitude and heading reference system in detecting the vehicle attitude.

[0259] Please refer to Figure 3 , Figure 3 which is a schematic structural diagram of an electronic device provided by the embodiments of the present application. As Figure 3 shown in, the electronic device 300 includes a processor 310, a memory 320, and a bus 330.

[0260] The memory 320 stores machine-readable instructions executable by the processor 310. When the electronic device 300 runs, the processor 310 communicates with the memory 320 through the bus 330. When the machine-readable instructions are executed by the processor 310, they can execute as described aboveFigure 1 The steps of the attitude correction method of the attitude and heading reference system in the method embodiment shown can be specifically implemented by referring to the method embodiment, which will not be elaborated here.

[0261] The embodiments of the present application also provide a computer-readable storage medium, on which a computer program is stored. When the computer program is run by a processor, it can execute the steps of the attitude correction method of the attitude and heading reference system in the method embodiment shown above. Figure 1 The steps of the attitude correction method of the attitude and heading reference system in the method embodiment shown can be specifically implemented by referring to the method embodiment, which will not be elaborated here.

[0262] Those skilled in the art can clearly understand that for the convenience and brevity of description, the specific working processes of the systems, devices, and units described above can refer to the corresponding processes in the foregoing method embodiments, which will not be elaborated here.

[0263] In several embodiments provided by the present application, it should be understood that the disclosed systems, devices, and methods can be implemented in other ways. The device embodiments described above are only illustrative. For example, the division of the units is only a logical function division, and there can be other division methods in actual implementation. For another example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed couplings or direct couplings or communication connections to each other can be through some communication interfaces. The indirect couplings or communication connections of the devices or units can be in electrical, mechanical, or other forms.

[0264] The units described as separate components may or may not be physically separated. The components displayed as units may or may not be physical units, that is, they can be located in one place or distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0265] In addition, in each embodiment of the present application, the functional units can be integrated into one processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit.

[0266] When the above-mentioned functions are implemented in the form of software function units and sold or used as independent products, they can be stored in a non-volatile computer-readable storage medium executable by a processor. Based on such understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to enable 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 methods described in various embodiments of this application. The aforementioned storage medium includes: various media that can store program codes, such as USB flash drives, mobile hard disks, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical discs.

[0267] Finally, it should be noted that the above-mentioned embodiments are only specific implementation manners of this application, used to illustrate the technical solutions of this application, rather than limiting it. The protection scope of this application is not limited thereto. Although this application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: any person skilled in the art within the technical scope disclosed in this application can still modify the technical solutions recorded in the foregoing embodiments, or can easily think of changes, or perform equivalent replacements on some of the technical features; and these modifications, changes, or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be covered by the protection scope of this application. Therefore, the protection scope of this application should be subject to the protection scope of the claims.

Claims

1. A method for attitude correction of an attitude and heading reference system, characterized in that, The described attitude correction method includes: During the process of detecting the carrier attitude using the attitude reference system set on the target carrier, obtaining the historical attitude data of the target carrier at the previous moment, as well as the acceleration data and angular velocity data of the target carrier at the current moment, and determining the attitude quaternion of the target carrier at the current moment based on the historical attitude data and the angular velocity data; Correspondingly comparing the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with a preset acceleration threshold and a preset angular velocity threshold respectively, to determine whether the target carrier is in a stationary state at the current moment, and obtaining a judgment result; Based on the judgment result, determining the noise covariance matrix of the accelerometer in the attitude reference system at the current moment; Based on the noise covariance matrix, the historical attitude data, and the observation vector of the target carrier at the current moment, determining the attitude correction number of the target carrier at the current moment; Using the attitude correction number to correct the attitude quaternion, to obtain the target attitude quaternion of the target carrier at the current moment, so as to correct the carrier attitude detection of the attitude reference system.

2. The method according to claim 1, wherein The determining the attitude quaternion of the target carrier at the current moment based on the historical attitude data and the angular velocity data includes: Determining the historical attitude quaternion and historical angular velocity data of the target carrier at the previous moment from the historical attitude data; Based on the angular velocity data and the historical angular velocity data, determining the gyro angle increment of the target carrier at the current moment, and calculating the equivalent rotation vector of the target carrier at the current moment based on the gyro angle increment and a preset coning compensation coefficient; Based on the historical attitude quaternion and the equivalent rotation vector, determining the attitude quaternion of the target carrier at the current moment.

3. The method according to claim 1, wherein The correspondingly comparing the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with a preset acceleration threshold and a preset angular velocity threshold respectively, to determine whether the target carrier is in a stationary state at the current moment, and obtaining a judgment result, includes: Correspondingly comparing the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with a preset acceleration threshold and a preset angular velocity threshold respectively, to determine whether the acceleration output modulus is less than or equal to the preset acceleration threshold, and to determine whether the angular velocity output modulus is equal to the preset angular velocity threshold; When the acceleration output modulus is less than or equal to the preset acceleration threshold and the angular velocity output modulus is equal to the preset angular velocity threshold, determining that the judgment result is that the target carrier is in a stationary state at the current moment; When the acceleration output modulus is greater than the preset acceleration threshold and / or the angular velocity output modulus is not equal to the preset angular velocity threshold, determining that the judgment result is that the target carrier is in a non-stationary state at the current moment.

4. The method according to claim 1, wherein The determining the noise covariance matrix of the accelerometer in the attitude reference system at the current moment based on the judgment result includes: When the judgment result indicates that the target carrier is in a stationary state at the current moment, determine the preset first covariance matrix as the noise covariance matrix of the accelerometer in the attitude reference system at the current moment; When the judgment result indicates that the target carrier is in a non-stationary state at the current moment, perform a numerical comparison on the obtained carrier vibration values, specific force estimation error values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier, and the motion acceleration of the target carrier, and based on the numerical comparison result, determine the noise covariance matrix of the accelerometer in the attitude reference system at the current moment.

5. The method according to claim 4, characterized in that, The performing a numerical comparison on the obtained carrier vibration values, specific force estimation error values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier, and the motion acceleration of the target carrier, and based on the numerical comparison result, determining the noise covariance matrix of the accelerometer in the attitude reference system at the current moment includes: Obtain the carrier vibration values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier respectively, and perform a numerical comparison on the carrier vibration values with the preset random walk values corresponding to the horizontal axis, vertical axis, and vertical axis respectively to obtain a first comparison result; wherein, the carrier vibration value represents the standard deviation of the acceleration data of the target carrier within a preset time window before the current moment; Select the first numerical values for which the first comparison result is greater from the carrier vibration values or the random walk values corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and construct the first numerical values into a second covariance matrix; Obtain the specific force estimation error values corresponding to the horizontal axis, vertical axis, and vertical axis of the target carrier respectively, and perform a numerical comparison on the specific force estimation error values with the first numerical values corresponding to the horizontal axis, vertical axis, and vertical axis respectively to obtain a second comparison result; Select the second numerical values for which the second comparison result is greater from the specific force estimation error values or the first numerical values corresponding to the horizontal axis, vertical axis, and vertical axis respectively, and construct the second numerical values into a third covariance matrix; Obtain the motion acceleration of the target carrier at the current moment, and perform a comparison on the motion acceleration with the sum of the second numerical values corresponding to the horizontal axis, vertical axis, and vertical axis respectively to obtain a third comparison result; When the third comparison result indicates that the motion acceleration is greater than the sum of the second numerical values, determine that the noise covariance matrix is infinite; When the third comparison result indicates that the motion acceleration is less than or equal to the sum of the second numerical values, determine the third covariance matrix as the noise covariance matrix.

6. The method according to claim 1, wherein The determining the attitude correction of the target carrier at the current moment based on the noise covariance matrix, the historical attitude data, and the observation vector of the target carrier at the current moment includes: Determine the state estimation vector and the estimation error covariance matrix of the target carrier at the previous moment in the historical attitude data, and based on the state estimation vector of the target carrier at the previous moment and the preset prediction matrix, determine the state prediction vector of the target carrier from the previous moment to the current moment; Determine the filtering gain matrix of the target vehicle at the current moment based on the noise covariance matrix, the estimated error covariance matrix, the preset observation matrix, and the preset system noise covariance matrix; Determine the state estimation vector of the target vehicle at the current moment based on the observation vector of the target vehicle at the current moment, the filtering gain matrix, the preset observation matrix, and the state prediction vector; Determine the attitude correction numbers corresponding to the roll angle, pitch angle, and heading angle of the target vehicle at the current moment in the state estimation vector at the current moment.

7. The method according to claim 1, characterized in that The correcting the attitude quaternion by using the attitude correction numbers to obtain the target attitude quaternion of the target vehicle at the current moment for correcting the vehicle attitude detection of the attitude reference system includes: Based on the attitude correction numbers, construct a three-dimensional attitude error matrix of the target vehicle at the current moment to determine the three-dimensional attitude error value of the target vehicle at the current moment; Based on the three-dimensional attitude error value, determine the attitude error quaternion of the target vehicle at the current moment; Perform quaternion multiplication calculation on the attitude error quaternion and the attitude quaternion to obtain the target attitude quaternion of the target vehicle at the current moment for correcting the vehicle attitude detection of the attitude reference system.

8. An attitude correction device for an attitude and heading reference system, characterized in that, The attitude correction device includes: An attitude determination module, configured to obtain the historical attitude data of the target vehicle at the previous moment and the acceleration data and angular velocity data of the target vehicle at the current moment during the process of performing vehicle attitude detection using an attitude reference system set by the target vehicle, and determine the attitude quaternion of the target vehicle at the current moment based on the historical attitude data and the angular velocity data; A static judgment module, configured to respectively compare the acceleration output modulus in the acceleration data and the angular velocity output modulus in the angular velocity data with a preset acceleration threshold and a preset angular velocity threshold, and judge whether the target vehicle is in a static state at the current moment to obtain a judgment result; A matrix determination module, configured to determine the noise covariance matrix of the accelerometer in the attitude reference system at the current moment based on the judgment result; An attitude filtering module, configured to determine the attitude correction numbers of the target vehicle at the current moment based on the noise covariance matrix, the historical attitude data, and the observation vector of the target vehicle at the current moment; An attitude correction module, configured to correct the attitude quaternion by using the attitude correction numbers to obtain the target attitude quaternion of the target vehicle at the current moment for correcting the vehicle attitude detection of the attitude reference system.

9. An electronic device, characterized in that, including: A processor, a memory, and a bus, where the memory stores machine-readable instructions executable by the processor. When the electronic device runs, the processor communicates with the memory through the bus. When the machine-readable instructions are run by the processor, the steps of the attitude correction method of the attitude reference system according to any one of claims 1 to 7 are executed.

10. A computer-readable storage medium, characterized in that, A computer program is stored on the computer-readable storage medium, and when the computer program is run by a processor, it executes the steps of the attitude correction method of the attitude and heading reference system according to any one of claims 1 to 7.

Images