KR101147133B1 - Complementary filter for angle measuring - Google Patents

Abstract

PURPOSE: A complementary filter for angle measurement and a method thereof are provided to accurately measure an angle with respect to the movements of a subject and prevent a delay of response speed. CONSTITUTION: A calculation device calculates a filter factor using a least square method. A low pass filter filters output values of a first sensor. A high pass filter filters output values of a second sensor. An adding device(220) adds up the output values of the low pass filter and the high pass filter. An integrating device(240) calculates an angle with respect to the movements of a subject by integrating results outputted from the adding device.

Description

Complementary filter for angle measurement and its method {Complementary Filter for Angle Measuring}

The present invention relates to a complementary filter for angular measurement and a method thereof, and more specifically, to apply a low pass filter and a high pass filter of order n (where n> = 1), and apply a least-square method to complement the order n of complement. The present invention relates to a complementary filter for angular measurement for measuring an angle with respect to the movement of a subject by forming a filter, and a method thereof.

Mobile robots such as unmanned aerial vehicles and balancing robots, which cannot use sensors such as encoders that measure relative angles based on fixed points, use inertial sensors such as gyro sensors to measure their posture. In order to improve the performance of low-cost MEMS (Micro Electro Mechanical Systems) inertial sensors, a lot of researches are being made to use the inertial sensors in small robots for attitude measurement. In particular, attention is focused on the gyro sensor for measuring the rotational angular velocity and the acceleration sensor for measuring the acceleration.

The method of measuring an angle using an acceleration sensor can be performed simply, but when the central axis of the sensor is not the same as the central axis of rotation, there is a problem in that an error occurs due to the translational motion component. In addition, the method of measuring an angle using a gyro sensor has a problem in that an error in which an integration error accumulates because an angle is calculated by integrating the measured angular velocity.

As such, due to a problem occurring when a single sensor is used, a Kalman filter or a complementary filter method using a gyro sensor and an acceleration sensor may be used.

The Kalman filter method has to deal with complex mathematical problems in the design of the Kalman filter, and it causes a problem of delayed response speed because it requires a large amount of computation in actual implementation.

In addition, the design of the complementary filter uses an angle estimation filter technique that is relatively simpler than the design of the Kalman filter, and then combines a high pass filter and a low pass filter to the gyro sensor and the acceleration sensor, respectively.

In general, the complementary filter is designed using a filter coefficient, which is not easy to construct an actual experimental environment, and it is difficult to design a large number of unknown factors to be determined when applying a high pass filter and a low pass filter of two or more orders. This happens.

In addition, since the filter coefficient is changed according to the type of sensor used for the angle measurement, the manufacturer of the sensor, and the like, an angle value according to the movement of the subject may be incorrect.

Therefore, it is possible to design more easily, and it is urgent to design a filter that is accurate for angle measurement while preventing a delay in response speed.

Accordingly, an object of the present invention is to prevent the delay of the response speed, to be more easily designed, and to achieve an accurate angle measurement for the movement of the subject, n (where n> = 1) and a low pass filter and The high pass filter and the least square method are applied to provide an n-th order complementary filter for angular measurement.

In order to achieve the above object, the complementary filter for the angle measurement according to the present invention is a means for calculating the filter coefficient of the order n (where n> = 1) by applying a least-squares method, filtering the output value of the first sensor N-th low pass filter for filtering, n-th high pass filter for filtering output value of second sensor, summing means for summing output values of the low-pass filter and high-pass filter, summing output from the summing means And integrating means for calculating an angle with respect to the movement of the subject by performing integration on the result.

In addition, it characterized in that it comprises a calculation means for calculating the output value and the filter coefficient of the high pass filter.

In addition, the least square method calculates a delta value of an angle of the angle with respect to a movement occurring in a subject and a delta value of the second sensor measurement value, and integrates n-1 times of the calculated delta value and the first sensor. It is characterized in that it is derived by using the measured value and the magnitude value for the actual measurement value.

The first sensor may be a gyro sensor.

The second sensor may be an acceleration sensor or an inclination sensor.

Therefore, according to the structure of the present invention, the design of the filter itself is more easily designed by designing the n-th order low pass filter, the n-th order equalizer filter, and the n-th order complementary filter to which the least-square method is applied. In addition, it is possible to prevent the delay of the response speed generated when measuring the angle of the movement of the subject, and at the same time, to perform a more accurate angle measurement.

1 is a circuit diagram showing a first complementary filter according to the prior art
2 is a circuit diagram illustrating an n (where n> = 1) complementary filter for angle measurement according to an exemplary embodiment of the present invention.
3 is a diagram illustrating experimental results of a tertiary complementary filter according to an exemplary embodiment of the present invention.

Hereinafter, with reference to the accompanying drawings will be described in detail an embodiment of the present invention. Prior to this, terms and words used in the present specification and claims should not be construed as limited to ordinary or dictionary terms, and the inventor should appropriately interpret the concept of the term appropriately in order to describe its own invention in the best way. The present invention should be construed in accordance with the meaning and concept consistent with the technical idea of the present invention.

Therefore, the embodiments described in the specification and the drawings shown in the drawings are only the most preferred embodiment of the present invention and do not represent all of the technical idea of the present invention, various modifications that can be replaced at the time of the present application It should be understood that there may be equivalents and variations.

1 is a circuit diagram illustrating a primary complementary filter according to the prior art.

In general, the first complementary filter according to the prior art design a high-pass filter to the integrated value of the output value from the gyro sensor (not shown) with good response characteristics in the high frequency region, acceleration sensor with good response characteristics in the low frequency region Design a low-pass filter on a value output from a (not shown) or a tilt sensor (not shown). Then, the output value from each designed filter is summed to derive the angle value according to the movement of the subject. This can be briefly summarized as in Equation 1 below, and Equation 1 is shown in FIG. 1 as a circuit diagram.

는 피사체의 움직임에 대한 각도 산출의 결과 값,

는 자이로 센서에서 측정된 각속도 값,

는 가속도 센서 또는 기울기 센서(미도시)에서 측정된 각도 값,

은 필터계수를 나타낸다. here,

Is the resulting value of the angle calculation for the subject's movement,

Is the angular velocity value measured by the gyro sensor,

Is the angle value measured by the accelerometer or tilt sensor (not shown),

Denotes a filter coefficient.

Referring to FIG. 1, the first complementary filter 100 according to the related art includes a first adding means 110, a calculating means 120, a second adding means 130, and an integrating means 140.

에서 각속도 센서 또는 기울기 센서로부터 출력된 각도 값이 저역통과필터(미도시)를 통과한 이후의 각도 값(

)을 뺀다. The first summing means 110

The angle value after the angle value output from the angular velocity sensor or the tilt sensor passes through the low pass filter (not shown)

Subtract).

)을 곱한다. The calculation means 120 has a filter coefficient (S) with a value derived from the first summing means 110.

Multiply by

)에서 연산수단(120)에서 도출된 값을 뺀다.The second summation unit 130 has an angular velocity value after the angular velocity value output from the gyro sensor passes through a high pass filter (not shown).

) Is subtracted from the value derived from the calculation means 120.

Thereafter, the integrating means 140 derives an angle value according to the movement of the subject by performing integration on the value derived from the second summing means 130.

2 is a circuit diagram illustrating an n (where n> = 1) complementary filter for angle measurement according to an exemplary embodiment of the present invention.

Referring to FIG. 2, the n-th complementary filter 200 (hereinafter, referred to as the n-th complementary filter) for angular measurement according to the present invention includes a first adding means 210, n calculating means 220, and a second summation. The means 230 is composed of n integral means 240, and includes a calculation means (not shown).

The n-th complementary filter 200 represents one of various main filters used to process and correct angle information. The n-th complementary filter 200 is designed by Equation 2 below, and FIG. 2 is a schematic diagram.

는 피사체의 움직임에 대한 각도 산출의 결과 값,

는 상기 자이로 센서(미도시)에서 측정된 각속도 값,

는 가속도 센서 또는 기울기 센서(미도시)에서 측정된 각도,

은 필터계수, k는 자이로 센서 및 가속도 센서 또는 자이로 센서 및 기울기 센서에서 측정된 측정 횟수를 나타낸다. here,

Is the resulting value of the angle calculation for the subject's movement,

Is an angular velocity value measured by the gyro sensor (not shown),

Is the angle measured by the accelerometer or tilt sensor (not shown),

Is the filter coefficient, k denotes the number of measurements measured by the gyro sensor and the acceleration sensor or the gyro sensor and the tilt sensor.

In more detail, the first summing means 210 is an angle value detected by the acceleration sensor or the tilt sensor output through the high pass filter of n (where n> = 1), and the angle with respect to the movement of the subject. The results of the calculation are summed up first.

The calculation unit 220 performs an operation of multiplying the filter coefficient by the output value summed by the first adding means 210. In this case, the calculation means 220 performs an operation of multiplying the n filter coefficients corresponding to the n th complementary filter 200.

When the calculation is completed in the calculation means 220, the second adding means 230 sums the angular velocity value detected by the gyro sensor and output through the low pass filter and the calculation value output from the calculation means 220.

Thereafter, the integrating means 240 performs integration on the finally summed values. The final value integrated by the integrating means 240 becomes a result value of the angle calculation for the movement of the subject.

,

, ..,

은 필터계수를 의미하고, 수학식 1에 기재된 와 동일한 의미이다. In addition, the calculation means calculates the filter coefficient described in Equation 2 using Equation 3 below. The calculating means calculates n filter coefficients,

,

, ..,

Denotes a filter coefficient and has the same meaning as described in Equation (1).

A and B described in Equation 3 are derived through Equations 4 and 5, respectively.

이고,

이다. At this time,

ego,

to be.

를 n-1번 적분한 결과 값이

이다. Denotes the true value of the angle with respect to the movement in the subject, that is, the value measured by the encoder.

The result of integrating n-1 times

to be.

이다. At this time,

to be.

는 자이로 센서에 획득된 값과 엔코더에서 측정된 값을 이용하여 산출한다.

Is calculated using the value obtained by the gyro sensor and the value measured by the encoder.

As described above, the present invention simply calculates n filter coefficients corresponding to the n-th order complementary filter 200 for the sensor used at the angle measurement using Equations 3 to 5, and calculates the calculated filter coefficients as Equation 2 below. By substituting for, the angle with respect to the movement occurring in the subject can be calculated accurately.

3 is a diagram illustrating an experimental result of a tertiary complementary filter according to an exemplary embodiment of the present invention.

Referring to FIG. 3, reference numeral 11 is an actual angle value of measuring an angle of a subject's movement using an encoder, and reference numeral 12 is an angle value of an angle of a subject's movement using a third-order complementary filter. to be. As shown in FIG. 3, it can be seen that the angle of the subject's movement calculated using the third complementary filter and the angle actually measured using the encoder are very similar.

As described above, the n-th complementary filter 200 may perform accurate angle measurement on the movement of the subject.

In addition, the low-cost MEMS sensor can be used to accurately measure the movement of the subject. Therefore, the cost of designing the angle measurement equipment for the movement of equipment such as unmanned aerial vehicles and balancing robots that cannot use sensors such as encoders can be increased. There is a saving effect.

As described above, although the present invention has been described by means of a limited embodiment and drawings, the present invention is not limited thereto and by those skilled in the art to which the present invention pertains, Of course, various modifications and variations are possible within the scope of equivalents of the claims to be described.

200: nth complementary filter
210: first adding means 220: calculating means
230: second adding means 240: integrating means

Claims (5)

Calculating means for calculating a filter coefficient of order n (where n> = 1) by applying the least square method;
An n-th order low pass filter for filtering the output value of the first sensor;
An n-th high pass filter for filtering the output value of the second sensor;
Summing means for summing output values of the low pass filter and the high pass filter;
Integrating means for integrating the sum result output from the adding means to calculate an angle with respect to the movement of the subject; And
Complementary filter for angle measurement, characterized in that it comprises a. The method of claim 1,
Calculating means for calculating an n-th order output value filtered by the n-th high pass filter and an n-th order filter coefficient calculated by the calculating means;
Complementary filter for angle measurement, characterized in that it comprises a. The method of claim 2,
The least square method is
Calculate a delta value for the actual measured value of the angle with respect to the movement occurring in the subject and the measured value by the second sensor, n-1 integration values for the calculated delta value, and the measured value by the first sensor And a complementary filter for angular measurement, which is derived using a magnitude value for the actual measured value. The method of claim 1,
The first sensor,
Complementary filter for angle measurement, characterized in that the gyro sensor. The method of claim 1,
The second sensor,
Complementary filter for angle measurement, characterized in that the acceleration sensor or the tilt sensor.

Images