A method for adjusting the gain of complementary filters based on KNN classifiers

Abstract

This invention relates to the field of attitude control technology, and more particularly to a complementary filter gain adjustment method based on a KNN classifier. The method includes: during the model training phase, acquiring triaxial accelerometer data; calculating the accelerometer output modulus and variance values ​​at each time step; constructing a dataset containing the modulus, variance, and adaptive threshold; inputting the training data into the KNN algorithm to construct a KNN model; during the real-time attitude calculation phase, calculating the modulus and variance values ​​using the real-time acquired triaxial accelerometer data, and then inputting them into the KNN model to obtain the adaptive threshold; multiplying the adaptive threshold by a scalar gain to obtain a corrected gain; and substituting the corrected gain into the final attitude estimation formula to calculate the attitude. This invention addresses the deficiency of existing complementary filter algorithms in accurately and adaptively adjusting the scalar gain during attitude calculation, leading to insufficient accuracy and reliability in attitude calculation.

Description

Technical Field

[0001] This invention relates to the field of attitude control technology, and in particular to a method for adjusting the gain of a complementary filter based on a KNN classifier. Background Technology

[0002] Attitude calculation is significant because it derives the orientation or attitude of an object in three-dimensional space from a series of measurements, providing a foundation for operations such as control, navigation, and positioning. It has wide applications in robotics, aircraft, navigation, and military and industrial fields, and has become an indispensable technology in many areas.

[0003] Inertial navigation is a tool that uses inertial sensors such as accelerometers and gyroscopes to measure the position, velocity, and attitude of an object through integral calculations.

[0004] Inertial navigation is used for attitude calculation, which has the advantages of being available in all weather conditions, high precision, high real-time performance, and no need for external signals. This has made inertial navigation widely used in aviation, navigation, robotics and other fields, bringing about technological progress and efficiency improvement.

[0005] In attitude assessment, complementary filtering is a commonly used method. Its advantage lies in its ability to fuse data from multiple sensors, improving the accuracy of attitude calculation. However, complementary filtering also has limitations in gain adjustment, mainly in the following aspects:

[0006] (1) Sensitive to noise: The application of gain adjustment will also increase the influence of sensor deviation and measurement error, making the attitude calculation results more sensitive to sensor noise. This will reduce the accuracy and stability of attitude calculation, so more stringent noise control is required.

[0007] (2) Difficult to achieve adaptive adjustment: The gain adjustment in the complementary filtering algorithm is fixed and cannot be automatically adjusted according to different scenarios and applications. The fixed gain adjustment method is difficult to adapt to complex and ever-changing environments, and also leads to poor adjustability. Summary of the Invention

[0008] To address the shortcomings of existing methods, this invention solves the defects of existing complementary filtering algorithms in scalar gain calculation for attitude determination, resulting in insufficient accuracy and reliability in attitude determination.

[0009] The technical solution adopted in this invention is: a method for adjusting the gain of a complementary filter based on a KNN classifier, comprising the following steps:

[0010] Step 1: During the model training phase, collect the output data from the triaxial accelerometer at each time step;

[0011] Step 2: Calculate the modulus and variance of the accelerometer at time i after removing the mean;

[0012] Furthermore, the formula for calculating the modulus difference is:

[0013]

[0014] Among them, a xi a yi a zi Let a be the measured values ​​of the accelerometer along the x, y, and z axes at time i, respectively. xk a yk a zk These are the measurements of the accelerometer along the x, y, and z axes at time k, respectively.

[0015] Furthermore, the formula for calculating the variance is:

[0016]

[0017] Among them, ||a k || represents the acceleration magnitude at time k. Let N be the magnitude of the acceleration data at time i; N is the total number of samples, 1 = <i<=N。

[0018] Step 3: Label the modulus and variance values ​​at each time step with adaptive thresholds, construct a dataset containing the modulus, variance, and adaptive thresholds; and divide it into training and test sets.

[0019] Step 4: After pre-setting the number of nearest neighbors K, input the training set data into the KNN algorithm to build the KNN model. This model is used to achieve adaptive adjustment of scalar gain in the subsequent pose calculation stage shown in the following steps.

[0020] Step 5: Calculate the modulus and variance values ​​using the real-time acquired triaxial acceleration data, input them into the KNN model, and obtain the adaptive threshold; multiply the adaptive threshold α by the scalar gain K. k Obtain the corrected gain K' k ; will K' k Substitute the values ​​into the final attitude estimation formula to calculate the attitude.

[0021] Furthermore, the final attitude estimation formula is as follows:

[0022] q k+1 / k+1 =(1-K') k )q k+1 / k +K' k q m (5)

[0023] Where, q k+1 / k+1 For the final attitude estimation at sampling time k+1, q k+1 / kThe attitude estimate is obtained by fusing the measurements from the gyroscope g, q m The attitude estimate, K', is obtained by fusing measurements from accelerometer a and magnetometer m. k It is a gain correction.

[0024] The beneficial effects of this invention are:

[0025] 1. Constructing a KNN model to correct the scalar gain is a simple method;

[0026] 2. The attitude calculations of the corrected gain and scalar gain are compared with the yaw rotation, pitch rotation and roll rotation indices, and the accuracy is significantly improved. Attached Figure Description

[0027] Figure 1 This is a flowchart of the complementary filter gain adjustment method based on KNN classifier of the present invention;

[0028] Figure 2 It is a diagram of yaw rotation, pitch rotation, and roll rotation corresponding to simple motion under scalar gain conditions.

[0029] Figure 3 This is a diagram of yaw rotation, pitch rotation, and roll rotation corresponding to simple motion under modified gain conditions.

[0030] Figure 4 This is a diagram of yaw rotation, pitch rotation, and roll rotation corresponding to complex rotations under scalar gain conditions.

[0031] Figure 5 It is a diagram of yaw rotation, pitch rotation, and roll rotation corresponding to complex rotation under modified gain conditions. Detailed Implementation

[0032] The present invention will be further described below with reference to the accompanying drawings and embodiments. The drawings are simplified schematic diagrams, which only illustrate the basic structure of the present invention in a schematic manner, and therefore only show the components related to the present invention.

[0033] like Figure 1 The flowchart for attitude calculation is shown below, with the improved parts indicated by the dashed boxes; the final attitude estimation formula for the existing attitude calculation is as follows:

[0034] q k+1 / k+1 = (1-K) k )q k+1 / k +K k q m (1)

[0035] Where, q k+1 / k+1 For the final attitude estimation at sampling time k+1, q k+1 / k The attitude estimate is obtained by fusing the measurements from the gyroscope g, q mThe attitude estimate, K, is obtained by fusing measurements from accelerometer a and magnetometer m. k It is a scalar gain.

[0036] scalar gain K k The calculation can be performed using the complementary filtering algorithm (CF). Madgwick et al. used K... k Set to:

[0037]

[0038] in, This represents the maximum measurement error of each axis of the gyroscope. It is a fixed value; it can be seen that when interference exists, the scalar gain K k The inability to adjust leads to errors in the final attitude estimation.

[0039] For example, the accelerometer has very high measurement noise, and when linear acceleration exists, due to the scalar gain K of CF... k The fixed value leads to a large attitude calculation error.

[0040] Therefore, when accelerometers have measurement noise, the scalar gain needs to be corrected.

[0041] A method for adjusting the gain of a complementary filter based on a KNN classifier includes the following steps:

[0042] The test data was collected by a 9-axis integrated MPU9150 with a sensor sampling rate of 100Hz. The test was conducted using an industrial robot, which can retrieve the orientation and position of the IMU unit at 15Hz to determine the true attitude value. The 9 axes include a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer.

[0043] Step 1: Collect data from the triaxial accelerometer;

[0044] Let the accelerometer readings on the x, y, and z axes at times i and k be a, , and , respectively. xi a yi a zi , and a xk a yk a zk ;

[0045] Step 2: Calculate the modulus difference M of the accelerometer at time i after removing the mean. i S and variance i ;

[0046] The formula for calculating the modulus difference at time i is:

[0047]

[0048] The formula for the variance value at time i is as follows:

[0049]

[0050] where ||a k || is the acceleration modulus value at time k, is the acceleration data modulus value at time i; N is the total number of samples, 1 <= i <= N, and in this embodiment, N = 20.

[0051] Step 3: Mark the adaptive thresholds for the modulus difference and variance value at each moment, and construct a data set including the modulus difference, variance value, and adaptive threshold; and divide it into a training set and a test set, and the ratio of the test set to the training set is 1:9;

[0052] In this embodiment, the adaptive threshold is divided into two categories. When there is no noise, the adaptive threshold is set to 1, and when there is noise, the adaptive threshold α is set to 0.002; the adaptive threshold α can be set fuzzily according to the noise situation of the acceleration signal. For example, the corresponding adaptive threshold can be set according to the severity of the noise.

[0053] Step 4: Set the value of K to 3, then input the training set data into the KNN algorithm to construct a KNN model, and verify the KNN model through the test set data.

[0054] Step 5: Calculate the modulus difference and variance value using the real-time collected three-axis acceleration data, and then input them into the KNN model to obtain the adaptive threshold; multiply the adaptive threshold α by the scalar gain K k to obtain the corrected gain K' k ; substitute K' k into the corrected final attitude estimation formula for attitude calculation.

[0055] q k+1 / k+1 =(1 - K' k )q k+1 / k + K' k q m (5)

[0056] where q k+1 / k+1 is the final attitude estimation at sampling time k + 1, q k+1 / k is the attitude estimation obtained by fusing the measurement value of the gyroscope g, q m is the attitude estimation obtained by fusing the measurement values of the accelerometer a and the magnetometer m, and K' k is the corrected gain.

[0057] [[ID=..]]Use an industrial robot to verify the attitude calculation. The industrial robot can retrieve the orientation and position of the IMU unit at 15 Hz to determine the true attitude value.

[0058] The experimental results of scalar gain and modified gain were compared using yaw-pitch-roll rotation indices.

[0059] The results are analyzed using the total error E of the quaternion; where q1 is the first value of the calculated quaternion, q 1ABB q is used to reference the first value of the quaternion; q2 is used to calculate the second value of the quaternion. 2ABB q is used to refer to the second value of the quaternion; q3 is used to calculate the third value of the quaternion. 3ABB The third value of the quaternion is used as a reference; q4 is used to calculate the fourth value of the quaternion, q 2ABB The fourth value of the reference quaternion.

[0060]

[0061] contrast Figure 2 and Figure 3 It is evident that the attitude is greatly improved in the 10-20 second range, especially for pitch and yaw angles, where the error is significantly reduced, indicating that the modified gain attitude adjustment method is indeed effective.

[0062] Figure 2 The total quaternion error of the scalar gain in the simple motion attitude calculation is 800.9918. Figure 3 The total quaternion error of the corrected gain for simple motion attitude calculation is 329.4117.

[0063] Figure 4 The total quaternion error of the scalar gain is 2754.3913; Figure 5 The total error of the quaternion for the corrected gain is 2475.3176.

[0064] contrast Figure 3 and Figure 4 It can be seen that the corrected gain attitude adjustment curve is smoother and more in line with the true value, especially noticeable at the 30-40 second mark.

[0065] The total error of the quaternion of the corrected gain was also significantly reduced, indicating that this method can effectively improve the accuracy of attitude calculation.

[0066] Based on the above-described preferred embodiments of the present invention, and through the foregoing description, those skilled in the art can make various changes and modifications without departing from the inventive concept. The technical scope of this invention is not limited to the contents of the specification, but must be determined according to the scope of the claims.

Claims

1. A complementary filter gain adjustment method based on KNN classifier, characterized in that, The method comprises the following steps: Step one, collecting triaxial accelerometer data; Step two, calculating the output module difference value and variance value of the accelerometer after removing the mean value at each time; Step three, marking an adaptive threshold value for the module difference value and variance value at each time, and constructing a data set containing the module difference value, variance value and adaptive threshold value; Step four, presetting the number K of nearest neighbors, inputting the training set data into the KNN algorithm, and thereby constructing a KNN model; Step five, in the real-time attitude solving stage, using the real-time collected triaxial acceleration data to calculate the module difference value and variance value, and inputting the KNN model to obtain the adaptive threshold value; The adaptive threshold value is multiplied by a scalar gain to obtain a correction gain; the correction gain is substituted into the final attitude estimation formula to solve the attitude in real time.

2. The KNN classifier based complementary filter gain adjustment method of claim 1, wherein, The calculation formula of the module difference value is: wherein a xi , a yi , a zi are the measured values of the accelerometer x-axis, y-axis and z-axis at the i time instant, respectively, a xk , a yk , a zk are the measured values of the accelerometer x-axis, y-axis and z-axis at the k time instant, respectively.

3. The KNN classifier based complementary filter gain adjustment method of claim 1, wherein, The calculation formula of the variance value is: wherein, is the acceleration data module value at time i, ||a k is the acceleration module value at time k; N is the total number of samples, 1 = < i <= N.

4. The KNN classifier based complementary filter gain adjustment method of claim 1, wherein, The final attitude estimation formula is: q k+1 / k+1 = (1 - K k )q k+1 / k + K k q m (5) where q k+1 / k+1 is the final pose estimate at sample time k + 1, q k+1 / k is the pose estimate obtained by fusing the measurements of the gyroscope g, q m is the pose estimate obtained by fusing the measurements of the accelerometer a and the magnetometer m, and K' k is the correction gain.

Images