KR101880940B1 - Method for controlling attitude and drone atapted the same - Google Patents

Abstract

The present invention provides a mechanism including: a step of calculating a current estimated angular speed by using an Euler angle measured from an accelerometer; a step of calculating a current estimated Euler angle by using an angular speed measured from a gyroscope; and a step of calculating a final Euler angle and a final angular speed by using the current estimated angular speed, the estimated Euler angle, the measured Euler angle, and the measured angular speed, and previous Euler angles and angular speeds, which have already been stored in a memory. Therefore, the present invention is capable of estimating and specifying a change direction and posture of a drone, thereby more precisely controlling the posture of the drone.

Description

METHOD FOR CONTROLLING ATTITUDE AND DRONE ATAPTED THE SAME,

More particularly, the present invention relates to a posture control method for estimating and controlling a posture of a drone, and a drone to which the posture control method is applied.

Generally, an unmanned aerial vehicle such as a drone is equipped with an accelerometer and a gyroscope, and directly acquires acceleration and angular velocity through them to utilize as information for attitude control.

At this time, the drone uses a filter algorithm for noise elimination and acquires a plurality of acceleration and angular velocity information through a plurality of homogeneous sensors and averages them by a method of increasing the reliability of each acceleration and angular velocity information , And attitude control is performed.

However, since the conventional drones are limited in the use of a large number of sensors due to the limitation of their specifications, they are exposed to a large amount of noise noise in the use environment. Therefore, There is a problem that accurate posture estimation is not easy.

Korean Patent Publication No. 2016-0068260, Publication Date: June 15, 2016 Title of invention: Posture stabilization and altitude control of a quadrotor type unmanned aerial vehicle in an indoor environment.

In order to solve the above-described problems, the present invention aims to provide a method for estimating and controlling a posture by more precisely complementing angular velocity and acceleration, and a dron to which the method is applied.

According to another aspect of the present invention, there is provided a method for estimating an angular velocity, comprising: calculating a current estimated angular velocity (Velocity _estimated ) using _measurement Euler _measurements measured from an accelerometer; Using the measured angular velocity (Velocity _measured) measured by the gyroscope calculating an estimate of the current Euler angles (Euler _estimated); And calculating the respective end-Euler (Euler _final) and final angular velocity (Velocity _final) using the current estimated angular velocity, the estimated Euler angles, group exchange Euler angle and the angular speed stored in the measuring Euler angles and the measured angular speed and the memory And provides a method of controlling the attitude of the drones.

The measured Euler angles may include angles of Euler _roll and Euler _pith calculated from the following equations (1) and (2) in the spatial coordinates (x, y, z)

식 (1)

Equation (1)

식 (2)

Equation (2)

Herein, atan2 is tan-1, and Accel _x , Accel _y, and Accel _z may be acceleration in the x-axis, y-axis, and z-axis.

The current estimated angular velocity can be calculated through the following equation (3).

Velocity _{roll_estimated} = (Euler _roll - Euler _roll-1 ) /? T

Velocity _{pitch_estimated} = (Euler _pitch - Euler _pitch-1 ) / DELTA T (3)

At this time, the current estimated angular velocity means a value (Velocity _{roll_estimated} , Velocity _{pitch_estimated} ) calculated for roll and pitch, respectively, and ΔT may mean time displacement.

The current estimated Euler angles can be calculated through the following equation (4).

식 (4)

Equation (4)

_Wherein F _hdr is a function of a Heuristic Drift Reduction Filter, F _lpf is a function of a Low Pass Filter, V _{angle_t} is acceleration at a specific measurement time of time (t), and ΔT is a time displacement can do.

Calculating the final Euler angles are pre-stored in the memory the N-th Euler angles of the Euler angles of the past (Euler _n) and the N-1 th Euler angles using the (Euler _n-1) past Euler angle difference (g _n ), And the past Euler angular difference value g _n can be calculated through the following equation (5).

g _n = Euler _n - Euler _n-1 Equation (5)

Calculating the final Euler angles are the measurement Euler angles (Euler _measured) and the N second Euler angle measured using the (Euler _n) and the past Euler angle difference (g _n) Euler angle difference value (g _measured) , And the measurement Euler angular difference value (g _measured ) can be calculated through the following equation (6).

g _measured = Abs (Euler _measured - Euler _n ) - Abs (g _n )

At this time, Abs may mean an absolute value.

The step of calculating the final Euler angles may further comprise calculating an estimated Euler angular difference value g _estimated using the estimated Euler _estimated , the Nth Euler angle Euler _n and the past Euler angular difference g _n , , And the estimated Euler angular difference value g _estimated can be calculated through the following equation (7).

g _estimated = Abs (Euler _estimated - Euler _n ) - Abs (g _n )

Calculating the final Euler angle is the final Euler angles (Euler _final) through (8) below, if the measured Euler angle difference value (g _measured) is less than the estimated Euler angle difference value (g _estimated) calculations, and the step of the measurement Euler angle difference value (g _measured) is calculating the final Euler angles (Euler _final) through the equation (9) below the estimate is greater than the Euler angle difference value (g _estimated), more .

Euler _final = Euler _measured x k _euler + Euler _estimated (1-k _euler )

Euler _final = Euler _estimated占 e _euler + Euler _measured (1-k _euler )

At this time, the k is the k _{euler euler} can mean Euler weight in the range _{(0 ≤ k euler ≤ 1)} .

The calculating the final angular velocity may include calculating a past angular velocity difference value V _n using an Nth angular velocity (Velocity _n ) and an N-1th angular velocity (Velocity _n-1 ) of the past angular velocity previously stored in the memory , And the past angular velocity difference value (V _n ) can be calculated through the following equation (10).

V _n = Velocity _n - Velocity _n-1 (10)

Calculating the final angular velocity is a step of calculating a difference value (V _measured) angular velocity measured by using the above N-th angular velocity (Velocity _n) and the past angular velocity difference (V _n) the measured angular velocity (Velocity _measured) , And the measured angular velocity difference value (V _measured ) can be calculated through the following equation (11).

V _measured = Abs (Abs (Velocity _measured - Velocity _n ) - Abs (v _n )

Calculating the final angular velocity is a step of calculating the estimated angular velocity difference (V _estimated) by using the estimated angular velocity (Velocity _estimated) and the N-th angular velocity (Velocity _n) and the past angular velocity difference (V _n) , And the estimated angular velocity difference value (V _estimated ) can be calculated through the following equation (12).

v _estimated = Abs (Velocity _estimated - Velocity _n ) - Abs (V _n )

Calculating the final angular velocity, and calculating the final angular velocity (Velocity _final) through formula (13) below, if the measured angular velocity difference value (V _measured) is less than the estimated angular velocity difference (V _estimated), the measured angular velocity difference value (V _measured) is greater than the estimated angular velocity difference (V _estimated), may further comprise the step of calculating the final angular velocity (Velocity _final) through (14) below.

Velocity _final = Velocity _measured × k _velocity + Velocity _estimated (1-k _velocity ) (13)

Velocity _final = V _estimated x k _velocity + V _measured (1-k _velocity )

Here, the k _velocity may mean an angular velocity weight having a range of (0? K _velocity ? 1).

Using a position control method to calculate the current Euler angles (Euler _N), and the angular velocity and the final angular velocity (Velocity _final) in the past using the Euler angles and the final Euler angles (Euler _final) of the history of the drone And calculating a current angular velocity (Velocity _N ).

Calculating the Euler angle of the current is the final Euler angles (Euler _final) and (15) to apply the N-th Euler weight (keuler _{_n)} the N-th Euler angles (Euler _n) of the Euler angle of the past And determining the current Euler angle (Euler _N ) through equation (17).

Euler _N = Euler _final x k _{euler_n} + Euler _n (1-k _{euler_n} )

Euler _n-1 = Euler _n (16)

Euler _n = Euler _N (17)

In this case, k _{euler_n} may mean an Nth Euler angular weight having a range of (0 ≦ k _{euler_n} ≦ 1).

Calculating the current angular velocity is the final angular velocity (Velocity _final) and (18) to apply the N-th angular weighting (Velocity _{_n)} the N-th angular velocity (Velocity _n) of the angular velocity of the past to (20 (Velocity _N ) of the current angular velocity.

Velocity _N = Velocity _final × k _{Velocity_n} + Velocity _n (1-k _{velocity_n} )

Velocity _n-1 = Velocity _n (19)

Velocity _n = Velocity _N (20)

Here, k _{velocity_n} may mean an Nth angular velocity weight having a range of (0? K _{velocity_n} ? 1).

According to the present invention, since the attitude of the drones is controlled more precisely by estimating the accelerations and the angular velocities obtained from the accelerometers and gyroscopes that have already been used and compensating for the angular velocities more accurately, it is possible to enhance the existing cost effectiveness.

The present invention also utilizes an algorithm for estimating the angular velocity and acceleration and estimates the actual attitude change using the similarity between the measured value and the estimated value related to the attitude change of the dron, Can be estimated and specified.

The best effects of the present invention are not limited to those described above, and include various other effects that can be clearly understood by those skilled in the art to which the present invention belongs.

BRIEF DESCRIPTION OF THE DRAWINGS The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of the specification, serve to explain the principles of the invention. It is to be understood, however, that the technical features of the present invention are not limited to the specific drawings, and the features disclosed in the drawings may be combined with each other to constitute a new invention.
Fig. 1 is a flowchart showing an example of the attitude control method of the present invention.
Fig. 2 is a diagram exemplifying the configuration of a dron for realizing the posture control method of Fig. 1. Fig.
3 is a flowchart illustrating an algorithm for computing a final Euler angle in step 130 according to the present invention.
4 is a flowchart illustrating an algorithm for calculating a final angular velocity in step 130 according to the present invention.
5 is a flowchart for explaining a posture specifying algorithm in step 140 according to the present invention.
FIG. 6 is a block diagram showing a configuration of a drones for attitude control according to the present invention.

In the following description and the claims, the terms "comprises", "comprising" and the like mean that the constituent elements are provided as one of the various constituent elements unless otherwise specifically stated, Is not to be excluded.

In addition, the term " part " described in the following description and claims may refer to a unit or block that performs a specific function by distinguishing hardware or software or a function performed by combining hardware and software have.

Generally, unmanned aerial vehicles include drones, unmanned helicopters, and unmanned aerial vehicles. Hereinafter, the method for controlling the attitude of the dron and the dron to which the dron is applied will be described in detail with reference to the related drawings. It should be noted, however, that the present invention can be applied to other unmanned aerial vehicles, though it will be described only in the following description and the drones described in the claims.

Fig. 1 is a flowchart exemplarily showing an attitude control method of the present invention, and Fig. 2 is a diagram exemplarily showing the configuration of a dron for implementing the attitude control method of Fig.

Fig. 2 is ancillary references when describing Fig.

Referring to FIG. 1, the attitude control method according to the present invention includes steps 110 through 140, which are realized through the controller of the drones of FIG. 2 may include an accelerometer 10 and gyroscope 20, a controller 30 and a memory 40. [ The accelerometer 10 measures the acceleration for the movement of the dron (posture change), and the gyroscope measures the angular velocity for the movement (posture change) of the dron.

In this case, the controller shown in FIG. 2 can perform precise attitude control according to the result of performing the attitude change algorithm for estimating and specifying the attitude and attitude of the dron by complementing the measured acceleration and angular velocity.

The memory shown in Fig. 2 can accumulate the acceleration and angular velocity measured by the accelerometer 10 and the gyroscope 20 as past data according to the command of the controller described above, and stores the data processed by the controller Can be stored.

Hereinafter, steps 110 to 140 of FIG. 1, which are realized by the above-described controller of the drones, are as follows.

In step 110, receives the controller 30 of the drone is an accelerometer (10) by measuring the Euler angles (Euler _measured) acceleration and measured, the acceleration and the measuring Euler angles (Euler _measured) measured by the accelerometer 10, the memory (40).

The measured Euler _measurements include the angles of the Euler _roll and the Euler _pith calculated from the following equations (1) and (2), which apply the acceleration of the spatial coordinates (x, y, z) .

(수학식 1)

(1)

(수학식 2)

(2)

At this time, atan2 in Equations 1 and 2 indicates tan-1, and Accel _x , Accel _y, and Accel _z can indicate acceleration in the x axis, y axis, and z axis.

Thereby, the controller 30 of the drone can calculate an estimated angular velocity (Velocity _estimated ) for estimating the current angular velocity using _measured Euler _measurements from the accelerometer 10. [

For example, the controller 30 of the drone can calculate an estimated angular velocity (Velocity _estimated ) for estimating the current angular velocity according to the time displacement [Delta] T for the above-mentioned measured Euler _measurements , The current estimated velocity velocity (Velocity _estimated ) can be calculated through Equation (3). It is a matter of course that the calculated current estimated angular velocity (Velocity _estimated ) is stored in the memory 40.

Velocity _{roll_estimated} = (Euler _roll - Euler _roll-1 ) /? T

Velocity _{pitch_estimated} = (Euler _pitch - Euler _{pitch - 1} ) /? T (Equation 3)

Here, the current estimated angular velocity (Velocity _estimated ) indicates a value (Velocity _{roll_estimated} , Velocity _{pitch_estimated} ) calculated for roll and pitch, the ΔT indicates a time displacement, and the Euler _roll and Euler _pith represent the current measurement Euler point to each (Euler _measured), Euler _{roll -1} and Euler _pith -1 may indicate the previous value of each of the current measuring Euler (Euler _measured).

In step 120, the controller 30 of the drone receives the gyroscope 20, the measured angular velocity and the measurement of the drone angular velocity (Velocity _measured), the angular velocity and the measured angular velocity (Velocity _measured) measured by the accelerometer 10 Can be stored in the memory (40).

Thereby, the controller 30 of the drone can calculate the _estimated Euler _estimate for estimating the current Euler angle using the _measured velocity velocities measured from the gyroscope 20.

For example, the controller 30 of the drones calculates the current estimated Euler angle (V) by using the measured angular velocity (V _{angle_t} ) at time t, the function of the Low Pass Filter, and the function of the Heuristic Drift Reduction Filter, Euler _estimated . The calculated current estimated Euler _estimated is of course stored in the memory 40.

(수학식 4)

(4)

In the equation (4), F _hdr is a function of Heuristic Drift Reduction Filter, F _lpf is a function of Low Pass Filter, V _{angle_t} is acceleration at a specific measurement time of time t, May indicate a time displacement.

The function (F _lpf ) of the Heuristic Drift Reduction Filter mentioned above may be a value for removing the heuristic drift noise component generated at the angular velocity measured by the gyroscope 20 through the Heuristic Drift Reduction Filter, The function F _lpf of the gyroscope 20 may be a value for removing the low-frequency noise component generated at each angular velocity measured by the gyroscope 20 through the low pass filter.

In step 130, the controller 30 of the drone calculates the current estimated angular velocity (Velocity _estimated ), the estimated Euler _estimated (Euler _estimated ) and the measured Euler angle (measured from the accelerometer 10) Euler _measured) and the gyroscope (the measured angular velocity (Velocity _measured) and a memory (40) end Euler angles (Euler _final) by the utilization of the past Euler angle and angular velocity are stored in and final angular velocity (Velocity _final) measured from 20) calculated (For example, the posture and posture direction of the drones) can be estimated by calculating (calculating) the posture of the drones.

Of these, in the memory 40 are stored past Euler angles using the cumulative acceleration of the past measured by the accelerometer 10, _{e.g., E 1, E 2, E} 3, ... E n-1 and E _n of the Euler angles it may mean, in the past, the angular velocity using the angular velocity of the accumulated past measured by the gyroscope 20, for _{example, V 1, V 2, V} 3, ... V n-1 and V _n And the angular speed of the motor.

In this case, a measuring Euler angles (Euler _measured) above may refer to the Euler angles of E _{n + 1} beonjjae generated Euler angles after E _n, the above-described measuring the angular velocity (Velocity _measured) is after the angular velocity of V _n And may indicate the generated angular velocity V _{n + 1} .

The above-described step 130 will be described in detail with reference to FIGS. 3 and 4. FIG.

3 is a flowchart illustrating an algorithm for calculating a final Euler angle in step 130 according to the present invention.

Referring to FIG. 3, the step 130 according to the present invention may include steps 131 through 136, which are realized by the controller 30 of the drones.

First, in the 131 step, the controller 30 of the drone is using the memory (40) N-th Euler angles (Euler _n) and the N-1 th Euler angles (Euler _n-1), the Euler angles of the history of the pre-stored in the history Euler angular difference values g _n can be calculated.

For example, the controller 30 of the drone may calculate the past Euler angular difference value g (g) by using the above-described Equation 5 using the N- _th Euler _n and the N-1th Euler _n-1 , _n ) can be calculated.

g _n = Euler _n - Euler _n-1 (Equation 5)

In step 132, the controller 30 of the drone calculates the past Euler angular difference value g _n ( _n ) using the Nth Euler angle Euler _n and the N-1th Euler angle _n-1 of the Euler angles of the past ) Can be calculated.

For example, the controller 30 may exchange Euler angles obtained by measuring Euler angles (Euler _measured) and the N-th Euler angles in the past Euler angle data (Euler _n) and the above-mentioned 131 phase measurement from the accelerometer 10 of the drone The measured Euler angular difference value g _measured can be calculated through Equation (6) using the difference value g _n .

g _measured = Abs (Euler _measured - Euler _n ) - Abs (g _n )

At this time, Abs in Equation (6) may indicate an absolute value.

In step 133, the controller 30 of the drones calculates an estimated Euler angular difference value g _estimated using the above-described estimated Euler _estimated , the Nth Euler angle Euler _n and the past Euler angular difference g _n , ) Can be calculated.

For example, the controller 30 is determined by the estimated Euler angles (Euler _estimated) and the N-th Euler angles (Euler _n) and the above-described 131 steps past Euler angle data obtained by the aforementioned step 120 past the Euler angles of the drone The estimated Euler angular difference value g _estimated can be calculated by Equation (7) using the difference value g _n .

g _estimated = Abs (Euler _estimated - Euler _n ) - Abs (g _n )

At this time, Abs in Equation (7) may indicate an absolute value.

In step 134, the controller 30 of the drone judges whether or not the measured Euler angular difference g _measured by the above step 132 is smaller than the estimated Euler angular difference g _estimated obtained in the above step 133 can do.

For example, in step 135, the controller 30 of the drones _computes the final Euler angles (Euler) through the following equation (8) if the measured Euler angular difference g _measured is less than the estimated Euler angular difference g _estimated : _final ) can be calculated.

Euler _final = Euler _measured占 k _euler + Euler _estimated (1-k _euler ) (8)

For example, in the 136 step, the drone of the controller 30 measure the Euler angle difference value (g _measured) is estimated Euler angle difference value (g _estimated) is greater than, the following equation (9) the final Euler angles (Euler _final via ) Can be calculated.

Euler _final = Euler _estimated占 e _euler + Euler _measured (1-k _euler ) (9)

At this time, the k _euler referred to in Equations (8) and (9) may indicate an Euler weight having a range of (0? K _euler ? 1).

As described above, in this embodiment, the final Euler _final can be accurately estimated through the above-described steps 131 through 136, by calculating the _final Euler _final .

4 is a flowchart illustrating an algorithm for calculating a final angular velocity in step 130 according to the present invention.

Referring to FIG. 4, step 130 according to the present invention may further include steps 131a to 136f which are realized by the controller 30 of the drones.

In step 131a, the controller 30 of the drone calculates the past angular velocity difference value V ( _n ) by using the Nth angular velocity (Velocity _n ) and the N-1th angular velocity (Velocity _n-1 ) of the past angular velocity previously stored in the memory 40 _n ) can be calculated.

For example, the controller 30 of the drone may calculate the past angular velocity difference value V ( _n ) by using the following equation (10) to which the Nth angular velocity (Velocity _n ) and the N-1th angular velocity _n ) can be calculated.

V _n = Velocity _n - Velocity _n-1 (10)

In step 132b, the controller 30 of the drone is the above-described measuring the angular velocity (Velocity _measured) and the N-th angular velocity (Velocity _n) and past angular velocity difference (V _n) to calculate a difference value (V _measured), the angular velocity measured by have.

For example, the controller 30 of the drones calculates the difference between the measured angular velocity (Velocity _measured ) obtained in step 120, the Nth angular velocity (Velocity _n ) of the past angular velocity data and the past angular velocity difference value V through _n) of equation (11) to apply may be calculated by measuring the angular velocity difference value (V _measured).

V _measured = Abs (Abs (Velocity _measured - Velocity _n ) - Abs (v _n )

At this time, the Abs mentioned in Equation (11) may indicate an absolute value.

At 133c step, the controller 30 of the drone is the above-described estimated angular velocity (Velocity _estimated) and the N-th angular velocity (Velocity _n) and past angular velocity difference (V _n) to be estimated to calculate the angular velocity difference (V _estimated) using have.

For example, the controller 30 of the drones calculates the estimated angular velocity (Velocity _estimated ) obtained in step 110, the Nth angular velocity (Velocity _n ) of the past angular velocity data, and the past angular velocity difference value V through the equation (12) below apply to _n) can be calculated to estimate the angular velocity difference (V _estimated).

V _estimated = Abs (Velocity _estimated - Velocity _n ) - Abs (V _n )

In step 134d, the controller 30 of the drone can determine whether the measured angular velocity difference value V _measured by the above-described step 132b is smaller than the estimated angular velocity difference value V _estimated obtained in step 133c have.

For example, in step 135e, when the measured angular velocity difference value V _measured is smaller than the estimated angular velocity difference value V _estimated , the controller 30 of the drones calculates a final final angular velocity (Velocity _final ) Can be calculated.

Velocity _final = Velocity _measured占_velocity占 Velocity _estimated (1-k _velocity ) (Equation 13)

For example, the final angular velocity (Velocity _final) from 135e step, the controller 30 is the difference between measured angular velocity values (V _measured) of the drone is greater than the estimated angular velocity difference (V _estimated), through (14) below the Can be calculated.

Velocity _final = V _estimated x k _velocity + V _measured (1-k _velocity ) (14)

In this case, the k- _velocity mentioned in equations (13) and (14) may indicate an angular velocity weight having a range of (0? K _velocity ? 1).

Thus, in the present embodiment, when calculating the final angular velocity (Velocity _final) through the above-described steps 131a to 136f step, it will be able to accurately estimate the changes in the direction of the current attitude of the drone through.

On the other hand, referring back to FIG. 1, the remaining step 140 will be described.

In step 140 according to the present invention, the controller 30 of the drones computes the past Euler angles (N-th Euler _n , N-1-th Euler angles Euler _n-1 )) and the final Euler _final , to calculate a more accurate current Euler angle (Euler _N ) with respect to the current position of the drones.

In addition, in step 140, the controller 30 of the drones calculates the past angular velocity (e.g., Nth angular velocity (Velocity _n ), N-1th angular velocity (Velocity _{n- 1} )) and the final velocity (Velocity _final ) to obtain a more precise current angular velocity (Velocity _N ) with respect to the changing direction of the dron's posture.

These 140 steps are as follows.

5 is a flowchart for explaining a posture specifying algorithm in step 140 according to the present invention.

Referring to FIG. 5, step 140 in accordance with the present invention may include steps 141 through 142, which are realized by the controller 30 of the drones in order to further complement the past values and the determined estimated values.

In step 141, the controller 30 of the drone will apply the final Euler angles (Euler _final) and the N-th Euler weight (keuler _{_n)} the N-th Euler angles (Euler _n) of the Euler angle of the past obtained by the above-mentioned 130 step The current Euler angle _N can be calculated through the following equations (15) to (17).

Euler _N = Euler _final x k _{euler_n} + Euler _n (1-k _{euler_n} ) (15)

Euler _n-1 = Euler _n (Equation 16)

Euler _n = Euler _N (Equation 17)

In this case, k _{euler_n} referred to in Equation (15) may indicate an Nth Euler angular weight having a range of 0 ≦ k _{euler_n} ≦ 1.

As described above, in the present embodiment, by specifying the current Euler angle (Euler _N ) as described above, it is possible to accurately predict the current attitude of the dron more precisely as compared with the step 130 described above.

In the 142 step, the controller 30 of the drone is mathematics to apply the final angular velocity (Velocity _final) and the N-th angular weighting (Velocity _{_n)} the N-th angular velocity (Velocity _n) of the angular velocity of the past obtained by the above-mentioned 130 step The current angular velocity (Velocity _N ) can be calculated through Equations (18) to (20).

Velocity _N = Velocity _final x k _{velocity_n} + Velocity _n (1-k _{velocity_n} ) (Equation 18)

Velocity _n-1 = Velocity _n (Equation 19)

Velocity _n = Velocity _N (Equation 20)

In this case, k _{velocity_n} referred to in Equation (18) may indicate an Nth angular velocity weight having a range of (0? K _{velocity_n} ? 1).

As described above, in this embodiment, by specifying the current angular velocity (Velocity _N ), it is possible to predict the change direction of the current posture of the dron more precisely as compared with the step 130 described above.

The method of controlling the attitude of the drones described above can be implemented in the form of program instructions that can be executed through various computer components and recorded on a computer-readable medium.

The computer readable medium may be any medium accessible by the processor. Such media can include both volatile and nonvolatile media, removable and non-removable media, storage media, and computer storage media.

(ROM), an electrically erasable read only memory ("EEPROM"), a register, a hard disk, a removable disk, a compact disk read-only memory ("CD- ROM"), Lt; RTI ID = 0.0 > a < / RTI > storage medium.

The computer storage media discussed include removable and non-removable, nonvolatile, and volatile storage media implemented in any method or technology for storage of information such as computer readable instructions, data structures, program modules or other data, And non-volatile media.

Such computer storage media may be embodied as program instructions, such as RAM, ROM, EPROM, EEPROM, flash memory, other solid state memory technology, CDROMs, digital versatile disks (DVDs) or other optical storage, magnetic cassettes, magnetic tape, magnetic disk storage, Lt; RTI ID = 0.0 > and / or < / RTI >

Examples of the mentioned program instructions may include machine language code such as those generated by the compiler, as well as high-level language code that may be executed by a computer using an interpreter or the like.

FIG. 6 is a block diagram showing a configuration of a drones for attitude control according to the present invention.

Referring to FIG. 6, a drone 200 according to the present invention includes an accelerometer 210, an angular velocity estimator 220, a gyroscope 230, an Euler angle estimator 240, a posture change estimator 250, A change specification unit 260 and a memory 270.

First, the accelerometers 210 can measure acceleration and measuring Euler angles (Euler _measured) for a motion (position change) of the drone. This accelerometer 210 may be at least one commonly known accelerometer.

The angular velocity estimating unit 220 may calculate an estimated angular velocity Velocity _estimated for estimating a current angular velocity according to the time displacement AT using the _measured Euler _measurements measured from the accelerometer 210. [

Because fully described in each of the mentioned measuring Euler (Euler _measured) and the estimated angular velocity calculated in the _(estimated Velocity) is a prior one, its description is omitted, but applied in this embodiment. FIG.

Gyroscope 230 may measure the angular velocity and the measured angular speed of the drone of the motion (position change) of the drone (Velocity _measured). Such a gyroscope 230 may be at least one commonly known angular velocity sensor.

Euler angle estimation unit 240 may compute a current estimate of the Euler angles (Euler _estimated) for estimating the current Euler angles using the measured angular velocity (Velocity _measured) measured by the gyroscope 230. Since the calculation of the current estimated Euler _estimated is fully described in FIG. 1, its explanation is omitted, but it goes without saying that the present embodiment also applies.

The posture change estimating unit 250 estimates the posture change based on the current estimated angular velocity Velocity _estimated , the estimated Euler _estimated , the measured Euler measured from the accelerometer 210, and the measured Euler _{measured by the} gyroscope 230 the can measure angular velocity (Velocity _measured) and a memory (270) calculates a final group Euler angles (Euler _final) and final angular velocity (Velocity _final) by an algorithm complementary to the Euler angle and the angular velocity history is stored (calculated) on.

Since the calculation of the _final Euler _final and the final velocity mentioned above has been fully described in FIGS. 1 to 4, its description is omitted, but it goes without saying that the present embodiment is also applicable.

Thus, when this embodiment, the end-Euler angles (Euler _final) is calculated, when This can accurately estimate the current position of the drone (Euler _final), the final angular velocity (Velocity _final) calculation, current drone through which The direction of change of the posture can be accurately estimated.

The posture change specifying unit 260 sets the posture change specifying unit 260 to the past Euler angles (the Nth Euler _n , the N-1th Euler _n-1 ) and the previous Euler angles The current Euler _N is calculated using the _final Euler _final and the previous angular velocity (e.g., Nth Velocity _n , N-1th Velocity _n-1 ) And the final velocity (Velocity _final ) can be used to calculate the current angular velocity (Velocity _N ).

Since the calculation of the current Euler _N and the current angular velocity Velocity _N is fully described in FIG. 5, the description is omitted, but it goes without saying that the present embodiment also applies.

As described above, in this embodiment, by specifying the current Euler angle (Euler _N ) as described above, it is possible to accurately predict the current attitude of the dron more precisely than the result value of the posture change estimator 250 described above , And by specifying the current angular velocity (Velocity _N ) as described above, the change direction of the current posture of the drone can be predicted more accurately than the resultant value of the posture change estimating unit 250 described above.

Lastly, the memory 270 includes the accelerometer 210, the angular velocity estimator 220, the gyroscope 230, the Euler angle estimator 240, the attitude change estimator 250, and the attitude change identifying unit 260 And may further store the past acceleration and angular velocity data measured from the accelerometer 210 and the gyroscope 230 and the past Euler angles and angular velocities.

Such memory 270 may be volatile memory or non-volatile memory or a combination thereof.

While the present invention has been described in connection with what is presently considered to be the most practical and preferred embodiment, it is evident that many alternatives, modifications and variations will be apparent to those skilled in the art. .

Therefore, the embodiments described above are merely illustrative and are not intended to limit the scope of the present invention to the foregoing embodiments. It is to be understood that the flowcharts shown in the drawings are merely illustrative examples for achieving the most desirable results in the practice of the present invention, and that other steps may be added or some steps may be deleted.

The scope of the present invention will be defined by the appended claims, but all changes or modifications derived from the equivalents, as well as those directly derived from the claims, are also included in the scope of the present invention. .

10,210 accelerometer 20,230 gyroscope
30: Controller 40,270: Memory
200: Drone 220: Angular velocity estimation unit
240: Euler angle estimator 250:
260: attitude change specific part

Claims (15)

Calculating a current estimated angular velocity (Velocity _estimated ) using _measured Euler _measurements from the accelerometer;
Using the measured angular velocity (Velocity _measured) measured by the gyroscope calculating an estimate of the current Euler angles (Euler _estimated); And
The current estimated angular velocity and the measured angular speed and to the utilization of past angular velocity stored in the memory and calculates the final angular velocity (Velocity _final) for predicting the current position of the drone, the estimated Euler angles and the measured Euler angles and a group in the memory Calculating a _final Euler _final for predicting a change direction of the current posture of the drone using the stored past Euler angles;
Lt; / RTI >
Wherein calculating the final Euler angles comprises:
Calculating a past Euler angular difference value g _n using the Nth Euler _n and the N-1th Euler _n-1 of the past Euler angles,
Wherein the past Euler angular difference value g _n is calculated by the following equation (5).
g _n = Euler _n - Euler _n-1 Equation (5) The method according to claim 1,
The measurement Euler angles
And the Euler _roll and Euler _pith angles calculated from the spatial coordinates (x, y, z) through the following equations (1) and (2).

Equation (1)

Equation (2)
The atan2 is tan-1, and the Accel _x , Accel _y, and Accel _z represent accelerations in the x-axis, the y-axis, and the z-axis. 3. The method of claim 2,
Wherein the current estimated angular velocity is calculated through the following equation (3).
Velocity _{roll_estimated} = (Euler _roll - Euler _roll-1 ) /? T
Velocity _{pitch_estimated} = (Euler _pitch - Euler _pitch-1 ) / DELTA T (3)
The current estimated angular velocity indicates a value (Velocity _{roll_estimated} , Velocity _{pitch_estimated} ) calculated for roll and pitch, respectively, and ΔT is time _varying . The method of claim 3,
Wherein the current estimated Euler angle is calculated through the following equation (4).

Equation (4)
The Euler _estimated represents the Velocity _{roll_estimated} and Velocity _{pitch_estimated,} the F _hdr is a function of Heuristic Drift Reduction Filter, the F _lpf is a function of the Low Pass Filter, the V _{angle_t} is in a given measurement point in time (t) Acceleration, and DELTA T represents time displacement. delete The method according to claim 1,
Wherein calculating the final Euler angles comprises:
Further comprising; calculating the measuring Euler angles (Euler _measured) and the N-th Euler angles (Euler _n) and the past Euler angle difference (g _n) measured Euler angle difference by using a value (g _measured) ,
Wherein the measured Euler angular difference (g _measured ) is calculated through the following equation (6).
g _measured = Abs (Euler _measured - Euler _n ) - Abs (g _n )
Abs represents an absolute value. The method according to claim 6,
Wherein calculating the final Euler angles comprises:
Further comprising; calculating the estimated estimated using the Euler angles (Euler _estimated) and the N-th Euler angles (Euler _n) and the past Euler angle difference (g _n) Euler angle difference value (g _estimated) ,
Wherein the estimated Euler angular difference value g _estimated is calculated through the following equation (7).
g _estimated = Abs (Euler _estimated - Euler _n ) - Abs (g _n )
Abs represents an absolute value. 8. The method of claim 7,
Wherein calculating the final Euler angles comprises:
If the measured Euler angular difference value g _measured is less than the estimated Euler angular difference value g _estimated , the final Euler _final is calculated through the following equation (8)
Calculating the _final Euler _final through the following equation (9) if the measured Euler angular difference g _measured is greater than the estimated Euler angular difference g _estimated ;
Further comprising the steps of:
Euler _final = Euler _measured x k _euler + Euler _estimated (1-k _euler )
Euler _final = Euler _estimated占 e _euler + Euler _measured (1-k _euler )
K _euler denotes an Euler weight having a range of (0 ≤ k _euler ≤ 1). The method according to claim 1,
Wherein the step of calculating the final angular velocity comprises:
Calculating a past angular velocity difference value (V _n ) using an Nth angular velocity (Velocity _n ) and an N-1th angular velocity (Velocity _n-1 ) of the past angular velocity stored in the memory,
Wherein the past angular velocity difference value (V _n ) is calculated through the following equation (10).
V _n = Velocity _n - Velocity _n-1 (10) 10. The method of claim 9,
Wherein the step of calculating the final angular velocity comprises:
Using the measured angular velocity (Velocity _measured) and the N-th angular velocity (Velocity _n) and the past value of the angular velocity difference (V _n) calculating a difference value (V _measured) measured angular velocity; further comprising,
Wherein the measured angular velocity difference value (V _measured ) is calculated by the following equation (11).
V _measured = Abs (Abs (Velocity _measured - Velocity _n ) - Abs (v _n )
Abs represents an absolute value. 11. The method of claim 10,
Wherein the step of calculating the final angular velocity comprises:
The estimated using the angular velocity (Velocity _estimated) and the N-th angular velocity (Velocity _n) and the past value of the angular velocity difference (V _n) calculating an estimated angular velocity difference (V _estimated); further comprising,
Wherein the estimated angular velocity difference value (V _estimated ) is calculated through the following equation (12).
v _estimated = Abs (Velocity _estimated - Velocity _n ) - Abs (V _n )
Abs represents an absolute value. 12. The method of claim 11,
Wherein the step of calculating the final angular velocity comprises:
The measured angular velocity difference value (V _measured) is the estimated angular velocity difference value is smaller than (V _estimated), calculating the final angular velocity (Velocity _final) through formula (13) below, and the measured angular speed difference value (V _measured ) calculating the final angular velocity (Velocity _final) through formula (14) in this to the estimated angular velocity is greater than the difference value (V _estimated),;
Further comprising the steps of:
Velocity _final = Velocity _measured × k _velocity + Velocity _estimated (1-k _velocity ) (13)
Velocity _final = V _estimated x k _velocity + V _measured (1-k _velocity )
The k _velocity represents an angular velocity weight having a range of (0? K _velocity ? 1). 13. The method according to any one of claims 1 to 12,
Using the Euler angles and the final Euler angles (Euler _final) in the past and calculates the current Euler angles (Euler _N), the current angular velocity (Velocity _N) using the angular velocity and the final angular velocity (Velocity _final) of the exchange ;
Further comprising the steps of: 14. The method of claim 13,
Wherein the calculating the current Euler angle comprises:
The current through the equation (15) to (17) to apply the N-th Euler weight (keuler _{_n)} in the final Euler angles (Euler _final) and the N-th Euler angles (Euler _n) of the Euler angle of the past Specifying an Euler angle (Euler _N );
Wherein the dorsal posture control method comprises:
Euler _N = Euler _final x k _{euler_n} + Euler _n (1-k _{euler_n} )
Euler _n-1 = Euler _n (16)
Euler _n = Euler _N (17)
K _{euler_n} denotes an Nth Euler angular weight having a range of (0 ≤ k _{euler_n} ≤ 1). 14. The method of claim 13,
Wherein the calculating the current angular velocity comprises:
The final angular velocity (Velocity _final) and the angular velocity of the current through the N-th angular weighting formula to apply (Velocity _{_n)} (18) to (20) to the N-th angular velocity (Velocity _n) of the angular velocity of the past (Velocity _N );
Further comprising the steps of:
Velocity _N = Velocity _final × k _{Velocity_n} + Velocity _n (1-k _{velocity_n} )
Velocity _n-1 = Velocity _n (19)
Velocity _n = Velocity _N (20)
K _{velocity_n} represents an Nth angular velocity weight having a range of (0? K _{velocity_n} ? 1).

Images