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

Method for controlling attitude and drone atapted the same Download PDF

Info

Publication number
KR101880940B1
KR101880940B1 KR1020170143963A KR20170143963A KR101880940B1 KR 101880940 B1 KR101880940 B1 KR 101880940B1 KR 1020170143963 A KR1020170143963 A KR 1020170143963A KR 20170143963 A KR20170143963 A KR 20170143963A KR 101880940 B1 KR101880940 B1 KR 101880940B1
Authority
KR
South Korea
Prior art keywords
euler
velocity
estimated
measured
final
Prior art date
Application number
KR1020170143963A
Other languages
Korean (ko)
Inventor
이흥신
김태희
Original Assignee
드로젠(주)
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 드로젠(주) filed Critical 드로젠(주)
Priority to KR1020170143963A priority Critical patent/KR101880940B1/en
Application granted granted Critical
Publication of KR101880940B1 publication Critical patent/KR101880940B1/en

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64C—AEROPLANES; HELICOPTERS
    • B64C39/00—Aircraft not otherwise provided for
    • B64C39/02—Aircraft not otherwise provided for characterised by special use
    • B64C39/024—Aircraft not otherwise provided for characterised by special use of the remote controlled vehicle type, i.e. RPV
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P15/00—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration
    • G01P15/14—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration by making use of gyroscopes
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/04—Control of altitude or depth
    • G05D1/042—Control of altitude or depth specially adapted for aircraft

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}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.

일반적으로, 무인 비행체, 예컨대, 드론은 가속도계(accelerometer) 및 자이로스코프(gyroscope)를 장착하고, 이들을 통해 가속도와 각속도를 직접 취득하여 자세 제어를 위한 정보로서 활용한다.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.

한국공개특허 : 제2016-0068260호, 공개일자 : 2016년 06월 15일, 발명의 명칭 : 실내환경에서의 쿼드로터형 무인비행체의 자세 안정화 및 고도 제어.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.

전술한 여러 목적을 달성하기 위한 본 발명은, 가속도계로부터 측정된 측정 오일러 각(Eulermeasured)을 이용하여 현재의 추정 각속도(Velocityestimated)를 계산하는 단계; 자이로스코프로부터 측정된 측정 각속도(Velocitymeasured)를 이용하여 현재의 추정 오일러 각(Eulerestimated)을 계산하는 단계; 및 상기 현재의 추정 각속도, 추정 오일러 각, 상기 측정 오일러 각 및 상기 측정 각속도와 메모리에 기 저장된 과거 오일러 각과 각속도를 이용하여 최종 오일러 각(Eulerfinal) 및 최종 각속도(Velocityfinal)를 계산하는 단계를 포함하는 드론의 자세 제어 방법을 제공한다.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.

상기 측정 오일러 각은 공간 좌표 상(x,y,z)에서 하기의 식 (1) 및 식 (2)를 통해 계산된 Eulerroll 및 Eulerpith의 각을 포함할 수 있다.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)

Figure 112017108046810-pat00001
식 (1)
Figure 112017108046810-pat00001
Equation (1)

Figure 112017108046810-pat00002
식 (2)
Figure 112017108046810-pat00002
Equation (2)

이때, 상기 atan2는 tan-1이고, 상기 Accelx, Accely 및 Accelz는 x축, y축 및 z축의 가속도를 의미할 수 있다.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.

상기 현재의 추정 각속도는 하기의 식 (3)을 통해 산출될 수 있다.The current estimated angular velocity can be calculated through the following equation (3).

Velocityroll_estimated = (Eulerroll - Eulerroll-1)/△ＴVelocity roll_estimated = (Euler roll - Euler roll-1 ) /? T

Velocitypitch_estimated = (Eulerpitch - Eulerpitch-1)/△Ｔ 식 (3)Velocity pitch_estimated = (Euler pitch - Euler pitch-1 ) / DELTA T (3)

이때, 상기 현재의 추정 각속도는 roll과 pitch에 대해서 각각 산출된 값(Velocityroll_estimated, Velocitypitch_estimated)을 의미하고, 상기 △Ｔ는 시간 변위를 의미할 수 있다.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.

상기 현재의 추정 오일러 각은 하기의 식 (4)를 통해 산출될 수 있다.The current estimated Euler angles can be calculated through the following equation (4).

Figure 112017108046810-pat00003
식 (4)
Figure 112017108046810-pat00003
Equation (4)

이때, 상기 Fhdr은 Heuristic Drift Reduction Filter의 함수이고, 상기 Flpf는 Low Pass Filter의 함수이며, 상기 Vangle_t는 시간(t)의 특정 측정 시점에서의 가속도이고, 상기 △Ｔ는 시간 변위를 의미할 수 있다. 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.

상기 최종 오일러 각을 계산하는 단계는 메모리에 기 저장된 과거의 오일러 각 중 N 번째 오일러 각(Eulern)과 N-1 번째 오일러 각(Eulern-1)을 이용하여 과거 오일러 각 차이값(gn)을 계산하는 단계를 포함하고, 상기 과거 오일러 각 차이값(gn)은 하기의 식 (5)를 통해 계산될 수 있다.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).

gn=Eulern- Eulern-1 식 (5)g n = Euler n - Euler n-1 Equation (5)

상기 최종 오일러 각을 계산하는 단계는 상기 측정 오일러 각(Eulermeasured)과 상기 N번째 오일러 각(Eulern) 및 상기 과거 오일러 각 차이값(gn)을 이용하여 측정 오일러 각 차이값(gmeasured)을 계산하는 단계를 더 포함하고, 상기 측정 오일러 각 차이값(gmeasured)은 하기의 식 (6)을 통해 계산될 수 있다.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).

gmeasured = Abs(Abs(Eulermeasured - Eulern) - Abs(gn)) 식 (6)g measured = Abs (Euler measured - Euler n ) - Abs (g n )

이때, 상기 Abs는 절대값을 의미할 수 있다.At this time, Abs may mean an absolute value.

상기 최종 오일러 각을 계산하는 단계는 상기 추정 오일러 각(Eulerestimated)과 상기 N번째 오일러 각(Eulern) 및 상기 과거 오일러 각 차이값(gn)을 이용하여 추정 오일러 각 차이값(gestimated)을 계산하는 단계를 더 포함하고, 상기 추정 오일러 각 차이값(gestimated)은 하기의 식 (7)를 통해 계산될 수 있다.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).

gestimated = Abs(Abs(Eulerestimated - Eulern) - Abs(gn)) 식 (7)g estimated = Abs (Euler estimated - Euler n ) - Abs (g n )

상기 최종 오일러 각을 계산하는 단계는 상기 측정 오일러 각 차이값(gmeasured)이 상기 추정 오일러 각 차이값(gestimated)보다 작으면, 하기의 식 (8)을 통해 상기 최종 오일러 각(Eulerfinal)을 계산하고, 상기 측정 오일러 각 차이값(gmeasured)이 상기 추정 오일러 각 차이값(gestimated)보다 크면, 하기의 식 (9)를 통해 상기 최종 오일러 각(Eulerfinal)을 계산하는 단계를 더 포함할 수 있다.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 .

Eulerfinal = Eulermeasured×keuler + Eulerestimated(1-keuler) 식 (8)Euler final = Euler measured x k euler + Euler estimated (1-k euler )

Eulerfinal = Eulerestimated×keuler + Eulermeasured(1-keuler) 식 (9)Euler final = Euler estimated占 e euler + Euler measured (1-k euler )

이때, 상기 keuler은 상기 keuler은 (0 ≤ keuler ≤ 1)의 범위를 갖는 오일러 가중치를 의미할 수 있다.At this time, the k is the k euler euler can mean Euler weight in the range (0 ≤ k euler ≤ 1) .

상기 최종 각속도를 계산하는 단계는 메모리에 기 저장된 상기 과거의 각속도 중 N번째 각속도(Velocityn)와 N-1번째 각속도(Velocityn-1)를 이용하여 과거 각속도 차이값(Vn)을 계산하는 단계를 포함하고, 상기 과거 각속도 차이값(Vn)은 하기의 식 (10)를 통해 계산될 수 있다.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).

Vn = Velocityn - Velocityn-1 식 (10)V n = Velocity n - Velocity n-1 (10)

상기 최종 각속도를 계산하는 단계는 상기 측정 각속도(Velocitymeasured)와 상기 N번째 각속도(Velocityn) 및 상기 과거 각속도 차이값(Vn)을 이용하여 측정 각속도 차이값(Vmeasured)을 계산하는 단계를 더 포함하고, 상기 측정 각속도 차이값(Vmeasured)은 하기의 식 (11)를 통해 계산될 수 있다.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).

Vmeasured = Abs(Abs(Velocitymeasured - Velocityn) - Abs(vn)) 식 (11)V measured = Abs (Abs (Velocity measured - Velocity n ) - Abs (v n )

상기 최종 각속도를 계산하는 단계는 상기 추정 각속도(Velocityestimated)와 상기 N번째 각속도(Velocityn) 및 상기 과거 각속도 차이값(Vn)을 이용하여 추정 각속도 차이값(Vestimated)을 계산하는 단계를 더 포함하고, 상기 추정 각속도 차이값(Vestimated)은 하기의 식 (12)을 통해 계산될 수 있다.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).

vestimated = Abs(Abs(Velocityestimated - Velocityn) - Abs(Vn)) 식 (12)v estimated = Abs (Velocity estimated - Velocity n ) - Abs (V n )

상기 최종 각속도를 계산하는 단계는 상기 측정 각속도 차이값(Vmeasured)이 상기 추정 각속도 차이값(Vestimated)보다 작으면, 하기의 식 (13)를 통해 상기 최종 각속도(Velocityfinal)를 계산하고, 상기 측정 각속도 차이값(Vmeasured)이 상기 추정 각속도 차이값(Vestimated)보다 크면, 하기의 식 (14)를 통해 상기 최종 각속도(Velocityfinal)를 계산하는 단계를 더 포함할 수 있다.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.

Velocityfinal = Velocitymeasured × kvelocity + Velocityestimated(1-kvelocity) (13)Velocity final = Velocity measured × k velocity + Velocity estimated (1-k velocity ) (13)

Velocityfinal = Vestimated × kvelocity + Vmeasured(1-kvelocity) 식 (14)Velocity final = V estimated x k velocity + V measured (1-k velocity )

이때, 상기 kvelocity는 (0 ≤ kvelocity ≤ 1)의 범위를 갖는 각속도 가중치를 의미할 수 있다.Here, the k velocity may mean an angular velocity weight having a range of (0? K velocity ? 1).

상기 드론의 자세 제어 방법은 상기 과거의 오일러 각과 상기 최종 오일러 각(Eulerfinal)을 이용하여 현재의 오일러 각(EulerN)을 계산하고, 상기 과거의 각속도와 상기 최종 각속도(Velocityfinal)를 이용하여 현재의 각속도(VelocityN)를 계산하는 단계를 더 포함할 수 있다.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 ).

상기 현재의 오일러 각을 계산하는 단계는 상기 최종 오일러 각(Eulerfinal)과 상기 과거의 오일러 각 중 N번째 오일러 각(Eulern)에 N번째 오일러 가중치(keuler_n)를 적용한 하기의 식 (15) 내지 식 (17)를 통해 상기 현재의 오일러 각(EulerN)을 특정하는 단계를 포함할 수 있다.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).

EulerN = Eulerfinal × keuler_n + Eulern(1-keuler_n) 식 (15)Euler N = Euler final x k euler_n + Euler n (1-k euler_n )

Eulern-1 = Eulern 식 (16)Euler n-1 = Euler n (16)

Eulern = EulerN 식 (17)Euler n = Euler N (17)

이때, 상기 keuler_n은 (0 ≤ keuler_n ≤ 1)의 범위를 갖는 N번째 오일러 각 가중치를 의미할 수 있다.In this case, k euler_n may mean an Nth Euler angular weight having a range of (0 ≦ k euler_n ≦ 1).

상기 현재의 각속도를 계산하는 단계는 상기 최종 각속도(Velocityfinal)와 상기 과거의 각속도 중 N번째 각속도(Velocityn)에 N번째 각속도 가중치(Velocity_n)를 적용한 하기의 식 (18) 내지 식 (20)을 통해 상기 현재의 각속도(VelocityN)를 특정하는 단계를 더 포함할 수 있다.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.

VelocityN = Velocityfinal × kvelocity_n + Velocityn(1-kvelocity_n) 식 (18)Velocity N = Velocity final × k Velocity_n + Velocity n (1-k velocity_n )

Velocityn-1 = Velocityn 식 (19)Velocity n-1 = Velocity n (19)

Velocityn = VelocityN 식 (20)Velocity n = Velocity N (20)

이때, 상기 kvelocity_n은 (0 ≤ kvelocity_n ≤ 1)의 범위를 갖는 N번째 각속도 가중치를 의미할 수 있다.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.

이하에 첨부되는 도면들은 본 발명의 이해를 돕기 위한 것으로, 상세한 설명과 함께 발명들을 제공한다. 다만, 본 발명의 기술적 특징이 특정 도면에 한정되는 것은 아니며, 각 도면에서 개시하는 특징들은 서로 조합되어 새로운 발명으로 구성될 수 있다.
도 1은 본 발명의 자세 제어 방법을 예시적으로 나타낸 순서도이다.
도 2는 도 1의 자세 제어 방법을 실현하는 드론의 구성을 예시적으로 나타낸 도면이다.
도 3은 본 발명에 따른 130 단계의 최종 오일러 각을 산출하기 위한 알고리즘을 설명하기 위한 순서도이다.
도 4는 본 발명에 따른 130 단계의 최종 각속도를 산출하기 위한 알고리즘을 설명하기 위한 순서도이다.
도 5는 본 발명에 따른 140 단계의 자세 특정화 알고리즘을 설명하기 위한 순서도이다.
도 6은 본 발명의 자세 제어를 위한 드론의 구성을 예시적으로 나타낸 블럭 구성도이다.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.

도 1은 본 발명의 자세 제어 방법을 예시적으로 나타낸 순서도이고, 도 2는 도 1의 자세 제어 방법을 실현하는 드론의 구성을 예시적으로 나타낸 도면이다.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.

도 2는 도 1를 설명할 때 보조적으로 인용된다.Fig. 2 is ancillary references when describing Fig.

도 1를 참조하면, 본 발명에 따른 자세 제어 방법은 도 2의 드론의 제어기를 통해 실현되는 110 단계 내지 140 단계를 포함한다. 도 2에 도시된 드론은 가속도계(10) 및 자이로스코프(20), 제어기(30) 및 메모리(40)를 포함할 수 있다. 가속도계(10)는 드론의 움직임(자세 변화)에 대한 가속도를 측정하고, 자이로스코프는 드론의 움직임(자세 변화)에 대한 각속도를 측정한다.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.

이러면, 도 2에 도시된 제어기는 측정된 가속도 및 각속도를 상호 보완하여 드론의 자세 및 자세 방향을 추정하고, 특정하는 자세 변화 알고리즘을 수행하고, 이 결과에 따라 정밀한 자세 제어를 실시할 수 있다.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.

도 2에 도시된 메모리는 전술한 제어기의 명령에 따라 가속도계(10) 및 자이로스코프(20)에 의해 측정된 가속도 및 각속도를 누적시켜 과거 데이터로서 저장할 수 있고, 전술한 제어기에 의해 처리된 데이터를 저장할 수 있다.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.

이하에서는, 전술한 드론의 제어기에 의해 실현되는 도 1의 110 단계 내지 140 단계는 다음과 같다.Hereinafter, steps 110 to 140 of FIG. 1, which are realized by the above-described controller of the drones, are as follows.

110 단계에서, 드론의 제어기(30)는 가속도계(10)가 가속도 및 측정 오일러 각(Eulermeasured)을 측정하면, 상기 가속도계(10)로부터 측정된 가속도 및 측정 오일러 각(Eulermeasured)을 수신하여 메모리(40)에 저장시킬 수 있다.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).

언급된 측정 오일러 각(Eulermeasured)은 공간 좌표 상(x, y, z)의 가속도를 오일러 법칙에 적용한 하기의 수학식 1 및 수학식 2를 통해 계산된 Eulerroll 및 Eulerpith의 각을 포함할 수 있다.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) .

Figure 112017108046810-pat00004
(수학식 1)
Figure 112017108046810-pat00004
(1)

Figure 112017108046810-pat00005
(수학식 2)
Figure 112017108046810-pat00005
(2)

이때, 수학식 1 및 2에서의 atan2는 tan-1를 가리키고, Accelx, Accely 및 Accelz는 x축, y축 및 z축의 가속도를 가리킬 수 있다.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.

이로써, 드론의 제어기(30)는 가속도계(10)로부터 측정된 측정 오일러 각(Eulermeasured)을 이용하여 현재의 각속도를 추정하기 위한 추정 각속도(Velocityestimated)를 계산할 수 있다.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. [

예를 들면, 드론의 제어기(30)는 전술한 측정 오일러 각(Eulermeasured)에 대해 시간 변위(△Ｔ )에 따른 현재의 각속도를 추정하기 위한 추정 각속도(Velocityestimated)를 계산할 수 있는데, 하기의 수학식 3을 통해 현재의 추정 각속도(Velocityestimated)를 계산할 수 있다. 계산된 현재의 추정 각속도(Velocityestimated)는 메모리(40)에 저장됨은 물론이다.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.

Velocityroll_estimated = (Eulerroll - Eulerroll-1)/△ＴVelocity roll_estimated = (Euler roll - Euler roll-1 ) /? T

Velocitypitch_estimated = (Eulerpitch - Eulerpitch-1)/△Ｔ (수학식 3)Velocity pitch_estimated = (Euler pitch - Euler pitch - 1 ) /? T (Equation 3)

여기서, 현재의 추정 각속도(Velocityestimated)는 roll과 pitch에 대해서 각각 산출된 값(Velocityroll_estimated, Velocitypitch_estimated)을 가리키고, 상기 △Ｔ는 시간 변위를 가리키며, 상기 Eulerroll 및 Eulerpith는 현재의 측정 오일러 각(Eulermeasured)을 가리키고, Eulerroll -1 및 Eulerpith -1은 현재의 측정 오일러 각(Eulermeasured)의 이전 값을 가리킬 수 있다.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).

120 단계에서, 드론의 제어기(30)는 자이로스코프(20)가 드론의 각속도 및 측정 각속도(Velocitymeasured)를 측정하면, 상기 가속도계(10)로부터 측정된 각속도 및 측정 각속도(Velocitymeasured)를 수신하여 메모리(40)에 저장시킬 수 있다.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).

이로써, 드론의 제어기(30)는 자이로스코프(20)로부터 측정된 측정 각속도(Velocitymeasured)를 이용하여 현재의 오일러 각을 추정하기 위한 추정 오일러 각(Eulerestimated)을 계산할 수 있다.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.

예를 들면, 드론의 제어기(30)는 시간 t에서의 측정된 각속도(Vangle_t)와 Low Pass Filter의 함수 및 Heuristic Drift Reduction Filter의 함수를 적용한 하기의 수학식 4를 통해 현재의 추정 오일러 각(Eulerestimated)을 계산할 수 있다. 계산된 현재의 추정 오일러 각(Eulerestimated)은 메모리(40)에 저장됨은 물론이다.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.

Figure 112017108046810-pat00006
(수학식 4)
Figure 112017108046810-pat00006
(4)

이때, 수학식 4에서, 상기 Fhdr은 Heuristic Drift Reduction Filter의 함수이고, 상기 Flpf은 Low Pass Filter의 함수이며, 상기 Vangle_t는 시간(t)의 특정 측정 시점에서의 가속도이고, 상기 △Ｔ는 시간 변위를 가리킬 수 있다.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.

언급된, Heuristic Drift Reduction Filter의 함수(Flpf)는 Heuristic Drift Reduction Filter를 통해 자이로스코프(20)로부터 측정된 각속도에서 발생된 Heuristic Drift 노이즈 성분을 제거하기 위한 값을 의미할 수 있고, Low Pass Filter의 함수(Flpf)는 Low Pass Filter를 통해 자이로스코프(20)로부터 측정된 각 각속도에서 발생된 저역 노이즈 성분을 제거하기 위한 값을 의미할 수 있다.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.

130 단계에서, 드론의 제어기(30)는 전술한 110 단계 및 120 단계에 의해 계산된 현재의 추정 각속도(Velocityestimated), 추정 오일러 각(Eulerestimated)과 가속도계(10)로부터 측정된 측정 오일러 각(Eulermeasured)과, 자이로스코프(20)로부터 측정된 측정 각속도(Velocitymeasured) 및 메모리(40)에 기 저장된 과거 오일러 각과 각속도를 이용하여 최종 오일러 각(Eulerfinal) 및 최종 각속도(Velocityfinal)를 계산(산출)함으로써, 드론의 자세 변화(예: 드론의 자세 및 자세 방향)를 추정할 수 있다.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.

이중에서, 메모리(40)에 기 저장된 과거 오일러 각은 가속도계(10)를 통해 측정된 과거의 누적 가속도를 이용하여 예컨대, E1, E2, E3, ... En-1 및 En의 오일러 각을 의미할 수 있고, 과거의 각속도는 자이로스코프(20)를 통해 측정된 과거의 누적 각속도를 이용하여 예컨대, V1, V2, V3, ... Vn-1 및 Vn의 각속도를 의미할 수 있다.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.

이런 경우, 전술한 측정 오일러 각(Eulermeasured)은 En의 오일러 각 이후에 발생하는 En+1 번째의 오일러 각을 가리킬 수 있고, 전술한 측정 각속도(Velocitymeasured)는 Vn의 각속도 이후에 발생하는 Vn+1 번째의 각속도를 가리킬 수 있다.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 .

전술한 130 단계에 대해 구체적으로 설명하면 도 3 및 도 4와 같다.The above-described step 130 will be described in detail with reference to FIGS. 3 and 4. FIG.

도 3은 본 발명에 따른 130 단계의 최종 오일러 각을 계산하기 위한 알고리즘을 설명하기 위한 순서도이다.3 is a flowchart illustrating an algorithm for calculating a final Euler angle in step 130 according to the present invention.

도 3을 참조하면, 본 발명에 따른 130 단계는 드론의 제어기(30)에 의해 실현되는 131 단계 내지 136 단계를 포함할 수 있다.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.

먼저, 131 단계에서, 드론의 제어기(30)는 메모리(40)에 기 저장된 과거의 오일러 각 중 N번째 오일러 각(Eulern)과 N-1번째 오일러 각(Eulern-1)을 이용하여 과거 오일러 각 차이값(gn)을 계산할 수 있다.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.

예를 들면, 드론의 제어기(30)는 전술한 N번째 오일러 각(Eulern)과 N-1번째 오일러 각(Eulern-1)을 적용한 하기의 수학식 5를 통해 과거 오일러 각 차이값(gn)을 계산할 수 있다.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.

gn = Eulern- Eulern-1 (수학식 5)g n = Euler n - Euler n-1 (Equation 5)

132 단계에서, 드론의 제어기(30)는 전술한 과거의 오일러 각 중 N 번째 오일러 각(Eulern)과 N-1 번째 오일러 각(Eulern-1)을 이용하여 과거 오일러 각 차이값(gn)을 계산할 수 있다.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.

예를 들면, 드론의 제어기(30)는 가속도계(10)로부터 측정된 측정 오일러 각(Eulermeasured)과 과거 오일러 각 데이터인 N번째 오일러 각(Eulern) 및 전술한 131 단계에 의해 구해진 과거 오일러 각 차이값(gn)을 적용한 하기의 수학식 6을 통해 측정 오일러 각 차이값(gmeasured)을 계산할 수 있다.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 .

gmeasured = Abs(Abs(Eulermeasured - Eulern) - Abs(gn)) (수학식 6)g measured = Abs (Euler measured - Euler n ) - Abs (g n )

이때, 수학식 6에서 Abs는 절대값을 가리킬 수 있다.At this time, Abs in Equation (6) may indicate an absolute value.

133 단계에서, 드론의 제어기(30)는 전술한 추정 오일러 각(Eulerestimated)과 N번째 오일러 각(Eulern) 및 과거 오일러 각 차이값(gn)을 이용하여 추정 오일러 각 차이값(gestimated)을 계산할 수 있다.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.

예를 들면, 드론의 제어기(30)는 전술한 120 단계에 의해 구해진 추정 오일러 각(Eulerestimated)과 과거 오일러 각 데이터인 N번째 오일러 각(Eulern) 및 전술한 131 단계에 의해 구해진 과거 오일러 각 차이값(gn)을 적용한 하기의 수학식 7를 통해 추정 오일러 각 차이값(gestimated)을 계산할 수 있다.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 .

gestimated = Abs(Abs(Eulerestimated - Eulern) - Abs(gn)) (수학식 7)g estimated = Abs (Euler estimated - Euler n ) - Abs (g n )

이때, 수학식 7에서 Abs는 절대값을 가리킬 수 있다.At this time, Abs in Equation (7) may indicate an absolute value.

134 단계에서, 드론의 제어기(30)는 전술한 132 단계에 의해 구해진 측정 오일러 각 차이값(gmeasured)이 전술한 133 단계에 의해 구해진 추정 오일러 각 차이값(gestimated)보다 작은지의 여부를 판단할 수 있다.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.

예를 들면, 135 단계에서, 드론의 제어기(30)는 측정 오일러 각 차이값(gmeasured)이 추정 오일러 각 차이값(gestimated)보다 작으면, 하기의 수학식 8를 통해 최종 오일러 각(Eulerfinal)을 계산할 수 있다.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.

Eulerfinal = Eulermeasured×keuler + Eulerestimated(1-keuler) (수학식 8)Euler final = Euler measured占 k euler + Euler estimated (1-k euler ) (8)

예를 들면, 136 단계에서, 드론의 제어기(30)는 측정 오일러 각 차이값(gmeasured)이 추정 오일러 각 차이값(gestimated)보다 크면, 하기의 수학식 9를 통해 최종 오일러 각(Eulerfinal)을 계산할 수 있다.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.

Eulerfinal = Eulerestimated×keuler + Eulermeasured(1-keuler) (수학식 9)Euler final = Euler estimated占 e euler + Euler measured (1-k euler ) (9)

이때, 수학식 8 및 9에서 언급된 keuler은 (0 ≤ keuler ≤ 1)의 범위를 갖는 오일러 가중치를 가리킬 수 있다.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).

이와 같이, 본 실시예에서는 전술한 131 단계 내지 136 단계를 통해 최종 오일러 각(Eulerfinal)을 계산하면, 이를 통해 드론의 현재 자세(Eulerfinal)를 정확히 추정할 수 있을 것이다.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는 본 발명에 따른 130 단계의 최종 각속도를 산출하기 위한 알고리즘을 설명하기 위한 순서도이다.4 is a flowchart illustrating an algorithm for calculating a final angular velocity in step 130 according to the present invention.

도 4를 참조하면, 본 발명에 따른 130 단계는 드론의 제어기(30)에 의해 실현되는 131a 단계 내지 136f 단계를 더 포함할 수 있다.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.

131a 단계에서, 드론의 제어기(30)는 메모리(40)에 기 저장된 과거의 각속도 중 N번째 각속도(Velocityn)와 N-1번째 각속도(Velocityn-1)를 이용하여 과거 각속도 차이값(Vn)을 계산할 수 있다.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.

예를 들면, 드론의 제어기(30)는 과거의 결과 데이터인 N번째 각속도(Velocityn)와 N-1번째 각속도(Velocityn-1)를 적용한 하기의 수학식 10을 통해 과거 각속도 차이값(Vn)을 계산할 수 있다.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.

Vn = Velocityn - Velocityn-1 (수학식 10)V n = Velocity n - Velocity n-1 (10)

132b 단계에서, 드론의 제어기(30)는 전술한 측정 각속도(Velocitymeasured)와 N번째 각속도(Velocityn) 및 과거 각속도 차이값(Vn)을 이용하여 측정 각속도 차이값(Vmeasured)을 계산할 수 있다.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.

예를 들면, 드론의 제어기(30)는 전술한 120 단계에 의해 구해진 측정 각속도(Velocitymeasured)와 과거 각속도 데이터인 N번째 각속도(Velocityn) 및 전술한 131a 단계에 의해 구해진 과거 각속도 차이값(Vn)을 적용한 하기의 수학식 11를 통해 측정 각속도 차이값(Vmeasured)을 계산할 수 있다.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).

Vmeasured = Abs(Abs(Velocitymeasured - Velocityn) - Abs(vn)) (수학식 11)V measured = Abs (Abs (Velocity measured - Velocity n ) - Abs (v n )

이때, 수학식 11에서 언급된 Abs는 절대값을 가리킬 수 있다.At this time, the Abs mentioned in Equation (11) may indicate an absolute value.

133c 단계에서, 드론의 제어기(30)는 전술한 추정 각속도(Velocityestimated)와 N번째 각속도(Velocityn) 및 과거 각속도 차이값(Vn)을 이용하여 추정 각속도 차이값(Vestimated)을 계산할 수 있다.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.

예를 들면, 드론의 제어기(30)는 전술한 110 단계에 의해 구해진 추정 각속도(Velocityestimated)와 과거 각속도 데이터인 N번째 각속도(Velocityn) 및 전술한 131a 단계에 의해 구해진 과거 각속도 차이값(Vn)을 적용한 하기의 수학식 12를 통해 추정 각속도 차이값(Vestimated)을 계산할 수 있다.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).

Vestimated = Abs(Abs(Velocityestimated - Velocityn) - Abs(Vn)) (수학식 12)V estimated = Abs (Velocity estimated - Velocity n ) - Abs (V n )

134d 단계에서, 드론의 제어기(30)는 전술한 132b 단계에 의해 구해진 측정 각속도 차이값(Vmeasured)이 전술한 133c 단계에 의해 구해진 추정 각속도 차이값(Vestimated)보다 작은지의 여부를 판단할 수 있다.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.

예를 들면, 135e 단계에서, 드론의 제어기(30)는 측정 각속도 차이값(Vmeasured)이 추정 각속도 차이값(Vestimated)보다 작으면, 하기의 수학식 13을 통해 최종 각속도(Velocityfinal)를 계산할 수 있다.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.

Velocityfinal = Velocitymeasured × kvelocity + Velocityestimated(1-kvelocity) (수학식 13)Velocity final = Velocity measured占velocity占 Velocity estimated (1-k velocity ) (Equation 13)

예를 들면, 135e 단계에서, 드론의 제어기(30)는 측정 각속도 차이값(Vmeasured)이 추정 각속도 차이값(Vestimated)보다 크면, 하기의 식 (14)를 통해 최종 각속도(Velocityfinal)를 계산할 수 있다.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.

Velocityfinal = Vestimated × kvelocity + Vmeasured(1-kvelocity) (수학식 14)Velocity final = V estimated x k velocity + V measured (1-k velocity ) (14)

이때, 수학식 13과 14에서 언급된 kvelocity는 (0 ≤ kvelocity ≤ 1)의 범위를 갖는 각속도 가중치를 가리킬 수 있다.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).

이와 같이, 본 실시예에서는 전술한 131a 단계 내지 136f 단계를 통해 최종 각속도(Velocityfinal)를 계산하면, 이를 통해 드론의 현재 자세의 변화 방향을 정확히 추정할 수 있을 것이다.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.

한편, 다시 도 1로 돌아와, 나머지 단계인 140 단계를 살펴본다.On the other hand, referring back to FIG. 1, the remaining step 140 will be described.

본 발명에 따른 140 단계에서, 드론의 제어기(30)는 측정 및 추정의 오차로 인한 영향을 최소화하기 위하여 앞서 설명한 과거의 오일러 각(N번째 오일러 각(Eulern), N-1번째 오일러 각(Eulern-1))과 최종 오일러 각(Eulerfinal)을 이용하여 한번 더 상호 보완함으로써, 드론의 현재 자세와 관련하여 보다 정밀한 현재의 오일러 각(EulerN)을 계산할 수 있다.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.

게다가, 140 단계에서, 드론의 제어기(30)는 측정 및 추정의 오차로 인한 영향을 최소화하기 위하여 앞서 설명한 과거의 각속도(예 : N번째 각속도(Velocityn), N-1번째 각속도(Velocityn-1))와 최종 각속도(Velocityfinal)를 이용하여 한번 더 상호 보완함으로써, 드론의 자세의 변화 방향과 관련하여 보다 정밀한 현재의 각속도(VelocityN)를 계산할 수 있다.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.

이러한 140 단계를 구체적으로 살펴보면 다음과 같다.These 140 steps are as follows.

도 5는 본 발명에 따른 140 단계의 자세 특정화 알고리즘을 설명하기 위한 순서도이다.5 is a flowchart for explaining a posture specifying algorithm in step 140 according to the present invention.

도 5를 참조하면, 본 발명에 따른 140 단계는 과거의 값과 확정된 추정값을 한번 더 상호 보완하기 위하여 드론의 제어기(30)에 의해 실현되는 141 단계 내지 142 단계를 포함할 수 있다.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.

141 단계에서, 드론의 제어기(30)는 전술한 130 단계에 의해 구해진 최종 오일러 각(Eulerfinal)과 과거의 오일러 각 중 N번째 오일러 각(Eulern)에 N번째 오일러 가중치(keuler_n)를 적용한 하기의 수학식 15 내지 수학식 17를 통해 현재의 오일러 각(EulerN)을 계산할 수 있다.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).

EulerN = Eulerfinal × keuler_n + Eulern(1-keuler_n) (수학식 15)Euler N = Euler final x k euler_n + Euler n (1-k euler_n ) (15)

Eulern-1 = Eulern (수학식 16)Euler n-1 = Euler n (Equation 16)

Eulern = EulerN (수학식 17)Euler n = Euler N (Equation 17)

이때, 수학식 15에서 언급된 keuler_n은 (0 ≤ keuler_n ≤ 1)의 범위를 갖는 N번째 오일러 각 가중치를 가리킬 수 있다.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.

이와 같이, 본 실시예에서는 전술한 바와 같이 현재의 오일러 각(EulerN)을 특정함으로써, 전술한 130 단계에 비하여 보다 더 정밀한 드론의 현재 자세를 정확히 예측할 수 있을 것이다.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.

142 단계에서, 드론의 제어기(30)는 전술한 130 단계에 의해 구해진 최종 각속도(Velocityfinal)와 과거의 각속도 중 N번째 각속도(Velocityn)에 N번째 각속도 가중치(Velocity_n)를 적용한 하기의 수학식 18 내지 수학식 20을 통해 현재의 각속도(VelocityN)를 계산할 수 있다.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).

VelocityN = Velocityfinal × kvelocity_n + Velocityn(1-kvelocity_n) (수학식 18)Velocity N = Velocity final x k velocity_n + Velocity n (1-k velocity_n ) (Equation 18)

Velocityn-1 = Velocityn (수학식 19)Velocity n-1 = Velocity n (Equation 19)

Velocityn = VelocityN (수학식 20)Velocity n = Velocity N (Equation 20)

이때, 수학식 18에서 언급된 kvelocity_n은 (0 ≤ kvelocity_n ≤ 1)의 범위를 갖는 N번째 각속도 가중치를 가리킬 수 있다.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).

이와 같이, 본 실시예에서는 현재의 각속도(VelocityN)를 특정함으로써, 전술한 130 단계에 비하여 보다 정밀한 드론의 현재 자세의 변화 방향을 예측할 수 있을 것이다.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.

언급된 저장 매체는 RAM, 플래시 메모리, ROM, EPROM, 전기적으로 소거 가능한 판독 전용 메모리("EEPROM"), 레지스터, 하드 디스크, 착탈식 디스크, 콤팩트 디스크 판독 전용 메모리("CD-ROM"), 또는 공지된 임의의 기타 형태의 저장 매체를 포함할 수 있다.(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.

언급된 컴퓨터 저장 매체는, 컴퓨터 판독 가능 명령어, 데이터 구조, 프로그램 모듈 또는 그 밖의 데이터와 같은 정보를 저장하기 위한 임의의 방법이나 기술로 구현되는 착탈형(removable)과 고정형(non-removable), 및 휘발성과 비휘발성 매체를 포함한다.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.

이러한 컴퓨터 저장 매체는 RAM, ROM, EPROM, EEPROM, 플래시 메모리, 다른 고체 메모리 기술, CDROM, 디지털 다용도 디스크(DVD), 또는 다른 광 저장 장치, 자기 카세트, 자기 테이프, 자기 디스크 저장 장치 등과 같은 프로그램 명령어를 저장하고 수행하도록 특별히 구성된 하드웨어 장치를 포함할 수 있다.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.

도 6은 본 발명의 자세 제어를 위한 드론의 구성을 예시적으로 나타낸 블럭 구성도이다.FIG. 6 is a block diagram showing a configuration of a drones for attitude control according to the present invention.

도 6을 참조하면, 본 발명에 따른 드론(200)은 가속도계(210), 각속도 추정부(220), 자이로스코프(230), 오일러 각 추정부(240), 자세 변화 추정부(250), 자세 변화 특정부(260) 및 메모리(270)를 포함할 수 있다.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.

먼저, 가속도계(210)는 드론의 움직임(자세 변화)에 대한 가속도 및 측정 오일러 각(Eulermeasured)을 측정할 수 있다. 이러한 가속도계(210)는 통상적으로 널리 알려진 적어도 하나의 가속도 센서일 수도 있다.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.

각속도 추정부(220)는 가속도계(210)로부터 측정된 측정 오일러 각(Eulermeasured)을 이용하여 시간 변위(△Ｔ )에 따른 현재의 각속도를 추정하기 위한 추정 각속도(Velocityestimated)를 계산할 수 있다. 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. [

언급된 측정 오일러 각(Eulermeasured) 및 추정 각속도(Velocityestimated)의 계산은 앞서 도 1에서 충분히 설명하였기 때문에, 그 설명은 생략하지만, 본 실시예에서도 적용됨은 물론이다.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.

자이로스코프(230)는 드론의 움직임(자세 변화)에 대한 드론의 각속도 및 측정 각속도(Velocitymeasured)를 측정할 수 있다. 이러한 자이로스코프(230)는 통상적으로 널리 알려진 적어도 하나의 각속도 센서일 수도 있다.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.

오일러 각 추정부(240)는 자이로스코프(230)로부터 측정된 측정 각속도(Velocitymeasured)를 이용하여 현재의 오일러 각을 추정하기 위한 현재의 추정 오일러 각(Eulerestimated)을 계산할 수 있다. 언급된 현재의 추정 오일러 각(Eulerestimated)의 계산 과정은 앞서 도 1에서 충분히 설명하였기 때문에 그 설명은 생략하지만, 본 실시예에서도 적용됨은 물론이다.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.

자세 변화 추정부(250)는 전술한 현재의 추정 각속도(Velocityestimated), 추정 오일러 각(Eulerestimated)과 가속도계(210)로부터 측정된 측정 오일러 각(Eulermeasured)과, 자이로스코프(230)로부터 측정된 측정 각속도(Velocitymeasured) 및 메모리(270)에 기 저장된 과거 오일러 각과 각속도를 상호 보완하는 알고리즘을 통해 최종 오일러 각(Eulerfinal) 및 최종 각속도(Velocityfinal)를 계산(산출)할 수 있다.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.

언급된 최종 오일러 각(Eulerfinal) 및 최종 각속도(Velocityfinal)의 계산 과정은 앞서 도 1, 내지 도 4에서 충분히 설명하였기 때문에 그 설명은 생략하지만, 본 실시예에서도 적용된은 물론이다.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.

이와 같이, 본 실시예에서는 최종 오일러 각(Eulerfinal)이 계산되면, 이를 통해 드론의 현재 자세(Eulerfinal)를 정확히 추정할 수 있고, 최종 각속도(Velocityfinal)가 계산되면, 이를 통해 드론의 현재 자세의 변화 방향을 정확히 추정할 수 있을 것이다.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.

자세 변화 특정부(260)는 측정 및 추정의 오차로 인한 영향을 최소화하기 위하여 전술한 과거의 오일러 각(N번째 오일러 각(Eulern), N-1번째 오일러 각(Eulern-1))과 최종 오일러 각(Eulerfinal)을 이용하여 현재의 오일러 각(EulerN)을 계산하고, 전술한 과거의 각속도(예 : N번째 각속도(Velocityn), N-1번째 각속도(Velocityn-1))와 최종 각속도(Velocityfinal)를 이용하여 현재의 각속도(VelocityN)를 계산할 수 있다.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 ).

언급된 현재의 오일러 각(EulerN)과 현재의 각속도(VelocityN)의 계산 과정은 앞서 도 5에서 충분히 설명하였기 때문에 그 설명은 생략하지만, 본 실시예에서도 적용됨은 물론이다.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.

이와 같이, 본 실시예에서는 전술한 바와 같이 현재의 오일러 각(EulerN)을 특정함으로써, 전술한 자세 변화 추정부(250)의 결과값에 비하여 비하여 보다 더 정밀한 드론의 현재 자세를 정확히 예측할 수 있고, 전술한 바와 같이 현재의 각속도(VelocityN)를 특정함으로써, 전술한 자세 변화 추정부(250)의 결과값에 비하여 보다 정밀한 드론의 현재 자세의 변화 방향을 예측할 수 있을 것이다.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.

마지막으로, 메모리(270)는 전술한 가속도계(210), 각속도 추정부(220), 자이로스코프(230), 오일러 각 추정부(240), 자세 변화 추정부(250) 및 자세 변화 특정부(260)에 의해 처리된 데이터를 저장하며, 가속도계(210) 및 자이로스코프(230)로부터 측정된 과거의 가속도와 각속도 데이터 및 과거의 오일러 각과 각속도를 더 저장할 수 있다.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.

이러한 메모리(270)는 휘발성 메모리 또는 비휘발성 메모리 또는 이들을 조합으로 이루어질 수 있다.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 : 가속도계 20,230: 자이로스코프
30 : 제어기 40,270 : 메모리
200 : 드론 220 : 각속도 추정부
240 : 오일러 각 추정부 250 : 자세 변화 추정부
260 : 자세 변화 특정부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)

가속도계로부터 측정된 측정 오일러 각(Eulermeasured)을 이용하여 현재의 추정 각속도(Velocityestimated)를 계산하는 단계;
자이로스코프로부터 측정된 측정 각속도(Velocitymeasured)를 이용하여 현재의 추정 오일러 각(Eulerestimated)을 계산하는 단계; 및
상기 현재의 추정 각속도와 상기 측정 각속도 및 메모리에 기 저장된 과거 각속도를 이용하여 드론의 현재 자세를 예측하기 위한 최종 각속도(Velocityfinal)를 계산하고, 상기 추정 오일러 각과 상기 측정 오일러 각 및 상기 메모리에 기 저장된 과거 오일러 각을 이용하여 드론의 현재 자세의 변화 방향을 예측하기 위한 최종 오일러 각(Eulerfinal)을 계산하는 단계;
를 포함하고,
상기 최종 오일러 각을 계산하는 단계는,
상기 과거 오일러 각 중 N 번째 오일러 각(Eulern)과 N-1 번째 오일러 각(Eulern-1)을 이용하여 과거 오일러 각 차이값(gn)을 계산하는 단계;를 포함하고,
상기 과거 오일러 각 차이값(gn)은, 하기의 식 (5)를 통해 계산되는 것을 특징으로 하는, 드론의 자세 제어 방법.
gn=Eulern- Eulern-1 식 (5)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) 제1항에 있어서,
상기 측정 오일러 각은,
공간 좌표 상(x,y,z)에서 하기의 식 (1) 및 식 (2)를 통해 계산된 Eulerroll 및 Eulerpith의 각을 포함하는 것을 특징으로 하는, 드론의 자세 제어 방법.
Figure 112017108046810-pat00007
식 (1)
Figure 112017108046810-pat00008
식 (2)
상기 atan2는 tan-1이고, 상기 Accelx, Accely 및 Accelz는 x축, y축 및 z축의 가속도를 나타냄.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).
Figure 112017108046810-pat00007
Equation (1)
Figure 112017108046810-pat00008
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. 제2항에 있어서,
상기 현재의 추정 각속도는, 하기의 식 (3)을 통해 산출되는 것을 특징으로 하는, 드론의 자세 제어 방법.
Velocityroll_estimated = (Eulerroll - Eulerroll-1)/△Ｔ
Velocitypitch_estimated = (Eulerpitch - Eulerpitch-1)/△Ｔ 식 (3)
상기 현재의 추정 각속도는 roll과 pitch에 대해서 각각 산출된 값(Velocityroll_estimated, Velocitypitch_estimated)을 가리키고, 상기 △Ｔ는 시간 변위임.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 . 제3항에 있어서,
상기 현재의 추정 오일러 각은, 하기의 식 (4)를 통해 산출되는 것을 특징으로 하는, 드론의 자세 제어 방법.
Figure 112018028887095-pat00009
식 (4)
상기 Eulerestimated는 상기 Velocityroll_estimated 및 Velocitypitch_estimated 를 나타내고, 상기 Fhdr은 Heuristic Drift Reduction Filter의 함수이고, 상기 Flpf은 Low Pass Filter의 함수이며, 상기 Vangle_t는 시간(t)의 특정 측정 시점에서의 가속도이고, 상기 △Ｔ는 시간 변위를 나타냄.The method of claim 3,
Wherein the current estimated Euler angle is calculated through the following equation (4).
Figure 112018028887095-pat00009
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 제1항에 있어서,
상기 최종 오일러 각을 계산하는 단계는,
상기 측정 오일러 각(Eulermeasured)과 상기 N번째 오일러 각(Eulern) 및 상기 과거 오일러 각 차이값(gn)을 이용하여 측정 오일러 각 차이값(gmeasured)을 계산하는 단계;를 더 포함하고,
상기 측정 오일러 각 차이값(gmeasured)은, 하기의 식 (6)을 통해 계산되는 것을 특징으로 하는, 드론의 자세 제어 방법.
gmeasured = Abs(Abs(Eulermeasured - Eulern) - Abs(gn)) 식 (6)
상기 Abs는 절대값을 나타냄.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. 제6항에 있어서,
상기 최종 오일러 각을 계산하는 단계는,
상기 추정 오일러 각(Eulerestimated)과 상기 N번째 오일러 각(Eulern) 및 상기 과거 오일러 각 차이값(gn)을 이용하여 추정 오일러 각 차이값(gestimated)을 계산하는 단계;를 더 포함하고,
상기 추정 오일러 각 차이값(gestimated)은, 하기의 식 (7)를 통해 계산되는 것을 특징으로 하는, 드론의 자세 제어 방법.
gestimated = Abs(Abs(Eulerestimated - Eulern) - Abs(gn)) 식 (7)
상기 Abs는 절대값을 나타냄.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. 제7항에 있어서,
상기 최종 오일러 각을 계산하는 단계는,
상기 측정 오일러 각 차이값(gmeasured)이 상기 추정 오일러 각 차이값(gestimated)보다 작으면, 하기의 식 (8)을 통해 상기 최종 오일러 각(Eulerfinal)을 계산하고,
상기 측정 오일러 각 차이값(gmeasured)이 상기 추정 오일러 각 차이값(gestimated)보다 크면, 하기의 식 (9)를 통해 상기 최종 오일러 각(Eulerfinal)을 계산하는 단계;
를 더 포함하는, 드론의 자세 제어 방법.
Eulerfinal = Eulermeasured×keuler + Eulerestimated(1-keuler) 식 (8)
Eulerfinal = Eulerestimated×keuler + Eulermeasured(1-keuler) 식 (9)
상기 keuler은 (0 ≤ keuler ≤ 1)의 범위를 갖는 오일러 가중치를 나타냄.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). 제1항에 있어서,
상기 최종 각속도를 계산하는 단계는,
메모리에 기 저장된 상기 과거의 각속도 중 N번째 각속도(Velocityn)와 N-1번째 각속도(Velocityn-1)를 이용하여 과거 각속도 차이값(Vn)을 계산하는 단계;를 포함하고,
상기 과거 각속도 차이값(Vn)은, 하기의 식 (10)를 통해 계산되는 것을 특징으로 하는, 드론의 자세 제어 방법.
Vn = Velocityn - Velocityn-1 식 (10)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) 제9항에 있어서,
상기 최종 각속도를 계산하는 단계는,
상기 측정 각속도(Velocitymeasured)와 상기 N번째 각속도(Velocityn) 및 상기 과거 각속도 차이값(Vn)을 이용하여 측정 각속도 차이값(Vmeasured)을 계산하는 단계;를 더 포함하고,
상기 측정 각속도 차이값(Vmeasured)은, 하기의 식 (11)를 통해 계산되는 것을 특징으로 하는, 드론의 자세 제어 방법.
Vmeasured = Abs(Abs(Velocitymeasured - Velocityn) - Abs(vn)) 식 (11)
상기 Abs는 절대값을 나타냄.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. 제10항에 있어서,
상기 최종 각속도를 계산하는 단계는,
상기 추정 각속도(Velocityestimated)와 상기 N번째 각속도(Velocityn) 및 상기 과거 각속도 차이값(Vn)을 이용하여 추정 각속도 차이값(Vestimated)을 계산하는 단계;를 더 포함하고,
상기 추정 각속도 차이값(Vestimated)은, 하기의 식 (12)을 통해 계산되는 것을 특징으로 하는, 드론의 자세 제어 방법.
vestimated = Abs(Abs(Velocityestimated - Velocityn) - Abs(Vn)) 식 (12)
상기 Abs는 절대값을 나타냄.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. 제11항에 있어서,
상기 최종 각속도를 계산하는 단계는,
상기 측정 각속도 차이값(Vmeasured)이 상기 추정 각속도 차이값(Vestimated)보다 작으면, 하기의 식 (13)를 통해 상기 최종 각속도(Velocityfinal)를 계산하고, 상기 측정 각속도 차이값(Vmeasured)이 상기 추정 각속도 차이값(Vestimated)보다 크면, 하기의 식 (14)를 통해 상기 최종 각속도(Velocityfinal)를 계산하는 단계;
를 더 포함하는, 드론의 자세 제어 방법.
Velocityfinal = Velocitymeasured × kvelocity + Velocityestimated(1-kvelocity) (13)
Velocityfinal = Vestimated × kvelocity + Vmeasured(1-kvelocity) 식 (14)
상기 kvelocity는 (0 ≤ kvelocity ≤ 1)의 범위를 갖는 각속도 가중치를 나타냄.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). 제1항 내지 제12항 중 어느 한 항에 있어서,
상기 과거의 오일러 각과 상기 최종 오일러 각(Eulerfinal)을 이용하여 현재의 오일러 각(EulerN)을 계산하고, 상기 과거의 각속도와 상기 최종 각속도(Velocityfinal)를 이용하여 현재의 각속도(VelocityN)를 계산하는 단계;
를 더 포함하는, 드론의 자세 제어 방법.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: 제13항에 있어서,
상기 현재의 오일러 각을 계산하는 단계는,
상기 최종 오일러 각(Eulerfinal)과 상기 과거의 오일러 각 중 N번째 오일러 각(Eulern)에 N번째 오일러 가중치(keuler_n)를 적용한 하기의 식 (15) 내지 식 (17)를 통해 상기 현재의 오일러 각(EulerN)을 특정하는 단계;
를 포함하는, 드론의 자세 제어 방법.
EulerN = Eulerfinal × keuler_n + Eulern(1-keuler_n) 식 (15)
Eulern-1 = Eulern 식 (16)
Eulern = EulerN 식 (17)
상기 keuler_n은 (0 ≤ keuler_n ≤ 1)의 범위를 갖는 N번째 오일러 각 가중치를 나타냄.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). 제13항에 있어서,
상기 현재의 각속도를 계산하는 단계는,
상기 최종 각속도(Velocityfinal)와 상기 과거의 각속도 중 N번째 각속도(Velocityn)에 N번째 각속도 가중치(Velocity_n)를 적용한 하기의 식 (18) 내지 식 (20)을 통해 상기 현재의 각속도(VelocityN)를 특정하는 단계;
를 더 포함하는, 드론의 자세 제어 방법.
VelocityN = Velocityfinal × kvelocity_n + Velocityn(1-kvelocity_n) 식 (18)
Velocityn-1 = Velocityn 식 (19)
Velocityn = VelocityN 식 (20)
상기 kvelocity_n은 (0 ≤ kvelocity_n ≤ 1)의 범위를 갖는 N번째 각속도 가중치를 나타냄.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).
KR1020170143963A 2017-10-31 2017-10-31 Method for controlling attitude and drone atapted the same KR101880940B1 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
KR1020170143963A KR101880940B1 (en) 2017-10-31 2017-10-31 Method for controlling attitude and drone atapted the same

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR1020170143963A KR101880940B1 (en) 2017-10-31 2017-10-31 Method for controlling attitude and drone atapted the same

Publications (1)

Publication Number Publication Date
KR101880940B1 true KR101880940B1 (en) 2018-07-23

Family

ID=63102849

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020170143963A KR101880940B1 (en) 2017-10-31 2017-10-31 Method for controlling attitude and drone atapted the same

Country Status (1)

Country Link
KR (1) KR101880940B1 (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112470091A (en) * 2018-08-22 2021-03-09 日本电气株式会社 Selection device, selection method, and selection program
KR20240019538A (en) 2022-08-04 2024-02-14 세종대학교산학협력단 Sensor fusion device and and multi-sensor system having same

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20060081509A (en) * 2005-01-07 2006-07-13 (주)마이크로인피니티 Method for measuring attitude of inputting device and apparatus thereof
WO2008142757A1 (en) * 2007-05-18 2008-11-27 Churyo Engineering Co., Ltd. Method for measuring vehicle posture angle by single gps and inertial data (acceleration, angular speed)
JP5610847B2 (en) * 2010-05-26 2014-10-22 三菱電機株式会社 Angular velocity estimation apparatus, computer program, and angular velocity estimation method
JP2016509211A (en) * 2013-01-07 2016-03-24 キオニクス，インコーポレイテッド Angular velocity estimation using magnetometer and accelerometer
KR20160068260A (en) 2014-12-05 2016-06-15 부산대학교 산학협력단 Attitude stabilization and altitude control of a quad-rotor type unmanned aerial vehicle in an indoor environment

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20060081509A (en) * 2005-01-07 2006-07-13 (주)마이크로인피니티 Method for measuring attitude of inputting device and apparatus thereof
WO2008142757A1 (en) * 2007-05-18 2008-11-27 Churyo Engineering Co., Ltd. Method for measuring vehicle posture angle by single gps and inertial data (acceleration, angular speed)
JP5610847B2 (en) * 2010-05-26 2014-10-22 三菱電機株式会社 Angular velocity estimation apparatus, computer program, and angular velocity estimation method
JP2016509211A (en) * 2013-01-07 2016-03-24 キオニクス，インコーポレイテッド Angular velocity estimation using magnetometer and accelerometer
KR20160068260A (en) 2014-12-05 2016-06-15 부산대학교 산학협력단 Attitude stabilization and altitude control of a quad-rotor type unmanned aerial vehicle in an indoor environment

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112470091A (en) * 2018-08-22 2021-03-09 日本电气株式会社 Selection device, selection method, and selection program
KR20240019538A (en) 2022-08-04 2024-02-14 세종대학교산학협력단 Sensor fusion device and and multi-sensor system having same

Similar Documents

Publication Publication Date Title
CN108573270B (en) Method and apparatus for synchronizing multi-sensor target information fusion and multi-sensor sensing, computer device, and recording medium
US7421343B2 (en) Systems and methods for reducing vibration-induced errors in inertial sensors
US7543392B2 (en) Motion estimation method and system for mobile body
US7860651B2 (en) Enhanced inertial system performance
EP3321631B1 (en) A inertial and terrain based navigation system
Ryu et al. Vehicle sideslip and roll parameter estimation using GPS
US6928382B2 (en) Method and apparatus for measuring speed of moving body using accelerometer
EP2519803B1 (en) Technique for calibrating dead reckoning positioning data
US9205742B2 (en) Apparatus and method for correcting offset of yaw rate sensor and system for controlling speed of vehicle with the said apparatus
KR20190042247A (en) Device and method to estimate position
US20050209775A1 (en) Method for determining altitude or road grade information in a motor vehicle
WO2010073300A1 (en) Travel route estimation device and travel route estimation method used in the device
CN110346824B (en) Vehicle navigation method, system and device and readable storage medium
KR20060048581A (en) System for navigation redundancy
KR101880940B1 (en) Method for controlling attitude and drone atapted the same
KR20210013526A (en) Apparatus and method for terrain aided navigation using inertial position
US7248948B2 (en) Apparatus and method for estimating attitude using inertial measurement equipment
RU2762143C2 (en) System for determining course and angular spatial position made with the possibility of functioning in polar region
RU2564380C1 (en) Correction method of strap-down inertial navigation system
JP6488860B2 (en) Gradient estimation apparatus and program
CN113465628A (en) Inertial measurement unit data compensation method and system
KR101390776B1 (en) Localization device, method and robot using fuzzy extended kalman filter algorithm
KR101160630B1 (en) Method for motion mode decision, navigation system using the method, and providing method thereof
EP2538176A2 (en) Motion-based adaptive frequency estimation of a doppler velocity sensor
KR102093743B1 (en) System for lane level positioning location information of ground vehicle using sensor fusion

Legal Events

Date Code Title Description
GRNT Written decision to grant