Angle calculation method and system based on three-axis accelerometer

Abstract

The application provides a three-axis accelerometer-based angle calculation method and system, relates to the technical field of inertial navigation, and comprises the following steps: obtaining an acceleration signal and preprocessing to obtain effective acceleration components, constructing a nonlinear error feature vector, using a recursive least square method to perform real-time parameter identification to establish a nonlinear error compensation model, using an adaptive sliding time window to perform dynamic correction, and finally calculating a pitch angle and a roll angle. The application can effectively inhibit the nonlinear error of the accelerometer, improve the angle calculation precision, and has the characteristics of good real-time performance and strong adaptability.

Description

Technical Field

[0001] This invention relates to inertial navigation technology, and more particularly to an angle calculation method and system based on a triaxial accelerometer. Background Technology

[0002] In modern navigation, attitude measurement, and motion analysis, triaxial accelerometers are widely used to measure the attitude angles of objects. Triaxial accelerometers offer advantages such as small size, low cost, and low power consumption. They can calculate an object's pitch and roll angles by measuring the components of gravitational acceleration along each axis. This angle measurement method has broad application prospects in smart wearable devices, drones, robot control, and human-computer interaction.

[0003] Accelerometers inherently possess nonlinear errors, including zero-bias error, scaling factor error, and cross-axis sensitivity error. These errors directly impact the accuracy of angle calculations, particularly in applications requiring high precision. Acceleration signals are susceptible to interference from environmental vibrations and shocks. During object motion, linear accelerations other than gravitational acceleration can severely affect the accuracy of angle measurements, leading to significant fluctuations in the calculation results. Traditional angle calculation algorithms typically employ fixed-parameter models, lacking adaptability to different operating environments. Under conditions of temperature changes or prolonged operation, the accuracy of angle calculations gradually decreases, failing to meet the stable measurement requirements of dynamic application environments.

[0004] Therefore, an angle calculation method is needed that can compensate for nonlinear errors in real time, suppress dynamic interference, and has environmental adaptability, in order to improve the accuracy and reliability of angle measurement based on triaxial accelerometers. Summary of the Invention

[0005] The present invention provides an angle calculation method and system based on a triaxial accelerometer, which can solve the problems in the prior art.

[0006] A first aspect of the present invention provides an angle calculation method based on a triaxial accelerometer, comprising: Acceleration signals from a triaxial accelerometer are acquired, and the acceleration signals are preprocessed to obtain the effective acceleration components of each axis. A nonlinear error feature vector is constructed based on the effective acceleration components, and the nonlinear error feature vector is used to identify parameters in real time through recursive least squares method to establish a nonlinear error compensation model. An adaptive sliding time window is used to dynamically correct the output of the nonlinear error compensation model. The acceleration signal after nonlinear error compensation and dynamic correction is substituted into the angle calculation equation to calculate the pitch and roll angles of the triaxial accelerometer. The pitch and roll angles are then output as the final angle calculation results.

[0007] Acquire acceleration signals from a triaxial accelerometer, preprocess the acceleration signals, and obtain the effective acceleration components for each axis, including: Wavelet threshold filtering and Kalman filtering were performed on the acceleration signals from the triaxial accelerometer, and weighted fusion was performed based on the covariance matrix of the filtering results. The fused acceleration signal is transformed according to the three orthogonal directions of the navigation coordinate system, and the effective acceleration components of each axis are extracted using an adaptive threshold segmentation detection method.

[0008] Based on the effective acceleration components, a nonlinear error feature vector is constructed. The nonlinear error compensation model is then established by real-time parameter identification of the nonlinear error feature vector using the recursive least squares method, including: The acceleration measurement value of the inertial navigation system is obtained, and the acceleration measurement value is filtered by digital filtering to extract the effective acceleration component from the acceleration measurement value; Based on the effective acceleration components, a Taylor series expansion is performed on the effective acceleration components and their corresponding temperature values ​​to construct a nonlinear error feature vector containing acceleration nonlinear terms, temperature nonlinear terms and their cross terms. The coefficients of second-order and higher nonlinear terms are obtained by Taylor series expansion of the nonlinear error feature vector. The recursive least squares method is used to identify the nonlinear error feature vector in real time. The historical data in the parameter identification process is weighted by introducing a time-related forgetting factor, and a nonlinear error compensation model is established based on the identification results.

[0009] The nonlinear error feature vector is identified in real time using the recursive least squares method. Historical data during the parameter identification process is weighted by introducing a time-dependent forgetting factor. A nonlinear error compensation model is established based on the identification results, including: The feature vector of the system to be identified is obtained, and the recursive least squares method is used to identify the parameters of the feature vector in real time. The recursive least squares method is calculated based on the exponential forgetting factor, wherein the value of the exponential forgetting factor is dynamically adjusted according to the data time interval to realize the differentiated weighting of historical data. A nonlinear error compensation model is constructed based on the result of the parameter identification.

[0010] An adaptive sliding time window is used to dynamically correct the output of the nonlinear error compensation model. The acceleration signal after nonlinear error compensation and dynamic correction is substituted into the angle calculation equation to calculate the pitch and roll angles of the triaxial accelerometer. The pitch and roll angles are output as the final angle calculation results, including: The output signal of the nonlinear error compensation model is obtained, and an adaptive sliding time window is constructed to dynamically correct the output signal. The length of the adaptive sliding time window is determined by calculating the variance change rate of the output signal. When the variance change rate is greater than a preset variance threshold, the window length is shortened, and when the variance change rate is less than the preset variance threshold, the window length is extended to achieve adaptive correction of the output signal. The acceleration signal, after nonlinear error compensation and dynamic correction, is substituted into the angle calculation equation. An attitude matrix is ​​constructed based on the projection value of the acceleration signal on the navigation coordinate system. The pitch and roll angles of the triaxial accelerometer are calculated by iteratively solving the attitude matrix. The pitch and roll angles are then output as the final angle calculation results.

[0011] Substituting the acceleration signal, after nonlinear error compensation and dynamic correction, into the angle calculation equation, and constructing an attitude matrix based on the projection value of the acceleration signal in the navigation coordinate system, the pitch and roll angles of the triaxial accelerometer are calculated iteratively by solving the attitude matrix, including: The acceleration signal, after nonlinear error compensation and dynamic correction, is projected onto the navigation coordinate system. An attitude matrix is ​​constructed based on the projected values. The pitch and roll angles of the triaxial accelerometer are calculated based on the orthogonal normalization result of the attitude matrix.

[0012] A second aspect of the present invention provides an angle calculation system based on a triaxial accelerometer, comprising: The first unit is used to acquire the acceleration signal from the triaxial accelerometer, preprocess the acceleration signal to obtain the effective acceleration components of each axis, construct a nonlinear error feature vector based on the effective acceleration components, identify the parameters of the nonlinear error feature vector in real time using the recursive least squares method, and establish a nonlinear error compensation model. The second unit is used to dynamically correct the output of the nonlinear error compensation model using an adaptive sliding time window. The acceleration signal after nonlinear error compensation and dynamic correction is substituted into the angle calculation equation to calculate the pitch and roll angles of the triaxial accelerometer, and the pitch and roll angles are output as the final angle calculation results.

[0013] A third aspect of the present invention provides an electronic device, comprising: processor; Memory used to store processor-executable instructions; The processor is configured to invoke instructions stored in the memory to execute the aforementioned method.

[0014] A fourth aspect of the present invention provides a computer-readable storage medium having stored thereon computer program instructions that, when executed by a processor, implement the aforementioned method.

[0015] The beneficial effects of this application are as follows: By constructing a nonlinear error feature vector and applying the recursive least squares method for real-time parameter identification, an accurate nonlinear error compensation model was established, which effectively overcomes the calculation error caused by the nonlinear characteristics of the accelerometer in the traditional angle calculation method and significantly improves the accuracy of angle calculation.

[0016] An adaptive sliding time window is used to dynamically correct the output of the nonlinear error compensation model. This allows the compensation parameters to be adjusted in real time according to changes in the measurement environment, enabling the system to maintain stable calculation accuracy under different operating conditions and enhancing the adaptability and robustness of the method.

[0017] By combining effective acceleration component preprocessing with nonlinear error compensation, interference from environmental noise and sensor internal errors is eliminated, making angle calculation results more reliable in complex dynamic environments. The overall method has high computational efficiency, making it suitable for real-time applications in resource-constrained embedded systems, while ensuring high accuracy and stability in pitch and roll angle calculations. Attached Figure Description

[0018] Figure 1 This is a flowchart illustrating the angle calculation method based on a triaxial accelerometer according to an embodiment of the present invention. Detailed Implementation

[0019] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0020] The technical solution of the present invention will be described in detail below with reference to specific embodiments. These specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described again in some embodiments.

[0021] Figure 1 This is a flowchart illustrating the angle calculation method based on a triaxial accelerometer according to an embodiment of the present invention. Figure 1 As shown, the method includes: Acceleration signals from a triaxial accelerometer are acquired, and the acceleration signals are preprocessed to obtain the effective acceleration components of each axis. A nonlinear error feature vector is constructed based on the effective acceleration components, and the nonlinear error feature vector is used to identify parameters in real time through recursive least squares method to establish a nonlinear error compensation model.

[0022] An adaptive sliding time window is used to dynamically correct the output of the nonlinear error compensation model. The acceleration signal after nonlinear error compensation and dynamic correction is substituted into the angle calculation equation to calculate the pitch and roll angles of the triaxial accelerometer. The pitch and roll angles are then output as the final angle calculation results.

[0023] In one optional implementation, acquiring the acceleration signal from the triaxial accelerometer and preprocessing the acceleration signal to obtain the effective acceleration components of each axis includes: Wavelet threshold filtering and Kalman filtering were performed on the acceleration signals from the triaxial accelerometer, and weighted fusion was performed based on the covariance matrix of the filtering results. The fused acceleration signal is transformed according to the three orthogonal directions of the navigation coordinate system, and the effective acceleration components of each axis are extracted using an adaptive threshold segmentation detection method.

[0024] Acquire the raw acceleration signal from a triaxial accelerometer. Triaxial accelerometers are typically mounted on mobile devices and can measure the device's acceleration in three orthogonal directions. The raw signal is represented as Ax(t), Ay(t), and Az(t), corresponding to the acceleration values ​​in the x-axis, y-axis, and z-axis directions, respectively, with units of meters per second. 2 .

[0025] After acquiring the original signal, wavelet threshold filtering is performed on the triaxial acceleration signal. Wavelet threshold filtering is an effective method for removing noise, especially suitable for processing non-stationary signals. The specific steps are as follows: Perform wavelet decomposition on the original acceleration signal, selecting a suitable wavelet basis function (such as the Daubechies wavelet) for multi-level decomposition; apply a threshold function to the decomposed wavelet coefficients, using either soft or hard thresholding methods; the threshold can be determined through signal variance estimation, such as using the empirical Bayesian thresholding method; finally, reconstruct the processed wavelet coefficients to obtain the filtered signal, denoted as Axw(t), Ayw(t), and Azw(t).

[0026] Next, Kalman filtering is performed on the same set of acceleration signals. Kalman filtering is a recursive estimation method that achieves optimal signal estimation through two stages: prediction and correction. First, a state-space model of the acceleration signal is established, with the state vector including the acceleration value and its rate of change. The process noise covariance matrix Q and the measurement noise covariance matrix R are set, which can be determined based on sensor specifications and experimental data. Then, the prediction and update steps of Kalman filtering are performed, iteratively calculating the optimal estimate. Finally, the Kalman-filtered signals are obtained, denoted as Axk(t), Ayk(t), and Azk(t).

[0027] After obtaining the two filtering results, a weighted fusion is performed based on the covariance matrices of the filtering results. The covariance matrices Pw and Pk of the wavelet threshold filtering and Kalman filtering results are calculated respectively. Weight coefficients are calculated based on the covariance matrices; the weights are inversely proportional to the covariance, meaning that the lower the noise level of the filtering result, the greater its weight. The calculated weight coefficients are then used to linearly combine the two filtering results to obtain the fused acceleration signals Axf(t), Ayf(t), and Azf(t). This fusion method fully utilizes the advantages of both filtering techniques, improving signal quality.

[0028] The fused acceleration signals undergo coordinate transformation. Since accelerometers are typically installed in the device's body coordinate system, but practical applications often require acceleration data in the navigation coordinate system, a coordinate transformation is necessary. First, the device's attitude information is acquired, which can be obtained from the device's rotation angles relative to the navigation coordinate system via a gyroscope or attitude sensor, including pitch, roll, and yaw angles. Then, a rotation matrix R is constructed from the device's body coordinate system to the navigation coordinate system. Finally, the fused acceleration signals are transformed back to the navigation coordinate system using this rotation matrix to obtain Axn(t), Ayn(t), and Azn(t).

[0029] An adaptive threshold segmented detection method is used to extract the effective acceleration components of each axis, calculate the noise baseline level under static conditions, and calculate the mean and standard deviation by collecting acceleration data when the equipment is stationary. Then, the detection threshold is dynamically adjusted according to the noise level. The threshold value can be set as a multiple of the noise standard deviation and can be adaptively adjusted according to the local characteristics of the signal. The acceleration signal is analyzed in segments using the sliding window method. When the signal amplitude exceeds the threshold and lasts for a certain period of time, it is determined to be effective acceleration. The acceleration segments that meet the conditions are extracted to obtain the effective acceleration components Axe(t), Aye(t), and Aze(t) of each axis.

[0030] In practical applications, such as gait analysis systems, the effective acceleration components during walking can be obtained using the methods described above. During walking, the human body's acceleration in the vertical direction exhibits clear periodicity and regularity. By extracting the effective acceleration components in the vertical direction, key events during walking can be identified, such as heel strike and toe lift-off, thereby allowing the calculation of gait parameters such as stride frequency and stride length.

[0031] Through the above steps, high-quality preprocessing of the triaxial accelerometer signal was achieved, effectively removing noise interference and extracting the effective acceleration components of each axis, providing a reliable data foundation for subsequent motion analysis, attitude estimation and other applications.

[0032] In one optional implementation, a nonlinear error feature vector is constructed based on the effective acceleration components, and the nonlinear error feature vector is used to perform real-time parameter identification through recursive least squares method to establish a nonlinear error compensation model, including:

[0033] The acceleration measurement value of the inertial navigation system is obtained, and the acceleration measurement value is filtered by digital filtering to extract the effective acceleration component from the acceleration measurement value;

[0034] Based on the effective acceleration components, a Taylor series expansion is performed on the effective acceleration components and their corresponding temperature values ​​to construct a nonlinear error feature vector containing acceleration nonlinear terms, temperature nonlinear terms and their cross terms. The coefficients of second-order and higher nonlinear terms are obtained by Taylor series expansion of the nonlinear error feature vector.

[0035] The recursive least squares method is used to identify the nonlinear error feature vector in real time. The historical data in the parameter identification process is weighted by introducing a time-related forgetting factor, and a nonlinear error compensation model is established based on the identification results.

[0036] Acceleration measurements of an inertial navigation system are acquired in practical applications using triaxial accelerometers mounted on the navigation platform. These measurements contain the true acceleration information of the navigation platform, but also include various error components such as quantization noise, random drift, and various nonlinear error terms.

[0037] The acquired acceleration measurements are digitally filtered to extract the effective acceleration components. Considering that acceleration measurement signals typically contain high-frequency noise, a low-pass filter is used for signal preprocessing. Specifically, a Butterworth low-pass filter can be used, with its cutoff frequency set to one-tenth of the dynamic characteristics of the navigation system, ensuring that high-frequency noise is filtered out while retaining the effective acceleration signal. The filtered acceleration signal is smoother, which is beneficial for subsequent nonlinear error feature extraction.

[0038] Based on the extracted effective acceleration components, a nonlinear error eigenvector is constructed. Due to the nonlinear error and temperature sensitivity of the accelerometer, the acceleration and temperature are expanded using a Taylor series to obtain an eigenvector containing higher-order nonlinear terms. Specifically, for a given axial accelerometer, its measured value can be expressed as the superposition of the true acceleration and the error term. The error term can be represented by a second-order or higher Taylor expansion, including square and cubic terms of acceleration, as well as linear and square terms of temperature and their interaction with acceleration.

[0039] Taking a triaxial accelerometer as an example, the x-axis accelerometer is modeled, and the constructed nonlinear error feature vector includes: the quadratic term a of the acceleration. 2 cubic term a 3 Temperature term T, temperature square term T 2 The terms include the interaction between temperature and acceleration, such as aT. These terms collectively form an eigenvector, used to describe the nonlinear error behavior of the accelerometer under different operating conditions.

[0040] A recursive least squares (RLS) method is used for real-time parameter identification of the constructed nonlinear error feature vector. RLS is an adaptive algorithm that can continuously update parameter estimates based on new observation data, making it suitable for scenarios requiring real-time processing, such as inertial navigation systems. To improve the algorithm's adaptability to time-varying systems, a time-dependent forgetting factor λ is introduced to weight historical data. The forgetting factor typically ranges from 0.95 to 0.99. Smaller values ​​mean greater weight is given to new data, resulting in a faster system response to changes, but leading to increased estimation fluctuations; larger values ​​result in smoother estimates, but a slower response to system changes.

[0041] In practical applications, the forgetting factor can be adjusted according to the working environment and dynamic characteristics of the navigation system. For example, in an environment with rapid temperature changes, a smaller forgetting factor can be selected to quickly adapt to parameter changes caused by temperature; while in a stable environment, a larger forgetting factor can be selected to obtain more stable estimation results.

[0042] The recursive least squares update process includes four steps: prediction error calculation, gain matrix update, parameter estimation update, and covariance matrix update. Each time a new set of acceleration data and corresponding temperature data is received, a parameter update is performed, enabling real-time identification of the parameters of the nonlinear error model.

[0043] Based on the identification results, a nonlinear error compensation model is established. This model includes the coefficients of various nonlinear terms of the accelerometer and temperature sensitivity parameters, and can be directly used to compensate for errors in the original acceleration measurements. The compensated acceleration value is closer to the true acceleration, which can significantly improve the accuracy of the navigation system.

[0044] In a static calibration experiment of an inertial navigation system, the temperature range increased from -20°C to 40°C, and accelerometer output data was collected across the entire temperature range. Applying this method to process the data, it was identified that the coefficient of the quadratic acceleration term is approximately 0.2% of the original output, the temperature sensitivity coefficient is approximately 0.2% per degree, and the coefficient of the cross-term between acceleration and temperature is approximately 0.05% per degree. After compensation, the accelerometer's nonlinear error was reduced by 85%, the static zero-bias stability was improved by three times, and the cumulative position error of the navigation system was reduced by 70%, significantly improving navigation accuracy.

[0045] The above method enables effective identification and compensation of nonlinear errors in the accelerometer of the inertial navigation system, improving the accuracy and reliability of the navigation system, and is especially suitable for long-term navigation and precise positioning requirements in complex environments.

[0046] In one optional implementation, the recursive least squares method is used to identify the nonlinear error feature vector in real time. A time-dependent forgetting factor is introduced to weight the historical data during the parameter identification process. Based on the identification results, a nonlinear error compensation model is established, including: The feature vector of the system to be identified is obtained, and the recursive least squares method is used to identify the parameters of the feature vector in real time. The recursive least squares method is calculated based on the exponential forgetting factor, wherein the value of the exponential forgetting factor is dynamically adjusted according to the data time interval to realize the differentiated weighting of historical data. A nonlinear error compensation model is constructed based on the result of the parameter identification.

[0047] The nonlinear error feature vector is obtained by acquiring raw data using a high-precision sensor. During acquisition, the sampling frequency is set to 200Hz and the sampling time window is set to 10 seconds to ensure that the data volume meets the requirements for parameter identification. After 16-bit AD conversion, the raw data is stored in a double-ended queue data structure with a queue capacity of 2048, and the data is dynamically updated using a circular write method.

[0048] The recursive least squares method is implemented using a state-space representation. The state vector contains the parameters to be identified, and the observation vector is the nonlinear error feature vector. The initial state estimate is set as the zero vector, and the initial error covariance matrix is ​​set as the identity matrix multiplied by a large positive number (e.g., 1000) to ensure a fast convergence speed. The gain matrix is ​​calculated using a recursive formula, where the observation noise variance is determined through statistical analysis during the data preprocessing stage.

[0049] The dynamic adjustment mechanism of the exponential forgetting factor is based on data time intervals. The initial value of the forgetting factor is set to 0.98, decreasing as the time interval increases and increasing as the time interval decreases. Specifically, the adjustment method is as follows: when the time interval between the current data and historical data is less than 100ms, the forgetting factor value changes linearly between 0.95 and 0.98; when the time interval is greater than 100ms but less than 500ms, the forgetting factor value changes linearly between 0.90 and 0.95; and when the time interval is greater than 500ms, the forgetting factor value is set to 0.90. The time interval is calculated using data timestamps.

[0050] The parameter identification process employs a batch processing method, with each batch consisting of 256 data points and an overlap of 128 data points between adjacent batches. For each batch, the predicted state value is first calculated, and then the estimated state value is updated based on the observed data. The convergence criterion for state estimation is that the relative error between two adjacent estimates is less than 0.001. If the convergence condition is not met and the number of iterations has not exceeded the maximum limit (set to 50), the iterative calculation continues.

[0051] The nonlinear error compensation model is constructed using a piecewise linear interpolation method. Feature points are determined based on parameter identification results. The spacing between feature points is non-uniform, with denser spacing (approximately 0.1g) near the zero point and sparser spacing (approximately 0.5g) further away from the zero point. Linear interpolation is used to calculate compensation values ​​between adjacent feature points. Model parameters are stored using a key-value pair structure, where the key is the acceleration value of the feature point and the value is the corresponding compensation value.

[0052] In a practical application case, calibration data from a triaxial accelerometer at 25°C was used as the validation dataset. The original data ranged from -2g to +2g, with a sampling interval of 5ms. The compensation model obtained after parameter identification exhibited a nonlinearity error of less than 0.1% of full scale within the ±1g range and less than 0.2% of full scale within the ±2g range. The model computation time on an embedded processor (100MHz) did not exceed 100 microseconds, meeting real-time requirements.

[0053] The online update cycle for the compensation model is set to 1 hour, and a dual-caching mechanism is used during the update process to ensure data continuity. Switching between old and new model parameters is achieved through atomic operations, with the switching time chosen at the batch boundary of data processing. Anomaly detection for model parameters is based on statistical features; when the compensation value of a feature point exceeds three times the standard deviation of the historical mean, the update of that feature point will be temporarily frozen.

[0054] Data storage and access are implemented using a circular buffer with a size of 8KB, managed according to the first-in, first-out (FIFO) principle. When the buffer is full, the oldest data is automatically overwritten. Each data frame contains a 16-bit timestamp, a 16-bit temperature value, and three 32-bit acceleration values, totaling 112 bits. Data integrity is guaranteed by CRC16 checksum. The communication interface uses an SPI bus with a clock frequency of 1MHz and operates in 4-wire full-duplex mode.

[0055] In one optional implementation, an adaptive sliding time window is used to dynamically correct the output of the nonlinear error compensation model. The acceleration signal after nonlinear error compensation and dynamic correction is substituted into the angle calculation equation to calculate the pitch and roll angles of the triaxial accelerometer. The pitch and roll angles are then output as the final angle calculation results, including:

[0056] The output signal of the nonlinear error compensation model is obtained, and an adaptive sliding time window is constructed to dynamically correct the output signal. The length of the adaptive sliding time window is determined by calculating the variance change rate of the output signal. When the variance change rate is greater than a preset variance threshold, the window length is shortened, and when the variance change rate is less than the preset variance threshold, the window length is extended to achieve adaptive correction of the output signal.

[0057] The acceleration signal, after nonlinear error compensation and dynamic correction, is substituted into the angle calculation equation. An attitude matrix is ​​constructed based on the projection value of the acceleration signal on the navigation coordinate system. The pitch and roll angles of the triaxial accelerometer are calculated by iteratively solving the attitude matrix. The pitch and roll angles are then output as the final angle calculation results.

[0058] The output signal of the nonlinear error compensation model is acquired through a high-precision data acquisition unit with a sampling frequency of 200Hz, a data bit width of 16 bits, and a signal amplitude range of ±2g. The acquired raw data undergoes digital low-pass filtering preprocessing with a cutoff frequency of 20Hz to eliminate high-frequency noise interference. The filtered data is stored in a circular buffer with a size of 1024 sampling points.

[0059] The adaptive sliding time window is implemented using a dual-buffering mechanism: the main buffer is used for current data processing, and the backup buffer is used for data transfer when the window length is adjusted. The initial window length is set to 256 sampling points, the minimum length is limited to 64 sampling points, and the maximum length is limited to 512 sampling points. The window length is adjusted in increments of 32 sampling points.

[0060] The variance change rate is calculated based on data from two adjacent non-overlapping windows. The variance within each window is obtained by subtracting the mean from the sum of squares and dividing by the window length. The variance change rate is defined as the difference between the variance of the subsequent window and the variance of the preceding window, divided by the time interval. The preset variance threshold is set to 0.01g. 2 / s, this threshold can be fine-tuned according to the actual application scenario, and the recommended adjustment range is within 0.005g. 2 / s to 0.02g 2 Between / s.

[0061] The dynamic adjustment strategy for the window length is implemented as follows: When the absolute value of the variance change rate is greater than the preset variance threshold, the window length is reduced by 32 sampling points, indicating that the signal changes rapidly and the time resolution needs to be improved; when the absolute value of the variance change rate is less than the preset variance threshold for 5 consecutive times, the window length is increased by 32 sampling points, indicating that the signal is relatively stable and a smoothing effect can be improved. The adjustment of the window length must be ensured to be within the minimum and maximum range.

[0062] The dynamically calibrated acceleration signal is input to the angle calculation module. The projection of the acceleration signal onto the navigation coordinate system is achieved through coordinate transformation, and the initial value of the transformation matrix is ​​determined by the initial attitude of the accelerometer. The projected values ​​are used to construct a 3×3 attitude matrix, with matrix elements maintained to six decimal places. The attitude matrix is ​​constructed using normalization to ensure its orthogonality.

[0063] The attitude matrix is ​​solved iteratively using Newton's iteration method. The initial value of the iteration is set to the result of the previous solution; if there is no historical data, the identity matrix is ​​used. The iteration terminates when the Euclidean distance between two consecutive iteration results is less than 0.0001, or when the number of iterations reaches the maximum value of 20. Each iteration involves approximately 200 floating-point operations, which takes no more than 1 ms on a 100MHz processor.

[0064] The pitch and roll angles are calculated based on the finally converged attitude matrix. The pitch angle ranges from -90 degrees to +90 degrees, and the roll angle ranges from -180 degrees to +180 degrees. The angles are calculated using the arctangent function and implemented through a lookup table interpolation method. The interpolation table has a size of 1024 points and an angle resolution better than 0.1 degrees.

[0065] The following are verification data from a real-world application case: The input acceleration signal amplitude is 1g, and the signal contains a 0.5Hz sinusoidal oscillation. The maximum value of the rate of change of variance during the oscillation process is 0.015g. 2 / s, minimum value is 0.003g 2 / s. The window length is dynamically adjusted accordingly between 128 and 384 sampling points. The maximum error of the pitch angle after processing is less than 0.5 degrees, and the maximum error of the roll angle is less than 0.8 degrees.

[0066] Data processing employs a single-threaded real-time processing mode with a processing latency of less than 5ms. Anomaly handling strategies include: signal over-range detection, variance calculation overflow protection, and iterative divergence judgment. All intermediate calculation results are stored as double-precision floating-point numbers, and the final output angle value is a single-precision floating-point number. The processing module's resource usage includes: 8KB of program storage space, 4KB of data storage space, and dynamic memory allocation not exceeding 2KB.

[0067] In one optional implementation, the acceleration signal, after nonlinear error compensation and dynamic correction, is substituted into the angle calculation equation. An attitude matrix is ​​constructed based on the projection values ​​of the acceleration signal onto the navigation coordinate system. The pitch and roll angles of the triaxial accelerometer are calculated iteratively by solving the attitude matrix, including:

[0068] The acceleration signal, after nonlinear error compensation and dynamic correction, is projected onto the navigation coordinate system. An attitude matrix is ​​constructed based on the projected values. The pitch and roll angles of the triaxial accelerometer are calculated based on the orthogonal normalization result of the attitude matrix.

[0069] In practical implementation, the raw acceleration signals output by the triaxial accelerometer are acquired. These signals contain errors and external interference. Nonlinear error compensation processing is performed on the accelerometer output signals, primarily considering the nonlinear characteristics of the accelerometer itself. For the output signals in the x, y, and z directions of the triaxial accelerometer, polynomial fitting is used to model the nonlinear errors, establishing a mapping relationship between the accelerometer output and the actual acceleration.

[0070] When performing nonlinear error compensation, a polynomial model is used to compensate for the nonlinear error of the accelerometer. The compensation model can be expressed as a polynomial function of the accelerometer output, and the coefficients of each polynomial are determined through experimental calibration. For each axis, the output signal is closer to the actual acceleration value after compensation.

[0071] After nonlinear error compensation, dynamic correction is performed on the acceleration signal. Dynamic correction primarily addresses the impact of external acceleration on the accelerometer during motion. This step uses a low-pass filter to process the acceleration signal, removing high-frequency noise and transient interference. The filter's cutoff frequency is adjusted based on the specific application scenario, typically selected to effectively remove noise without affecting the frequency range of the useful signal.

[0072] Dynamic calibration also considers the cross-axis sensitivity of the accelerometer during motion, and performs calibration by establishing a coupling model between the three axes. This model is based on the accelerometer's installation error and the sensor's own characteristics, and the calibration coefficient matrix is ​​obtained through experimental calibration to eliminate the mutual influence between the three axes.

[0073] The acceleration signals, after nonlinear error compensation and dynamic correction, more accurately reflect the actual acceleration of the object. These optimized signals are then used in the subsequent attitude calculation process. In attitude calculation, the optimized acceleration signals need to be transformed from the vehicle coordinate system to the navigation coordinate system.

[0074] In a stationary or uniform linear motion state, the accelerometer primarily measures the components of gravitational acceleration along three axes. Projecting these components onto the navigation coordinate system provides the basis for constructing the attitude matrix. Specifically, let the processed triaxial accelerometer signals be ax, ay, and az. Ideally, these signals reflect the projections of gravitational acceleration along the three axes of the carrier coordinate system.

[0075] When constructing the attitude matrix, the acceleration signal is first normalized so that its magnitude equals a gravitational acceleration. The normalized acceleration vector is then used as a column or row of the attitude matrix (depending on the specific definition). Since the attitude matrix is ​​an orthogonal matrix, mathematical methods are needed to ensure that it satisfies the orthogonality property.

[0076] An iterative method is used to solve for the attitude matrix. After initializing the attitude matrix, optimization algorithms such as least squares or gradient descent are used to continuously adjust the matrix elements to minimize the error between the calculated acceleration projection and the actual measured value. During the iteration process, the attitude matrix needs to be orthogonally normalized after each iteration to ensure that it satisfies the property of an orthogonal matrix.

[0077] Orthogonal normalization can be achieved using the Schmitt orthogonalization method, which processes each column of the matrix to ensure that the column vectors are mutually orthogonal and have a magnitude of 1. Through multiple iterations, a satisfactory attitude matrix is ​​finally obtained.

[0078] Based on the attitude matrix obtained from the final solution, the pitch and roll angles of the triaxial accelerometer can be calculated. Let the attitude matrix be R, and its elements be Rij (i, j=1, 2, 3). Then the pitch angle θ and roll angle φ can be calculated as follows: the pitch angle θ equals arcsin(-R31); the roll angle φ equals arctan(R32 / R33).

[0079] In practical applications, to improve the accuracy of the calculation, data from other sensors, such as gyroscope data, can be fused using complementary filtering or Kalman filtering. This can overcome the limitations of using accelerometers alone for attitude calculation, especially in the presence of external acceleration interference.

[0080] Before performing attitude calculation, it is recommended to perform static detection on the acceleration signal to determine whether the current state is one of rest or uniform linear motion. Only in these states will the accelerometer measure primarily gravitational acceleration, resulting in more accurate attitude calculations. Static detection can be performed by calculating the standard deviation or variance of the acceleration signal within a short time window; if this deviation is less than a preset threshold, the state is considered static.

[0081] Through the above steps, accurate calculation of pitch and roll angles from triaxial accelerometer signals is achieved, providing reliable attitude information for various inertial navigation, attitude control and other applications.

[0082] The method further includes: The accelerometer acquires triaxial acceleration data in real time at a sampling frequency of 200Hz. The signal is acquired via a 16-bit analog-to-digital converter. The acceleration data is preprocessed using an IIR low-pass filter with a cutoff frequency of 20Hz to suppress high-frequency noise. The filtered data is then fed into a circular buffer with a size of 1024 points.

[0083] The nonlinear error compensation model is based on polynomial fitting. The input to the compensation model is the original triaxial acceleration values, and the output is the corrected acceleration values. The model coefficients are obtained through offline calibration using the least squares method. During calibration data acquisition, the accelerometer is placed on a precision turntable for ±1g scanning. The calibration temperature is 25℃, and the number of calibration data sets is no less than 1000 sets.

[0084] The adaptive sliding window implementation employs a dual-buffer structure: the main buffer is used for current data processing, and the backup buffer is used for data transfer during window length adjustments. The initial window length is 256 points, the minimum length is 64 points, and the maximum length is 512 points. The window length adjustment step is 32 points, and the adjustment period is 1 second.

[0085] The variance change rate is calculated based on data from two adjacent non-overlapping windows. The variance of the data within each window is obtained by subtracting the mean, summing the squares, and then dividing by the window length. The variance change rate is defined as the difference between the variance of the subsequent window and the variance of the preceding window, divided by the time interval. The preset variance threshold is 0.01g. 2 / s, this threshold can be set between 0.005-0.02g depending on the actual application scenario. 2 Adjust within the range of / s.

[0086] Window length dynamic adjustment strategy: When the absolute value of the variance change rate is greater than a preset threshold, the window length is reduced by 32 points; when the absolute value of the variance change rate is less than the threshold for 5 consecutive times, the window length is increased by 32 points. The window length change must be ensured to be within the range of minimum and maximum values.

[0087] The dynamically corrected acceleration signal is input to the angle calculation module. The acceleration signal is projected onto the navigation coordinate system through a coordinate transformation matrix, the initial value of which is determined by the initial attitude. The projected values ​​are used to construct a 3×3 attitude matrix, with matrix elements maintained to 6 decimal places. The attitude matrix construction employs normalization to ensure orthogonality.

[0088] The attitude matrix is ​​solved iteratively using a modified Newton's method. The initial value for each iteration is the result of the previous solution; if no historical data is available, the identity matrix is ​​used. The iteration terminates when the Euclidean distance between two consecutive results is less than 0.0001, or when the number of iterations reaches the maximum value of 20. Each iteration involves approximately 200 floating-point operations, and the computation time on a 100MHz processor is less than 1ms.

[0089] Pitch and roll angles are calculated based on the finally converged attitude matrix. The pitch angle ranges from -90 to +90 degrees, and the roll angle ranges from -180 to +180 degrees. The angles are calculated using the arctangent function and interpolated through a 1024-point lookup table, achieving an angle resolution better than 0.1 degrees.

[0090] The actual application verification data is as follows: the input acceleration signal amplitude is 1g, containing a 0.5Hz sinusoidal oscillation, and the maximum variance change rate is 0.015g. 2 / s, minimum value 0.003g 2 / s. The window length is dynamically adjusted between 128 and 384 points. The maximum error of the pitch angle after processing is less than 0.5 degrees, and the maximum error of the roll angle is less than 0.8 degrees.

[0091] Data processing employs a single-threaded real-time mode with a processing latency of less than 5ms. Anomaly handling includes signal over-range detection, variance calculation overflow protection, and iterative divergence detection. Intermediate calculation results are processed using double-precision floating-point numbers, while the final angle output is a single-precision floating-point number. Processing module resource usage: 8KB program storage, 4KB data storage, and less than 2KB of dynamic memory.

[0092] A second aspect of the present invention provides an angle calculation system based on a triaxial accelerometer, comprising:

[0093] The first unit is used to acquire the acceleration signal from the triaxial accelerometer, preprocess the acceleration signal to obtain the effective acceleration components of each axis, construct a nonlinear error feature vector based on the effective acceleration components, identify the parameters of the nonlinear error feature vector in real time using the recursive least squares method, and establish a nonlinear error compensation model.

[0094] The second unit is used to dynamically correct the output of the nonlinear error compensation model using an adaptive sliding time window. The acceleration signal after nonlinear error compensation and dynamic correction is substituted into the angle calculation equation to calculate the pitch and roll angles of the triaxial accelerometer, and the pitch and roll angles are output as the final angle calculation results.

[0095] A third aspect of the present invention provides an electronic device, comprising: processor;

[0096] Memory used to store processor-executable instructions;

[0097] The processor is configured to invoke instructions stored in the memory to execute the aforementioned method.

[0098] A fourth aspect of the present invention provides a computer-readable storage medium having stored thereon computer program instructions that, when executed by a processor, implement the aforementioned method.

[0099] This invention can be a method, apparatus, system, and / or computer program product. The computer program product may include a computer-readable storage medium having computer-readable program instructions loaded thereon for performing various aspects of the invention.

[0100] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. An angle calculation method based on a triaxial accelerometer, characterized in that, include: Acquire acceleration signals from a triaxial accelerometer, preprocess the acceleration signals, and obtain the effective acceleration components for each axis; Based on the effective acceleration components, a nonlinear error feature vector is constructed. The nonlinear error feature vector is then used for real-time parameter identification via recursive least squares method to establish a nonlinear error compensation model, including: The acceleration measurement value of the inertial navigation system is obtained, and the acceleration measurement value is filtered by digital filtering to extract the effective acceleration component from the acceleration measurement value; Based on the effective acceleration components, a Taylor series expansion is performed on the effective acceleration components and their corresponding temperature values ​​to construct a nonlinear error feature vector containing acceleration nonlinear terms, temperature nonlinear terms and their cross terms. The coefficients of second-order and higher nonlinear terms are obtained by Taylor series expansion of the nonlinear error feature vector. The recursive least squares method is used to identify the nonlinear error feature vector in real time. The historical data in the parameter identification process is weighted by introducing a time-related forgetting factor. A nonlinear error compensation model is established based on the identification results. An adaptive sliding time window is used to dynamically correct the output of the nonlinear error compensation model. The acceleration signal after nonlinear error compensation and dynamic correction is substituted into the angle calculation equation to calculate the pitch and roll angles of the triaxial accelerometer. The pitch and roll angles are then output as the final angle calculation results.

2. The method according to claim 1, characterized in that, The nonlinear error feature vector is identified in real time using the recursive least squares method. Historical data during the parameter identification process is weighted by introducing a time-dependent forgetting factor. A nonlinear error compensation model is established based on the identification results, including: The feature vector of the system to be identified is obtained, and the recursive least squares method is used to identify the parameters of the feature vector in real time. The recursive least squares method is calculated based on the exponential forgetting factor, wherein the value of the exponential forgetting factor is dynamically adjusted according to the data time interval to realize the differentiated weighting of historical data. A nonlinear error compensation model is constructed based on the result of the parameter identification.

3. The method according to claim 1, characterized in that, An adaptive sliding time window is used to dynamically correct the output of the nonlinear error compensation model. The acceleration signal after nonlinear error compensation and dynamic correction is substituted into the angle calculation equation to calculate the pitch and roll angles of the triaxial accelerometer. The pitch and roll angles are output as the final angle calculation results, including: The output signal of the nonlinear error compensation model is obtained, and an adaptive sliding time window is constructed to dynamically correct the output signal. The length of the adaptive sliding time window is determined by calculating the variance change rate of the output signal. When the variance change rate is greater than a preset variance threshold, the window length is shortened, and when the variance change rate is less than the preset variance threshold, the window length is extended to achieve adaptive correction of the output signal. The acceleration signal, after nonlinear error compensation and dynamic correction, is substituted into the angle calculation equation. An attitude matrix is ​​constructed based on the projection value of the acceleration signal on the navigation coordinate system. The pitch and roll angles of the triaxial accelerometer are calculated by iteratively solving the attitude matrix. The pitch and roll angles are then output as the final angle calculation results.

4. The method according to claim 3, characterized in that, Substituting the acceleration signal, after nonlinear error compensation and dynamic correction, into the angle calculation equation, and constructing an attitude matrix based on the projection value of the acceleration signal in the navigation coordinate system, the pitch and roll angles of the triaxial accelerometer are calculated iteratively by solving the attitude matrix, including: The acceleration signal, after nonlinear error compensation and dynamic correction, is projected onto the navigation coordinate system. An attitude matrix is ​​constructed based on the projected values. The pitch and roll angles of the triaxial accelerometer are calculated based on the orthogonal normalization result of the attitude matrix.

5. An angle calculation system based on a triaxial accelerometer, used to implement the method of any one of claims 1-4, characterized in that, include: The first unit is used to acquire the acceleration signal from the triaxial accelerometer, preprocess the acceleration signal, and acquire the effective acceleration components of each axis. Based on the effective acceleration components, a nonlinear error feature vector is constructed. The nonlinear error feature vector is then used to identify parameters in real time through the recursive least squares method to establish a nonlinear error compensation model. The second unit is used to dynamically correct the output of the nonlinear error compensation model using an adaptive sliding time window. The acceleration signal after nonlinear error compensation and dynamic correction is substituted into the angle calculation equation to calculate the pitch and roll angles of the triaxial accelerometer, and the pitch and roll angles are output as the final angle calculation results.

6. An electronic device, characterized in that, include: processor; Memory used to store processor-executable instructions; The processor is configured to invoke instructions stored in the memory to execute the method according to any one of claims 1 to 4.

7. A computer-readable storage medium having computer program instructions stored thereon, characterized in that, When the computer program instructions are executed by the processor, they implement the method described in any one of claims 1 to 4.

Images