High-frequency output method and system for attitude sensor based on dual-loop control

Abstract

The present invention relates to the technical field of inertial navigation and attitude estimation, and discloses a high-frequency output method and system for an attitude sensor based on dual-loop control. The method comprises: high-frequency calculation of an inner-loop output of an attitude angle using a data fusion algorithm based on a three-axis acceleration and a three-axis gyroscope; accurate calculation of an outer-loop output of an attitude angle using a data fusion algorithm based on nine-axis data of a nine-axis sensor, and correction of the accumulated error of the inner-loop; dynamic resetting the corrected inner-loop output of the attitude angle to the inner-loop calculation to achieve high-frequency update of the attitude angle; the system comprises: a control module, an attitude sensor module, a level conversion module, a voltage stabilization module, and an input / output module. A dual-loop output control method is designed to address the fact that the output frequency of the nine-axis attitude sensor is affected by a magnetometer, thereby effectively improving the use accuracy of the gyroscope and the output frequency of the nine-axis attitude sensor.

Description

Technical Field

[0001] The present invention belongs to the technical field of inertial navigation and attitude estimation, and in particular relates to a high-frequency output method and system of an attitude sensor based on dual-loop control. Background Art

[0002] With the rapid development of embedded technology, the demand for high-precision attitude control in areas such as drones, humanoid robots, virtual reality, and wearable devices is increasing. Currently, there are many mainstream attitude estimation solutions, such as optical motion capture systems, visual inertial odometry, lidar, and inertial measurement units. However, although optical motion capture systems are extremely accurate, they cannot be used outdoors. Visual inertial odometry and lidar both require large amounts of data. However, inertial navigation units have low computational complexity, low cost, and good accuracy, and are widely used in various fields requiring attitude estimation. The mainstream attitude estimation solution uses a six-axis inertial measurement unit to measure three-axis acceleration and a three-axis gyroscope, respectively, combined with a three-axis magnetometer to form a nine-axis sensor. The attitude angle is ultimately calculated through a data fusion algorithm.

[0003] Three-axis acceleration and gyroscope data are generated by inertial measurement sensors, which convert acceleration and angular velocity into voltage signals. These signals are then passed through an analog-to-digital converter to produce six-axis data. The update frequency is typically above 1 kHz. Due to the Earth's weak magnetic field and the need to filter out ambient magnetic noise, the magnetometer sampling time is typically 2-10ms, resulting in an overall output frequency of no more than 500 Hz for the nine-axis sensor. However, control frequencies in areas such as drones, robotics, virtual reality, and wearable devices are typically above 1 kHz.

[0004] Therefore, there is an urgent need to develop a high-frequency output method and system for attitude sensors based on dual-loop control, which can achieve high-frequency output of nine-axis sensor data and solve the problem of slow update of nine-axis sensor data during the control process. Summary of the Invention

[0005] In order to solve the above technical problems, the present invention provides a high-frequency output method and system for a posture sensor based on dual-loop control, which can achieve high-frequency output of nine-axis sensor data and solve the problem of slow update of nine-axis sensor data during the control process.

[0006] The present invention provides a high-frequency output method for a posture sensor based on dual-loop control, the method comprising the following steps:

[0007] S1, based on the six-axis data of the three-axis accelerometer and the three-axis gyroscope, the attitude angle inner loop output is calculated at high frequency through the first data fusion algorithm;

[0008] S2. Based on the nine-axis data of the nine-axis sensor, the second data fusion algorithm is used to calculate the attitude angle outer loop output at a low frequency, and the attitude angle outer loop output is used to correct the accumulated error of the attitude angle inner loop output;

[0009] S3. Dynamically reset the corrected attitude angle inner loop output to the inner loop calculation to achieve high-frequency update of the attitude angle.

[0010] Furthermore, in S1, the first data fusion algorithm includes:

[0011] S11. Calculate the roll angle roll_acc around the x-axis and the pitch angle pitch_acc around the y-axis using the three-axis acceleration data;

[0012] S12. Calculate the roll angle roll_gyro around the x-axis, the pitch angle pitch_gyro around the y-axis, and the heading angle yaw_gyro around the z-axis by integrating the three-axis gyroscope data;

[0013] S13. Dynamically adjust the weight factor α based on the acceleration modulus value, fuse roll_acc and roll_gyro to obtain the inner loop roll angle, fuse pitch_acc and pitch_gyro to obtain the inner loop pitch angle, and directly output yaw_gyro as the inner loop heading angle.

[0014] Furthermore, in S2, the second data fusion algorithm includes:

[0015] S21. Calculate the outer ring roll angle and the outer ring pitch angle using the three-axis acceleration and the three-axis gyroscope according to the method in S11-S13;

[0016] S22, calculate the heading angle yaw_mag around the z-axis using the three-axis magnetometer data;

[0017] S23, integrate yaw_mag and yaw_gyro in the inner loop to obtain the precise heading angle;

[0018] S24: Synchronize the outer ring roll angle, outer ring pitch angle, and precise heading angle to the inner ring, and reset the initial integral value of the inner ring.

[0019] Furthermore, in S13, dynamically adjusting the weight factor α based on the acceleration modulus value includes:

[0020] When the acceleration modulus is greater than the first preset value or less than the second preset value, the weight factor α is reduced by the preset value; otherwise, the weight factor α remains unchanged;

[0021] The first preset value is greater than the second preset value.

[0022] The present invention also provides a high-frequency output system for a posture sensor based on dual-loop control, which is used to execute the above-mentioned high-frequency output method for a posture sensor based on dual-loop control. The system includes the following modules:

[0023] The control module is used to read the raw data of the nine-axis sensor and execute the dual-loop control algorithm;

[0024] The attitude sensor module is used to collect raw data from the nine-axis sensor;

[0025] Level conversion module, used to bridge devices with different voltages to achieve cross-voltage communication;

[0026] Voltage stabilization module, used to provide stable voltage for the system;

[0027] Input and output modules are used to provide external communication, power supply and debugging interfaces.

[0028] Furthermore, the control module includes a first control chip U1, pin 1 of the first control chip U1 is connected to the first capacitor C1, pin 9 of the first control chip U1 is connected to the second capacitor C2 and the third capacitor C3, pin 24 of the first control chip U1 is connected to the fourth capacitor C4, pin 36 of the first control chip U1 is connected to the fifth capacitor C5, pin 48 of the first control chip U1 is connected to the sixth capacitor C6, pin 5 and pin 6 of the first control chip U1 are connected to the first non-crystalline oscillator X1 and two co-oscillator capacitors, the seventh capacitor C7 and the eighth capacitor C8, pin 7 of the first control chip U1 is connected to the reset circuit composed of the first resistor R1, the ninth capacitor C9 and the first switch SW1, the second resistor R2 is connected between the digital power supply and the analog power supply, and the third resistor R3 is connected between the digital ground and the analog ground.

[0029] Furthermore, the posture sensor module includes a second chip U2, pin 8 of the second chip U2 is connected to the tenth capacitor C10 and the eleventh capacitor C11, pin 10 of the second chip U2 is connected to the twelfth capacitor C12, the other end of the twelfth capacitor C12 is grounded, and pin 13 of the second chip U2 is connected to the thirteenth capacitor C13.

[0030] Furthermore, the level conversion module includes a third chip U3, pin 8 of the third chip U3 is connected to the fourth resistor R4, and the other end of the fourth resistor R4 is connected to a 1.8V power supply, pin 2, pin 3, pin 4 and pin 5 of the third chip U3 are respectively connected to pin 22, pin 9, pin 24 and pin 23 of the second chip U2, and pin 10, pin 11, pin 12 and pin 13 of the third chip U3 are respectively connected to pin 26, pin 28, pin 27 and pin 25 of the first control chip U1.

[0031] Furthermore, the voltage stabilizing module includes a fourth chip U4 and a fifth chip U5, wherein pin 1 of the fourth chip U4 is connected to two parallel capacitors, a fourteenth capacitor C14 and a fifteenth capacitor C15, and the other ends of the fourteenth capacitor C14 and the fifteenth capacitor C15 are connected to pin 6 of the fourth chip U4, and pin 3 of the fourth chip U4 is connected to a feedback circuit composed of a sixth resistor R6, a seventh resistor R7 and a nineteenth capacitor C19, and the other end of the feedback circuit is connected to the output power supply, and pin 4 of the fourth chip U4 is connected to the fifth resistor R5, and the other end of the fifth resistor R5 is connected to pin 5 of the fourth chip U4, and pin 5 of the fourth chip U4 is connected to Connect to the first Schottky diode D1, the other end of the first Schottky diode D1 is connected to the power supply VCC, pin 5 of the fourth chip U4 is connected to the sixteenth capacitor C16, pin 6 of the fourth chip U4 is connected to the first inductor L1, the other end of the first inductor L1 is connected to the output power supply, and at the same time, two capacitors connected in parallel to the ground, the seventeenth capacitor C17 and the eighteenth capacitor C18, are connected, pin 6 of the fourth chip U4 is connected to the second Schottky diode D2, the other end of the second Schottky diode D2 is connected to the ground, pin 1 of the fifth chip U5 is connected to the twentieth capacitor C20, and pin 5 of the fifth chip U5 is connected to the twenty-first capacitor C21.

[0032] Furthermore, the input-output module includes a first interface H1, a second interface H2 and a third interface H3, pin 2 and pin 3 of the first interface H1 are respectively connected to pin 37 and pin 34 of the first control chip U1, pin 4 of the first interface H1 is connected to the twenty-second capacitor C22, pin 1 and pin 2 of the second interface H2 are respectively connected to pin 13 and pin 12 of the first control chip U1, and pin 2 of the third interface H3 is connected to the twenty-third capacitor C23 and the twenty-fourth capacitor C24.

[0033] The embodiments of the present invention have the following technical effects:

[0034] The present invention is based on dual-loop control. Through a high-frequency inner loop based on the data of the six-axis attitude sensor and a low-frequency inner loop based on the data of the nine-axis attitude sensor, the update frequency of the nine-axis sensor is no longer limited by the update frequency of the magnetometer, achieving high-frequency update of the attitude angle, thereby achieving a more stable and smooth control effect. At the same time, the high-frequency output system of the attitude sensor based on dual-loop control provided by the present invention is connected to the second chip U2 through the first control chip U1 after level conversion by the third chip U3, and obtains data from the second chip U2 through the hardware SPI. Because SPI is a push-pull output, the communication frequency of SPI can easily reach 7MHZ, thereby achieving high-speed data reading. After the first control chip U1 obtains the data through SPI, it estimates the attitude angle through the inner loop to achieve high-frequency output of the inner loop. When the returned magnetometer value is updated, the outer loop is used to accurately calculate the attitude angle and correct the inner loop at the same time, achieving low-frequency and accurate output of the outer loop, thereby achieving high-frequency output of the attitude angle, ensuring that other modules have real-time and high-frequency attitude angle updates, effectively avoiding problems such as slow error data update, data jump, error accumulation and poor system stability. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] In order to more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the specific embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0036] Figure 1 Flowchart of a high-frequency output method of a posture sensor based on dual-loop control provided by an embodiment of the present invention;

[0037] Figure 2 A logic diagram of a high-frequency output method for a posture sensor based on dual-loop control provided by an embodiment of the present invention;

[0038] Figure 3 A circuit schematic diagram of a control module provided in an embodiment of the present invention;

[0039] Figure 4 A circuit schematic diagram of a posture sensor module provided by an embodiment of the present invention;

[0040] Figure 5 A circuit schematic diagram of a level conversion module provided by an embodiment of the present invention;

[0041] Figure 6 A circuit schematic diagram of a voltage stabilizing module provided in an embodiment of the present invention;

[0042] Figure 7A circuit schematic diagram of an input / output module provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0043] To make the objectives, technical solutions, and advantages of the present invention more clear, the technical solutions of the present invention are described clearly and completely below. Obviously, the embodiments described are only some of the embodiments of the present invention, not all of them. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the present invention without inventive effort are also within the scope of protection of the present invention.

[0044] The embodiment of the present invention provides a high-frequency output method of a posture sensor based on dual-loop control. Figure 1 is a flow chart of a high-frequency output method of a posture sensor based on dual-loop control provided by an embodiment of the present invention. Figure 2 For a logic diagram of a high-frequency output method for a posture sensor based on dual-loop control provided by an embodiment of the present invention, see Figure 1 and Figure 2 , the method comprises the following steps:

[0045] S1. Based on the six-axis data of the three-axis accelerometer and the three-axis gyroscope, the attitude angle inner loop output is calculated at high frequency through the first data fusion algorithm.

[0046] In some embodiments, the first data fusion algorithm includes:

[0047] S11 . Calculate the roll angle roll_acc around the x-axis and the pitch angle pitch_acc around the y-axis using the three-axis acceleration data.

[0048] The calculation formula is as follows:

[0049] (1)

[0050] (2)

[0051] Among them, a x 、a y 、a z They represent the acceleration components of the nine-axis sensor on the x-axis, y-axis, and z-axis respectively.

[0052] S12. Calculate the roll angle roll_gyro around the x-axis, the pitch angle pitch_gyro around the y-axis, and the heading angle yaw_gyro around the z-axis by integrating the three-axis gyroscope data.

[0053] The calculation formula is as follows:

[0054] (3)

[0055] (4)

[0056] (5)

[0057] Among them, roll_gyro(k), pitch_gyro(k), and yaw_gyro(k) respectively represent the k-th calculation of the carrier's attitude angle around the x-axis, the carrier's attitude angle around the y-axis, and the carrier's attitude angle around the z-axis using the three-axis gyroscope, k represents the k-th calculation, roll_gyro(k-1), pitch_gyro(k-1), and yaw_gyro(k-1) represent the last calculation of the carrier's attitude angle around the x-axis, the carrier's attitude angle around the y-axis, and the carrier's attitude angle around the z-axis using the three-axis gyroscope, ω x 、ω y and ω z They represent the angular velocity of the carrier along the x-axis, the angular velocity of the carrier along the y-axis, and the angular velocity of the carrier along the z-axis measured by the three-axis gyroscope, Indicates the time interval between two measurements.

[0058] S13. Dynamically adjust the weight factor α based on the acceleration modulus value, fuse roll_acc and roll_gyro to obtain the inner loop roll angle, fuse pitch_acc and pitch_gyro to obtain the inner loop pitch angle, and directly output yaw_gyro as the inner loop heading angle.

[0059] The calculation formula is as follows:

[0060] roll=α×roll_acc+(1-α)×roll_gyro(k)(6)

[0061] pitch=α×pitch_acc+(1-α)×pitch_gyro(k)(7)

[0062] yaw = yaw_gyro(k) (8)

[0063] Among them, roll represents the attitude angle of the carrier around the x-axis after the fusion of the three-axis acceleration and the three-axis gyroscope, that is, the roll angle; pitch represents the attitude angle of the carrier around the y-axis after the fusion of the three-axis acceleration and the three-axis gyroscope, that is, the pitch angle; yaw represents the heading angle, and α is a constant between 0 and 1.

[0064] Furthermore, according to the modulus of the three-axis acceleration Whether the carrier is moving is determined by whether it is greater than a first preset value or less than a second preset value. If it is moving, the weight factor α is reduced by a preset value; otherwise, the weight factor α remains unchanged. The size of the preset value can be set according to actual conditions; for example, the first preset value can be 1.1g, where g is the acceleration of gravity, and the second preset value can be 0.9g.

[0065] S2. Based on the nine-axis data of the nine-axis sensor, the attitude angle outer loop output is calculated at a low frequency through the second data fusion algorithm, and the attitude angle outer loop output is used to correct the accumulated error of the attitude angle inner loop output.

[0066] In some embodiments, the second data fusion algorithm includes:

[0067] S21. Calculate the outer ring roll angle and the outer ring pitch angle using the three-axis acceleration and the three-axis gyroscope according to the method in S11-S13.

[0068] S22. Calculate the heading angle yaw_mag around the z-axis using the three-axis magnetometer data.

[0069] The calculation formula is as follows:

[0070] (9)

[0071] Among them, m x 、m y and m z They represent the x-axis magnetic field, y-axis magnetic field, and z-axis magnetic field measured by the magnetometer, respectively.

[0072] S23. Fuse yaw_mag with yaw_gyro in the inner loop to obtain the precise heading angle.

[0073] Select an appropriate α to correct the heading angle in the inner loop. The calculation formula is as follows:

[0074] yaw=α×yaw_mag+(1-α)×yaw_gyro(k)(10)

[0075] Among them, yaw represents the heading angle of the vehicle after the heading angle measured by the three-axis magnetometer and the heading angle calculated by the three-axis gyroscope are fused, that is, the attitude angle of the vehicle around the z-axis.

[0076] S24: Synchronize the outer ring roll angle, outer ring pitch angle, and precise heading angle to the inner ring, and reset the initial integral value of the inner ring.

[0077] At this point, the accurate three-axis attitude angles have been calculated and provided to the inner loop to calibrate the inner loop data. That is, the roll_gyro(k) value in the inner loop is equal to the roll in the outer loop, the pitch_gyro(k) value in the inner loop is equal to the pitch in the outer loop, and the yaw_gyro(k) value in the inner loop is equal to the yaw in the outer loop.

[0078] S3. Dynamically reset the corrected attitude angle inner loop output to the inner loop calculation to achieve high-frequency update of the attitude angle.

[0079] Furthermore, before S1, a data initialization phase is required. During this initialization phase, an outer loop data update is completed. The initial value of roll is calculated by taking α = 1 using Equations (1) and (6), and roll_gyro(0) = roll; the initial value of pitch is calculated by taking α = 1 using Equations (2) and (7), and pitch_gyro(0) = pitch; the initial value of yaw is calculated by taking α = 1 using Equations (9) and (10), and yaw_gyro(0) = yaw. With the initial values, high-frequency updates of the attitude angle can be achieved through steps S1-S3.

[0080] The embodiment of the present invention further provides a high-frequency output system for a posture sensor based on dual-loop control, which is used to execute the above-mentioned high-frequency output method for a posture sensor based on dual-loop control, see Figure 3-Figure 7 , the system includes the following modules:

[0081] The control module is used to read the raw data of the nine-axis sensor and execute the dual-loop control algorithm;

[0082] The attitude sensor module is used to collect raw data from the nine-axis sensor;

[0083] Level conversion module, used to bridge devices with different voltages to achieve cross-voltage communication;

[0084] Voltage stabilization module, used to provide stable voltage for the system;

[0085] Input and output modules are used to provide external communication, power supply and debugging interfaces.

[0086] Furthermore, the control module includes a first control chip U1, pin 1 of the first control chip U1 is connected to the first capacitor C1, pin 9 of the first control chip U1 is connected to the second capacitor C2 and the third capacitor C3, pin 24 of the first control chip U1 is connected to the fourth capacitor C4, pin 36 of the first control chip U1 is connected to the fifth capacitor C5, pin 48 of the first control chip U1 is connected to the sixth capacitor C6, pin 5 and pin 6 of the first control chip U1 are connected to the first non-crystalline oscillator X1 and two co-oscillator capacitors, the seventh capacitor C7 and the eighth capacitor C8, pin 7 of the first control chip U1 is connected to the reset circuit composed of the first resistor R1, the ninth capacitor C9 and the first switch SW1, the second resistor R2 is connected between the digital power supply and the analog power supply, and the third resistor R3 is connected between the digital ground and the analog ground.

[0087] The crystal oscillator circuit composed of the first random crystal oscillator X1 and two co-oscillation capacitors C7 and C8 provides an external clock signal for the first control chip U1; the reset circuit composed of the first resistor R1, the ninth capacitor C9 and the first switch SW1 provides a reset signal for the first control chip U1; the second resistor R2 and the third resistor R3 are used to prevent digital noise from interfering with the analog circuit; the first capacitor C1, the second capacitor C2, the third capacitor C3, the fourth capacitor C4, the fifth capacitor C5 and the sixth capacitor C6 are used to filter the first control chip U1.

[0088] Furthermore, the posture sensor module includes a second chip U2, pin 8 of the second chip U2 is connected to the tenth capacitor C10 and the eleventh capacitor C11, pin 10 of the second chip U2 is connected to the twelfth capacitor C12, the other end of the twelfth capacitor C12 is grounded, and pin 13 of the second chip U2 is connected to the thirteenth capacitor C13.

[0089] The second chip U2 is a nine-axis attitude sensor chip that can obtain three-axis acceleration, three-axis gyroscope and three-axis magnetometer information; the tenth capacitor C10, the eleventh capacitor C11 and the thirteenth capacitor C13 are used for filtering the second chip U2, and the twelfth capacitor C12 is used for filtering the voltage regulator inside the second chip U2.

[0090] Furthermore, the level conversion module includes a third chip U3, pin 8 of the third chip U3 is connected to the fourth resistor R4, and the other end of the fourth resistor R4 is connected to a 1.8V power supply, pin 2, pin 3, pin 4 and pin 5 of the third chip U3 are respectively connected to pin 22, pin 9, pin 24 and pin 23 of the second chip U2, and pin 10, pin 11, pin 12 and pin 13 of the third chip U3 are respectively connected to pin 26, pin 28, pin 27 and pin 25 of the first control chip U1.

[0091] The third chip, U3, is a level converter chip. One end is connected to an SPI pin on the first control chip, U1, and the other end is connected to an SPI pin on the second chip, U2. This allows communication between the two chips, U1 and U2, which operate at different power levels. Because hardware SPI is used, the pins are configured as push-pull outputs, allowing the SPI communication frequency to easily reach the 7 MHz limit of the second chip, U2, in this embodiment. This allows the first control chip, U1, to perform other calculations even within the inner loop with ample time. A fourth resistor, R4, provides a weak pull-up signal for the third chip, U3, enabling it.

[0092] Furthermore, the voltage stabilizing module includes a fourth chip U4 and a fifth chip U5, wherein pin 1 of the fourth chip U4 is connected to two parallel capacitors, a fourteenth capacitor C14 and a fifteenth capacitor C15, and the other ends of the fourteenth capacitor C14 and the fifteenth capacitor C15 are connected to pin 6 of the fourth chip U4, and pin 3 of the fourth chip U4 is connected to a feedback circuit composed of a sixth resistor R6, a seventh resistor R7 and a nineteenth capacitor C19, and the other end of the feedback circuit is connected to the output power supply, and pin 4 of the fourth chip U4 is connected to the fifth resistor R5, and the other end of the fifth resistor R5 is connected to pin 5 of the fourth chip U4, and pin 5 of the fourth chip U4 is connected to Connect to the first Schottky diode D1, the other end of the first Schottky diode D1 is connected to the power supply VCC, pin 5 of the fourth chip U4 is connected to the sixteenth capacitor C16, pin 6 of the fourth chip U4 is connected to the first inductor L1, the other end of the first inductor L1 is connected to the output power supply, and at the same time, two capacitors connected in parallel to the ground, the seventeenth capacitor C17 and the eighteenth capacitor C18, are connected, pin 6 of the fourth chip U4 is connected to the second Schottky diode D2, the other end of the second Schottky diode D2 is connected to the ground, pin 1 of the fifth chip U5 is connected to the twentieth capacitor C20, and pin 5 of the fifth chip U5 is connected to the twenty-first capacitor C21.

[0093] The core of the voltage stabilization module is the fourth chip U4 and the fifth chip U5. The fourth chip U4 is a switching power supply chip, and the fifth chip U5 is a linear low-voltage dropout regulator chip. The fourteenth capacitor C14 and the fifteenth capacitor C15 are two bootstrap capacitors used to drive the high-side MOSFET in the fourth chip U4. The sixteenth capacitor C16 is used for input power filtering. The first Schottky diode D1 prevents reverse connection of the input power supply, thus protecting the circuit. The fifth resistor R5 provides a pull-up signal for the fourth chip U4, enabling the fourth chip U4. The first inductor L1 is used for energy storage. The second Schottky diode D2 is used for freewheeling to ensure the formation of a loop. The seventeenth capacitor C17 and the eighteenth capacitor C18 are used for filtering the fourth chip U4. The sixth resistor R6, the seventh resistor R7, and the nineteenth capacitor C19 form a feedback circuit, providing a feedback signal to the fourth chip U4 to determine the output voltage. The fifth chip U5 converts the 3.3V voltage into 1.8V, which is provided to the second chip U2 and the third chip U3. The twentieth capacitor C20 and the twenty-first capacitor C21 provide filtering for the fifth chip U5 .

[0094] Furthermore, the input-output module includes a first interface H1, a second interface H2 and a third interface H3, pin 2 and pin 3 of the first interface H1 are respectively connected to pin 37 and pin 34 of the first control chip U1, pin 4 of the first interface H1 is connected to the twenty-second capacitor C22, pin 1 and pin 2 of the second interface H2 are respectively connected to pin 13 and pin 12 of the first control chip U1, and pin 2 of the third interface H3 is connected to the twenty-third capacitor C23 and the twenty-fourth capacitor C24.

[0095] The input / output module includes a first interface H1, which provides a code download interface for the first control chip U1. A 22nd capacitor C22 is used to filter the 3.3V power supply. A second interface H2 provides serial communication for the first control chip U1. A third interface H3 provides the main power supply for the entire control circuit. A 23rd capacitor C23 and a 24th capacitor C24 are used for main power supply filtering.

[0096] The present invention is based on dual-loop control. Through a high-frequency inner loop based on the six-axis attitude sensor data and a low-frequency inner loop based on the nine-axis attitude sensor data, the update frequency of the nine-axis sensor is no longer limited by the update frequency of the magnetometer, and high-frequency update of the attitude angle is achieved, thereby achieving a more stable and smooth control effect. At the same time, the high-frequency output system of the attitude sensor based on dual-loop control provided by the present invention is connected to the second chip U2 through the first control chip U1 after level conversion by the third chip U3, and the data of the second chip U2 is obtained through the hardware SPI. Because SPI is a push-pull output, the communication frequency of SPI can easily reach 7MHZ, thereby achieving high-speed data reading. After the first control chip U1 obtains the data through SPI, it estimates the attitude angle through the inner loop to achieve high-frequency output of the inner loop. When the returned magnetometer value is updated, the outer loop is used to accurately calculate the attitude angle and correct the inner loop at the same time, achieving low-frequency and accurate output of the outer loop, thereby achieving high-frequency output of the attitude angle, ensuring that other modules have real-time and high-frequency attitude angle updates, and effectively avoiding problems such as slow error data update, data jump, error accumulation and poor system stability.

[0097] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the technical solutions of the embodiments of the present invention.

Claims

1. A high-frequency output method for an attitude sensor based on dual-loop control, characterized in that: The method comprises the following steps: S1, based on the six-axis data of the three-axis accelerometer and the three-axis gyroscope, the attitude angle inner loop output is calculated at high frequency through the first data fusion algorithm; The first data fusion algorithm includes: S11. Calculate the roll angle roll_acc around the x-axis and the pitch angle pitch_acc around the y-axis using the three-axis acceleration data; S12. Calculate the roll angle roll_gyro around the x-axis, the pitch angle pitch_gyro around the y-axis, and the heading angle yaw_gyro around the z-axis by integrating the three-axis gyroscope data; S13. Dynamically adjust the weight factor α based on the acceleration modulus, fuse the roll_acc and roll_gyro to obtain the inner loop roll angle, fuse the pitch_acc and pitch_gyro to obtain the inner loop pitch angle, and directly output yaw_gyro as the inner loop heading angle; The dynamic adjustment of the weight factor α based on the acceleration modulus value includes: When the acceleration modulus is greater than the first preset value or less than the second preset value, the weight factor α is reduced by the preset value; otherwise, the weight factor α remains unchanged; Wherein, the first preset value is greater than the second preset value; S2. Based on the nine-axis data from the nine-axis sensor, calculate the attitude angle outer loop output at a low frequency using a second data fusion algorithm, and use the attitude angle outer loop output to correct the accumulated error of the attitude angle inner loop output; S3. Dynamically reset the corrected attitude angle inner loop output to the inner loop calculation to achieve high-frequency update of the attitude angle.

2. The high-frequency output method of the attitude sensor based on dual-loop control according to claim 1, characterized in that: In S2, the second data fusion algorithm includes: S21. Calculate the outer ring roll angle and the outer ring pitch angle using the three-axis acceleration and the three-axis gyroscope according to the method in S11-S13; S22, calculate the heading angle yaw_mag around the z-axis using the three-axis magnetometer data; S23, fusing the yaw_mag and the yaw_gyro in the inner loop to obtain a precise heading angle; S24: Synchronize the outer ring roll angle, outer ring pitch angle, and precise heading angle to the inner ring, and reset the initial integral value of the inner ring.

3. A high-frequency output system for a posture sensor based on dual-loop control, configured to execute the high-frequency output method for a posture sensor based on dual-loop control according to any one of claims 1 to 2, characterized in that: The system includes the following modules: The control module is used to read the raw data of the nine-axis sensor and execute the dual-loop control algorithm; The attitude sensor module is used to collect raw data from the nine-axis sensor; Level conversion module, used to bridge devices with different voltages to achieve cross-voltage communication; A voltage stabilizing module, used for providing a stable voltage for the system; Input and output modules are used to provide external communication, power supply and debugging interfaces.

4. The high-frequency output system of the attitude sensor based on dual-loop control according to claim 3 is characterized in that: The control module includes a first control chip U1, pin 1 of the first control chip U1 is connected to the first capacitor C1, pin 9 of the first control chip U1 is connected to the second capacitor C2 and the third capacitor C3, pin 24 of the first control chip U1 is connected to the fourth capacitor C4, pin 36 of the first control chip U1 is connected to the fifth capacitor C5, pin 48 of the first control chip U1 is connected to the sixth capacitor C6, pins 5 and 6 of the first control chip U1 are connected to the first non-crystalline oscillator X1 and two co-oscillator capacitors, the seventh capacitor C7 and the eighth capacitor C8, pin 7 of the first control chip U1 is connected to the reset circuit composed of the first resistor R1, the ninth capacitor C9 and the first switch SW1, the second resistor R2 is connected between the digital power supply and the analog power supply, and the third resistor R3 is connected between the digital ground and the analog ground.

5. The high-frequency output system of the attitude sensor based on dual-loop control according to claim 3 is characterized in that: The posture sensor module includes a second chip U2, pin 8 of the second chip U2 is connected to the tenth capacitor C10 and the eleventh capacitor C11, pin 10 of the second chip U2 is connected to the twelfth capacitor C12, the other end of the twelfth capacitor C12 is grounded, and pin 13 of the second chip U2 is connected to the thirteenth capacitor C13.

6. The high-frequency output system of the attitude sensor based on dual-loop control according to claim 3 is characterized in that: The level conversion module includes a third chip U3, pin 8 of the third chip U3 is connected to the fourth resistor R4, and the other end of the fourth resistor R4 is connected to a 1.8V power supply, pin 2, pin 3, pin 4 and pin 5 of the third chip U3 are respectively connected to pin 22, pin 9, pin 24 and pin 23 of the second chip U2, and pin 10, pin 11, pin 12 and pin 13 of the third chip U3 are respectively connected to pin 26, pin 28, pin 27 and pin 25 of the first control chip U1.

7. The high-frequency output system of the attitude sensor based on dual-loop control according to claim 3 is characterized in that: The voltage stabilizing module includes a fourth chip U4 and a fifth chip U5, wherein the pin 1 of the fourth chip U4 is connected to two parallel capacitors, a fourteenth capacitor C14 and a fifteenth capacitor C15, and the other ends of the fourteenth capacitor C14 and the fifteenth capacitor C15 are connected to the pin 6 of the fourth chip U4, and the pin 3 of the fourth chip U4 is connected to a feedback circuit composed of a sixth resistor R6, a seventh resistor R7 and a nineteenth capacitor C19, and the other end of the feedback circuit is connected to the output power supply, and the pin 4 of the fourth chip U4 is connected to the fifth resistor R5, and the other end of the fifth resistor R5 is connected to the pin 5 of the fourth chip U4, and the pin 5 of the fourth chip U4 is connected to the first Schottky diode D1, the other end of the first Schottky diode D1 is connected to the power supply VCC, pin 5 of the fourth chip U4 is connected to the sixteenth capacitor C16, pin 6 of the fourth chip U4 is connected to the first inductor L1, the other end of the first inductor L1 is connected to the output power supply, and at the same time, two capacitors connected in parallel to the ground, the seventeenth capacitor C17 and the eighteenth capacitor C18, pin 6 of the fourth chip U4 is connected to the second Schottky diode D2, the other end of the second Schottky diode D2 is connected to the ground, pin 1 of the fifth chip U5 is connected to the twentieth capacitor C20, and pin 5 of the fifth chip U5 is connected to the twenty-first capacitor C21.

8. The attitude sensor high-frequency output system based on dual-loop control according to claim 3 is characterized in that: The input-output module includes a first interface H1, a second interface H2 and a third interface H3. Pin 2 and pin 3 of the first interface H1 are respectively connected to pin 37 and pin 34 of the first control chip U1, pin 4 of the first interface H1 is connected to the twenty-second capacitor C22, pin 1 and pin 2 of the second interface H2 are respectively connected to pin 13 and pin 12 of the first control chip U1, and pin 2 of the third interface H3 is connected to the twenty-third capacitor C23 and the twenty-fourth capacitor C24.

Images