Nine-axis inertial measurement unit attitude solving method and device based on observation decoupling

Abstract

The application provides a nine-axis inertial measurement unit attitude solving method and device based on observation decoupling, the method comprising collecting acceleration data, angular velocity data and magnetic field data, using the acceleration data to only compensate for the error of the X-axis and Y-axis angular velocity, and updating to obtain target attitude quaternions; calculating the heading angle error according to the magnetic field data, constructing a modified quaternion with zero X and Y components and only rotating around the Z-axis of the world coordinate system; multiplying the modified quaternion and the target attitude quaternion to obtain the final attitude quaternion. In this way, by completely decoupling the gravity observation and the geomagnetic observation, the error cross-talk is avoided from the root, the heading correction does not change the horizontal attitude, the roll angle and the pitch angle can be stabilized in complex environments such as magnetic field distortion and ferromagnetic interference, and the attitude drift is effectively suppressed. The overall architecture is simple, the operation amount is small, it is suitable for low-power embedded devices, and the attitude solving accuracy, real-time performance and environmental adaptability are considered.

Description

Technical Field

[0001] This application relates to the field of inertial navigation and attitude measurement technology, and in particular to a method and apparatus for attitude calculation of a nine-axis inertial measurement unit based on observation decoupling. Background Technology

[0002] Currently, with the rapid development of attitude calculation technology for nine-axis inertial measurement units, this technology has been widely applied in scenarios such as drones, robots, and wearable devices. Furthermore, classic multi-sensor fusion algorithms such as complementary filtering and extended Kalman fusion have become the mainstream in the industry, and supporting technologies such as sensor calibration and signal processing are also gradually maturing.

[0003] However, traditional attitude calculation fusion methods for nine-axis inertial measurement units still have significant shortcomings in practical applications. First, current mainstream algorithms incorporate gravity and geomagnetic observations into the same optimization system. Errors caused by external magnetic field distortion and ferromagnetic interference are directly coupled to horizontal attitude parameters such as roll and pitch angles, causing attitude drift and instantaneous jumps, severely affecting measurement reliability. Second, algorithms such as extended Kalman filtering involve complex operations such as matrix inversion, resulting in high computational complexity and overhead, making it difficult to achieve high-frequency attitude updates due to limitations in the computing power of embedded devices. Furthermore, existing improvement schemes cannot simultaneously ensure anti-interference capability, computational efficiency, and long-term stability of attitude measurements under complex conditions such as high-speed maneuvers and time-varying magnetic fields. Summary of the Invention

[0004] This application proposes a nine-axis inertial measurement unit (IMU) attitude calculation method and apparatus based on observation decoupling. This aims to decouple gravity and geomagnetic observations, ensuring that heading corrections only affect the Z-axis and do not interfere with roll or pitch attitude. It effectively resists magnetic field distortion and ferromagnetic interference, preventing attitude drift. The overall computational architecture is simple with low computational overhead, improving the stability and accuracy of attitude measurement while ensuring real-time calculation, and is compatible with various embedded devices.

[0005] In a first aspect, embodiments of this application provide a nine-axis inertial measurement unit (IMU) attitude calculation method based on observation decoupling, applied to an electronic device in an attitude calculation system. The electronic device is communicatively connected to the nine-axis IMU in the attitude calculation system. The nine-axis IMU integrates a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, and is rigidly fixed to the measured carrier. The method includes: Acquire acceleration data output from a triaxial accelerometer, angular velocity data output from a triaxial gyroscope, and magnetic field data output from a triaxial magnetometer; Error compensation is performed only on the X-axis and Y-axis angular velocities in the angular velocity data based on the acceleration data, and the current attitude quaternion is updated based on the compensated angular velocity to obtain the target attitude quaternion. The attitude quaternion is used to characterize the three-dimensional rotational attitude of the measured vehicle. Error compensation refers to generating a compensation amount based on the deviation between the acceleration data and the predicted gravity vector and superimposing it on the corresponding axis angular velocity. The predicted gravity vector is a vector obtained by rotating and transforming the reference gravity vector based on the current attitude quaternion. The current attitude quaternion is an attitude characterization parameter obtained by iteratively updating the angular velocity data of the previous moment through the quaternion differential equation. The heading angle error is calculated based on the magnetic field data, and a corrected quaternion is constructed based on the heading angle error, which rotates only around the Z-axis of the world coordinate system. The X and Y components of the vector part of the corrected quaternion are both zero. The corrected quaternion and the target attitude quaternion are fused by quaternion multiplication to obtain the final attitude quaternion.

[0006] Secondly, embodiments of this application provide a nine-axis inertial measurement unit attitude calculation device based on observation decoupling, applied to an electronic device in an attitude calculation system. The electronic device is communicatively connected to the nine-axis inertial measurement unit in the attitude calculation system. The nine-axis inertial measurement unit integrates a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, and is rigidly fixed to the measured carrier. The device includes: The data acquisition unit is used to acquire acceleration data output by the triaxial accelerometer, angular velocity data output by the triaxial gyroscope, and magnetic field data output by the triaxial magnetometer. The horizontal attitude calculation unit is used to perform error compensation only on the X-axis angular velocity and Y-axis angular velocity in the angular velocity data based on the acceleration data, and update the current attitude quaternion based on the compensated angular velocity to obtain the target attitude quaternion. The attitude quaternion is used to characterize the three-dimensional rotational attitude of the measured vehicle. Error compensation refers to generating a compensation amount based on the deviation between the acceleration data and the predicted gravity vector and superimposing it on the corresponding axis angular velocity. The predicted gravity vector is a vector obtained by rotating and transforming the reference gravity vector based on the current attitude quaternion. The current attitude quaternion is an attitude characterization parameter obtained by iteratively updating the angular velocity data of the previous moment through quaternion differential equations. The heading and attitude calculation unit is used to calculate the heading angle error based on the magnetic field data, and construct a corrected quaternion that rotates only around the Z-axis of the world coordinate system based on the heading angle error. The X and Y components of the vector part of the corrected quaternion are both zero. The attitude fusion unit is used to perform quaternion multiplication and fusion of the corrected quaternion and the target attitude quaternion to obtain the final attitude quaternion.

[0007] Thirdly, embodiments of this application provide an electronic device including a processor, a memory, and one or more programs, the one or more programs being stored in the memory and configured to be executed by the processor, the programs including instructions for performing the steps as described in the first aspect of embodiments of this application.

[0008] Fourthly, embodiments of this application provide a computer-readable storage medium having a computer program / instructions stored thereon, which, when executed by a processor, implement the steps in the first aspect of embodiments of this application.

[0009] Fifthly, embodiments of this application provide a computer program product, including a computer program / instructions, which, when executed by a processor, implement some or all of the steps described in the first aspect of embodiments of this application.

[0010] As can be seen from the embodiments of this application, by completely separating the gravity observation channel and the geomagnetic observation channel of the nine-axis inertial measurement unit, using acceleration data to compensate for errors only in the X and Y axis angular velocities, and constructing a corrected quaternion that rotates only around the Z-axis of the world coordinate system based on magnetometer data, the two types of observation data are decoupled from the algorithmic root. Errors caused by magnetic field interference in traditional attitude calculations cannot be coupled to horizontal attitudes such as roll angle and pitch angle. This approach can effectively suppress attitude drift and data jumps caused by magnetic field distortion and interference from surrounding ferromagnetic equipment, significantly improving the measurement stability of horizontal attitude. At the same time, this solution adopts a lightweight computing architecture, without complex calculations such as matrix inversion, resulting in low overall computational load and enabling high-frequency attitude updates on low-power embedded hardware. This design balances anti-interference capability, measurement accuracy, and operational efficiency, and can be adapted to various attitude measurement application scenarios such as drones, robots, and wearable devices. Attached Figure Description

[0011] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0012] Figure 1 This is a schematic diagram of the structure of a nine-axis inertial measurement unit attitude calculation system based on observation decoupling provided in an embodiment of this application; Figure 2 This is a flowchart illustrating a nine-axis inertial measurement unit attitude calculation method based on observation decoupling provided in an embodiment of this application. Figure 3This is a functional unit block diagram of a nine-axis inertial measurement unit attitude calculation device based on observation decoupling provided in an embodiment of this application; Figure 4 This is a functional unit block diagram of another observation-decoupled nine-axis inertial measurement unit attitude calculation device provided in this application embodiment; Figure 5 This is a structural block diagram of an electronic device provided in an embodiment of this application. Detailed Implementation

[0013] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present application, and not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present application.

[0014] The terms "first," "second," etc., in the specification, claims, and accompanying drawings of this application are used to distinguish different objects, not to describe a specific order. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or apparatus that includes a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to these processes, methods, products, or apparatuses.

[0015] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0016] This application provides a method for attitude calculation of a nine-axis inertial measurement unit based on observation decoupling. The embodiments of this application will be described in detail below with reference to the accompanying drawings.

[0017] Please see Figure 1 , Figure 1 This is a schematic diagram of a nine-axis inertial measurement unit attitude calculation system based on observation decoupling, provided in an embodiment of this application. Figure 1As shown, the attitude calculation system 10 includes a nine-axis inertial measurement unit 120, an electronic device 110, and a test vehicle 130. The nine-axis inertial measurement unit 120 is an integrated sensing device that integrates a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer. The nine-axis inertial measurement unit 120 is rigidly fixed on the test vehicle 130 and can collect three types of raw sensing data in real time: acceleration data, angular velocity data, and magnetic field data during the operation of the test vehicle 130.

[0018] Specifically, electronic device 110 is a core control device integrating data reception, algorithm calculation, and attitude output. It can adopt hardware forms such as embedded controllers, microcontrollers, and industrial terminals. It is both the sensor data receiving end and the core execution unit of the attitude calculation algorithm. The nine-axis inertial measurement unit 120 establishes a communication connection with electronic device 110 to realize real-time interactive transmission of sensor data. The three work together to form a closed-loop attitude measurement system of data acquisition, algorithm calculation, and attitude output.

[0019] In the attitude calculation system 10, the nine-axis inertial measurement unit 120 is responsible for continuously collecting various raw sensor data corresponding to the tested carrier 130 and transmitting the collected data to the electronic device 110 in real time. The electronic device 110 has a built-in data acquisition unit, horizontal attitude calculation unit, heading attitude calculation unit and attitude fusion unit. Relying on each functional unit, it completes the entire process of sensor preprocessing, attitude calculation, quaternion fusion, data validity detection and fault degradation, and finally outputs the three-dimensional attitude information of the tested carrier 130.

[0020] Furthermore, after the nine-axis inertial measurement unit 120 completes the acquisition of raw sensor data and uploads it to the electronic device 110, the electronic device 110 first performs gyroscope zero-bias calibration, six-point orthogonal static calibration, ellipsoid fitting calibration, filtering, and acceleration data magnitude normalization preprocessing on the received sensor data in sequence. Subsequently, the horizontal attitude calculation unit performs error compensation only on the X-axis and Y-axis angular velocities based on the acceleration data, and obtains the target attitude quaternion by combining the attitude quaternion update rules; the heading attitude calculation unit calculates the heading angle error based on the magnetic field data, and constructs a corrected quaternion with zero vector X and Y components and rotating only around the Z-axis of the world coordinate system; the attitude fusion unit performs quaternion multiplication and fusion with the corrected quaternion and the target attitude quaternion to obtain the final attitude quaternion. The electronic device 110 simultaneously performs validity determination on the sensor data and the final attitude quaternion, and executes the corresponding fault degradation strategy according to the determination result, and finally outputs stable three-dimensional attitude data of the tested carrier 130.

[0021] Typically, the attitude calculation system 10 can be configured with one or more nine-axis inertial measurement units 120. A single electronic device 110 can simultaneously interface with multiple sets of nine-axis inertial measurement units 120, supporting parallel calculation of attitude data from multiple measured carriers 130. Based on an observation decoupling architecture design, the system completely separates gravity observation and geomagnetic observation channels, fundamentally avoiding error coupling problems. It also employs lightweight algorithms to reduce hardware computational load. The overall architecture is simple and easy to deploy, possessing both strong anti-interference capabilities and high operating efficiency. It can be widely applied to various three-dimensional attitude measurement scenarios involving drones, service robots, wearable devices, motion monitoring, and other devices carrying the measured carrier 130.

[0022] Please see Figure 2 , Figure 2 This is a flowchart illustrating a nine-axis inertial measurement unit (IMU) attitude calculation method based on observation decoupling, provided in an embodiment of this application. The method is applied to an electronic device 110 in an attitude calculation system 10, which is communicatively connected to a nine-axis IMU 120 within the system. The nine-axis IMU 120 integrates a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, and is rigidly fixed to the measured carrier 130. The method includes: Step S201: Obtain acceleration data output by the three-axis accelerometer, angular velocity data output by the three-axis gyroscope, and magnetic field data output by the three-axis magnetometer.

[0023] In step S201, the nine-axis inertial measurement unit 120 deployed on the carrier under test 130 collects sensing information in real time and outputs data to the electronic device 110 connected to it. Specifically, it acquires acceleration data output by the three-axis accelerometer, angular velocity data output by the three-axis gyroscope, and magnetic field data output by the three-axis magnetometer.

[0024] In this embodiment, the nine-axis inertial measurement unit 120 is an integrated MEMS sensing module (Micro-Electro-Mechanical System, a mainstream technology for fabricating micro-sensors such as IMUs, characterized by small size, low power consumption, and high integration, suitable for embedded devices and various mobile carriers). It integrates three types of functional sensors: a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer. This module is rigidly fixed to the surface of the carrier 130 under test. This rigid installation method prevents relative displacement of the nine-axis inertial measurement unit 120 during the movement and vibration of the carrier 130, ensuring the stability of data acquisition from a hardware perspective. Each of the three sensors performs its specific function: the three-axis accelerometer senses the gravity and linear acceleration experienced by the carrier 130; the three-axis gyroscope detects the rotational angular rate of the carrier 130; and the three-axis magnetometer senses the distribution of the surrounding geomagnetic field. The three sensors work together to comprehensively collect raw information related to the carrier's motion and the environmental magnetic field.

[0025] The working principle of this step is as follows: the nine-axis inertial measurement unit 120, relying on the physical sensing characteristics of the MEMS chip, completes continuous sampling and outputs raw sensing data without any correction or processing. This raw data is the foundational input for the subsequent sensor calibration, filtering, and attitude calculation processes. The raw angular velocity data of the gyroscope's three axes is... , , The raw triaxial accelerometer data is , , The original data vector of the magnetometer is ; where the subscript in the symbol The term "uniform" refers to the raw sampled values ​​that are directly output by the sensor and have not undergone calibration or filtering.

[0026] In one possible embodiment, after acquiring the acceleration data output by the triaxial accelerometer, the angular velocity data output by the triaxial gyroscope, and the magnetic field data output by the triaxial magnetometer, before performing error compensation only on the X-axis angular velocity and Y-axis angular velocity data based on the acceleration data, the method further includes: performing gyroscope zero-bias calibration on the angular velocity data to obtain calibrated angular velocity data; performing six-point orthogonal static calibration based on the acceleration data to obtain calibrated acceleration data; performing ellipsoidal fitting calibration based on the magnetic field data to obtain calibrated magnetic field data; performing filtering processing on the calibrated angular velocity data, calibrated acceleration data, and calibrated magnetic field data respectively to obtain filtered acceleration data, angular velocity data, and magnetic field data; and performing modulus normalization processing on the filtered acceleration data to obtain normalized acceleration data.

[0027] Among them, gyroscope zero-bias calibration, six-point orthogonal static calibration, and ellipsoid fitting calibration are three types of sensor-specific error correction methods. Gyroscope zero-bias calibration is used to compensate for the long-term static output deviation of the gyroscope; six-point orthogonal static calibration can simultaneously correct the zero-bias error, scale inconsistency error, and orthogonality error of the accelerometer; ellipsoid fitting calibration can eliminate the interference of hard magnetic bias and soft magnetic distortion on the magnetometer. Filtering is used to filter out high-frequency noise, vibration spikes, and random interference signals generated during data sampling; modulus normalization is used to normalize the acceleration vector and unify the data input magnitude. All the above data correction and processing operations are completed by the electronic device 110 based on the raw data output by the nine-axis inertial measurement unit 120.

[0028] The calibration principle of gyroscope zero-bias calibration is based on the ideal static angular velocity of 0. After long-term static sampling, the average value of the three-axis output is statistically analyzed as a fixed zero-bias error, and real-time bias compensation is performed. Correction is achieved by calculating the average deviation through multiple sets of static sampling. The corresponding calculation equations are as follows: ; in, , , For the first i The raw angular velocity data of the gyroscope along the X, Y, and Z axes in the group sampling. i Indicates the sampling group number. i =1,2,..., N , N This represents the total number of sample groups. , , This represents the zero-bias value for the three axes of the gyroscope. The formula for data correction using the zero-bias value is: ; In the formula, This is the original angular velocity vector of the gyroscope. This is the calibrated angular velocity vector.

[0029] Furthermore, the calibration principle of the six-point orthogonal static calibration is to use the Earth's constant gravity vector as a reference to correct the accelerometer's three-axis zero bias, scaling inconsistency, and orthogonality error. The specific calibration steps are as follows: the sensor is horizontally placed in six orthogonal attitudes in sequence: up, down, front, back, left, and right; after each attitude stabilizes, multiple sets of data are continuously sampled and averaged; based on the standard gravity g constraint, the three-axis zero bias and three-axis scale correction coefficients are solved; finally, the bias and scale factor are combined to complete the comprehensive correction.

[0030] Specifically, the operational equations of the accelerometer integrated calibration model are as follows: ; in, , , These are the zero bias values ​​for the X, Y, and Z axes of the accelerometer, respectively. , , These are the scale correction factors for the X, Y, and Z axes of the accelerometer, respectively.

[0031] Its matrix abbreviation is as follows: , The accelerometer scale diagonal matrix, For the raw acceleration data, For the zero-biased acceleration vector, This is the calibrated acceleration data.

[0032] Furthermore, the magnetometer relies on the ellipsoid equation to complete distortion correction. Specifically, the calibration principle is that the magnetometer is affected by hard iron bias (fixed magnetic field interference) and soft magnetic distortion (induced magnetic field distortion). The ideal magnetic field trajectory is a standard sphere, which becomes a distorted ellipsoid after interference. The bias vector and distortion correction matrix are solved by least-squares ellipsoid fitting to restore the standard spherical magnetic field distribution. The calibration steps involve maintaining the IMU in space and slowly rotating it in multiple dimensions and directions, covering pitch, roll, and yaw angles; continuously collecting a large number of original three-axis sampling points of the magnetometer under all attitudes; using the least-squares method to perform ellipsoid fitting on the massive number of sampling points to calculate the ellipsoid center (hard magnetic bias), ellipsoid axis length, and distortion coefficient (soft magnetic error); and constructing a magnetometer error correction model to complete the integrated hard and soft magnetic calibration.

[0033] Specifically, the general constraint equations for the ellipsoid are: ; in, A , B,C,D,E,F,G,H,I,J The coefficients of the general constraint equations for the ellipsoid are: The correction formula is: In the formula This is the original data vector from the magnetometer. To obtain the hard magnetic bias vector. , M This is the soft magnetic distortion correction matrix. This is the calibrated magnetic field data.

[0034] This scheme employs a combination of first-order low-pass filtering and moving average filtering for noise reduction. The formula for the first-order low-pass filtering is as follows: ; Moving average filter formula: ; in, This is the filtering smoothing coefficient, typically set to 0.1~0.3 to balance delay and smoothing effect. The filtered output data for the current moment. This is the original sampled data at the current moment. The filtered output data from the previous time step. This is the output value of the moving average filter. N This is the size of the sliding window.

[0035] Furthermore, after calibration filtering, normalization is performed based on the gravitational modulus. The specific formula for normalizing the acceleration modulus is as follows: ; In the formula, This is the filtered acceleration data. This is the normalized acceleration data. A unified vector magnitude is used to eliminate hardware amplitude discrepancies and adapt to subsequent quaternion, complementary filtering, and Kalman filtering attitude calculations.

[0036] As can be seen, in this example, multi-dimensional calibration methods at this stage can completely eliminate interference from inherent sensor bias, scale errors, and external magnetic field distortions. Combined with integrated filtering, the sampled data is effectively purified, improving data purity. Acceleration modulus normalization unifies the vector data magnitude, providing standardized input data for subsequent attitude error compensation and quaternion operations. The entire preprocessing workflow optimizes data quality from the source, effectively reducing the impact of raw errors on attitude calculation, significantly improving overall calculation accuracy and operational stability. Furthermore, the entire computational logic is simple and adaptable to the operational requirements of low-power embedded hardware.

[0037] Step S202: Based on the acceleration data, only the X-axis angular velocity and Y-axis angular velocity in the angular velocity data are compensated for errors, and the current attitude quaternion is updated based on the compensated angular velocity to obtain the target attitude quaternion.

[0038] In this application, attitude quaternions are used to characterize the three-dimensional rotational attitude of the tested vehicle. Error compensation refers to generating a compensation amount based on the deviation between the acceleration data and the predicted gravity vector and superimposing it onto the corresponding axis angular velocity. The predicted gravity vector is a vector obtained by rotating and transforming the reference gravity vector according to the current attitude quaternion. The current attitude quaternion is an attitude characterization parameter obtained by iteratively updating the angular velocity data at the previous moment through quaternion differential equations. Specifically, this application uses multiple quaternions, including: 1. Current attitude quaternion: the basic parameter before attitude iteration, used to characterize the real-time three-dimensional rotational attitude of the tested vehicle; 2. Target attitude quaternion: the attitude parameter obtained after horizontal attitude compensation and quaternion update, characterizing the updated three-dimensional rotational attitude of the tested vehicle; 3. Correction quaternion: a heading-specific rotation correction operator, used only to compensate for heading deviations and not to characterize the attitude of the tested vehicle itself; 4. Final attitude quaternion: the final attitude parameter after quaternion fusion, characterizing the complete three-dimensional rotational attitude of the tested vehicle.

[0039] Among them, the current attitude quaternion is a mathematical model used to characterize the real-time motion state of the tested carrier 130. It can completely describe the three-dimensional rotational attitude of the tested carrier 130 relative to the world coordinate system, and is also the core computing carrier for the electronic device 110 to carry out attitude calculation and data iteration.

[0040] In one possible embodiment, error compensation is performed only on the X-axis angular velocity and Y-axis angular velocity in the angular velocity data based on the acceleration data, and the current attitude quaternion is updated based on the compensated angular velocity to obtain the target attitude quaternion. This includes: performing a rotation transformation on the reference gravity vector based on the current attitude quaternion to obtain a predicted gravity vector; calculating the cross product error between the predicted gravity vector and the measured vector corresponding to the acceleration data; performing proportional and integral joint operations on the cross product error through a proportional-integral controller to obtain the angular velocity compensation amount; superimposing the angular velocity compensation amount onto the X-axis angular velocity and Y-axis angular velocity respectively, while keeping the Z-axis angular velocity as the original measured value; and updating the current attitude quaternion through a quaternion differential equation based on the superimposed compensated angular velocity data to obtain the target attitude quaternion.

[0041] Among them, the reference gravity vector is the constant gravity direction vector in the world coordinate system. This vector is a fixed vector and is the benchmark for judging the attitude deviation of the measured carrier 130. It is also the basis for completing coordinate transformation and solving attitude error.

[0042] This step relies on the gravity observation channel to complete the horizontal attitude error correction and quaternion iterative update. The overall computational logic unfolds sequentially around coordinate transformation, error solving, proportional-integral adjustment, and quaternion evolution. First, the current attitude quaternion is used... With respect to the reference gravity vector Perform coordinate transformations and solve for the predicted gravity vector. The corresponding calculation formula is as follows: ; in, , , , The four components of the current attitude quaternion, This is the reference gravity vector in the world coordinate system; , , The predicted gravity vector obtained after transformation The three-axis components.

[0043] Furthermore, the predicted gravity vector is then calculated. With measured acceleration vector cross product error The calculation formula is: ; in, , , The normalized measured acceleration components, , , The triaxial cross product error is used to characterize the deviation between the actual attitude and the ideal attitude of the carrier.

[0044] Specifically, a proportional-integral controller is used to adjust the cross product error, and the error is fed back to compensate for the angular velocity of the gyroscope. The above generates compensated angular velocity data, and the calculation formula is as follows: ; In the formula, , , These are the three-axis components corresponding to the original angular velocity vector. , , The compensated triaxial angular velocity; This is the scaling factor for the horizontal attitude channel. The horizontal attitude channel integral coefficient; , These are the integral terms of the cross product errors of the X and Y axes, used to eliminate long-standing static attitude deviations. Only the X and Y axis angular velocities of the gyroscope are compensated, while the Z-axis angular velocity remains unchanged, ensuring that the accelerometer's correction effect only applies to the horizontal attitude angle and does not directly affect the heading angle.

[0045] Finally, combining the compensated angular velocity, the attitude is updated using quaternion differential equations, as follows: ; in, The sampling time interval, The rate of change of quaternions, The current pose quaternion before the update. The updated target pose quaternion. This represents quaternion multiplication. This step only compensates for the X-axis and Y-axis angular velocities, while retaining the original value of the Z-axis angular velocity, thus isolating the calculations for horizontal attitude and heading attitude.

[0046] To clearly explain the quaternion operation rules involved in the attitude update process of this embodiment, the following sections will provide a complete explanation of the standard definition of quaternions, the imaginary unit primitive operation, the general quaternion multiplication, and the special quaternion derivative expansion for attitude adaptation.

[0047] First, the standard definition of quaternions. In this embodiment, all quaternions uniformly adopt the standardized expression form of "real part first, imaginary part second," and the complete definition expression is: ; In the formula, The current attitude quaternion, is the real part of the quaternion, corresponding to the scalar component; , , The imaginary part of the quaternion corresponds to the three-dimensional vector components; symbol , , For each quaternion, there are three sets of orthogonal unit bases corresponding to the imaginary part of the quaternion. All quaternion multiplication operations follow the Hamiltonian basis multiplication rule.

[0048] Furthermore, the basic arithmetic rules for quaternion multiplication ⊗. Quaternion imaginary part unit. , , Multiplication is the foundation of all quaternion expansion calculations, and the fixed operational rules are as follows: .

[0049] Furthermore, the complete and general expansion formula for quaternion multiplication ⊗ is as follows: When performing multiplication on any two sets of quaternions, the component expansion calculation can be completed using the following system of equations. Let the two independent quaternions involved in the operation be... , The result of the multiplication operation is denoted as The resulting system of equations for calculating each component of the quaternion is as follows: ; Among them, a dedicated quaternion derivative expansion adapted for attitude update operations is used. The core differential formula for attitude update in this embodiment is: This formula contains two types of quaternions: one is the carrier attitude quaternion. The second is the pure imaginary angular velocity quaternion. The real part of this quaternion is always 0, and it is only used to support the angular velocity after triaxial compensation.

[0050] Furthermore, substituting the two sets of quaternions into the aforementioned general multiplication expansion equations and simplifying, we obtain the specific quaternion derivative matrix expansion for this scheme: ; The matrix expansion is a direct calculation formula for the attitude quaternion change rate, providing a complete and reproducible mathematical basis for attitude iteration updates in each frame. This embodiment adopts the rotation transformation rule from the body coordinate system to the world coordinate system, with the real part of the quaternion uniformly placed first, and the whole coordinate system follows the right-front-up standard orthogonal coordinate system.

[0051] As can be seen, in this example, this step accurately solves for attitude deviation through coordinate transformation and cross product error. Combined with proportional-integral control and integral limiting strategies, it effectively suppresses attitude drift. Furthermore, by employing a design that compensates only for the X-axis and Y-axis angular velocities, the gravity observation channel is computed independently at the algorithm level, avoiding interference from heading motion in horizontal attitude calculations. The entire computation primarily utilizes basic arithmetic and vector operations, resulting in low computational complexity. It can run efficiently in low-power embedded hardware, ensuring the accuracy and stability of horizontal attitude calculations under normal operating conditions such as vehicle acceleration / deceleration and minor vibrations.

[0052] In one possible embodiment, the proportional-integral controller performs proportional and integral calculations on the cross product error to obtain the angular velocity compensation amount, including: calculating the proportional component of the angular velocity based on the cross product error and a first preset proportional coefficient; iteratively accumulating the angular velocity integral term of the proportional-integral controller based on the cross product error and a first sampling period to obtain the accumulated angular velocity integral term; and if the accumulated angular velocity integral term exceeds a first integral limiting threshold, clamping the accumulated angular velocity integral term within the first integral limiting threshold; and synthesizing the angular velocity compensation amount based on the proportional component of the angular velocity and the limited angular velocity integral term.

[0053] The first preset proportional coefficient is the horizontal attitude channel proportional adjustment parameter corresponding to acceleration, used to set the compensation weight corresponding to instantaneous attitude error; the first sampling period is the time interval of a single loop operation of the algorithm, which is the basic timing parameter of the integral iteration process. The first integral limiting threshold is the maximum allowable value of the angular velocity integral term, used to limit the effective operating range of the integral term. The angular velocity proportional component is mainly used to respond to instantaneous attitude deviations and achieve dynamic and rapid compensation; the angular velocity integral term is used to accumulate fixed attitude errors that exist for a long time and eliminate static offsets. Clamping is a numerical constraint processing method that can forcibly limit the amount of computation exceeding the threshold range to the threshold boundary, avoiding abnormal accumulation of values.

[0054] This step relies on the proportional-integral (PI) control principle to solve for the error compensation. First, the proportional component is generated using the cross-product error combined with the proportional coefficient. Then, the integral term is iterated based on the sampling period, and the range of the integral term is constrained by clamping rules. Finally, the two types of components are fused to obtain the complete compensation. The iterative accumulation formula for the angular velocity integral term is as follows: ; In the formula, The cross product error corresponds to the X and Y axes. For the first sampling period, The first integral limiting threshold (typically 0.5 rad / s) ensures that even under conditions where the accelerometer is subjected to long-term interference and the cross product error is continuously non-zero, the integral term will not exceed the effective compensation range, thereby maintaining the stability of the roll and pitch angle calculations.

[0055] Furthermore, the piecewise function corresponding to the clamping operation is: ; In the formula, For the integral term of the angular velocity to be constrained, + L - L These represent the upper and lower boundaries of the integral limiting threshold, respectively. The angular velocity proportional component is obtained by multiplying the cross product error by the first preset proportional coefficient. The final angular velocity compensation amount is synthesized by adding the angular velocity proportional component to the angular velocity integral term after limiting.

[0056] As can be seen, in this example, the proportional component can quickly respond to instantaneous attitude changes of the carrier, while the integral component can effectively correct long-standing static attitude deviations. The combination of the two significantly improves the overall accuracy of angular velocity error compensation. Simultaneously, through integral term clamping and limiting, integral saturation is prevented, avoiding attitude calculation loss caused by abnormal compensation amounts. This stage primarily involves basic iterations and numerical calculations, resulting in low computational load. It can run stably on low-power embedded hardware, ensuring continuous and reliable attitude compensation under complex conditions such as continuous carrier tilting and frequent start-stop movements.

[0057] Step S203: Calculate the heading angle error based on the magnetic field data, and construct a corrected quaternion that rotates only around the Z-axis of the world coordinate system based on the heading angle error.

[0058] In this design, the X and Y components of the modified quaternion vector are both set to zero, which is the core constraint of the scheme. Since the X and Y axes correspond to the roll and pitch attitude dimensions of the measured carrier 130, setting these components to zero ensures that the modified quaternion only has the function of rotating around the Z axis. This structurally eliminates the influence of heading correction on the horizontal attitude and is a key design for decoupling gravity observation and geomagnetic observation.

[0059] In one possible embodiment, the heading angle error is calculated based on the magnetic field data, and a corrected quaternion that rotates only around the Z-axis of the world coordinate system is constructed based on the heading angle error. This includes: extracting the roll and pitch angles from the target attitude quaternion; performing tilt compensation on the magnetic field data based on the roll and pitch angles to obtain the magnetometer horizontal component; calculating the magnetometer observed heading angle based on the magnetometer horizontal component and extracting the current heading angle from the target attitude quaternion; calculating the difference between the magnetometer observed heading angle and the current heading angle to obtain the heading angle error; performing amplitude limiting processing on the heading angle error to constrain it within a single circumference; and calculating the amplitude-limited heading angle error using a proportional-integral controller to obtain the heading angle correction amount; and constructing the corrected quaternion based on the heading angle correction amount.

[0060] Among them, the roll angle and pitch angle represent the current horizontal tilt state of the tested vehicle 130. The purpose of tilt compensation is to eliminate the influence of the vehicle's attitude tilt on the geomagnetic field sampling data, convert the three-dimensional magnetic field data into effective components in the horizontal plane, and restore the true horizontal distribution characteristics of the geomagnetic field. The single-cycle angle range is [−π,π], which limits the heading angle error and avoids numerical abrupt changes caused by angle jumps and cross-cycle calculations, ensuring the continuous and reliable calculation of heading error. At the same time, the correction quaternion is constructed in the form of trigonometric functions. Its real part is the cosine value of half the heading angle correction amount, and its imaginary part Z component is the sine value of half the heading angle correction amount, further limiting it to only perform Z-axis rotation.

[0061] This step sequentially completes magnetic field tilt compensation, heading angle calculation, error calculation, error limiting, correction calculation, and correction quaternion construction. The corresponding calculation formulas and symbols are explained below: Among these steps, tilt compensation is performed on the magnetometer data: using the roll angle obtained in the previous step. and pitch angle The actual measured value of the magnetometer Projected onto a horizontal plane, the magnetometer tilt compensation formula is as follows: ; In the formula, , , To calibrate the triaxial components corresponding to the filtered magnetic field data, This is the roll angle. The pitch angle, , This represents the horizontal component of the magnetometer obtained after tilt compensation.

[0062] The formula for calculating the heading angle observed by the magnetometer is as follows: ; In the formula, The observed heading angle is obtained from the magnetometer calculation.

[0063] Furthermore, from the target pose quaternion Formula for extracting the current heading angle: ; in, , , , For each component of the target pose quaternion, The heading angle is extracted from the current attitude quaternion.

[0064] Among them, the heading angle error is calculated. And limit the error to To avoid error accumulation; specifically, the formula for limiting the heading angle error in segments: ; In the formula, This represents the original heading angle error. To constrain the heading angle error after limiting the amplitude within a single circumference angle range.

[0065] The formula for calculating the heading angle correction is as follows: ; In the formula, This is the heading channel ratio factor, used for rapid response to instantaneous heading deviations. This is the integral coefficient for the heading channel, used to eliminate long-standing static heading deviations. δ is the integral term of the heading angle error, and δ is the final heading angle correction.

[0066] Finally, construct the corrected quaternion that rotates only around the Z-axis of the world coordinate system. To ensure that the correction does not affect the roll and pitch angles, the quaternion construction formula is corrected as follows: ; In the formula, It is a corrected quaternion that rotates only around the Z-axis of the world coordinate system.

[0067] As can be seen in this example, this step first corrects the geomagnetic data deviation caused by attitude tilt through tilt compensation, ensuring the accuracy of the heading angle calculation; it then uses an angle limiting mechanism to avoid the problem of heading angle jumps across cycles, improving the continuity of error calculation. Heading correction is performed using a correction quaternion that rotates only around the Z-axis, completely isolating the geomagnetic observation channel from the horizontal attitude channel. Even if there is interference in the magnetic field data, it will not cause anomalies in roll or pitch angles. The overall computational logic is simple, without complex matrix operations, balancing heading calculation accuracy with the operating efficiency of embedded devices.

[0068] In one possible embodiment, the heading angle correction is obtained by calculating the heading angle error after the amplitude limit using a proportional-integral controller, including: calculating the heading proportional component based on the heading angle error after the amplitude limit and a second preset proportional coefficient; iteratively accumulating the heading integral term of the proportional-integral controller based on the heading angle error after the amplitude limit and the second sampling period to obtain the accumulated heading integral term; and if the accumulated heading integral term exceeds the second integral amplitude limit threshold, clamping the accumulated heading integral term within the second integral amplitude limit threshold; and synthesizing the heading proportional component and the amplitude limit heading integral term to obtain the heading angle correction.

[0069] The second preset proportional coefficient is a dedicated proportional adjustment parameter for the heading and attitude channel, used to match the instantaneous compensation weight of heading error. The second sampling period is the time interval for the integral iteration of the heading channel, consistent with the overall sampling sequence of the system. The second integral limiting threshold is the maximum allowable value of the heading integral term, and this threshold is less than the first integral limiting threshold. Since the geomagnetic environment is susceptible to interference from ferromagnetic objects and electromagnetic equipment, and this interference is random and sudden, a more conservative threshold constraint is applied to the heading integral term to reduce the risk of integral saturation. The heading proportional component is used for rapid response to instantaneous heading deviations, while the heading integral term is used to eliminate long-term fixed heading offsets; the two work together to achieve synchronous correction of dynamic and static errors.

[0070] Among them, the magnetometer is susceptible to the influence of surrounding ferromagnetic materials and electromagnetic interference. To prevent the integration term from being affected by abnormal magnetic field conditions, The continuous accumulation leads to the heading angle correction. If the system becomes uncontrollable, independent limiting processing needs to be applied to the integral component of the yaw axis (heading angle). The specific limiting rules are as follows: The heading integral term adopts an iterative accumulation combined with clamping constraints, and the corresponding integral iteration formula is as follows: ; In the formula, This refers to the heading angle error after amplitude limiting. For the second sampling period, This is the second integration limiting threshold (typically 0.1 rad). This threshold is set conservatively because the yaw axis correction relies on magnetometer observations, and environmental magnetic field interference is complex and uncertain, leading to a much higher risk of integration saturation compared to other axes (Roll / Ritch axes). This section reuses clamping functions to complete numerical constraints: ; By forcibly constraining the heading integral term that exceeds the threshold range to the upper and lower boundaries, and finally adding the heading proportional component to the clamped heading integral term, the heading angle correction can be synthesized.

[0071] As can be seen, in this example, setting a smaller integral limit threshold for the heading channel can adapt to complex and variable magnetic field environments, effectively suppressing the accumulation of integral anomalies caused by magnetic field distortion and electromagnetic interference. The proportional-integral (PI) combined control method can both quickly correct instantaneous heading deviations and gradually eliminate long-term static heading drift. This stage involves only basic iterative calculations and arithmetic operations, resulting in low computational overhead. It ensures stable and reliable heading angle correction in various scenarios, including indoor metallic environments and outdoor electromagnetic interference.

[0072] Step S204: Perform quaternion multiplication and fusion of the corrected quaternion and the target attitude quaternion to obtain the final attitude quaternion.

[0073] Quaternion multiplication fusion is the core computational method for achieving attitude information integration in this scheme. The target attitude quaternion carries horizontal attitude information such as roll and pitch obtained from acceleration data, while the correction quaternion only carries attitude correction information in the heading dimension. After multiplying and fusing the two, heading correction can be completed without changing the horizontal attitude, further ensuring the design goal of decoupling gravity observation and geomagnetic observation.

[0074] Specifically, the corrected quaternion is fused with the updated quaternion through quaternion multiplication to obtain the final attitude quaternion: ; in, The final pose quaternion after fusion.

[0075] Furthermore, regarding the final pose quaternion Normalization is performed to ensure the standardization of quaternions.

[0076] ; in, The final pose quaternion after normalization. For the final pose quaternion The modulus is calculated using the following formula: ; In the formula, For the final pose quaternion The real part; , , For the final pose quaternion The imaginary component. The result after normalization. This is the final pose quaternion that conforms to the specification.

[0077] In one possible embodiment, the method further includes: normalized final pose quaternions. The attitude angles, including the roll angle, can be calculated for practical applications. Pitch angle Heading angle The specific solution formula is derived based on the conversion relationship between quaternions and Euler angles, as shown below: First, let's define the expression for the normalized final pose quaternion: ; in, Let be the real part of the final pose quaternion after normalization; , , Let be the imaginary component of the final pose quaternion after normalization, and satisfy . (Normalization property).

[0078] Furthermore, the specific calculation formulas for each attitude angle are as follows: ; Furthermore, in the above formula The two-parameter arctangent function can completely cover the entire circumference angle range, avoiding the quadrant loss and angle jump defects that exist in the single-parameter arctangent function; the attitude angles obtained by directly solving this system of equations are in radians (rad), where the roll angle is... With heading angle The numerical range is [−π,π], pitch angle The numerical range is [−π / 2, π / 2]. If the engineering scenario requires converting radians to the commonly used angle unit (°), the obtained attitude angle value can be multiplied by a conversion factor to complete the unit conversion. The roll angle is obtained by solving the above system of equations. Pitch angle Heading angle After obtaining three sets of attitude angle data, the complete three-dimensional attitude information of the tested carrier 130 can be output, completing the entire nine-axis sensing attitude calculation process in a closed loop.

[0079] In one possible embodiment, the method further includes: calculating a first data magnitude based on acceleration data and comparing it with a preset gravitational acceleration confidence interval; if the first data magnitude exceeds the preset gravitational acceleration confidence interval, then determining that the acceleration data is invalid, freezing the angular velocity integral term, and retaining the angular velocity proportional component; calculating a second data magnitude based on magnetic field data and comparing it with a preset geomagnetic field strength confidence interval, while simultaneously determining whether the magnetic field data is in a state of all zeros; if the second data magnitude exceeds the preset geomagnetic field strength confidence interval, or if the magnetic field data is in a state of all zeros, then determining that... If the magnetic field data is invalid, freeze the heading integral term; based on the angular velocity data, compare the values ​​of each axis with the preset upper limit threshold of angular velocity; if any axis value exceeds the preset upper limit threshold of angular velocity, the angular velocity data is determined to be abnormal, and the valid angular velocity data of the previous frame is assigned to the current frame; based on the final attitude quaternion, calculate the quaternion modulus and compare it with the preset lower safety limit threshold; if the quaternion modulus is less than the preset lower safety limit threshold, the quaternion value is determined to be corrupted, the final attitude quaternion is reset to the initial unit quaternion, and the angular velocity integral term and heading integral term are cleared.

[0080] The first data magnitude is the vector magnitude of the raw acceleration data, used to characterize the overall amplitude of the acceleration vector. The preset gravitational acceleration confidence interval is the normal range defined based on standard gravitational acceleration, serving as the basis for judging whether the acceleration sampling data is distorted. The second data magnitude is the vector magnitude of the calibrated magnetic field data. The preset geomagnetic field strength confidence interval corresponds to the amplitude range of the geomagnetic field under normal conditions; a zero magnetic field data state indicates magnetometer sampling failure. The preset angular velocity upper limit threshold is used to define the normal output range of the gyroscope; exceeding this threshold indicates extreme motion of the carrier or sensor malfunction. The quaternion safety lower limit threshold is used to determine whether the quaternion has numerical distortion; the initial unit quaternion is the baseline initial state for attitude calculation. Freezing the integral term means stopping the iterative accumulation operation of the integral term, retaining only the proportional component for compensation calculation; clearing the integral term means directly setting the accumulated integral value to zero; resetting the quaternion restores the attitude calculation baseline to its initial state.

[0081] This step relies on vector magnitude calculation and logical judgment to detect anomalies in multi-source sensor data and attitude calculation results. The corresponding calculation formulas and operation principles for each step are as follows: The effectiveness of the accelerometer is tested based on the principle that, under static or quasi-static conditions, the accelerometer measurement should be approximately equal to the Earth's gravitational acceleration g (approximately 9.8 m / s²). When the system is subjected to linear acceleration disturbances or sensor malfunctions, the modulus will deviate from the normal range. In this case, the cross product error no longer reflects the true gravity deviation, and directly using it for integration compensation will lead to Roll / Pitch angle drift. First, the modulus of the accelerometer data is calculated. The formula for calculating the modulus of the first acceleration data is: ; In the formula, , , For the raw triaxial data of the accelerometer, This is the first data modulus, i.e., the modulus of the accelerometer data. Set the confidence interval: That is, the lower and upper limits of the confidence interval for weight acceleration; preferably, , The rationale for this setting is based on the gravitational acceleration g: when stationary or moving at a constant speed, the modulus measured by the accelerometer should equal g. An error tolerance of ±20% is allowed: this can accommodate common sensor noise, zero bias, temperature drift, and slight tilt or motion interference. It distinguishes between static and dynamic states: when an object has significant acceleration, the resulting acceleration will exceed this range and be deemed invalid. It also represents a trade-off based on engineering experience: a range that is too narrow is prone to misjudgment, while a range that is too wide will miss real motion. ±20% is a commonly used, reliable, and sensitive equilibrium value.

[0082] Furthermore, the formula for determining the validity of acceleration data is as follows: ; In the formula, , These represent the lower and upper limits of the confidence interval for gravitational acceleration, respectively. This is a valid acceleration indicator. When the indicator is 0, the accelerometer is deemed unreliable (possible causes include: linear acceleration generated during high-maneuver flight, sensor malfunction, severe vibration, etc.), and an angular velocity integral term freeze operation is performed. , Keep the previous frame value unchanged to prevent erroneous error values ​​from continuously contaminating the integration state; retain the proportional compensation term: still allow , It participates in gyroscope angular velocity compensation to maintain a certain fast response; gyroscope integral prediction continues to run; quaternion differential updates are uninterrupted, and the system degenerates into a "pure gyroscope + weak proportional correction" mode.

[0083] Furthermore, the magnetometer's effectiveness is tested. The principle behind this test is that the Earth's magnetic field strength has a baseline range (typically 25~65 μT) at different geographical locations. When the magnetometer is affected by hard magnetic interference from ferromagnetic materials, electromagnetic soft magnetic distortion, or sensor saturation, its calibrated modulus will deviate from the normal range of geomagnetic field strength. In addition, raw magnetometer data that is all zero or close to all zero also indicates that the sensor is not working or there is a communication failure.

[0084] First, the modulus of the magnetometer data is calculated. The formula for calculating the modulus of the second magnetic field data is as follows: ; In the formula, , , For the calibrated magnetic field data, This is the second data modulus. The geomagnetic field confidence threshold is set to... ,in This is the lower limit of the geomagnetic field (adjusted according to the geographical latitude of the deployment area). The upper limit of the geomagnetic field (including a certain margin) is determined by the following rules: ; At the same time, zero-value protection of the original data is added as a prerequisite: ; Furthermore, the overall effectiveness of the magnetometer is obtained by combining the all-zero state determination: ; In the formula, The magnetic field data is marked as all zeros. This serves as a comprehensive and valid indicator for the magnetometer. If the magnetometer is deemed unreliable (possible causes include: presence of ferromagnetic objects nearby, electromagnetic interference, sensor saturation, hardware failure, etc.), a heading integration term freeze operation is performed, i.e. Keeping the previous frame value unchanged, and the yaw angle calculated solely by the gyroscope: the yaw angle obtains its natural drift characteristic only through gyroscope integration, and automatically reconnects for correction once the magnetometer regains its effectiveness. Also, no construction... Quaternions do not perform Yaw-axis quaternion multiplication fusion.

[0085] Furthermore, the validity of gyroscope data is tested based on the principle that the gyroscope measures the angular rate of rotation of the carrier relative to the inertial frame. Physical constraints determine that its angular velocity in normal motion scenarios has a reasonable upper limit. If the output of a certain axis shows a jump far exceeding the physical limit, it is highly likely to be a noise spike or a transient sensor failure.

[0086] Among them, the absolute upper limit threshold of the three-axis angular velocity is set. (Typical values ​​are taken as 90%~95% of the range, for example, for a range of ±2000° / s, take...) Formula for determining the validity of angular velocity data: ; In the formula, The upper limit threshold for angular velocity. This is a valid indicator for the gyroscope. When an abnormality is detected, the gyroscope data of the current frame is discarded: the abnormal value is not used to participate in the quaternion differential update; the historical effective angular velocity data is reused, and the angular velocity after compensation in the previous frame is used to replace the value of the current frame for quaternion extrapolation to ensure that the attitude calculation is not interrupted.

[0087] Furthermore, a safety check is performed on the quaternion values. The detection principle is that during the attitude calculation process, due to floating-point cumulative errors, extreme inputs, or numerical calculation overflows, the quaternion modulus may deviate significantly from the unit length (theoretically it should always be 1), or even approach zero. If a normalized division operation is performed on a near-zero quaternion, it will cause a division-by-zero error, leading to the collapse of the entire attitude calculation link.

[0088] First, calculate the quaternion module length. The formula for calculating the final attitude quaternion module length is as follows: ; Among them, the formula for determining the safety status of quaternions is: ; In the formula, The lower safety threshold for quaternions is typically set to 10. -6 , For quaternion security flags, when When a numerical crash is detected, the quaternion is reset to its initial unit quaternion. (Corresponding to a horizontal north orientation), and clear all integral terms. , , To prevent historical erroneous integration from affecting the reconvergence process; set the restart convergence flag to notify the upper-layer application system that it is in the attitude reinitialization phase, and the short-term output is for reference only.

[0089] As can be seen in this example, the hierarchical anomaly detection and graded fault handling strategy can achieve accurate fault identification for four types of objects: acceleration, magnetic field, angular velocity, and attitude quaternions. Differentiated processing methods, such as freezing integral terms, reusing historical data, and resetting the computational baseline, are adopted for different fault types. This can prevent the continuous spread of faults under conditions such as short-term sensor failure, data distortion, and abnormal numerical calculations, ensuring that the attitude calculation process is not directly interrupted and effectively improving the continuity of system operation.

[0090] The following describes the specific application scenarios to which this solution can be applied: The tested carrier 130 covers a variety of typical application scenarios, including indoor mobile devices, outdoor aircraft, human-computer interaction wearable devices, industrial fixed monitoring equipment, and human motion monitoring protective gear. Each scenario has geomagnetic field distortion interference caused by structures such as metal shells, metal brackets, metal fasteners, and building steel bars. At the same time, it corresponds to different operating conditions such as high-speed movement of the carrier, long-term static placement, small-amplitude high-frequency swing of the human body, and joint deformation.

[0091] The attitude calculation method provided in this embodiment relies on a core architecture that is completely decoupled between gravity observation and geomagnetic observation channels. Combined with proportional-integral control with differentiated thresholds and hierarchical fault degradation and fault tolerance logic, it can ensure that magnetic field interference only affects the attitude in the heading dimension and does not interfere with the calculation results of roll angle and pitch angle. At the same time, the lightweight operation logic of the entire algorithm can be adapted to low-power embedded hardware on various carriers.

[0092] Summary of Adaptation Effects by Scenario: 1. For indoor service robots, it can isolate the continuous geomagnetic distortion caused by indoor metal shelves and steel walls, freeze the corresponding integral term when acceleration data is abnormal, and ensure no horizontal attitude drift during walking; 2. For aerial drones, it can complete attitude calculation based on gyroscopes under high maneuver conditions such as diving and turning, and can temporarily shut down heading correction when there are sudden changes in magnetic field caused by outdoor iron towers and buildings to avoid tilting of the drone screen; 3. For VR / AR headsets, the local weak magnetic field interference caused by the metal components inside the device only changes the heading value and does not cause horizontal screen jitter, ensuring smooth head interaction; 4. For industrial monitoring gimbals, the fixed static geomagnetic bias caused by metal brackets can be slowly eliminated through the integral term, and the roll and pitch attitudes remain stable during long-term static operation; 5. For smart sports knee braces, the local magnetic field distortion generated by the metal fasteners of the brace will not interfere with the measurement of roll and pitch angles corresponding to joint flexion and extension, accurately capturing human motion posture.

[0093] Therefore, it can be seen that the technical solution of this application has universality for various carrier scenarios with geomagnetic interference, and the attitude calculation accuracy and system operation stability are significantly improved in complex environments.

[0094] As can be seen, in this embodiment, the solution forms a complete closed loop from data acquisition, preprocessing, decoupled attitude calculation, heading correction, quaternion fusion to full-dimensional fault detection. Through the decoupled design of dual-channel gravity and geomagnetic observation, the influence of magnetic field interference on horizontal attitude is eliminated at the source. Combined with proportional-integral control and integral limiting mechanisms with differentiated parameters, attitude drift and integral saturation problems are effectively suppressed. Furthermore, the use of lightweight computational logic and a tiered fault degradation strategy significantly reduces overall computational overhead, making it compatible with various low-power embedded devices. It also significantly improves the system's anti-interference capability, stability, and fault tolerance under complex conditions such as magnetic field distortion, sensor anomalies, and large-scale carrier maneuvers. It can be widely applied to various test carriers 130 equipped with a nine-axis inertial measurement unit 120, such as drones, service robots, and smartwatches.

[0095] The following are embodiments of the apparatus of this application. These embodiments of the apparatus and the embodiments of the method of this application belong to the same concept and are used to execute the methods described in the embodiments of this application. For ease of explanation, only the parts related to the apparatus embodiments of this application are shown in the embodiments of this application. For specific technical details not disclosed, please refer to the description of the embodiments of the method of this application, which will not be repeated here.

[0096] This application provides an observation-decoupling-based attitude calculation device for a nine-axis inertial measurement unit (IMU). Specifically, the observation-decoupling-based nine-axis IMU attitude calculation device is used to execute the steps performed by the server in the above-described observation-decoupling-based nine-axis IMU attitude calculation method. The observation-decoupling-based nine-axis IMU attitude calculation device of this application may include modules corresponding to the respective steps.

[0097] This application embodiment can divide the attitude calculation device of a nine-axis inertial measurement unit based on observation decoupling into functional modules according to the above method example. For example, each function can be divided into a separate functional module, or two or more functions can be integrated into one processing module. The integrated module can be implemented in hardware or as a software functional module. The module division in this application embodiment is illustrative and only represents one logical functional division. In actual implementation, there may be other division methods.

[0098] When dividing each function into modules according to its corresponding function. Figure 3 This is a functional block diagram of a nine-axis inertial measurement unit attitude calculation device based on observation decoupling provided in this application embodiment; the nine-axis inertial measurement unit attitude calculation device 30 based on observation decoupling is applied to, for example... Figure 1The attitude calculation system 10 shown includes an electronic device 110, which is communicatively connected to a nine-axis inertial measurement unit 120 within the system. The nine-axis inertial measurement unit 120 integrates a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, and is rigidly fixed to the measured carrier 130. The device includes: a data acquisition unit 301, used to acquire acceleration data output from the three-axis accelerometer, angular velocity data output from the three-axis gyroscope, and magnetic field data output from the three-axis magnetometer; and a horizontal attitude calculation unit 302, used to perform error compensation only on the X-axis and Y-axis angular velocities in the angular velocity data based on the acceleration data, and update the current attitude quaternion based on the compensated angular velocity to obtain the target attitude quaternion. The attitude quaternion is used to characterize the measured carrier. The three-dimensional rotational attitude is calculated. Error compensation refers to generating a compensation amount based on the deviation between the acceleration data and the predicted gravity vector and superimposing it on the corresponding axis angular velocity. The predicted gravity vector is a vector obtained by rotating the reference gravity vector based on the current attitude quaternion. The current attitude quaternion is an attitude characterization parameter obtained by iteratively updating the angular velocity data of the previous moment through quaternion differential equations. The heading attitude calculation unit 303 is used to calculate the heading angle error based on the magnetic field data and construct a corrected quaternion that rotates only around the Z-axis of the world coordinate system based on the heading angle error. The X and Y components of the vector part of the corrected quaternion are both zero. The attitude fusion unit 304 is used to perform quaternion multiplication and fusion of the corrected quaternion and the target attitude quaternion to obtain the final attitude quaternion.

[0099] In one possible embodiment, regarding the error compensation performed only on the X-axis and Y-axis angular velocities in the angular velocity data based on the acceleration data, and the update of the current attitude quaternion based on the compensated angular velocities to obtain the target attitude quaternion, the horizontal attitude calculation unit 302 is specifically used to: perform a rotation transformation on the reference gravity vector based on the current attitude quaternion to obtain a predicted gravity vector, where the reference gravity vector is a constant gravity direction vector in the world coordinate system; calculate the cross product error between the predicted gravity vector and the measured vector corresponding to the acceleration data; perform proportional and integral joint operations on the cross product error through a proportional-integral controller to obtain the angular velocity compensation amount; superimpose the angular velocity compensation amount onto the X-axis and Y-axis angular velocities respectively, while keeping the Z-axis angular velocity as the original measured value; and update the current attitude quaternion through a quaternion differential equation based on the superimposed compensated angular velocity data to obtain the target attitude quaternion.

[0100] In one possible embodiment, in obtaining the angular velocity compensation amount by performing proportional and integral joint calculations on the cross product error using a proportional-integral controller, the horizontal attitude calculation unit 302 is specifically used to: calculate the proportional component of angular velocity based on the cross product error and a first preset proportional coefficient; iteratively accumulate the angular velocity integral term of the proportional-integral controller based on the cross product error and a first sampling period to obtain the accumulated angular velocity integral term; and if the accumulated angular velocity integral term exceeds a first integral limiting threshold, clamp the accumulated angular velocity integral term within the first integral limiting threshold; and synthesize the angular velocity compensation amount based on the proportional component of angular velocity and the limited angular velocity integral term.

[0101] In one possible embodiment, the heading and attitude calculation unit 303 is specifically used to: extract roll and pitch angles from the target attitude quaternion; perform tilt compensation on the magnetic field data based on the roll and pitch angles to obtain the magnetometer horizontal component; calculate the magnetometer observed heading angle based on the magnetometer horizontal component and extract the current heading angle from the target attitude quaternion; calculate the difference between the magnetometer observed heading angle and the current heading angle to obtain the heading angle error; perform amplitude limiting processing on the heading angle error to constrain it within a single angle range; and calculate the amplitude-limited heading angle error by a proportional-integral controller to obtain the heading angle correction; construct a corrected quaternion based on the heading angle correction, wherein the real part of the corrected quaternion is the cosine of half the heading angle correction and the Z component of the imaginary part is the sine of half the heading angle correction.

[0102] In one possible embodiment, in calculating the heading angle correction by the proportional-integral controller after limiting the heading angle error, the heading attitude calculation unit 303 is specifically used to: calculate the heading proportional component based on the limited heading angle error and a second preset proportional coefficient; iteratively accumulate the heading integral term of the proportional-integral controller based on the limited heading angle error and the second sampling period to obtain the accumulated heading integral term; and if the accumulated heading integral term exceeds a second integral limiting threshold, clamp the accumulated heading integral term within the second integral limiting threshold, the second integral limiting threshold being less than a first integral limiting threshold; and synthesize the heading angle correction by combining the heading proportional component and the limited heading integral term.

[0103] In one possible embodiment, the attitude fusion unit 304 is further configured to: calculate a first data magnitude based on acceleration data and compare it with a preset gravitational acceleration confidence interval; if the first data magnitude exceeds the preset gravitational acceleration confidence interval, determine that the acceleration data is invalid, freeze the angular velocity integral term, and retain the angular velocity proportional component; calculate a second data magnitude based on magnetic field data and compare it with a preset geomagnetic field strength confidence interval, and simultaneously determine whether the magnetic field data is in a state of all zeros; if the second data magnitude exceeds the preset geomagnetic field strength confidence interval, or, determine whether the magnetic field data is in a state of all zeros. If the magnetic field data is deemed invalid, the heading integral term is frozen. Based on the angular velocity data, the values ​​of each axis are compared with the preset upper threshold of angular velocity. If any axis value exceeds the preset upper threshold of angular velocity, the angular velocity data is deemed abnormal, and the valid angular velocity data from the previous frame is assigned to the current frame. Based on the final attitude quaternion, the quaternion modulus is calculated and compared with the preset lower threshold of safety. If the quaternion modulus is less than the preset lower threshold of safety, the quaternion value is deemed to have collapsed, the final attitude quaternion is reset to the initial unit quaternion, and the angular velocity integral term and heading integral term are cleared.

[0104] In one possible embodiment, after acquiring the acceleration data output by the triaxial accelerometer, the angular velocity data output by the triaxial gyroscope, and the magnetic field data output by the triaxial magnetometer, before performing error compensation only on the X-axis angular velocity and Y-axis angular velocity data based on the acceleration data, the data acquisition unit 301 is further configured to: perform gyroscope zero-bias calibration on the angular velocity data to obtain calibrated angular velocity data; and perform six-point orthogonal static calibration based on the acceleration data to obtain calibrated acceleration data; and perform ellipsoidal fitting calibration based on the magnetic field data to obtain calibrated magnetic field data; perform filtering processing on the calibrated angular velocity data, calibrated acceleration data, and calibrated magnetic field data respectively to obtain filtered acceleration data, angular velocity data, and magnetic field data; and perform modulus normalization processing on the filtered acceleration data to obtain normalized acceleration data.

[0105] When using integrated units, such as Figure 4 As shown, Figure 4 This is a functional unit block diagram of another observation-decoupled nine-axis inertial measurement unit attitude calculation device provided in this application embodiment. Figure 4The observation-decoupling-based nine-axis inertial measurement unit (IMU) attitude calculation device 40 includes a processing module 402 and a communication module 401. The processing module 402 controls and manages the actions of the observation-decoupling-based nine-axis IMU attitude calculation device 40, such as the steps of the data acquisition unit 301, the horizontal attitude calculation unit 302, the heading attitude calculation unit 303, and the attitude fusion unit 304, and / or other processes for executing the techniques described herein. The communication module 401 supports interaction between the observation-decoupling-based nine-axis IMU attitude calculation device and other devices. Figure 4 As shown, the attitude calculation device of the nine-axis inertial measurement unit based on observation decoupling may include a storage module 403, which is used to store the program code and data of the attitude calculation device of the nine-axis inertial measurement unit based on observation decoupling.

[0106] The processing module 402 can be a processor or processing module, such as a central processing unit (CPU), a general-purpose processor, a digital signal processor (DSP), an ASIC, an FPGA, or other programmable logic devices, transistor logic devices, hardware components, or any combination thereof. It can implement or execute the various exemplary logic blocks, modules, and circuits described in conjunction with the disclosure of this application. The processor can also be a combination that implements computational functions, such as a combination of one or more microprocessors, a combination of a DSP and a microprocessor, etc. The communication module 401 can be a transceiver, RF circuitry, or a communication interface, etc. The storage module 403 can be a memory.

[0107] All relevant content in each scenario involved in the above method embodiments can be referenced from the functional descriptions of the corresponding functional modules, and will not be repeated here. The above-mentioned nine-axis inertial measurement unit attitude calculation device 40 based on observation decoupling can all perform the above-mentioned... Figure 2 The method for attitude calculation of a nine-axis inertial measurement unit based on observation decoupling is shown.

[0108] The above embodiments can be implemented, in whole or in part, by software, hardware, firmware, or any other combination thereof. When implemented using software, the above embodiments can be implemented, in whole or in part, as a computer program product. A computer program product includes one or more computer instructions or computer programs. When the computer instructions or computer programs are loaded or executed on a computer, all or part of the processes or functions according to the embodiments of this application are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. Computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired or wireless means. A computer-readable storage medium can be any available medium that a computer can access or a data storage device such as a server or data center that includes one or more sets of available media. Available media can be magnetic media (e.g., floppy disks, hard disks, magnetic tapes), optical media (e.g., DVDs), or semiconductor media. Semiconductor media can be solid-state drives.

[0109] Figure 5 This is a structural block diagram of an electronic device provided in an embodiment of this application. For example... Figure 5 As shown, the electronic device 110 may include one or more of the following components: a processor 501 and a memory 502 coupled to the processor 501, wherein the memory 502 may store one or more computer programs 503, and the one or more computer programs 503 may be configured to implement the methods described in the above embodiments when executed by one or more processors 501.

[0110] Processor 501 may include one or more processing cores. Processor 501 connects to various parts within the electronic device 110 using various interfaces and lines, and performs various functions and processes data of the electronic device 110 by running or executing instructions, programs, code sets, or instruction sets stored in memory 502, and by calling data stored in memory 502. Optionally, processor 501 may be implemented using at least one hardware form of Digital Signal Processing (DSP), Field-Programmable Gate Array (FPGA), or Programmable Logic Array (PLA). Processor 501 may integrate one or more of the following: Central Processing Unit (CPU), Graphics Processing Unit (GPU), and modem. The CPU primarily handles the operating system, user interface, and applications; the GPU is responsible for rendering and drawing the displayed content; and the modem handles wireless communication. It is understood that the modem may also not be integrated into processor 501 and may be implemented separately using a communication chip.

[0111] The memory 502 may include random access memory (RAM) or read-only memory (ROM). The memory 502 can be used to store instructions, programs, code, code sets, or instruction sets. The memory 502 may include a program storage area and a data storage area. The program storage area may store instructions for implementing an operating system, instructions for implementing at least one function (such as touch functionality, sound playback functionality, image playback functionality, etc.), and instructions for implementing the various method embodiments described above. The data storage area may also store data created by the electronic device 110 during use.

[0112] It is understood that the electronic device 110 may include more or fewer structural elements than those shown in the above block diagram, and this is not limited thereto.

[0113] This application also provides a computer storage medium storing a computer program / instructions thereon, which, when executed by a processor, implements some or all of the steps of any of the methods described in the above method embodiments.

[0114] This application also provides a computer program product, which includes a non-transitory computer-readable storage medium storing a computer program operable to cause a computer to perform some or all of the steps of any of the methods described in the above method embodiments.

[0115] It should be understood that in the various embodiments of this application, the order of the above-mentioned processes does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0116] In the several embodiments provided in this application, it should be understood that the disclosed methods, apparatuses, and systems can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for example, the division of units is merely a logical functional division, and there may be other division methods in actual implementation; for example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces, and the indirect coupling or communication connection between devices or units may be electrical, mechanical, or other forms.

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

[0118] Furthermore, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can be physically comprised separately, or two or more units can be integrated into one unit. The integrated unit described above can be implemented in hardware or in the form of hardware plus software functional units.

[0119] The integrated units implemented as software functional units described above can be stored in a computer-readable storage medium. These software functional units, stored in a storage medium, include several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute partial steps of the methods of the various embodiments of the present invention. The aforementioned storage medium includes: a USB flash drive, a portable hard drive, a magnetic disk, an optical disk, volatile memory, or non-volatile memory. The non-volatile memory can be read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), or flash memory. The volatile memory can be random access memory (RAM), which is used as an external cache. By way of example, but not limitation, many forms of random access memory (RAM) are available, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous linked DRAM (SLDRAM), and direct rambus RAM (DR RAM), etc., which are various media capable of storing program code.

[0120] While the present invention has been disclosed above, it is not limited thereto. Any person skilled in the art can easily conceive of variations or substitutions without departing from the spirit and scope of the present invention, and various modifications and alterations can be made, including combinations of the different functions and implementation steps described above, as well as software and hardware implementation methods, all of which are within the protection scope of the present invention.

Claims

1. A method for attitude calculation of a nine-axis inertial measurement unit based on observation decoupling, characterized in that, An electronic device used in an attitude calculation system, wherein the electronic device is communicatively connected to a nine-axis inertial measurement unit in the attitude calculation system; The nine-axis inertial measurement unit integrates a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, and is rigidly fixed to the carrier under test; the method includes: Acquire the acceleration data output by the triaxial accelerometer, the angular velocity data output by the triaxial gyroscope, and the magnetic field data output by the triaxial magnetometer; Based on the acceleration data, error compensation is performed only on the X-axis angular velocity and Y-axis angular velocity in the angular velocity data, and the current attitude quaternion is updated based on the compensated angular velocity to obtain the target attitude quaternion. The attitude quaternion is used to characterize the three-dimensional rotational attitude of the tested vehicle. The error compensation refers to generating a compensation amount based on the deviation between the acceleration data and the predicted gravity vector and superimposing it on the corresponding axis angular velocity. The predicted gravity vector is a vector obtained by rotating and transforming the reference gravity vector based on the current attitude quaternion. The current attitude quaternion is an attitude characterization parameter obtained by iteratively updating the angular velocity data at the previous moment through quaternion differential equations. The process involves calculating the heading angle error based on the magnetic field data and constructing a corrected quaternion that rotates only around the Z-axis of the world coordinate system. This includes: extracting roll and pitch angles from the target attitude quaternion; performing tilt compensation on the magnetic field data based on the roll and pitch angles to obtain the magnetometer horizontal component; calculating the magnetometer observed heading angle based on the magnetometer horizontal component and extracting the current heading angle from the target attitude quaternion; calculating the difference between the magnetometer observed heading angle and the current heading angle to obtain the heading angle error; limiting the heading angle error to confine it within a single circumference; and calculating the limited heading angle error using a proportional-integral controller to obtain a heading angle correction; constructing the corrected quaternion based on the heading angle correction, where the real part of the corrected quaternion is the cosine of half the heading angle correction, the Z-component of the imaginary part is the sine of half the heading angle correction, and the X and Y components of the vector part of the corrected quaternion are both zero. The modified quaternion and the target attitude quaternion are fused by quaternion multiplication to obtain the final attitude quaternion.

2. The attitude calculation method for a nine-axis inertial measurement unit based on observation decoupling according to claim 1, characterized in that, The step of performing error compensation only on the X-axis angular velocity and Y-axis angular velocity in the angular velocity data based on the acceleration data, and updating the current attitude quaternion based on the compensated angular velocity to obtain the target attitude quaternion, includes: The reference gravity vector is rotated and transformed according to the current attitude quaternion to obtain the predicted gravity vector, wherein the reference gravity vector is the constant gravity direction vector in the world coordinate system. Calculate the cross product error between the predicted gravity vector and the measured vector corresponding to the acceleration data; The angular velocity compensation amount is obtained by performing proportional and integral calculations on the cross product error using a proportional-integral controller. The angular velocity compensation is superimposed on the X-axis angular velocity and the Y-axis angular velocity respectively, while keeping the Z-axis angular velocity at its original measured value; Based on the superimposed and compensated angular velocity data, the current attitude quaternion is updated through a quaternion differential equation to obtain the target attitude quaternion.

3. The attitude calculation method for a nine-axis inertial measurement unit based on observation decoupling according to claim 2, characterized in that, The step of performing proportional and integral joint calculations on the cross product error using a proportional-integral controller to obtain the angular velocity compensation amount includes: The proportional component of angular velocity is calculated based on the cross product error and the first preset proportional coefficient. Based on the cross product error and the first sampling period, the angular velocity integral term of the proportional-integral controller is iteratively accumulated to obtain the accumulated angular velocity integral term; and if the accumulated angular velocity integral term exceeds the first integral limiting threshold, the accumulated angular velocity integral term is clamped within the first integral limiting threshold. The angular velocity compensation amount is obtained by combining the proportional component of the angular velocity with the integral term of the angular velocity after limiting.

4. The attitude calculation method for a nine-axis inertial measurement unit based on observation decoupling according to claim 3, characterized in that, The step of calculating the heading angle error after amplitude limiting using a proportional-integral controller to obtain the heading angle correction includes: Based on the heading angle error after amplitude limiting and the second preset proportional coefficient, the heading proportional component is calculated; Based on the heading angle error after the amplitude limit and the second sampling period, the heading integral term of the proportional-integral controller is iteratively accumulated to obtain the accumulated heading integral term; and if the accumulated heading integral term exceeds the second integral amplitude limit threshold, the accumulated heading integral term is clamped within the second integral amplitude limit threshold, wherein the second integral amplitude limit threshold is less than the first integral amplitude limit threshold. The heading angle correction is obtained by combining the heading proportional component and the heading integral term after the heading is limited.

5. The attitude calculation method for a nine-axis inertial measurement unit based on observation decoupling according to claim 4, characterized in that, The method further includes: The first data modulus is calculated based on the acceleration data and compared with a preset gravitational acceleration confidence interval; If the first data modulus exceeds the preset gravitational acceleration confidence range, the acceleration data is determined to be invalid, the angular velocity integral term is frozen, and the angular velocity proportional component is retained. The second data modulus is calculated based on the magnetic field data and compared with the preset geomagnetic field strength confidence interval. At the same time, it is determined whether the magnetic field data is in a state of all zero. If the second data modulus exceeds the preset geomagnetic field strength confidence range, or if the magnetic field data is in the all-zero state, then the magnetic field data is determined to be invalid, and the heading integral term is frozen; Based on the angular velocity data, the values ​​of each axis are compared with the preset upper limit threshold of angular velocity; If any axis value exceeds the preset angular velocity upper limit threshold, the angular velocity data is determined to be abnormal, and the valid angular velocity data of the previous frame is assigned to the current frame. Based on the final attitude quaternion, calculate the quaternion modulus and compare it with the preset safety lower limit threshold. If the quaternion modulus is less than the preset safety lower limit threshold, the quaternion value is determined to have collapsed. The final attitude quaternion is then reset to the initial unit quaternion, and the angular velocity integral term and the heading integral term are cleared.

6. The attitude calculation method for a nine-axis inertial measurement unit based on observation decoupling according to any one of claims 1-5, characterized in that, After acquiring the acceleration data output by the triaxial accelerometer, the angular velocity data output by the triaxial gyroscope, and the magnetic field data output by the triaxial magnetometer, and before performing error compensation only on the X-axis angular velocity and Y-axis angular velocity in the angular velocity data based on the acceleration data, the method further includes: Perform gyroscope zero-bias calibration on the angular velocity data to obtain calibrated angular velocity data; and perform six-point orthogonal stationary calibration on the acceleration data to obtain calibrated acceleration data; and perform ellipsoidal fitting calibration on the magnetic field data to obtain calibrated magnetic field data. The calibrated angular velocity data, the calibrated acceleration data, and the calibrated magnetic field data are respectively filtered to obtain the filtered acceleration data, the angular velocity data, and the magnetic field data. The filtered acceleration data is subjected to modulus normalization to obtain the normalized acceleration data.

7. A nine-axis inertial measurement unit attitude calculation device based on observation decoupling, characterized in that, An electronic device used in an attitude calculation system, wherein the electronic device is communicatively connected to a nine-axis inertial measurement unit in the attitude calculation system; The nine-axis inertial measurement unit integrates a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, and is rigidly fixed to the carrier being measured; the device includes: The data acquisition unit is used to acquire the acceleration data output by the triaxial accelerometer, the angular velocity data output by the triaxial gyroscope, and the magnetic field data output by the triaxial magnetometer. The horizontal attitude calculation unit is used to perform error compensation only on the X-axis angular velocity and Y-axis angular velocity in the angular velocity data based on the acceleration data, and update the current attitude quaternion based on the compensated angular velocity to obtain the target attitude quaternion. The attitude quaternion is used to characterize the three-dimensional rotational attitude of the measured carrier. The error compensation refers to generating a compensation amount based on the deviation between the acceleration data and the predicted gravity vector and superimposing it on the corresponding axis angular velocity. The predicted gravity vector is a vector obtained by rotating and transforming the reference gravity vector based on the current attitude quaternion. The current attitude quaternion is an attitude characterization parameter obtained by iteratively updating the angular velocity data at the previous moment through quaternion differential equations. The heading and attitude calculation unit is used to calculate the heading angle error based on the magnetic field data, and construct a corrected quaternion that rotates only around the Z-axis of the world coordinate system based on the heading angle error. This includes: extracting the roll and pitch angles from the target attitude quaternion; performing tilt compensation on the magnetic field data based on the roll and pitch angles to obtain the magnetometer horizontal component; calculating the magnetometer observed heading angle based on the magnetometer horizontal component, and extracting the current heading angle from the target attitude quaternion; and calculating the magnetometer observed heading angle relative to the target attitude quaternion. The difference in the current heading angle is used to obtain the heading angle error; the heading angle error is subjected to amplitude limiting processing to constrain it within a single angle range; and the amplitude-limited heading angle error is calculated by a proportional-integral controller to obtain the heading angle correction; the correction quaternion is constructed based on the heading angle correction, wherein the real part of the correction quaternion is the cosine of half the heading angle correction, the Z component of the imaginary part is the sine of half the heading angle correction, and the X and Y components of the vector part of the correction quaternion are both zero; The attitude fusion unit is used to perform quaternion multiplication fusion on the corrected quaternion and the target attitude quaternion to obtain the final attitude quaternion.

8. An electronic device, characterized in that, It includes a processor, a memory, a communication interface, and one or more programs, said one or more programs being stored in the memory and configured to be executed by the processor, said programs including instructions for performing the steps of the method as described in any one of claims 1-6.

9. A computer-readable storage medium, characterized in that, A computer program for storing electronic data interchange is provided, wherein the computer program causes a computer to perform the method as described in any one of claims 1-6.

Images