CN116482982A - A Complementary Filter Gain Adjustment Method Based on KNN Classifier - Google Patents

Abstract

The invention relates to the technical field of attitude control, in particular to a complementary filter gain adjustment method based on a KNN classifier, which comprises the following steps: in a model training stage, acquiring triaxial accelerometer data; calculating an accelerometer output module difference value and a variance value at each moment; constructing a data set containing a modulus value, a variance value and an adaptive threshold; inputting training set data into a KNN algorithm so as to construct a KNN model; in a real-time attitude calculation stage, calculating a modulus value and a variance value by utilizing triaxial accelerometer data acquired in real time, and then inputting the modulus value and the variance value into a KNN model to obtain a self-adaptive threshold; multiplying the adaptive threshold by the scalar gain to obtain a correction gain; substituting the correction gain into a final attitude estimation formula to calculate the attitude. The invention solves the problem that the accuracy and reliability of the gesture resolving are insufficient due to the defect that the scale gain can not be accurately and adaptively adjusted when the gesture resolving is carried out by the traditional complementary filtering algorithm.

Description

Complementary filter gain adjustment method based on KNN classifier

Technical Field

The invention relates to the technical field of attitude control, in particular to a complementary filter gain adjustment method based on a KNN classifier.

Background

The gesture resolving has the meaning that the direction or gesture of the object in the three-dimensional space is deduced through a series of measured values, and a basis is provided for operations such as control, navigation and positioning. The method is widely applied in the fields of robots, aircrafts, navigation, military, industry and the like, and becomes an indispensable technology in many fields.

Inertial navigation is a tool for measuring the position, speed and attitude of an object through integral calculation based on inertial sensors such as accelerometers and gyroscopes.

The inertial navigation is utilized to carry out gesture calculation, and the method has the advantages of availability in all weather, high precision, high real-time performance and no need of external signals, so that the inertial navigation is widely applied to the fields of aviation, navigation, robots and the like, and the technical progress and the efficiency are improved.

In the gesture resolving, the complementary filtering algorithm is a common gesture resolving method, and has the advantages that multiple sensors can be adopted to fuse data, so that the accuracy of gesture resolving is improved. However, complementary filtering algorithms also suffer from the disadvantage of gain adjustment, mainly in terms of:

(1) Sensitivity to noise: the application of gain adjustment also increases the effects of sensor bias and measurement errors, making the attitude solution more sensitive to sensor noise, which reduces the accuracy and stability of the attitude solution, thus requiring more stringent noise control.

(2) It is difficult to achieve adaptive adjustment: the gain adjustment in the complementary filtering algorithm is fixed and cannot be adaptively and automatically adjusted according to different scenes and applications; the fixed gain adjustment method is difficult to adapt to complex and changeable environments, and meanwhile, the problem of poor adjustability is caused.

Disclosure of Invention

Aiming at the defects of the existing method, the invention solves the problems of the existing complementary filtering algorithm that the gesture is solved to obtain scalar gain and the gesture is solved to have insufficient accuracy and reliability.

The technical scheme adopted by the invention is as follows: the complementary filter gain adjustment method based on the KNN classifier comprises the following steps:

step one, in a model training stage, acquiring triaxial accelerometer output data at each moment;

step two, calculating a module difference value and a variance value of the accelerometer at the moment i after removing the mean value;

further, the calculation formula of the modulus value is:

wherein a is _xi 、a _yi 、a _zi Measurements of the accelerometer x, y and z axes at time i, a, respectively _xk 、a _yk 、a _zk The measurements of the accelerometer x-axis, y-axis and z-axis at time k, respectively.

Further, the formula of the variance value is:

wherein, ||a _k The value of the acceleration modulus at the moment k is the value of the acceleration modulus at the moment k,the acceleration data module value at the moment i; n is the total number of samples, 1 =<i<＝N。

Marking self-adaptive thresholds for the modulus difference value and the variance value at each moment, and constructing a data set containing the modulus difference value, the variance value and the self-adaptive thresholds; dividing the training set into a training set and a testing set;

step four, after the nearest neighbor number K value is preset, inputting training set data into a KNN algorithm to construct a KNN model, wherein the model is used for realizing the self-adaptive adjustment of scalar gain in a subsequent gesture resolving stage shown in the following steps;

calculating a modulus value and a variance value by utilizing triaxial acceleration data acquired in real time, and inputting the modulus value and the variance value into a KNN model to obtain a self-adaptive threshold value; multiplying the adaptive threshold α by a scalar gain K _k Obtain the correction gain K' _k The method comprises the steps of carrying out a first treatment on the surface of the Will K' _k Substituting the calculated pose into a final pose estimation formula, and calculating the pose.

Further, the final pose estimation formula is:

q _k+1/k+1 ＝(1-K' _k )q _k+1/k +K' _k q _m (5)

wherein q _k+1/k+1 For final pose estimation at sampling time k+1, q _k+1/k Is an attitude estimation obtained by fusing the measured values of the gyroscope g, q _m Is an attitude estimate, K 'obtained by fusing measurements of accelerometer a and magnetometer m' _k Is the correction gain.

The invention has the beneficial effects that:

1. the KNN model is built to correct the scalar gain, and the method is simple;

2. and the gesture solutions of the correction gain and the scalar gain are compared with yaw rotation, pitch rotation and roll rotation indexes, so that the accuracy is obviously improved.

Drawings

FIG. 1 is a flow chart of a complementary filter gain adjustment method based on a KNN classifier of the present invention;

FIG. 2 is a yaw rotation-pitch rotation-roll rotation diagram for simple motion under scalar gain conditions;

FIG. 3 is a yaw-pitch-roll rotation plot for simple motion under modified gain conditions;

FIG. 4 is a yaw rotation-pitch rotation-roll rotation plot for complex rotations under scalar gain conditions;

FIG. 5 is a yaw rotation-pitch rotation-roll rotation diagram corresponding to complex rotations under modified gain conditions.

Detailed Description

The invention will be further described with reference to the accompanying drawings and examples, which are simplified schematic illustrations showing only the basic structure of the invention and thus showing only those constructions that are relevant to the invention.

FIG. 1 is a gesture resolving flow chart, and the improvement part is shown by a dotted line box; the final pose estimation formula of the existing pose calculation:

q _k+1/k+1 ＝(1-K _k )q _k+1/k +K _k q _m (1)

wherein q _k+1/k+1 For final pose estimation at sampling time k+1, q _k+1/k Is an attitude estimation obtained by fusing the measured values of the gyroscope g, q _m Is an attitude estimate, K, obtained by fusing measurements of accelerometer a and magnetometer m _k Is a scalar gain.

Scalar gain K _k The calculation of (c) may employ a complementary filtering algorithm (CF), madgwick et al, describe K _k The method comprises the following steps:

wherein,,for maximum measurement error of each axis of the gyroscope, < >>Is a fixed value; it can be seen that the scalar gain K when interference is present _k Failing to adjust, the final pose estimate is subject to error.

Measurement noise, such as an accelerometer, is very loud, due to scalar gain K of CF when linear acceleration is present _k Is a fixed value, resulting in a large attitude solution error.

The scalar gain needs to be corrected when there is measurement noise in the accelerometer.

The complementary filter gain adjustment method based on the KNN classifier comprises the following steps:

the test data source adopts data acquired by the 9-axis integrated MPU9150, the sampling rate of a sensor is 100Hz, the test is carried out by using an industrial robot, and the robot can search the direction and the position of an IMU unit under 15Hz, so that the attitude true value is determined; the 9-axis includes a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer.

Step one, acquiring triaxial accelerometer data;

the measurement values of the x axis, the y axis and the z axis of the accelerometer at the moment i and the moment k are respectively a _xi 、a _yi 、a _zi And a _xk 、a _yk 、a _zk ；

Step two, calculating the addition of the moment iModel difference M after removing mean value by speedometer _i Sum of variances S _i ；

The calculation formula of the i moment modulus difference value is as follows:

the calculation formula of the moment variance value i is as follows:

wherein, ||a _k The value of the acceleration modulus at the moment k is the value of the acceleration modulus at the moment k,the acceleration data module value at the moment i; n is the total number of samples, 1 =<i<N, this embodiment n=20.

Marking self-adaptive thresholds for the modulus difference value and the variance value at each moment, and constructing a data set containing the modulus difference value, the variance value and the self-adaptive thresholds; dividing the training set into a training set and a testing set, wherein the ratio of the testing set to the training set is 1:9;

in the embodiment, the adaptive threshold is divided into two types, the adaptive threshold is set to 1 when no noise exists, and the adaptive threshold alpha is set to 0.002 when noise exists; the adaptive threshold α may be set in a fuzzy manner according to the noise-containing condition of the acceleration signal, for example, the corresponding adaptive threshold may be set according to the severity of the noise.

And fourthly, setting the K value to be 3, inputting training set data into a KNN algorithm, constructing a KNN model, and verifying the KNN model through testing set data.

Calculating a modulus value and a variance value by utilizing triaxial acceleration data acquired in real time, and then inputting the modulus value and the variance value into a KNN model to obtain a self-adaptive threshold; multiplying the adaptive threshold α by a scalar gain K _k Obtain the correction gain K' _k The method comprises the steps of carrying out a first treatment on the surface of the Will K' _k And (5) carrying out resolving gesture in a final gesture estimation formula corrected by self substitution.

q _k+1/k+1 ＝(1-K' _k )q _k+1/k +K' _k q _m (5)

Wherein q _k+1/k+1 For final pose estimation at sampling time k+1, q _k+1/k Is an attitude estimation obtained by fusing the measured values of the gyroscope g, q _m Is an attitude estimate, K 'obtained by fusing measurements of accelerometer a and magnetometer m' _k Is the correction gain.

The industrial robot is utilized to carry out resolving gesture verification, and the industrial robot can search the direction and the position of the IMU unit at 15Hz so as to determine the gesture true value.

The experimental effects of scalar gain and correction gain are compared by yaw rotation-pitch rotation-roll rotation index.

Analyzing results through the quaternion total error E; wherein q is ₁ To calculate the quaternion first value, q _1ABB A first value that is a reference quaternion; q ₂ To calculate the quaternion second value, q _2ABB A second value that is a reference quaternion; q ₃ To calculate the third value of the quaternion, q _3ABB A third value that is a reference quaternion; q ₄ To calculate the fourth value of the quaternion, q _2ABB And is the fourth value of the reference quaternion.

As is apparent from comparing fig. 2 and 3, the posture is greatly improved in the interval of 10-20 seconds, and particularly, the error is greatly reduced for pitch and yaw angles, which indicates that the correction gain posture adjustment method is effective.

The quaternion total error of the scalar gain for the simple motion pose solution in fig. 2 is 800.9918; the quaternion total error of the correction gain for the simple motion pose solution in fig. 3 is 329.4117.

The quaternion total error for the scalar gain of fig. 4 is 2754.3913; the quaternion total error of the correction gain in fig. 5 is 2475.3176.

Comparing fig. 3 and 4, it can be seen that the corrected gain attitude adjustment curve is smoother and more consistent with the true value, especially at 30-40 seconds.

The total error of quaternion of correction gain is also greatly reduced, which indicates that the method can effectively improve the accuracy of gesture calculation.

With the above-described preferred embodiments according to the present invention as an illustration, the above-described descriptions can be used by persons skilled in the relevant art to make various changes and modifications without departing from the scope of the technical idea of the present invention. The technical scope of the present invention is not limited to the description, but must be determined according to the scope of claims.

Claims (4)

1. The complementary filter gain adjustment method based on the KNN classifier is characterized by comprising the following steps of:

step one, acquiring triaxial accelerometer data;

step two, calculating an output module difference value and a variance value of the accelerometer at each moment after removing the mean value;

marking the self-adaptive threshold value for the modulus difference value and the variance value at each moment, and constructing a data set containing the modulus difference value, the variance value and the self-adaptive threshold value;

step four, presetting a nearest neighbor number K value, and inputting training set data into a KNN algorithm so as to construct a KNN model;

step five, in the real-time attitude resolving stage, calculating a modulus value and a variance value by utilizing three-axis acceleration data acquired in real time, and inputting a KNN model to obtain a self-adaptive threshold value; multiplying the adaptive threshold by the scalar gain to obtain a correction gain; and substituting the correction gain into a final attitude estimation formula to perform real-time attitude calculation.

2. The KNN classifier-based complementary filter gain adjustment method of claim 1, wherein the modulus value is calculated as:

wherein a is _xi 、a _yi 、a _zi Measurements of the accelerometer x, y and z axes at time i, a, respectively _xk 、a _yk 、a _zk The measurements of the accelerometer x-axis, y-axis and z-axis at time k, respectively.

3. The KNN classifier-based complementary filter gain adjustment method of claim 1, wherein the variance value is calculated by the formula:

wherein,,for the acceleration data module value at the moment i, ||a _k The I is the acceleration module value at the moment 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 pose estimation formula is:

q _k+1/k+1 ＝(1-K' _k )q _k+1/k +K' _k q _m (5)

wherein q _k+1/k+1 For final pose estimation at sampling time k+1, q _k+1/k Is an attitude estimation obtained by fusing the measured values of the gyroscope g, q _m Is an attitude estimate, K 'obtained by fusing measurements of accelerometer a and magnetometer m' _k Is the correction gain.