CN113218391A - Attitude calculation method based on EWT algorithm - Google Patents

Attitude calculation method based on EWT algorithm Download PDF

Info

Publication number
CN113218391A
CN113218391A CN202110310526.XA CN202110310526A CN113218391A CN 113218391 A CN113218391 A CN 113218391A CN 202110310526 A CN202110310526 A CN 202110310526A CN 113218391 A CN113218391 A CN 113218391A
Authority
CN
China
Prior art keywords
ewt
empirical
algorithm
wavelet
attitude
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202110310526.XA
Other languages
Chinese (zh)
Inventor
刘春�
陈豪
葸生宝
何敏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hefei University of Technology
Original Assignee
Hefei University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hefei University of Technology filed Critical Hefei University of Technology
Priority to CN202110310526.XA priority Critical patent/CN113218391A/en
Publication of CN113218391A publication Critical patent/CN113218391A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Gyroscopes (AREA)

Abstract

In an inertial navigation system, the problem of solving the attitude angle relates to navigation accuracy, the invention provides an attitude solving method based on an EWT algorithm, aiming at the problem of low attitude solving accuracy caused by low-frequency noise and drift error of a gyroscope, the EWT algorithm is adopted to process data of the gyroscope, a quaternion is used for solving a gravity vector, vector cross product is carried out on accelerometer data and the gravity vector to obtain an error, then, complementary filtering controlled by PID is used for correcting the gyroscope data, and finally, a Longge Kutta method is used for solving the quaternion to solve a more accurate attitude angle.

Description

Attitude calculation method based on EWT algorithm
Technical Field
The invention belongs to the technical field of navigation, particularly relates to the field of inertial navigation, and particularly relates to an attitude calculation method based on an EWT algorithm.
Background
Since the 20 th century, the rapid development of inertial navigation technology has gradually become a popular research direction. The inertial navigation technology is a technology for acquiring instantaneous speed and position data of an object by measuring the angular velocity/linear acceleration of an aircraft and utilizing a correlation calculation method. The inertial navigation technique relates to automatic control, precision machinery, optics and other subjects and fields[1]The method has the advantages of autonomy, concealment, anti-interference and the like, and is widely applied to the fields of aviation, aerospace, navigation, guided missile, aircraft navigation and the like[2]. Attitude calculation is an important component of an inertial navigation system, and generally, angular velocity and acceleration are measured by using an inertial measurement element (accelerometer/gyroscope), and an attitude angle is obtained by calculation through integral operation[3]. When the attitude angle is calculated, errors are accumulated inevitably due to the existence of integral, so that the calculated value of the attitude angle is drifted, and the precision of a navigation system is influenced[4]. Therefore, before the attitude angle is calculated, noise reduction processing needs to be performed on data of the gyroscope, and meanwhile, drift errors of the gyroscope also need to be corrected.
Many scholars have made relevant studies on how to improve the accuracy of attitude solution. Such as horsepower[5]The attitude calculation algorithm based on complementary filtering is provided, the calculation amount of the algorithm is small, the algorithm is suitable for being used for the small unmanned aerial vehicle, but the filtering coefficient is fixed, and the influence of sensor noise is not fully considered, so the algorithm precision is low. Yaohansuka[6]And the RBF neural network based attitude calculation algorithm is provided, the RBF neural network is fused with Kalman to establish a filtering model, the attitude calculation precision is improved to a certain extent, but the neural network has training uncertainty, and further the filtering effect has uncertainty. Zhangdong (a Chinese character of 'zhang' an)[7]And the like, an improved attitude solution method based on Kalman filtering and complementary filtering is proposed, and although the solution precision can be improved, the method needs the assistance of a magnetometer. Chen Guangwu[8]The method has good denoising effect on random noise of positions and azimuth angles, the result of wavelet transformation depends on selection of wavelet functions, and the selection of the wavelet functions has no adaptivity.
Disclosure of Invention
Aiming at various defects of the prior art indicated by the background art, the invention provides an attitude calculation method based on an EWT algorithm, which comprises the following steps:
an attitude calculation method based on an EWT algorithm is carried out according to the following steps:
step 1: performing frequency spectrum self-adaptive segmentation on angular velocity data measured by a gyroscope by using an EWT algorithm, and constructing a proper wavelet filter on a segmentation interval so as to extract Empirical Mode Functions (EMFs);
step 2: performing noise reduction processing and signal reconstruction by using a wavelet denoising method;
and step 3: complementary filtering is carried out on the data measured by the accelerometer and the reconstructed signal to reduce drift errors, and PID controlled complementary filtering is adopted to realize the complementary filtering;
and 4, step 4: and calculating a more accurate attitude angle by using a quaternion calculating method, and outputting a result.
Further, in step 1, the measured gyroscope data is adaptively spectrally decomposed using empirical wavelet decomposition (EWT) to obtain empirical mode components (EMFs)
In step 2, aiming at the noise problem, a wavelet soft threshold denoising method is adopted, different soft thresholds are selected according to different components for denoising, and then signals are reconstructed to obtain denoised gyroscope data;
in step 3, a gravity vector is obtained from the initial quaternion, the gravity vector and the data of the accelerometer are subjected to vector cross product to obtain an error, and PID control is performed to correct the data of the reconstructed gyroscope;
in step 4, a quaternion differential equation is constructed from the corrected gyroscope data, the quaternion differential equation is solved by using a four-order Runge Kutta method, and a more accurate attitude angle is finally solved and output.
Further, in step 1: normalizing the frequency spectrum range of the signal to be measured to [0, pi ] according to Shannon sampling theorem]And dividing the region into N intervals, each interval being ΛnAs in formula (8):
Figure BDA0002989496880000021
there are N +1 dividing lines in the signal spectrum; let omeganIs a boundary line, Λn＝[ωn-1，ωn]Also, from the formula (8), ω isnIn the range of [0, π]To (c) to (d); around each of the lines of demarcation, there is defined a transitionPhase interval with width Tn＝2τn。
Further, in step 1, the EWT is a signal analysis method, and a wavelet window function is required to be constructed for the divided intervals, where the wavelet window function includes: empirical scale function
Figure BDA0002989496880000022
And empirical wavelet function
Figure BDA0002989496880000023
The expressions are respectively shown in formulas (9) and (10):
Figure BDA0002989496880000024
Figure BDA0002989496880000031
β(x)＝x4(35-84x+70x2-20x3) (11)
Figure BDA0002989496880000032
further, in step 1: the Empirical Wavelet Transform (EWT) can be formed by the signal to be measured and the empirical wavelet function psinAnd empirical scale function
Figure BDA0002989496880000033
To obtain the detail coefficient thereof
Figure BDA0002989496880000034
And approximation coefficient
Figure BDA0002989496880000035
The expressions are shown in the following formulas (13) and (14):
Figure BDA0002989496880000036
Figure BDA0002989496880000037
from this original signal, the expression is reconstructed as follows:
Figure BDA0002989496880000038
equation 15 is an expression of the reconstructed signal.
In the above formula, denotes convolution; in formulas 14 and 15
Figure BDA0002989496880000039
In order to approximate the coefficients of the coefficients,
Figure BDA00029894968800000310
for detail coefficients, #nIn order to be an empirical wavelet function,
Figure BDA00029894968800000311
is an empirical scale function.
Figure BDA00029894968800000312
Represents the fourier transform and (V) represents the inverse fourier transform. Empirical mode function f is obtained from the above formulak(t) the expression is as shown in formula (17):
Figure BDA00029894968800000313
the formula 16 is an empirical mode function obtained by decomposing the original signal f (t), and the total number is N.
Figure BDA00029894968800000314
In formula 16
Figure BDA00029894968800000315
In order to approximate the coefficients of the coefficients,
Figure BDA00029894968800000316
for detail coefficients, #nIn order to be an empirical wavelet function,
Figure BDA00029894968800000317
is an empirical scale function.
Further, in step 3: the PID control mode judges the trend of errors by utilizing a differential process so as to improve the accuracy of attitude angle calculation by matching with a PI process; the PID calculation formula is as follows (18):
Figure BDA00029894968800000318
in equation 18, u (t) is an output signal of the PID controller, e is an input deviation, Kp is a proportional coefficient, Ki integral coefficient, and Kd derivative coefficient.
Advantageous technical effects
In the inertial navigation system, the problem of resolving the attitude angle is related to the navigation accuracy. Aiming at the problem of low attitude calculation precision caused by the problems of low-frequency noise and drift error of the gyroscope, the invention adopts the EWT algorithm to process the data of the gyroscope, uses quaternion to calculate the gravity vector, and uses the accelerometer data and the gravity vector to perform vector cross product to obtain the error. And then, correcting gyroscope data by utilizing complementary filtering controlled by PID, finally solving quaternion by using a Runge Kutta method, and solving an attitude angle.
The experimental simulation result shows that:
(1) the EWT algorithm is fused with the gyroscope/accelerometer, so that the attitude angle resolving precision can be improved, and the resolving with higher precision can be realized after the PID control complementary filtering is added.
(2) The improved attitude calculation method has good effect on the course angle and the pitch angle, and can improve the course angle and the pitch angle by about 60 percent and can improve the roll angle by about 50 percent.
Drawings
Fig. 1 is a diagram of a spectrum division principle.
FIG. 2 is a block diagram of the denoising principle of the EWT algorithm.
Fig. 3 is a diagram illustrating a conventional complementary filtering structure.
Fig. 4 is a flowchart of the overall procedure.
FIG. 5 is a diagram illustrating the X-axis angular velocity spectrum division.
Fig. 6 is a diagram of empirical mode components EMFs.
FIG. 7 is a diagram illustrating the effect of filtering the X-axis signal.
FIG. 8 is a schematic of method 1 solving for attitude angle (without the use of the EWT algorithm and using conventional complementary filtering to solve for attitude angle).
FIG. 9 is a schematic of method 2 solving for attitude angle (using the EWT algorithm and using conventional complementary filtering to solve for attitude angle).
FIG. 10 is a schematic of method 3 solving for attitude angle (using PID control complementary filtering without using the EWT algorithm to solve for attitude angle).
FIG. 11 is a schematic of method 4 solving for attitude angle (using the EWT algorithm and PID control complementary filtering to solve for attitude angle).
Detailed Description
Technical features of the present invention will now be described in detail with reference to the accompanying drawings.
An attitude calculation method based on an EWT algorithm is carried out according to the following steps:
step 1: performing frequency spectrum self-adaptive segmentation on angular velocity data measured by a gyroscope by utilizing an EWT algorithm, and constructing a proper wavelet filter on a segmentation interval so as to extract Empirical Mode Functions (EMFs);
step 2: performing noise reduction processing and signal reconstruction by using a wavelet denoising method;
and step 3: complementary filtering is carried out on the data measured by the accelerometer and the reconstructed signal to reduce drift errors, and PID complementary filtering is adopted to realize the complementary filtering;
and 4, step 4: and calculating a more accurate attitude angle by using a quaternion calculating method, and outputting a result.
To better illustrate the invention, an angle of change will be continued.
Posture generally means, a sitting postureThe conversion relation between the standard system and another coordinate system. The attitude of a carrier is usually described by a heading angle ψ, a pitch angle θ, and a roll angle γ. The common attitude calculation method mainly comprises an Euler angle method, a quaternion method and a direction cosine method[9]. Wherein:
euler angle method: euler angles are a common method of describing orientation and are proposed by euler. The basic idea is to decompose the transformation of two coordinate systems into a sequence of three consecutive rotations around three different coordinate axes. The euler angle rotation rule is two consecutive rotations, which must be rotated around different axes of rotation, so there are 12 rotation sequences in total. What is described primarily herein is the sequence of rotation of Z-Y-X, i.e., rotation of heading angle (YAW) ψ about the Z axis, ROLL angle (ROLL) γ about the X axis, and PITCH angle (PITCH) θ about the Y axis.
The differential equation of the euler angle is shown in formula (1).
Figure BDA0002989496880000051
The Euler angle method has the problems of singular points and dead locking of universal joints, and cannot be used for determining the attitude of the full attitude. Quaternion method: compared with the direction cosine method, the quaternion method has small calculated amount and simple algorithm[10]The problem of dead locking of the universal joint is avoided, and the method is a practical engineering method.
Quaternion, defining a quaternion as in formula (2):
q＝[q0 q1 q2 q3]T (2)
the mathematical expression of quaternion is as follows (3):
q＝q0+q1i+q2j+q3k type (3)
In the formula q0，q1，q2，q3Is a constant, i, j, k are pairwise orthogonal unit vectors, | q 21. The quaternion-to-rotation matrix is as follows:
Figure BDA0002989496880000061
the conversion relation between quaternion and Euler angle is used to obtain the mathematical formulas of course angle, pitch angle and roll angle as formula (5)
Figure BDA0002989496880000062
Solving quaternion: solving quaternion differential equations, commonly used are Euler method, median method, Picard algorithm, Longgusta method[11]。
In the process of resolving the differential equation of the quaternion, the fourth-order Rungestota method carries out interpolation processing in the integral interval, optimizes the total slope to obtain an updated result, so the accuracy is far higher than that of direct integration, the truncation error is smaller along with the increase of the order, and the calculation accuracy can be improved[12]. For the quaternion differential equation, the fourth order Rungestota formula is as in equation (6).
Figure BDA0002989496880000063
In the formula, q is a quaternion vector, h is a simulation step length, and f is a quaternion differential equation. Since there is a calculation error, the quaternion must be normalized after a number of updates.
The EWT algorithm: for convenience of explanation, provisions are made herein
Figure BDA0002989496880000064
In order to perform the fourier transformation, the method,
Figure BDA0002989496880000065
is an inverse fourier transform. For example, formula (7):
Figure BDA0002989496880000066
where j represents the imaginary unit, ω represents frequency, and t represents time.
Dividing the frequency spectrum boundary of the signal to be measured: normalizing the frequency spectrum range of the signal to be measured to [0, pi ] according to Shannon sampling theorem]And dividing the region into N intervals, each interval being ΛnAs shown in formula (8).
Figure BDA0002989496880000067
There are N +1 dividing lines in the signal spectrum, as shown in fig. 1; let omeganIs a boundary line, Λn＝[ωn-1，ωn]Also, from the formula (8), ω isnIn the range of [0, π]In the meantime. Around each boundary line, a transition phase region with a width T is definedn＝2τn. Whether the transition zone can contain valid information as much as possible determines the good or bad effect of the EWT algorithm to a certain extent[13]。
Wavelet filter bank: EWT is a new signal analysis method proposed by scholars in France[14]Has certain adaptivity and simultaneously has strict support of wavelet analysis theory, but needs to construct wavelet basis functions[15-16]Mainly referring to the construction idea of Littlewood-Paley and Meyer wavelet basis functions[17]. And adding a wavelet window function to the divided intervals. Empirical scale function
Figure BDA00029894968800000710
And empirical wavelet function
Figure BDA00029894968800000711
Are expressed by the formulas (9) and (10).
Figure BDA0002989496880000071
Figure BDA0002989496880000072
β(x)＝x4(35-84x+70x2-20x3) Formula (11)
Figure BDA0002989496880000073
In the formula 9, ω is frequency, ωnIs a demarcation frequency; formula 11 β is a coefficient function, and the currently mainly used form is as formula 11. γ in formula 12 is a proportionality coefficient at τn＝γωnThe window width is determined by the method, and the range is (0-1).
Signal reconstruction: the Empirical Wavelet Transform (EWT) is defined in the same way as the classical wavelet transform, and can be formed by the signal to be measured and the empirical wavelet function psinAnd empirical scale function
Figure BDA0002989496880000074
To obtain the detail coefficient thereof
Figure BDA0002989496880000075
And approximation coefficient
Figure BDA0002989496880000076
The expressions are as follows (13) and (14).
Figure BDA0002989496880000077
Figure BDA0002989496880000078
Reconstructing an expression from the original signal as follows
Figure BDA0002989496880000079
Where denotes convolution. The empirical mode function f can be obtained from the above formulak(t) is represented by formula (17).
Figure BDA0002989496880000081
The formula 16 is an empirical mode function obtained by decomposing the original signal f (t), and the total number is N.
Figure BDA0002989496880000082
Denoising by wavelet threshold: the EWT algorithm itself uses a wavelet filter bank, which has a certain filtering effect, but the effect is not ideal. Therefore, in order to better eliminate noise in signals, the method for denoising by using wavelet threshold and the EWT algorithm are combined, and the method for denoising by using wavelet soft threshold is used[18]The method carries out denoising processing aiming at Empirical Modes (EMFs) of each decomposition, and then carries out signal reconstruction, thereby obtaining denoised signals. The signal denoising principle diagram is shown in fig. 2.
Complementary filtering: in the traditional complementary filtering structure, attitude updating is mainly solved by using data of a gyroscope, the gyroscope has good dynamic performance, the accuracy of the measured data is high in a short time, but accumulated errors can be generated during attitude calculation, and random drift of the data occurs[19]. When the accelerometer measures the attitude, the problem of accumulated errors does not exist, but the measurement precision in a short time is poor, so the measurement precision and the dynamic performance are improved by utilizing the characteristic complementation and the complementary filtering[20]. A conventional complementary filtering structure is shown in fig. 3.
The invention adopts a complementary filtering structure controlled by PID: the conventional complementary filtering selects PI to eliminate errors, but the commonly solved attitude angle has the possibility of causing data to vibrate due to the existence of accumulated errors. In order to solve the problem, a PID control mode is adopted, and the trend of errors is judged by utilizing a differential process, so that the accuracy of attitude angle calculation is improved by matching with a PI process. The PID calculation formula is as follows (18):
Figure BDA0002989496880000083
an attitude calculation method based on an EWT algorithm comprises the following steps: according to the method, firstly, empirical wavelet decomposition (EWT) is utilized to perform self-adaptive spectrum decomposition on measured gyroscope data, empirical mode components (EMFs) are obtained through decomposition, aiming at the noise problem, a wavelet soft threshold denoising method is adopted, different soft thresholds are selected according to different components to perform denoising, then signals are reconstructed, and denoised gyroscope data are obtained. And (3) solving a gravity vector from the initial quaternion, performing vector cross product on the gravity vector and the data of the accelerometer to obtain an error, and performing PID control to correct the data of the reconstructed gyroscope. And finally, constructing a quaternion differential equation from the corrected gyroscope data, solving the quaternion differential equation by using a four-order Runge Kutta method, and finally solving a more accurate attitude angle. The overall program flow chart is shown in fig. 4.
Experimental analysis and study
In order to verify the practicability of the invention, a 6-axis inertial sensor MPU6050 is adopted to obtain accelerometer and gyroscope data, and Matlab software is used for carrying out simulation analysis on the measured data. Firstly, for the adaptive segmentation of the gyro data spectrum, fig. 5 is an X-axis angular velocity spectrum segmentation graph.
The signal is divided into 14 EMFs according to the display shown in the figure, and the EMFs are shown in FIG. 6.
And (3) adopting wavelet denoising, taking a self-adaptive soft threshold value for each modal component, and then reconstructing a signal. The original and reconstructed signal pairs for the angular velocity of the X-axis are shown in fig. 7.
As can be seen from FIG. 7, the angular velocity fluctuation of the X-axis is significantly reduced, and the fluctuation range is from-5X 10-3～5×10- 3rad/s is changed to-4X 10-3～4×10-3rad/s, which shows that the EWT algorithm is combined with wavelet denoising effect obviously and the error is reduced. The Y axis and the Z axis are subjected to noise reduction treatment by the same method, and the noise reduction effect is obvious.
Here, a signal-to-noise ratio of the signal after noise reduction processing, an autocorrelation coefficient, and a mean square error are added.
TABLE 1 analysis of reconstructed signals
Signal Signal to noise ratio Mean square error Coefficient of autocorrelation
Angular velocity of X axis 7.3103 0.000788 0.9668
Angular velocity of Y axis 7.2742 0.000811 0.9663
Angular velocity of Z axis 8.0922 0.000724 0.9727
Aiming at the efficiency problem of a filtering algorithm, comparing the EWT algorithm with commonly used EMD, EEMD and CEEMD algorithms, wherein the filtering object is an angular velocity signal of an X axis, the noise standard deviation ratio NSTD of the EEMD algorithm is 0.2, and the number NE of times of executing the kernel EMD algorithm is 500; the noise standard deviation ratio NSTD of the CEEMD algorithm is 0.2, the number of realizations NR is 100, and the maximum number of iterations maxter is 500.
TABLE 2 Filter Algorithm comparison
Method Signal to noise ratio/dB Mean square error rad/s Run time/s
EMD 1.3948 0.001557 0.7609
EEMD 5.6884 0.001505 14.3826
CEEMD 5.6369 0.001510 8.8791
EWT 7.3103 0.000788 0.7532
As can be seen from table 2, the EWT algorithm performs well both from the filtering effect and at run-time.
Next, 4 methods are used for solving the attitude angle, the method 1 is called in the figure without using the EWT algorithm and by using the conventional complementary filter, the method 2 is called in the figure with using the EWT algorithm and by using the conventional complementary filter, the method 3 is called in the figure with using the PID-controlled complementary filter and by using the EWT algorithm, and the method 4 is called in the figure with using the EWT algorithm and by using the PID-controlled complementary filter. The parameters of the PID control are set to Kp-1, Ki-0.01 and Kd-0.5, which can achieve better attitude angle resolving effect.
Analysis of results
TABLE 1 course Angle comparison
Method Fluctuation range/° c Maximum error angle/° Error angle standard deviation/° f
Method 1 -0.05～0.05 0.04579 0.0015
Method 2 -0.05～0.05 0.02790 0.0009
Method 3 -0.05～0.05 0.03279 0.0012
Method 4 -0.02～0.02 0.01903 0.0007
From Table 1 and FIGS. 8-11 (FIG. 8 is a schematic view of method 1 solving for attitude angle, i.e., without using the EWT algorithm and using conventional complementary filtering; FIG. 9 is a schematic view of method 2 solving for attitude angle, i.e., using the EWT algorithm and using conventional complementary filtering; FIG. 10 is a schematic view of method 3 solving for attitude angle, i.e., using PID controlled complementary filtering and without using the EWT algorithm; FIG. 11 is a schematic view of method 4 solving for attitude angle, i.e., using the EWT algorithm and using PID controlled complementary filtering)
When only the traditional complementary filtering is used, the error fluctuation is large, the maximum error is large, and the course angle resolving effect is poor; although the maximum error angle can be greatly reduced by adopting the EWT algorithm, the standard deviation is small, but the image vibration is large; when the complementary filtering of PID control is adopted, although the waveform is smooth, the maximum error angle is still large; and finally, complementary filtering of an EWT algorithm and PID control is adopted, the waveform effect is best, the error range is reduced to-0.02 degrees, the maximum error angle is reduced by 60 percent, and the standard deviation is reduced by 50 percent. The resolving precision of the course angle is improved.
TABLE 2 Pitch Angle comparison
Method Fluctuation range/° c Maximum error angle/° Error angle standard deviation/° f
Method 1 -0.05～0.05 0.02721 0.00070
Method 2 -0.05～0.05 0.02503 0.00066
Method 3 -0.02～0.02 0.01482 0.00041
Method 4 -0.02～0.02 0.01111 0.00036
From table 2 and fig. 8 to fig. 11, for the pitch angle, when only the conventional complementary filtering is adopted, the error range is-0.05 ° to 0.05 °, the waveform vibration is large in view of the waveform, and the maximum error angle is maximum; if the effect of correcting the pitching error is not great by only adopting the EWT algorithm; if the improved complementary filtering is combined with the EWT algorithm, the correction effect on the pitch angle error is good, the error range is-0.02 degrees, the maximum error angle is reduced by about 60 percent compared with the traditional complementary filtering, and the standard deviation is reduced by 50 percent.
TABLE 3 comparison of roll angles
Method Fluctuation range/° c Maximum error angle/° Error angle standard deviation/° f
Method 1 -0.05～0.05 0.02088 0.00064
Method 2 -0.05～0.05 0.02066 0.00062
Method 3 -0.02～0.02 0.01290 0.00037
Method 4 -0.02～0.02 0.01091 0.00034
As can be seen from table 3, the traditional complementary filtering and the improved PID complementary filtering fused with EWT have no obvious effect on the improvement of the roll angle, the maximum error angle is reduced by about 50%, the standard deviation of the error is also reduced by 50%, and the resolving accuracy of the roll angle can be improved.
Conclusion
In the inertial navigation system, the problem of resolving the attitude angle is related to the navigation accuracy. Aiming at the problem of low attitude calculation precision caused by the problems of low-frequency noise and drift error of the gyroscope, the invention adopts the EWT algorithm to process the data of the gyroscope, uses quaternion to calculate the gravity vector, and uses the accelerometer data and the gravity vector to perform vector cross product to obtain the error. And then, correcting gyroscope data by utilizing complementary filtering controlled by PID, finally solving quaternion by using a Runge Kutta method, and solving an attitude angle. The experimental simulation result shows that: (1) the EWT algorithm is fused with the gyroscope/accelerometer, so that the attitude angle resolving precision can be improved, and the resolving with higher precision can be realized after the PID control complementary filtering is added. (2) The improved attitude calculation method has good effect on the course angle and the pitch angle, and can improve the course angle and the pitch angle by about 60 percent and can improve the roll angle by about 50 percent.
Reference to the literature
[1] Chenoding, zhongpenhao, huzhenhao, tangyuesheng, qinyanfei four-axis hardware attitude solution studies based on MPU6050 [ J ] electromechanical engineering, 2018, 35 (01): 95-100.
Chen Guoding，Zhou Penghao，et al.Research on four-axis hardware attitude calculation based on MPU6050[J].Mechanical and Electrical Engineering，2018，35(01)：95-100.
[2] An MEMS navigation computer [ J ] electronic test, applicable to unmanned aerial vehicles, 2018, 000 (001): 44-48.
Jin Yuhang，Wang Tengfei.A MEMS navigation computer suitable for unmanned aerial vehicles[J].Electronic Test，2018，000(001)：44-48.
[3] Research on waru missile-borne strapdown inertial navigation algorithms and devices [ D ]. university of sienna industry, 2012.
Chen Wu.Research on missile-borne strapdown inertial navigation algorithm and device[D].Xi'an Technological University,2012.
[4] AHRS attitude solution design [ J ] electromechanical technique based on cubic spline and improved QKF 2015,000(003):24-27.
Zhou Hailing,Li Ting.AHRS attitude calculation design based on cubic spline and improved QKF[J].Electromechanical Technology,2015,000(003):24-27.
[5] Horsepower, Lensong, et al. unmanned aerial vehicle attitude algorithm [ J ] based on enhanced explicit complementary filtering, proceedings of Guilin electronics science and technology university, 2019,39(05): 396-.
Ma Li,Li Tiansong,et al.UAV attitude algorithm based on enhanced explicit complementary filtering[J].Journal of Guilin University of Electronic Technology,2019,39(05):396-401.
[6] MiMU attitude solution algorithm research [ J ] digital world based on RBF neural network, 2019(12): 41-42.
Yao Wenkai,Xing Liwen.Research on MIMU attitude calculation algorithm based on RBF neural network[J].Digital World,2019(12):41-42.
[7] Zhangiang, Songlong, et al. complementary filtering and Kalman filtering fusion attitude solution method [ J ]. sensor and microsystems, 2017,36(03):62-65+69.
Zhang Dong,Jiao Songming,et al.A fusion attitude calculation method based on complementary filtering and Kalman filtering[J].Sensors and Microsystems,2017,36(03):62-65+69.
[8] Chenguangwu, Liuxiao, Wangdi, etc. MEMS gyro signal denoising algorithm [ J ] based on improved wavelet transform, electron and informatics declaration, 2019,41(05):14-20.
Chen Guangwu,Liu Xiaobo,et al.MEMS gyroscope signal denoising algorithm based on improved wavelet transform[J].Journal of Electronics&Information Technology,2019,41(05):14-20.
[8] Zhang Zeng, Banguanren, strapdown inertial navigation four-subsample rotation vector attitude update algorithm [ J ] control engineering, 2010,17(003):272 and 274.
Zhang Ze,Duan Guangren.Four-sample rotation vector attitude update algorithm for strapdown inertial navigation[J].Control Engineering,2010,17(003):272-274.
[9] Liu Zhen Di is based on course attitude measurement technology research [ D ] 2016 of composite sensor.
Liu Zhendi.Research on heading and attitude measurement technology based on composite sensors[D].2016.
[10] Performance analysis of Xuxihao, Zhou Suo, et al, high precision strapdown inertial navigation attitude algorithm [ J ]. electro-optic and control,2019(7).
Xu Zhihao, Zhou Zaofa, et al.Performance analysis of high-precision strand initial observation attribute algorithm [ J ]. Electro-Optics and Control,2019(7) [11] Huangsheng, Jianshou Zihui, et al.
Huang Sheng,Jiang Zihui,et al.Commonly used numerical algorithms for ordinary differential equations and their applications[J].Progress in Applied Mathematics,2019,8(12):5.
[12] Skyo, Liuma Bao, four-step Runge Kuta attitude algorithm [ J ] of strapdown inertial navigation quaternion, 2019,41(03).
Shi Kai, Liu Mabao, Straddown inertial navigation quadron four-order Runge-Kutta atte algorithm [ J ] Journal of Detection and Control,2019,41(03) [13] Penglonghui, Zhao Zhidong, et al.
Peng Ronghui,Zhao Zhidong,et al.Research on EWT algorithm in single-lead ECG signal denoising[J].Journal of Hangzhou Dianzi University,2019,039(005):13-18.
[14]Gilles J.Empirical Wavelet Transform[J].IEEE Transactions on Signal Processing,2013,61(16):3999-4010.
[15] Cinnamylamine, a novel self-adaptive time-frequency distribution method and application research [ D ].2015 in fault diagnosis.
Zhu Ming.Novel adaptive time-frequency distribution method and its application in fault diagnosis[D].2015.
[16] Li Shinong, Zhuming, et al, mechanical failure diagnosis method based on empirical wavelet transform study [ J ] Instrument and Meter bulletin, 2014,000(011) 2423-.
Li Zhinong,Zhu Ming,et al.Research on mechanical fault diagnosis method based on empirical wavelet transform[J].Chinese Journal of Scientific Instrument,2014,000(011):2423-2432.
[17] Study of the algorithm eow in ECG signal filtering [ J ] electronic measurement and instrumentation, 2017(11).
Liu Chun,Xie Hao,et al.Research on EWT algorithm in ECG signal filtering[J].Journal of Electronic Measurement and Instrument,2017(11).
[18] High positive, rig drill pipe fault identification studies based on EWT and feature fusion [ D ].2019.
Gao Zheng.Research on Drill Pipe Failure Recognition Based on EWT and Feature Fusion[D].2019.
[19] Modeling and Analysis of MEMS Gyro Random Errors [ J ]. University of aerospace, Beijing University of Wang Xinlong, Li Na. modeling and Analysis of Random Errors in MEMS Gyroscopies [ J ]. Journal of Beijing University of aerospace of Aeronautics and Astronetics, 2012.
[20] Wanpengfei.Crane brake slip amount detection system based on dynamics modeling and MEMS technology was developed [ D ].2016.Wang Pengfei.development of a crane brake slip detection system based on dynamic modeling and MEMS technology [ D ].2016.

Claims (6)

1. An attitude calculation method based on an EWT algorithm is characterized in that: the method comprises the following steps:
step 1: performing self-adaptive segmentation of frequency spectrum on angular velocity data measured by a gyroscope by utilizing an EWT algorithm, and constructing a proper wavelet filter on a segmentation interval so as to extract EMFs;
step 2: performing noise reduction processing and signal reconstruction by using a wavelet denoising method;
and step 3: complementary filtering is carried out on the data measured by the accelerometer and the reconstructed signal to reduce drift errors, and PID controlled complementary filtering is adopted to realize the complementary filtering;
and 4, step 4: and calculating a more accurate attitude angle by using a quaternion calculating method, and outputting a result.
2. The EWT algorithm-based attitude solution method according to claim 1, wherein: the method comprises the following specific steps:
in step 1, performing self-adaptive spectrum decomposition on the measured gyroscope data by using EWT to obtain EMFs;
in step 2, aiming at the noise problem, a wavelet soft threshold denoising method is adopted, different soft thresholds are selected according to different frequency band components for denoising, and then signals are reconstructed to obtain denoised gyroscope data;
in step 3, a gravity vector is obtained from the initial quaternion, the gravity vector and the data of the accelerometer are subjected to vector cross product to obtain an error, and PID control is performed to correct the data of the reconstructed gyroscope;
in step 4, a quaternion differential equation is constructed from the corrected gyroscope data, the quaternion differential equation is solved by using a four-order Runge Kutta method, and a more accurate attitude angle is finally solved and output.
3. The EWT algorithm-based attitude solution method according to claim 1, wherein: in step 1: normalizing the frequency spectrum range of the signal to be measured to [0, pi ] according to Shannon sampling theorem]And dividing the region into N intervals, each interval being ΛnAs in formula (8):
Figure FDA0002989496870000011
there are N +1 dividing lines in the signal spectrum; let omeganIs a boundary line, Λn＝[ωn-1,ωn]Also, from the formula (8), ω isnIn the range of [0, π]To (c) to (d); around each boundary line, a transition phase region with a width T is definedn＝2τn。
4. The EWT algorithm-based attitude solution method according to claim 1, wherein: in step 1, the EWT used is a signal analysisThe method needs to construct a wavelet window function for the divided intervals, and the wavelet window function comprises the following steps: empirical scale function
Figure FDA0002989496870000012
And empirical wavelet function
Figure FDA0002989496870000013
The expressions are respectively shown in formulas (9) and (10):
Figure FDA0002989496870000021
Figure FDA0002989496870000022
β(x)＝x4(35-84x+70x2-20x3) (11)
Figure FDA0002989496870000023
in the above formula, ω is the frequency, ωnIs a demarcation frequency; beta is a coefficient function, and is mainly written by formula 11 at present; gamma is a proportionality coefficient at taun＝γωnThe window width is determined by the method, and the range is (0-1).
5. The EWT algorithm-based attitude solution method according to claim 1, wherein: in step 1: the Empirical Wavelet Transform (EWT) can be formed by the signal to be measured and the empirical wavelet function psinAnd empirical scale function
Figure FDA0002989496870000024
To obtain the detail coefficient thereof
Figure FDA0002989496870000025
And nearCoefficient of similarity
Figure FDA0002989496870000026
The expressions are shown in the following formulas (13) and (14):
Figure FDA0002989496870000027
Figure FDA0002989496870000028
the expression for reconstructing a signal from this original signal is as follows:
Figure FDA0002989496870000029
in the above formula, denotes convolution;
Figure FDA00029894968700000210
in order to approximate the coefficients of the coefficients,
Figure FDA00029894968700000211
for detail coefficients, #nIn order to be an empirical wavelet function,
Figure FDA00029894968700000212
in the form of a function of an empirical scale,
Figure FDA00029894968700000213
represents Fourier transform, ()∨Represents an inverse fourier transform;
empirical mode function f is obtained from the above formulak(t) the expression is as shown in formula (16):
Figure FDA00029894968700000214
the formula 17 is empirical mode functions obtained by decomposing the original signal f (t), and the number of the empirical mode functions is N;
Figure FDA00029894968700000215
in the above formula, the first and second carbon atoms are,
Figure FDA0002989496870000031
in order to approximate the coefficients of the coefficients,
Figure FDA0002989496870000032
for detail coefficients, #nIn order to be an empirical wavelet function,
Figure FDA0002989496870000033
is an empirical scale function.
6. The EWT algorithm-based attitude solution method according to claim 1, wherein: in step 3: the PID control mode judges the trend of errors by utilizing a differential process so as to improve the accuracy of attitude angle calculation by matching with a PI process; the PID calculation formula is as follows (18):
Figure FDA0002989496870000034
in the above formula, u (t) is the output signal of the PID controller, e is the input deviation, Kp is the proportional coefficient, Ki integral coefficient, Kd differential coefficient.
CN202110310526.XA 2021-03-23 2021-03-23 Attitude calculation method based on EWT algorithm Pending CN113218391A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110310526.XA CN113218391A (en) 2021-03-23 2021-03-23 Attitude calculation method based on EWT algorithm

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110310526.XA CN113218391A (en) 2021-03-23 2021-03-23 Attitude calculation method based on EWT algorithm

Publications (1)

Publication Number Publication Date
CN113218391A true CN113218391A (en) 2021-08-06

Family

ID=77083875

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110310526.XA Pending CN113218391A (en) 2021-03-23 2021-03-23 Attitude calculation method based on EWT algorithm

Country Status (1)

Country Link
CN (1) CN113218391A (en)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114608516A (en) * 2022-01-28 2022-06-10 北京航天发射技术研究所 Appearance equipment is surveyed to miniaturized radar developments
CN114894043A (en) * 2022-03-30 2022-08-12 北京航天飞腾装备技术有限责任公司 Precise guidance ammunition attitude filtering method and system based on wavelet packet transformation
CN115063945A (en) * 2022-06-20 2022-09-16 浙江科技学院 Fall detection alarm method and system based on attitude fusion calculation
CN118393290A (en) * 2024-06-27 2024-07-26 武汉豪迈光电科技有限公司 Power grid fault detection method based on 5G4G short sharing wireless communication
CN118570492A (en) * 2024-07-25 2024-08-30 长春工程学院 Depth stereo matching method based on PSMNet optimized feature extraction

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060265120A1 (en) * 2005-04-02 2006-11-23 American Gnc Corporation Method and system for automatic stabilization and pointing control of a device
CN102150203A (en) * 2008-03-20 2011-08-10 弗劳恩霍夫应用研究促进协会 Apparatus and method for converting an audio signal into a parameterized representation, apparatus and method for modifying a parameterized representation, apparatus and method for synthensizing a parameterized representation of an audio signal
JP2011227017A (en) * 2010-04-23 2011-11-10 Univ Of Tokyo Device and method for attitude estimation of moving body using inertial sensor, magnetic sensor, and speed meter
JP2013104665A (en) * 2011-11-10 2013-05-30 Toyota Motor Corp Attitude angle calculation device, attitude angle calculation method, and program
CN103676650A (en) * 2013-11-29 2014-03-26 上海交通大学 Method for PID (proportion integration differentiation) optimization control with dead zone for two-wheeled self-balancing intelligent vehicle
CN103818526A (en) * 2014-02-21 2014-05-28 广州中国科学院先进技术研究所 Water platform with thrusters
CN104682789A (en) * 2013-11-28 2015-06-03 哈尔滨功成科技创业投资有限公司 PID (Proportion Integration Differentiation) controller applied to two-wheeled robots
CN106482734A (en) * 2016-09-28 2017-03-08 湖南优象科技有限公司 A kind of filtering method for IMU Fusion
KR20170121379A (en) * 2016-04-22 2017-11-02 가천대학교 산학협력단 A position recognition and control device for a remote control device
CN108827299A (en) * 2018-03-29 2018-11-16 南京航空航天大学 A kind of attitude of flight vehicle calculation method based on improvement quaternary number second order complementary filter
CN109766798A (en) * 2018-12-27 2019-05-17 武汉灏存科技有限公司 Gesture data processing method, server and awareness apparatus based on experience small echo
CN109931929A (en) * 2019-01-25 2019-06-25 南京薄幕软件科技有限公司 A kind of UAV Attitude calculation method based on quaternary number

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060265120A1 (en) * 2005-04-02 2006-11-23 American Gnc Corporation Method and system for automatic stabilization and pointing control of a device
CN102150203A (en) * 2008-03-20 2011-08-10 弗劳恩霍夫应用研究促进协会 Apparatus and method for converting an audio signal into a parameterized representation, apparatus and method for modifying a parameterized representation, apparatus and method for synthensizing a parameterized representation of an audio signal
JP2011227017A (en) * 2010-04-23 2011-11-10 Univ Of Tokyo Device and method for attitude estimation of moving body using inertial sensor, magnetic sensor, and speed meter
JP2013104665A (en) * 2011-11-10 2013-05-30 Toyota Motor Corp Attitude angle calculation device, attitude angle calculation method, and program
CN104682789A (en) * 2013-11-28 2015-06-03 哈尔滨功成科技创业投资有限公司 PID (Proportion Integration Differentiation) controller applied to two-wheeled robots
CN103676650A (en) * 2013-11-29 2014-03-26 上海交通大学 Method for PID (proportion integration differentiation) optimization control with dead zone for two-wheeled self-balancing intelligent vehicle
CN103818526A (en) * 2014-02-21 2014-05-28 广州中国科学院先进技术研究所 Water platform with thrusters
KR20170121379A (en) * 2016-04-22 2017-11-02 가천대학교 산학협력단 A position recognition and control device for a remote control device
CN106482734A (en) * 2016-09-28 2017-03-08 湖南优象科技有限公司 A kind of filtering method for IMU Fusion
CN108827299A (en) * 2018-03-29 2018-11-16 南京航空航天大学 A kind of attitude of flight vehicle calculation method based on improvement quaternary number second order complementary filter
CN109766798A (en) * 2018-12-27 2019-05-17 武汉灏存科技有限公司 Gesture data processing method, server and awareness apparatus based on experience small echo
CN109931929A (en) * 2019-01-25 2019-06-25 南京薄幕软件科技有限公司 A kind of UAV Attitude calculation method based on quaternary number

Non-Patent Citations (8)

* Cited by examiner, † Cited by third party
Title
JI MA: "Application and Optimization ofWavelet Transform Filter for North-Seeking Gyroscope Sensor Exposed to Vibration", SENSORS *
刘春等: "EWT 算法在ECG 信号滤波中的研究", 电子测量与仪器学报, pages 1836 *
刘春等: "改进Ａｌｌａｎ 方差的自适应滤波在姿态解算中的应用", 传感技术学报, pages 683 *
崔冰波: "GNSS拒止环境下FOG-S丨NS/GNSS组合导航关键技术研究", 中国博士学位论文全文数据库 (信息科技辑) *
崔晓阳;白茹;吴涛;钱正洪;: "姿态测量系统中两种姿态解算方法的分析比较", 杭州电子科技大学学报(自然科学版), no. 01 *
彭荣辉等: "ＥＷＴ算法在单导联心电信号去噪中的研究", 杭州电子科技大学学报（自然科学版）, pages 13 *
彭蕴博;袁亮;: "基于小波滤波的低成本无人机姿态信息融合", 计算机仿真, no. 11 *
邓传远;刘春;谢皓;张国强;: "基于PID自适应卡尔曼的惯导姿态算法", 传感器与微系统, no. 04 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114608516A (en) * 2022-01-28 2022-06-10 北京航天发射技术研究所 Appearance equipment is surveyed to miniaturized radar developments
CN114894043A (en) * 2022-03-30 2022-08-12 北京航天飞腾装备技术有限责任公司 Precise guidance ammunition attitude filtering method and system based on wavelet packet transformation
CN114894043B (en) * 2022-03-30 2023-12-22 北京航天飞腾装备技术有限责任公司 Accurate guided ammunition attitude filtering method and system based on wavelet packet transformation
CN115063945A (en) * 2022-06-20 2022-09-16 浙江科技学院 Fall detection alarm method and system based on attitude fusion calculation
CN115063945B (en) * 2022-06-20 2023-12-29 浙江科技学院 Fall detection alarm method and system based on attitude fusion calculation
CN118393290A (en) * 2024-06-27 2024-07-26 武汉豪迈光电科技有限公司 Power grid fault detection method based on 5G4G short sharing wireless communication
CN118393290B (en) * 2024-06-27 2024-09-13 武汉豪迈光电科技有限公司 Power grid fault detection method based on 5G4G short sharing wireless communication
CN118570492A (en) * 2024-07-25 2024-08-30 长春工程学院 Depth stereo matching method based on PSMNet optimized feature extraction

Similar Documents

Publication Publication Date Title
CN113218391A (en) Attitude calculation method based on EWT algorithm
Wu et al. Generalized linear quaternion complementary filter for attitude estimation from multisensor observations: An optimization approach
CN104898681B (en) A kind of quadrotor attitude acquisition method for approximately finishing card quaternary number using three ranks
CN109974714B (en) Sage-Husa self-adaptive unscented Kalman filtering attitude data fusion method
CN108827299B (en) Aircraft attitude calculation method based on improved quaternion second-order complementary filtering
CN111551174A (en) High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system
CN105806363B (en) The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF
CN108458709B (en) Airborne distributed POS data fusion method and device based on vision-aided measurement
CN110174121A (en) A kind of aviation attitude system attitude algorithm method based on earth's magnetic field adaptive correction
CN110702113B (en) Method for preprocessing data and calculating attitude of strapdown inertial navigation system based on MEMS sensor
CN108871319B (en) Attitude calculation method based on earth gravity field and earth magnetic field sequential correction
CN112525191A (en) Airborne distributed POS transfer alignment method based on relative strapdown calculation
CN106595669A (en) Attitude calculation method of rotating body
CN113325865B (en) Unmanned aerial vehicle control method, control device and control system
CN112097728B (en) Inertial double-vector matching deformation measurement method based on inverse solution combined inertial navigation system
CN116182871B (en) Sea cable detection robot attitude estimation method based on second-order hybrid filtering
CN110095118A (en) A kind of method for real-time measurement and system at body gesture angle
CN115839726B (en) Method, system and medium for jointly calibrating magnetic sensor and angular velocity sensor
CN115655285B (en) Unscented quaternion attitude estimation method for correcting weight and reference quaternion
Liu et al. LGC-Net: A lightweight gyroscope calibration network for efficient attitude estimation
CN112304309B (en) Method for calculating combined navigation information of hypersonic vehicles based on cardiac array
Peng et al. Online Self-calibration of Camera-IMU External Parameters and IMU Initialization for Stereo VI-SLAM
Lu et al. Segmented Angular Rate Joint Estimation of Inertial Sensor Arrays for UAV Navigation
CN117128956B (en) Dynamic inclination angle acquisition method based on angular velocity conversion and equipment applying method
Lin et al. Research on the quadrotor of AHRS based on gradient descent algorithm

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination