Attitude sensor high-frequency output method and system based on double-loop control

Abstract

The invention relates to the technical field of inertial navigation and attitude estimation, and discloses an attitude sensor high-frequency output method and system based on double-loop control, and the method comprises the following steps: calculating attitude angle inner loop output at high frequency through a data fusion algorithm based on a three-axis acceleration and a three-axis gyroscope; based on nine-axis data of the nine-axis sensor, accurate attitude angle outer ring output is calculated through a data fusion algorithm, and accumulative errors of an inner ring are corrected; the corrected attitude angle inner ring output is dynamically reset to inner ring calculation, and high-frequency updating of the attitude angle is achieved; the system comprises a control module, an attitude sensor module, a level conversion module, a voltage stabilization module and an input and output module, a double-loop output control method is designed for the influence of a magnetometer on the output frequency of the nine-axis attitude sensor, and the use precision of a gyroscope and the output frequency of the nine-axis attitude sensor can be effectively improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of inertial navigation and attitude estimation, and particularly relates to a high-frequency output method and system for 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 fields such as unmanned aerial vehicles, humanoid robots, virtual reality, and wearable devices is increasing day by day. At present, there are many mainstream attitude estimation schemes, such as optical motion capture systems, visual inertial odometers, lidar, inertial measurement units, etc. However, although the optical motion capture system has extremely high precision, it cannot be used in outdoor venues. Both visual inertial odometers and lidar have large amounts of data. However, inertial navigation units have low computational requirements, low cost, and good accuracy, and are widely used in various fields that require attitude estimation. The mainstream attitude estimation scheme uses a six-axis inertial measurement unit to measure three-axis acceleration and three-axis gyroscopes, and is paired with a three-axis magnetometer to form a nine-axis sensor. The attitude angle is finally calculated through a data fusion algorithm.

[0003] The data of the three-axis acceleration and the three-axis gyroscope are obtained by an inertial measurement sensor converting acceleration and angular velocity into voltage signals, and then six-axis data is obtained after passing through an analog-to-digital converter. The update frequency is usually above 1KHZ. Due to the weak magnetic characteristics of the earth and the need to filter out environmental magnetic noise, the sampling time required for the magnetometer is usually 2 - 10ms, resulting in the overall output frequency of the nine-axis sensor generally not being higher than 500HZ. However, the control frequencies in fields such as unmanned aerial vehicles, robots, virtual reality, and wearable devices are usually above 1KHZ.

[0004] Therefore, there is an urgent need to develop a high-frequency output method and system for an attitude 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. 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 an attitude 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 an attitude sensor based on dual-loop control. The method includes the following steps: S1. Based on the six-axis data of the three-axis accelerometer and the three-axis gyroscope, high-frequency calculate the output of the attitude angle inner loop through the first data fusion algorithm; S2. Based on the nine-axis data of the nine-axis sensor, low-frequency calculate the output of the attitude angle outer loop through the second data fusion algorithm, and use the output of the attitude angle outer loop to correct the cumulative error of the output of the attitude angle inner loop; S3. Dynamically reset the output of the inner loop of the corrected attitude angle to the inner loop calculation to achieve high-frequency update of the attitude angle.

[0007] Furthermore, in S1, 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 through 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 yaw angle yaw_gyro around the z-axis by integrating the three-axis gyroscope data; S13. Dynamically adjust the weight factor α based on the acceleration magnitude, 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 yaw angle.

[0008] Furthermore, in S2, the second data fusion algorithm includes: S21. Calculate the outer loop roll angle and the outer loop pitch angle through the three-axis acceleration and the three-axis gyroscope according to the methods in S11 - S13; S22. Calculate the yaw angle yaw_mag around the z-axis through the three-axis magnetometer data; S23. Fuse yaw_mag and yaw_gyro in the inner loop to obtain the accurate yaw angle; S24. Synchronize the outer loop roll angle, the outer loop pitch angle, and the accurate yaw angle to the inner loop and reset the integral initial value of the inner loop.

[0009] Furthermore, in S13, dynamically adjusting the weight factor α based on the acceleration magnitude includes: When the acceleration magnitude is greater than the first preset value or less than the second preset value, reduce the weight factor α by a preset value; otherwise, the weight factor α remains unchanged; Wherein, the first preset value is greater than the second preset value.

[0010] The present invention also provides a high-frequency output system of an attitude sensor based on double-loop control for executing the above-mentioned high-frequency output method of an attitude sensor based on double-loop control. The system includes the following modules: A control module for reading the original data of the nine-axis sensor and executing the double-loop control algorithm; An attitude sensor module for collecting the original data of the nine-axis sensor; A level conversion module for bridging devices with different voltages to achieve cross-voltage communication; A voltage stabilization module for providing a stable voltage for the system; An input / output module for providing external communication, power supply, and debugging interfaces.

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

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

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

[0014] Further, the voltage stabilizing module includes a fourth chip U4 and a fifth chip U5. The pin 1 of the fourth chip U4 is connected to two parallel capacitors, i.e., the fourteenth capacitor C14 and the fifteenth capacitor C15. The other ends of the fourteenth capacitor C14 and the fifteenth capacitor C15 are connected to the pin 6 of the fourth chip U4. 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. The pin 4 of the fourth chip U4 is connected to a fifth resistor R5, and the other end of the fifth resistor R5 is connected to the pin 5 of the fourth chip U4. The pin 5 of the fourth chip U4 is connected to a first Schottky diode D1, and the other end of the first Schottky diode D1 is connected to the power supply VCC. The pin 5 of the fourth chip U4 is connected to a sixteenth capacitor C16. The pin 6 of the fourth chip U4 is connected to a first inductor L1, and the other end of the first inductor L1 is connected to the output power supply and is also connected to two capacitors in parallel to the ground, i.e., the seventeenth capacitor C17 and the eighteenth capacitor C18. The pin 6 of the fourth chip U4 is connected to a second Schottky diode D2, and the other end of the second Schottky diode D2 is connected to the ground. The pin 1 of the fifth chip U5 is connected to a twentieth capacitor C20, and the pin 5 of the fifth chip U5 is connected to a twenty-first capacitor C21.

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

[0016] The embodiments of the present invention have the following technical effects: The present invention is based on dual-loop control. By means of a high-frequency inner loop based on six-axis attitude sensor data and a low-frequency inner loop based on nine-axis attitude sensor data, 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, and thus realizing a more stable and smooth control effect. At the same time, for the attitude sensor high-frequency output system based on dual-loop control provided by the present invention, the first control chip U1 is connected to the second chip U2 after level conversion by the third chip U3, and the data of the second chip U2 is obtained through hardware SPI. Since SPI is a push-pull output, the communication frequency of SPI can easily reach 7MHZ, thus achieving high-speed data reading. After the first control chip U1 obtains the data through SPI, the attitude angle is estimated through the inner loop to achieve high-frequency output of the inner loop. When the updated magnetometer value is transmitted back, the attitude angle is accurately calculated through the outer loop while correcting the inner loop, achieving low-frequency accurate output of the outer loop, thereby realizing high-frequency output of the attitude angle, ensuring that other modules have real-time high-frequency update of the attitude angle, and effectively avoiding problems such as slow update of error data, data jitter, error accumulation, and poor system stability. Description of the Drawings

[0017] In order to more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the following will briefly introduce the drawings required for use in the description of the specific embodiments or the prior art. Obviously, the following drawings are some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0018] Figure 1 It is a flowchart of the method for high-frequency output of an attitude sensor based on dual-loop control provided by an embodiment of the present invention; Figure 2 It is a logic diagram of the method for high-frequency output of an attitude sensor based on dual-loop control provided by an embodiment of the present invention; Figure 3 It is a circuit schematic diagram of a control module provided by an embodiment of the present invention; Figure 4 It is a circuit schematic diagram of an attitude sensor module provided by an embodiment of the present invention; Figure 5 It is a circuit schematic diagram of a level conversion module provided by an embodiment of the present invention; Figure 6 It is a circuit schematic diagram of a voltage stabilization module provided by an embodiment of the present invention; Figure 7 It is a circuit schematic diagram of an input / output module provided by an embodiment of the present invention. Detailed Embodiments

[0019] To make the objectives, technical solutions, and advantages of the present invention clearer, the technical solutions of the present invention will be described clearly and completely below. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts fall within the scope protected by the present invention.

[0020] The embodiment of the present invention provides a high-frequency output method for an attitude sensor based on dual-loop control. Figure 1 It is a flowchart of the high-frequency output method for an attitude sensor based on dual-loop control provided by the embodiment of the present invention. Figure 2 It is a logic diagram of the high-frequency output method for an attitude sensor based on dual-loop control provided by the embodiment of the present invention. Refer to Figure 1 and Figure 2 , the method includes the following steps: S1. Based on the six-axis data of a three-axis accelerometer and a three-axis gyroscope, calculate the high-frequency output of the attitude angle inner loop through a first data fusion algorithm.

[0021] In some embodiments, 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 through the three-axis acceleration data.

[0022] The calculation formulas are as follows: (1) (2) where a x , a y , a z respectively represent the acceleration components of the nine-axis sensor on the x-axis, y-axis, and z-axis.

[0023] S12. Calculate the roll angle roll_gyro around the x-axis, the pitch angle pitch_gyro around the y-axis, and the yaw angle yaw_gyro around the z-axis through the integration of the three-axis gyroscope data.

[0024] The calculation formulas are as follows: (3) (4) (5) Among them, roll_gyro(k), pitch_gyro(k), and yaw_gyro(k) respectively represent the attitude angle of the carrier around the x-axis, the attitude angle of the carrier around the y-axis, and the attitude angle of the carrier around the z-axis calculated by the three-axis gyroscope for the k-th time. k represents the k-th time. roll_gyro(k - 1), pitch_gyro(k - 1), and yaw_gyro(k - 1) represent the attitude angle of the carrier around the x-axis, the attitude angle of the carrier around the y-axis, and the attitude angle of the carrier around the z-axis calculated by the three-axis gyroscope for the previous time. ω x 、ω y and ω z respectively 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. represents the time interval between two measurements.

[0025] 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.

[0026] The calculation formula is as follows: roll = α × roll_acc + (1 - α) × roll_gyro(k) (6) pitch = α × pitch_acc + (1 - α) × pitch_gyro(k) (7) yaw = yaw_gyro(k) (8) Among them, roll represents the attitude angle of the carrier around the x-axis after fusing 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 fusing the three-axis acceleration and the three-axis gyroscope, that is, the pitch angle. yaw represents the heading angle. α is a constant between 0 and 1.

[0027] Furthermore, according to the modulus value of the three-axis acceleration judge whether the carrier is moving by whether it is greater than the first preset value or less than the second preset value. If it is moving, reduce the weight factor α by a preset value; otherwise, the weight factor α remains unchanged. The size of the preset value can be set according to the actual situation. Exemplarily, the first preset value can be 1.1g, where g is the acceleration due to gravity, and the second preset value can be 0.9g.

[0028] S2. Based on the nine-axis data of the nine-axis sensor, low-frequency calculate the attitude angle outer-loop output through the second data fusion algorithm, and use the attitude angle outer-loop output to correct the cumulative error of the attitude angle inner-loop output.

[0029] In some embodiments, the second data fusion algorithm includes: S21. Calculate the outer-loop roll angle and outer-loop pitch angle according to the methods in S11 - S13 using the three-axis accelerometer and three-axis gyroscope.

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

[0031] The calculation formula is as follows: (9) where m x 、m y and m z respectively represent the magnetic field in the x-axis, y-axis, and z-axis measured by the magnetometer.

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

[0033] Select an appropriate α to correct the angle of the heading angle in the inner loop. The calculation formula is as follows: yaw = α × yaw_mag + (1 - α) × yaw_gyro(k) (10) where yaw represents the heading angle of the carrier after fusing the heading angle measured by the three-axis magnetometer and the heading angle calculated by the three-axis gyroscope, that is, the attitude angle of the carrier around the z-axis.

[0034] S24. Synchronize the outer-loop roll angle, outer-loop pitch angle, and accurate heading angle to the inner loop and reset the integral initial value of the inner loop.

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

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

[0037] Further, before S1, it is also necessary to first perform a data initialization phase. During the initialization phase, an outer loop data update needs to be completed. By taking α = 1 in equations (1) and (6), the initial value of roll is calculated, and roll_gyro(0) = roll is set; by taking α = 1 in equations (2) and (7), the initial value of pitch is calculated, and pitch_gyro(0) = pitch is set; by taking α = 1 in equations (9) and (10), the initial value of yaw is calculated, and yaw_gyro(0) = yaw is set. With the initial values, the high-frequency update of the attitude angle can be achieved through steps S1 - S3.

[0038] The embodiment of the present invention also provides a high-frequency output system of an attitude sensor based on double-loop control, which is used to execute the above-mentioned high-frequency output method of an attitude sensor based on double-loop control. Refer to Figures 3 - 7 , the system includes the following modules: A control module, which is used to read the original data of the nine-axis sensor and execute the double-loop control algorithm; An attitude sensor module, which is used to collect the original data of the nine-axis sensor; A level conversion module, which is used to bridge devices with different voltages to achieve cross-voltage communication; A voltage stabilization module, which is used to provide a stable voltage for the system; An input / output module, which is used to provide external communication, power supply, and debugging interfaces.

[0039] Further, the control module includes a first control chip U1. The pin 1 of the first control chip U1 is connected to the first capacitor C1, the pin 9 of the first control chip U1 is connected to the second capacitor C2 and the third capacitor C3, the pin 24 of the first control chip U1 is connected to the fourth capacitor C4, the pin 36 of the first control chip U1 is connected to the fifth capacitor C5, the pin 48 of the first control chip U1 is connected to the sixth capacitor C6, the pins 5 and 6 of the first control chip U1 are connected to the first quartz crystal oscillator X1 and two resonant capacitors, the seventh capacitor C7 and the eighth capacitor C8. The pin 7 of the first control chip U1 is connected to a 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.

[0040] The crystal oscillator circuit composed of the first quartz crystal oscillator X1 and two resonant 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 avoid the interference of digital noise on 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 for the first control chip U1.

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

[0042] The second chip U2 is a nine-axis attitude sensor chip, which can obtain triaxial acceleration, triaxial gyroscope, and triaxial magnetometer information. The tenth capacitor C10, the eleventh capacitor C11, and the thirteenth capacitor C13 filter the second chip U2, and the twelfth capacitor C12 filters the voltage regulator inside the second chip U2.

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

[0044] The third chip U3 is a level converter chip, with one end connected to the SPI pins of the first control chip U1 and the other end connected to the SPI pins of the second chip U2, enabling the two chips U1 and U2 powered by different levels to communicate with each other. Since hardware SPI is used, the pins are configured as push-pull outputs, enabling the communication frequency of SPI to easily reach the limit frequency of 7MHZ of the second chip U2 in this embodiment, allowing the first control chip U1 to have sufficient time for other calculations even in the inner loop. The fourth resistor R4 provides a weak pull-up signal for the third chip U3 to enable the third chip U3.

[0045] Further, the voltage regulation module includes a fourth chip U4 and a fifth chip U5. The pin 1 of the fourth chip U4 is connected to two parallel capacitors, the fourteenth capacitor C14 and the fifteenth capacitor C15. The other ends of the fourteenth capacitor C14 and the fifteenth capacitor C15 are connected to the pin 6 of the fourth chip U4. 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. The other end of the feedback circuit is connected to the output power supply. The pin 4 of the fourth chip U4 is connected to a fifth resistor R5. The other end of the fifth resistor R5 is connected to the pin 5 of the fourth chip U4. The pin 5 of the fourth chip U4 is connected to a first Schottky diode D1. The other end of the first Schottky diode D1 is connected to the power supply VCC. The pin 5 of the fourth chip U4 is connected to a sixteenth capacitor C16. The pin 6 of the fourth chip U4 is connected to a first inductor L1. The other end of the first inductor L1 is connected to the output power supply and is also connected to two parallel capacitors to the ground, the seventeenth capacitor C17 and the eighteenth capacitor C18. The pin 6 of the fourth chip U4 is connected to a second Schottky diode D2. The other end of the second Schottky diode D2 is connected to the ground. The pin 1 of the fifth chip U5 is connected to a twentieth capacitor C20. The pin 5 of the fifth chip U5 is connected to a twenty-first capacitor C21.

[0046] The core of the voltage regulation 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 dropout regulator chip. The fourteenth capacitor C14 and the fifteenth capacitor C15 are two bootstrap capacitors used to drive the high-side MOSFET inside the fourth chip U4. The sixteenth capacitor C16 is for input power supply filtering. The first Schottky diode D1 prevents the input power supply from being reverse-connected and plays a role in protecting the circuit. The fifth resistor R5 provides a pull-up signal for the fourth chip U4 to enable the fourth chip U4. The first inductor L1 is used to store energy. The second Schottky diode D2 is used for freewheeling to ensure a closed loop. The seventeenth capacitor C17 and the eighteenth capacitor C18 filter the fourth chip U4. The sixth resistor R6, the seventh resistor R7, and the nineteenth capacitor C19 form a feedback to provide a feedback signal for the fourth chip U4 to determine the output voltage. The fifth chip U5 converts the 3.3V voltage into a 1.8V voltage and supplies it to the second chip U2 and the third chip U3. The twentieth capacitor C20 and the twenty-first capacitor C21 filter the fifth chip U5.

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

[0048] The input / output module includes a first interface H1 that provides a code download interface for the first control chip U1. The twenty-second capacitor C22 is used to filter the 3.3V power supply. The second interface H2 provides serial communication for the first control chip U1. The third interface H3 provides the main power supply for the entire control circuit. The twenty-third capacitor C23 and the twenty-fourth capacitor C24 filter the main power supply.

[0049] Based on dual-loop control, the present invention enables the update frequency of the nine-axis sensor to be no longer limited by the update frequency of the magnetometer through a high-frequency inner loop based on six-axis attitude sensor data and a low-frequency inner loop based on nine-axis attitude sensor data, achieving high-frequency updates of the attitude angle, and thus realizing a more stable and smooth control effect. At the same time, the attitude sensor high-frequency output system based on dual-loop control provided by the present invention is connected to the second chip U2 after level conversion by the first control chip U1 through the third chip U3, and the data of the second chip U2 is obtained through hardware SPI. Since SPI is a push-pull output, the communication frequency of SPI can easily reach 7MHZ, thus achieving high-speed data reading. After obtaining the data through SPI, the first control chip U1 estimates the attitude angle through the inner loop to achieve high-frequency output of the inner loop. When the updated magnetometer value is transmitted back, the outer loop accurately calculates the attitude angle and corrects the inner loop at the same time, achieving low-frequency accurate output of the outer loop, thereby realizing high-frequency output of the attitude angle, ensuring that other modules have real-time high-frequency updates of the attitude angle, and effectively avoiding problems such as slow update of error data, data jitter, error accumulation, and poor system stability. Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some or all of the technical features. However, such modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the technical solutions of the embodiments of the present invention.

Claims

1. A high-frequency output method of an attitude sensor based on dual-loop control, characterized in that The method includes the following steps: S1. Based on the six-axis data of a triaxial accelerometer and a triaxial gyroscope, high-frequency calculate the output of the attitude angle inner loop through a first data fusion algorithm; S2. Based on the nine-axis data of a nine-axis sensor, low-frequency calculate the output of the attitude angle outer loop through a second data fusion algorithm, and correct the cumulative error of the output of the attitude angle inner loop by using the output of the attitude angle outer loop; S3. Dynamically reset the corrected output of the attitude angle inner loop 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, wherein, In the above S1, 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 through triaxial acceleration data; S12. Integrate the triaxial gyroscope data to calculate the roll angle roll_gyro around the x-axis, the pitch angle pitch_gyro around the y-axis, and the yaw angle yaw_gyro around the z-axis; S13. Dynamically adjust the weight factor α based on the acceleration modulus value, fuse the roll_acc and the roll_gyro to obtain the inner loop roll angle, fuse the pitch_acc and the pitch_gyro to obtain the inner loop pitch angle, and directly output the yaw_gyro as the inner loop yaw angle.

3. The high-frequency output method of the attitude sensor based on dual-loop control according to claim 2, wherein In the above S2, the second data fusion algorithm includes: S21. Calculate the outer loop roll angle and the outer loop pitch angle through the triaxial acceleration and the triaxial gyroscope according to the methods in S11 - S13; S22. Calculate the yaw angle yaw_mag around the z-axis through the triaxial magnetometer data; S23. Fuse the yaw_mag and the yaw_gyro in the inner loop to obtain the accurate yaw angle; S24. Synchronize the outer loop roll angle, the outer loop pitch angle, and the accurate yaw angle to the inner loop, and reset the integral initial value of the inner loop.

4. The high-frequency output method of the attitude sensor based on dual-loop control according to claim 2, wherein In the above S13, dynamically adjusting the weight factor α based on the acceleration modulus value includes: When the acceleration modulus value is greater than a first preset value or less than a second preset value, reduce the weight factor α by a preset value, otherwise the weight factor α remains unchanged; Wherein, the first preset value is greater than the second preset value.

5. A high-frequency output system of an attitude sensor based on double-loop control, which is used to execute the high-frequency output method of the attitude sensor based on double-loop control according to any one of the above claims 1-4, characterized in that, The system includes the following modules: A control module, which is used to read the original data of the nine-axis sensor and execute the double-loop control algorithm; An attitude sensor module, which is used to collect the original data of the nine-axis sensor; A level conversion module, which is used to bridge devices with different voltages to achieve cross-voltage communication; A voltage stabilization module, which is used to provide a stable voltage for the system; An input / output module, which is used to provide external communication, power supply, and debugging interfaces.

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

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

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

9. The high-frequency output system of the attitude sensor based on dual-loop control according to claim 5, characterized in that, The voltage regulation module includes a fourth chip U4 and a fifth chip U5. Pin 1 of the fourth chip U4 is connected to two parallel capacitors, a fourteenth capacitor C14 and a fifteenth capacitor C15. The other ends of the fourteenth capacitor C14 and the fifteenth capacitor C15 are connected to pin 6 of the fourth chip U4. 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. The other end of the feedback circuit is connected to the output power supply. Pin 4 of the fourth chip U4 is connected to a fifth resistor R5. The other end of the fifth resistor R5 is connected to pin 5 of the fourth chip U4. Pin 5 of the fourth chip U4 is connected to a 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 a sixteenth capacitor C16. Pin 6 of the fourth chip U4 is connected to a first inductor L1. The other end of the first inductor L1 is connected to the output power supply and is also connected to two parallel capacitors to the ground, a seventeenth capacitor C17 and an eighteenth capacitor C18. Pin 10. The high-frequency output system of the attitude sensor based on dual-loop control according to claim 5, 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 a first control chip U1. Pin 4 of the first interface H1 is connected to a 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. Pin 2 of the third interface H3 is connected to a twenty-third capacitor C23 and a twenty-fourth capacitor C24.

Images