A micro dynamic inclination measuring module and a solution method thereof

Abstract

The application discloses a kind of miniature dynamic inclination measurement module and its solving method, the application includes mechanical structure, circuit and solving algorithm.The module is stacked by the mechanical structure of core board and bottom plate, and the overall size is centimeter level.The overall structure is kept in the operating temperature of master control by patch technology and reasonable planning of front and back device installation position, reduces the error caused by temperature drift of inertial sensor.Embedded microcontroller master control chip realizes the data output of multiple communication modes, including serial output and bus output.Kalman filtering algorithm is used for error compensation and noise filtering, to realize the fusion of estimated inclination and measured inclination to obtain stable optimal estimated inclination.Dynamic output data includes six-axis data of inertial sensor and calculated two-axis inclination, with the advantages of small size, high precision and strong robustness.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of inertial measurement, and particularly relates to a micro dynamic inclination measurement module and a calculation method thereof. BACKGROUND

[0002] Inclination measurement has important applications in the fields of building, mechanical processing, robot technology, agricultural production, aerospace, etc. In practical engineering, the most common is mechanical inclination measurement instrument. With the progress of measurement technology, there are many measurement methods for inclination measurement, such as optical type and physical sensor. Fusion measurement technology is mainly aimed at dynamic inclination measurement. The micro gyroscope is used to measure the angular rate of the carrier in a short time, and the angle is obtained by integrating the angular rate. However, the micro gyroscope has random drift and temperature drift, and the error will accumulate with time, which will increase the measurement error. Therefore, the output information of the micro accelerometer is used to correct the output of the gyroscope. SUMMARY

[0003] To solve the above problems, the present application discloses a dynamic inclination measurement module based on a micro electro-mechanical gyroscope accelerometer and a calculation method thereof. The overall size of the module is centimeter level, which is convenient for application in different special scenes. The present application also provides an inclination calculation algorithm based on Kalman filter, which can output two-axis inclination and realize accurate dynamic inclination measurement.

[0004] To achieve the above purpose, the technical scheme of the present application is as follows:

[0005] A micro dynamic inclination measurement module, which is composed of an embedded single-chip microcomputer main control chip, an inertial sensor, a power conversion circuit and a power indicator, a reset button, an external clock crystal oscillator and a special plug-in interface. The design adopts a four-layer board processing technology. The largest embedded single-chip microcomputer main control chip is placed on the front surface of the module, and the inertial sensor with similar volume is placed on the back surface opposite to the main control chip, so that the overall module volume is reduced to the minimum.

[0006] The specific technical scheme is as follows:

[0007] Firstly, the mechanical structure layout of the inclination measurement module is designed. The system framework is composed of a micro accelerometer, a signal acquisition, processing and calculation circuit board and an external expansion interface, which are integrated in a four-layer circuit board. Considering the miniaturization of the designed system, the length, width and height of the mechanical structure of the measurement system are centimeter level. Due to the need of fixed installation, the bottom surface of the designed system is used as the installation reference surface during measurement, and the flatness of the bottom surface is ensured during mechanical processing. The micro accelerometer and the main control chip are respectively installed on the two surfaces in the system mechanical structure, and the rest of the system circuit is placed in the structure, and power supply and communication are realized through four-layer board wiring.

[0008] The design adopts four-layer board layer design, the top layer is set as a signal layer, the top layer power supply adopts copper cladding method. The middle layer one is a ground layer, adopts copper cladding method, and the digital ground and analog ground in the signal layer are separated. The middle layer two is a power supply layer, adopts copper cladding method, and uses the signal layer. The bottom layer is a signal layer, and the bottom layer power supply adopts copper cladding method. The via inner diameter is one half of the outer diameter, and a via is used. The chip is placed on the top layer, and the resistor and capacitor are placed on the bottom layer.

[0009] In the power conversion circuit, the inertial sensor and the mainboard are mainly connected for power supply and SPI communication. The inertial sensor module is powered by an external 5V power supply, and the module has a voltage stabilizing chip for power supply of the entire module. The power rail outputs 3V3 voltage output by the voltage stabilizing chip for power supply of the main control chip and the inertial sensor. Considering the miniaturization and convenience of system integration, a low-dropout linear voltage stabilizing chip is selected to stabilize the external power supply. The microcontroller of the data processing module needs +3.3V digital power supply, which is converted and output at the conversion output end of the voltage conversion chip, and a 2.2uF capacitor is connected in parallel for passive filtering processing of the output, so that the output voltage is more stable. In the entire circuit, there are digital ground and analog ground. The main control circuit, data transmission, and analog-to-digital conversion circuit require digital ground, while the power supply circuit requires analog ground. For the entire system circuit, there can only be one ground wire, so the analog ground and digital ground in the circuit need to be connected to make the overall circuit have a common reference potential. If the analog ground and digital ground are connected in a large area in the PCB, it may cause signal interference in the circuit, so in the circuit PCB design, the analog ground and digital ground are connected through a 0 ohm resistor. At this time, the 0 ohm resistor in the circuit is equivalent to a very narrow current path and has impedance itself, which can limit the loop current and suppress noise.

[0010] The module uses a board-to-board connector and is installed in a corresponding special connector. The interface is designed on the symmetrical two sides of the inertial sensor to reduce installation errors.

[0011] The inertial sensor and the main control chip are installed on the front and opposite back of the module through the SMT process, and the heat dissipation of the main control chip is transmitted to the inertial sensor, so that the overall temperature of the inertial sensor is maintained near the operating temperature of the main control, reducing the error caused by temperature drift. The peripheral resistors of the main control chip are arranged around the inertial sensor to ensure the consistency of the overall module temperature.

[0012] The application further provides a dynamic inclination angle calculation method based on a Kalman filter.

[0013] The inclination angle calculation comprises gyroscopic quaternion inclination angle calculation, accelerometer attitude measurement, and inclination angle estimation based on Kalman filtering. The quaternion inclination angle calculation obtains an initial inclination angle under a static condition by using a three-axis accelerometer, calculates a current attitude matrix i is an angular velocity in an inertial system, ie is an earth rotation angular velocity, en is a navigation system relative earth rotation angular velocity, and L is an inclination angle. is a rotation matrix, and the angular rate ω of the attitude matrix is calculated as follows:

[0014]

[0015] The influence of the earth angular rate is not considered, and thus the above formula can be approximately considered as: w = w i The fourth-order Runge-Kutta method is used to update the quaternion. The quaternion at the current time is calculated and normalized. The real-time quaternion value converted from the attitude matrix corresponds to the value of T, so that the rotation matrix is obtained, and three inclination angles are further obtained.

[0016] The accelerometer attitude measurement: when the attitude is measured by using the accelerometer, the accelerometer is used as an inclination meter. The three-axis accelerometer can measure the gravity in a static state. When the accelerometer is inclined, the gravity acceleration on the three axes will change, and the inclination angle is calculated. In a static state, for a three-axis accelerometer, the values of the three-axis accelerations at any time are:

[0017]

[0018] It is assumed that the influence of linear acceleration is not considered:

[0019]

[0020] The three-axis accelerometer is used to measure the inclination angle, the roll angle β, and the pitch angle ψ:

[0021] β = arcsin (G py )

[0022]

[0023] The Kalman filter-based inclination angle estimation algorithm is used to establish a state process:

[0024] The process of calculating the inclination angle according to the output angular velocity value of the gyroscope is the prediction of the current state, and the state equation of the filter system is established. According to the quaternion differential equation, the inclination angle is back calculated, and the inclination angle is taken as the current predicted value:

[0025]

[0026] The attitude quaternion at the current time is obtained through the fourth-order Runge-Kutta method, and then the pitch angle and roll angle at the current time are obtained. G represents the gyroscope, T represents the true value, k represents the time, and the output predicted angle is the synthesis of the true angle and the error angle: The true angle is: And the error angle is:

[0027] The measurement equation is established: the inclination angle at the current time is calculated according to the three-axis gravity field acceleration measured by the accelerometer as the observation. Among them, the three-axis accelerometer is used as a static inclination angle instrument, and the pitch angle and roll angle in the attitude angle are obtained by measuring the gravity field.

[0028] The inclination angle calculated by the accelerometer is represented by A:

[0029] The filter parameters are designed: the process noise covariance Q, i.e. the process error covariance matrix in the process of predicting the attitude angle by the gyroscope, is obtained by experimental method, and the measurement noise covariance matrix R can be calculated by fusion update. Through mutually independent zero-mean white noise sequences, the difference between the angle estimates at adjacent two times is calculated:

[0030]

[0031]

[0032] The fusion error at two times is obtained by subtracting the two equations, and the fusion error covariance is calculated:

[0033] The beneficial effects of the present application are:

[0034] (1) The dynamic inclination angle module based on MEMS inertial sensors is designed to be light in weight, small in size and low in power consumption, compared with the traditional electronic inclination angle module, the overall size is smaller, and is suitable for more complex measurement scenes. And through reasonable structure design, the overall sensor temperature consistency is higher, and the error caused by temperature drift is reduced.

[0035] ​(2) The original data collected in the application is compensated for error and filtered, the sensor self noise is reduced, and the quaternion is used to solve the attitude, so as to eliminate the singularity of the attitude solved by the gyroscope using Euler angles. The inclination angle calculated according to the gyroscope is used as the prediction value of the Kalman filter, and the process noise covariance Q is estimated; the inclination angle calculated by the accelerometer is used as the measured value, and the measurement noise covariance matrix R is estimated in combination with the gyroscope error. The fusion of multi-sensor information is realized through Kalman filtering, the cumulative error of the gyroscope can be effectively corrected, the accuracy of the angle measurement of the system under dynamic condition can be effectively improved, but the system parameters are complicated, and the real-time operation amount is large. Through algorithm simplification, two-dimensional inclination measurement can be realized, and accurate inclination measurement under dynamic condition can be realized. BRIEF DESCRIPTION OF DRAWINGS

[0036] Figure 1 is the module design framework of the application.

[0037] Figure 2 is a structural diagram of the dynamic inclination measurement module of the application.

[0038] Figure 3 is an internal circuit diagram of the dynamic inclination measurement module of the application.

[0039] Figure 4 is a 3D simulation diagram of the dynamic inclination measurement module of the application.

[0040] Figure 5 is a dynamic inclination solving flowchart based on the Kalman filter of the application. DETAILED DESCRIPTION

[0041] The application will be further illustrated below in combination with the drawings and specific embodiments, and it should be understood that the following specific embodiments are only used to illustrate the application and not to limit the scope of the application.

[0042] A dynamic inclination measurement module based on a micro-electromechanical gyroscope accelerometer is composed of an embedded single-chip microcomputer master control chip, an inertial sensor, a power transformation circuit and a power indicator, a reset button, an external clock crystal oscillator and a special plug-in interface. The design adopts a four-layer board processing technology, and the largest embedded single-chip microcomputer master control chip is placed on the front surface of the module, and the inertial sensor with similar volume is placed on the back surface opposite to the master control chip.

[0043] In combination with Figure 1 , the software framework and hardware framework of the structure of the application are shown in the drawings.

[0044] In combination with Figure 2The mechanical structure layout of the inclination measurement module is shown in the figure, the system framework is composed of micro-accelerometer, signal acquisition, processing, solving circuit board and external expansion interface, and the system is integrated in a four-layer circuit board. Considering the miniaturization of the designed system, the length, width and height of the mechanical structure of the measurement system are in centimeter level. Due to the need of fixed installation, the bottom surface of the system is designed as the installation reference surface during measurement, and the flatness of the bottom surface needs to be ensured during mechanical processing. The micro-accelerometer and the main control chip are respectively installed on the two surfaces in the system mechanical structure, and the rest of the circuit of the system is placed in the structure and is powered and communicated through the four-layer board wiring.

[0045] In combination Figure 3 The inertial sensor and the main control chip are installed on the front surface and the opposite back surface of the module through the patch process, the easily-heated resistors around the main control chip are arranged around the inertial sensor to ensure the consistency of the temperature of the module. The module uses a board-to-board connector and is installed in a corresponding special connector. The interface is designed on the two symmetrical sides of the inertial sensor to reduce the installation error.

[0046] In combination Figure 4 The four-layer board layer design is adopted in the design, the top layer is designed as a signal layer, the top layer power supply adopts a copper cladding method. The first middle layer is a ground layer, the copper cladding method is adopted, and the digital ground and the analog ground in the signal layer are separated and wired. The second middle layer is a power supply layer, the copper cladding method is adopted, and the signal layer is used. The bottom layer is a signal layer, and the power supply of the bottom layer adopts a copper cladding method. The inner diameter of the via is one half of the outer diameter, and the via is used to place the chip on the top layer and the resistor and capacitor on the bottom layer.

[0047] In combination Figure 5 The dynamic inclination calculation method based on the Kalman filter is adopted in the present application, and the inclination calculation is divided into gyro four-element number inclination calculation, accelerometer attitude measurement and Kalman filter-based inclination estimation algorithm.

[0048] Four-element number inclination calculation: the three-axis accelerometer is used to obtain the initial inclination under the static condition, and the current attitude matrix is calculated. The fourth-order Runge-Kutta method is used to update the four-element number. The four-element number at the current time is calculated and normalized. The value of the real-time four-element number converted from the attitude matrix corresponds to the value of T, so that the rotation matrix is obtained, and then three attitude angles can be obtained.

[0049] When the accelerometer is used to measure the inclination, the accelerometer is used as an inclinometer. The accelerometer can measure the gravity under the static state, when the accelerometer is inclined, the gravity acceleration on the three axes will change, and then the inclination is calculated. Under the static state, for a three-axis accelerometer, the value of the three-axis acceleration at any time is:

[0050]

[0051] Assuming the effects of linear acceleration are neglected:

[0052]

[0053] Using a triaxial accelerometer to measure tilt angle:

[0054] The tilt angle estimation algorithm based on Kalman filtering establishes the state process:

[0055] The process of calculating the tilt angle based on the gyroscope's output angular velocity value is a prediction of the current state, establishing the state equation of the filtering system. The tilt angle is then inversely derived using the quaternion differential equation and taken as the current predicted value. The attitude quaternion at the current moment is obtained using the fourth-order Runge-Kutta method, from which the pitch and roll angles at that moment can be calculated. G represents the gyroscope, T represents the true value, k represents the time, and the output predicted angle. It's from the real perspective. and error angle Synthesis:

[0056]

[0057] Establish the measurement equation: The tilt angle at the current moment is calculated based on the triaxial gravitational field acceleration measured by the accelerometer, serving as the observation. The triaxial accelerometer is used as a static tilt meter; by measuring the gravitational field, the pitch and roll angles in the attitude angles are determined. The tilt angle calculated by the accelerometer, where A represents the accelerometer reading:

[0058] The filter parameters are designed as follows: the process noise covariance Q, which is the process error covariance matrix in the gyroscope's attitude angle prediction process, obtained experimentally; and the measurement noise covariance matrix R, which can be calculated through fusion and update. The difference Δ between angle estimates at adjacent time points is calculated using independent zero-mean white noise sequences.

[0059]

[0060]

[0061] Subtracting the two equations, we obtain the fusion error at the two time points. We then calculate the covariance of the fusion error.

[0062] The specific process in programming software algorithms is as follows:

[0063] (1) Parameter initialization: Let the state variable be k. In the initial state, i.e., when k = 1, the sensor is in a stationary state.

[0064] Two initial tilt angles, roll angle β and pitch angle ψ, were calculated using a triaxial accelerometer. pyGy is the accelerometer y-axis measurement value, G px Gx is the accelerometer x-axis measurement value, G pz Gz is the accelerometer z-axis measurement value:

[0065] β = arcsin(Gz / G) py

[0066] (2) When k = 2, the current inclination is predicted by using the three-axis angular velocity data output by the gyroscope. According to the previous gyroscope data experiment, the prior error covariance P(k|k-1) is obtained. F is the state transition matrix:

[0067] P(k|k-1) = F·P(k-1|k-1)·F' + Q

[0068] (3) Calculate the Kalman gain K. H is the observation matrix:

[0069] K(k) = P(k|k-1)·H / H·P(k|k-1)·H + R

[0070] (4) Update the predicted value according to the inclination value measured by the accelerometer at k = 2, and output the fused inclination value X(k|k):

[0071] X(k|k) = X(k|k-1) + K(k)·(Z(k) - H·X(k|k-1))

[0072] (5) Calculate the predicted value at k + 1: by collecting the angular velocity data output by the gyroscope at k + 1, solve the quaternion differential equation, and obtain the inclination at that time as the predicted value, then repeat the above steps.

[0073] In each cycle, the relative accurate inclination estimation value at that time is obtained. First, collect the original data of the gyroscope and accelerometer for Kalman filtering processing, error compensation and filtering. If it is the first time, the angle calculated by the gyroscope and accelerometer is taken as the initial angle, and the attitude quaternion is calculated according to the initial inclination. Then, according to the angular velocity data output by the gyroscope, the inclination is obtained by solving the quaternion differential equation as the estimated angle. According to the angle calculated by the accelerometer as the measurement value, the process noise covariance and the measurement noise covariance are calculated. The fusion of the estimated angle and the measured angle is realized by using the Kalman filtering algorithm to obtain the optimal estimation angle. The optimal estimation angle obtained is taken as the starting angle for the next calculation, and a series of stable optimal estimation angles are obtained by repeatedly performing the data fusion algorithm.

[0074] ​It should be noted that the above content only illustrates the technical idea of the present application, and cannot limit the protection scope of the present application. For ordinary skilled in the art, without departing from the principle of the present application, a number of improvements and refinements can be made, which fall within the protection scope of the claims of the present application.

Claims

1. A miniature dynamic tilt angle measurement module, characterized in that, It is composed of an embedded microcontroller main control chip, an inertial sensor, a power transformer circuit, a power indicator light, a reset button, an external clock crystal oscillator, and special connectors. The embedded microcontroller main control chip is located on the front of the module, the inertial sensor is mounted on the back of the module opposite the embedded microcontroller main control chip using a surface mount technology, and the power transformer circuit, power indicator light, reset button, and external clock crystal oscillator are located around the embedded microcontroller main control chip. The design adopts a four-layer board manufacturing process. The four-layer board design includes a top layer configured as a signal layer and a top power layer using copper plating. The first intermediate layer is the ground layer, which uses copper pouring and separates the digital ground and analog ground in the signal layer. The second intermediate layer is the power layer, which uses copper pouring and is a signal layer. The bottom layer is the signal layer, and the bottom layer uses copper pouring for power. The inner diameter of the via is half the outer diameter and through-holes are used. The embedded microcontroller main control chip is placed on the top layer, and the resistors and capacitors are placed on the bottom layer. Connect the analog ground and digital ground through a 0-ohm resistor; The heat dissipation of the embedded microcontroller main control chip is transferred to the inertial sensor, keeping the overall temperature of the inertial sensor close to the operating temperature of the embedded microcontroller main control chip. The heat-generating resistors around the embedded microcontroller main control chip are arranged around the inertial sensor to ensure the temperature consistency of the entire module. The method for dynamic tilt angle calculation based on Kalman filter using the aforementioned miniature dynamic tilt angle measurement module includes: obtaining the optimal linear minimum variance estimate using Kalman filtering, correcting the cumulative error of the gyroscope by fusion measurement, and improving the accuracy of angle measurement of the module under dynamic conditions; the tilt angle calculation is divided into gyroscope quaternion tilt angle calculation, accelerometer attitude measurement, and tilt angle estimation algorithm based on Kalman filter; The process of calculating the tilt angle from the angular velocity value output by the gyroscope in the inertial sensor is as follows: Quaternions are used to solve for the attitude from the acquired raw data, eliminating the singularity of using Euler angles to solve the attitude using the gyroscope. The gyroscope quaternion tilt angle calculation includes: obtaining the initial tilt angle under static conditions using the triaxial accelerometer in the inertial sensor, and calculating the current attitude matrix. And convert it into a quaternion q, ω i Let ω be the angular velocity in the inertial frame. ie Let ω be the angular velocity of Earth's rotation. en Let L be the angular velocity of the navigation system relative to the Earth's rotation, and L be the tilt angle. Given the rotation matrix, the angular rate ω of the attitude matrix is ​​calculated as follows: Attitude measurement using a triaxial accelerometer in an inertial sensor: When the accelerometer tilts, the gravitational acceleration on its three axes changes, and the tilt angle can be calculated accordingly; The tilt angle estimation algorithm based on Kalman filtering includes the following steps: (1-1) The process of calculating the tilt angle based on the angular velocity value output by the gyroscope in the inertial sensor: Assume the predicted state of the current state and establish the state equation of the filtering system; deduce the tilt angle using the quaternion differential equation and take this tilt angle as the current predicted value; calculate the attitude quaternion at the current moment using the fourth-order Runge-Kutta method, and then calculate the pitch and roll angles at the current moment; G represents the gyroscope, T represents the true value, k represents the time, and the output predicted angle. It's from the real perspective. and error angle Synthesis: (1-2) Establish the measurement equation: Calculate the tilt angle at the current moment based on the triaxial gravitational field acceleration measured by the accelerometer as the observation; among which, the triaxial accelerometer in the inertial sensor is used as a static tilt meter, and the pitch angle and roll angle in the attitude angle are obtained by measuring the gravitational field; (1-3) The tilt angle calculated by the accelerometer, where A represents the accelerometer: (1-4) Design of filtering parameters: The process noise covariance Q, i.e., the process error covariance matrix in the gyroscope's attitude angle prediction process, is obtained experimentally; the measurement noise covariance matrix R is calculated through fusion and update; the difference Δ between the angle estimates at adjacent time points is calculated using mutually independent zero-mean white noise sequences. (1-5) Subtract the two equations in step (6-4) to obtain the fusion error at the two time points. Calculate the covariance of the fusion error:

2. The miniature dynamic tilt measurement module according to claim 1, characterized in that: The module uses board-to-board connectors and is installed in corresponding special connectors; the special connectors are designed on both sides of the inertial sensor, with a height slightly higher than the sensor height.

3. The miniature dynamic tilt measurement module according to claim 1, characterized in that: The dynamic tilt angle calculation method includes the following steps: (3-1) Parameter initialization: Let the state variable be k. In the initial state, i.e., when k = 1, the sensor is in a stationary state. Two initial tilt angles, roll angle β and pitch angle ψ, were calculated using a triaxial accelerometer. py For the y-axis measurement of the accelerometer, G px For the accelerometer x-axis measurement, G pz For accelerometer z-axis measurements: β=arcsin(G py ), (3-2) When k=2, the current tilt angle is predicted using the three-axis angular velocity data output by the gyroscope. Based on previous gyroscope data experiments, the prior error covariance P(k|k-1) is obtained, and F is the state transition matrix: P(k|k-1)=F·P(k-1|k-1)·F'+Q (3-3) Calculate the Kalman gain K, where H is the observation matrix: K(k)=P(k|k-1)·H' / H·P(k|k-1)·H'+R (3-4) Update the predicted value based on the tilt angle measured by the accelerometer at time k=2, and output the fused tilt angle value. X(k|k):X(k|k)=X(k|k-1)+K(k)·(Z(k)-H·X(k|k-1)) (3-5) Calculate the predicted value at time k+1: By collecting the angular velocity data output by the gyroscope at time k+1, solve the quaternion differential equation to obtain the tilt angle at that time as the predicted value, and then repeat the above steps iteratively.

Images