KR20150107664A - Quad-Rotor position control system and control method thereof - Google Patents

Abstract

Disclosed are a quad-rotor control system and a control method thereof. The present invention uses a nine axis inertial navigation device based on a quad-rotor with four motors to measure the posture and motion of a flying object, and controls a modularized controller to improve the performance and efficiency of a system. Thereby, the stability of quad-rotor is increased. The adjustment/control of the quad-rotor can be easily carried out. Especially, the performance of a hovering can be improved.

Description

[0001] The present invention relates to a quad rotor position control system and a control method thereof,

The present invention relates to a quad rotor attitude control system and a control method thereof.

Small unmanned aerial vehicles are used in various fields such as traffic control, video shooting, reconnaissance missions, and fire surveillance. With the development of processors, sensors and communication technologies, performance and functions have been improved, and as they have become smaller and more cost-effective, they have expanded their presence in various fields and will accelerate in the future.

Especially, in case of multi - copter, it is considered to be suitable as a platform of small unmanned aerial vehicle because it is free to move up, down, left and right. Although the multi-copter maintains the attitude by the output of each motor and the motor output of each axis is assisted by the automatic control system according to the attitude, it is relatively easy to control compared to other airplanes, It is never easy to steer to the desired behavior.

In other words, in order to make the multi-copter more useful and easy to use, it is necessary to provide a control system that can prevent the flow phenomenon, which makes the flight difficult. Flow stabilization is required to control this flow.

Korean Patent Laid-Open No. 10-2013-0081260 [entitled: Unmanned Aerial Vehicle Control Device and Unmanned Aerial Vehicle Having the Same]

The object of the present invention is to provide a system for measuring the attitude and motion of a vehicle using a 9-axis inertial navigation system based on a quadrotor equipped with four motors, Rotor position control system and a control method thereof.

In a quadrotor attitude control system in which a position control function and an attitude control function are modularized, a quadrotor attitude control system according to an embodiment of the present invention receives a reference value and a previous displacement value on the basis of X axis, Y axis, and Z axis, respectively A setta reference value for the position control of the quadrotor, a theta value in the previous loop, a sipe reference value, a sipe value in the previous loop and a pie reference value in the previous loop through the PID control function for the received reference value and the previous displacement value, A position control unit for outputting pi values; A control for attitude control of the quadrotor through the PID control function for theta reference value output from the position control unit, theta value in the previous loop, the sipe reference value, the sipe value and the pie reference value in the previous loop, A posture control unit for outputting a signal; A quadrotor control unit for outputting a set value in a current loop, a sigh value in a current loop, and a pie value in a current loop for attitude and position control of the quadrotor, based on a control signal output from the attitude control unit; And a sensor unit for measuring theta value in the current loop including the noise component output from the quadrotor control unit, the sipe value in the current loop, and the pie value in the current loop.

As an example related to the present invention, the sensor unit may perform a predetermined integration function on the measured theta value in the measured current loop, the sipe value in the current loop, and the pie value in the current loop to calculate a displacement value And can transmit the displacement value in the extracted current loop to the input value of the position control unit.

A control method of a quadrotor attitude control system according to an embodiment of the present invention is a control method of a quadrotor attitude control system in which a position control function and an attitude control function are modularized, Receiving a reference value and a previous displacement value, respectively; The position control unit controls the PID control function for the received reference value and the previous displacement value to determine the setta reference value for the position control of the quadrotor, the theta value in the previous loop, the sipe reference value, the sipe value in the previous loop, And a pie value in a previous loop, respectively; The PID control function of theta reference value outputted from the position control unit, theta value in the previous loop, the sipe reference value, the sipe value in the previous loop, the pie reference value and the pi value in the previous loop, Outputting a control signal for attitude control; A set value in the current loop, a sigh value in the current loop, and a pi value in the current loop for attitude and position control of the quadrotor are output through the quad-rotor control unit, respectively, based on the control signal output from the attitude control unit ; Measuring acceleration including a noise component output from the quad rotor control unit through a sensor unit; And extracting a displacement value in the current loop based on the measured acceleration through the sensor unit and transmitting the displacement value in the extracted current loop to the input value of the position control unit.

In one embodiment of the present invention, the step of measuring the acceleration includes measuring a set value in a current loop including a noise component output from the quad rotor control unit, a sigh value in a current loop, and a pie value in a current loop .

In one embodiment of the present invention, the step of extracting the displacement values in the current loop based on the measured acceleration comprises: comparing the measured values in the measured current loop with the syt values in the current loop and the pi values in the current loop, So that the displacement values in the current loop can be respectively extracted.

The present invention is based on a quadrotor equipped with four motors, measures the attitude and motion of a vehicle using a 9-axis inertial navigation system, and controls a modular controller to realize a system having a high efficiency as well as a performance, This increases the stability of the rotor, which makes it easier to adjust / control the quad rotor, and in particular, improves hovering performance.

1 is a diagram illustrating a quadrotor model according to an embodiment of the present invention.
FIG. 2 is a diagram illustrating a quadrotor control according to an embodiment of the present invention. Referring to FIG.
3 is a view for explaining a quadrotor control according to an embodiment of the present invention.
4 is a view for explaining control of the posture of the quadrotor according to the embodiment of the present invention.
5 is a view illustrating an inertial sensor of a quadrotor according to an embodiment of the present invention.
FIG. 6 is a block diagram showing a configuration of a flying body having a plurality of rotor blades according to an embodiment of the present invention.
7 is a view for explaining a quadrotor attitude control system according to an embodiment of the present invention.
8 is a view illustrating an axial force in a tilted state in the quad rotor position control system according to the embodiment of the present invention.
9 is a diagram illustrating a simulink block list of a MATLAB in a quadrotor attitude control system according to an embodiment of the present invention.
10 is a view for explaining a quadrotor attitude control system according to an embodiment of the present invention.
11 is a view for explaining a reaction according to state variables of a quad rotor attitude control system according to an embodiment of the present invention.
12 is a view for explaining a quadrotor attitude control system according to an embodiment of the present invention.
13 is a view for explaining a position controller according to an embodiment of the present invention.
14 is a view for explaining a posture control unit according to an embodiment of the present invention.
15 is a block diagram of outputs and motions of respective rotors of a quad rotor according to an embodiment of the present invention.
16 is a block diagram for determining a quadrotor motion state according to an embodiment of the present invention.
FIG. 17 is a view for explaining a case where only a displacement is input in a noise-free state according to an embodiment of the present invention.
FIGS. 18 and 19 are diagrams for explaining a noise only state according to an embodiment of the present invention.
FIGS. 20 and 21 are diagrams for explaining a case where noise and displacement are input together according to an embodiment of the present invention.
Figs. 22 and 23 are diagrams for explaining a state in which desired displacement and irregular disturbance are present in all three axes according to the embodiment of the present invention. Fig.
24 is a flowchart illustrating a control method of the quadrotor attitude control system according to the embodiment of the present invention.
25 is a diagram illustrating input / output data of a quadrotor attitude control system according to an embodiment of the present invention.

It is noted that the technical terms used in the present invention are used only to describe specific embodiments and are not intended to limit the present invention. In addition, the technical terms used in the present invention should be construed in a sense generally understood by a person having ordinary skill in the art to which the present invention belongs, unless otherwise defined in the present invention, Should not be construed to mean, or be interpreted in an excessively reduced sense. In addition, when a technical term used in the present invention is an erroneous technical term that does not accurately express the concept of the present invention, it should be understood that technical terms that can be understood by a person skilled in the art can be properly understood. In addition, the general terms used in the present invention should be interpreted according to a predefined or prior context, and should not be construed as being excessively reduced.

Furthermore, the singular expressions used in the present invention include plural expressions unless the context clearly dictates otherwise. The term "comprising" or "comprising" or the like in the present invention should not be construed as necessarily including the various elements or steps described in the invention, Or may further include additional components or steps.

Furthermore, terms including ordinals such as first, second, etc. used in the present invention can be used to describe elements, but the elements should not be limited by terms. Terms are used only for the purpose of distinguishing one component from another. For example, without departing from the scope of the present invention, the first component may be referred to as a second component, and similarly, the second component may also be referred to as a first component.

Hereinafter, exemplary embodiments of the present invention will be described in detail with reference to the accompanying drawings, wherein like reference numerals refer to like or similar elements throughout the several views, and redundant description thereof will be omitted.

In the following description, well-known functions or constructions are not described in detail since they would obscure the invention in unnecessary detail. It is to be noted that the accompanying drawings are only for the purpose of facilitating understanding of the present invention, and should not be construed as limiting the scope of the present invention with reference to the accompanying drawings.

The multi-copter constructs the frame so that the motors can be arranged at the same distance with respect to the center axis, and attaches the motor and the rotor. Generally, the control unit, the power unit, and the battery are placed on the central axis to secure a space, and the weight is further concentrated on the central axis.

Especially, multi-copter is popular as a platform of small unmanned aerial vehicle because it is free to move up and down and left and right. In the case of multi-copter, since there are at least 4 power units, the loads that can be mounted are relatively large because of the good output relative to the mass.

Although the energy consumption is large, the efficiency of the brushless motor has been improved due to the development of battery technology.

As shown in FIG. 1, the multi-copter 10 is a quadrotor having four rotors and four motors. Since the quad rotor 11 has a shape in which the motor shafts are arranged at 90 degrees each, stable flight performance can be expected using a minimum motor. As for the motor rotation, the quadrotor reacts as shown in FIG.

The quadrotor adjusts the rotation speed of the four blades, and the output is controlled by the output. Fig. 2 illustrates how the attitude is controlled. FIGS. 2 (a) and 2 (b) show that the two motors rotating left and right rotate rapidly to rotate right or left, respectively. FIG. 2 (C) shows an elevation of the altitude by increasing the output of all the rotors. The remaining figures in FIG. 2 can also control the six-axis motion with this simple mechanical principle.

However, the quadrotor system usually used only controls the rotation. That is, depending on the feeling of the person who controls the ability to efficiently cope with the inertia and external force in order to accurately stop the quad rotor that is in motion, or hover at a fixed position.

In other words, for the automation or more convenient control of the quadrotor, it suggests the necessity of the controller that can control the quadrotor using the displacement instead of the control through the existing angle. In addition, external disturbance input should be able to counteract external shocks and noise.

Here, the entire system for dynamic modeling of the quadrotor may be set to the following assumption.

That is, the overall system follows the general assumptions considered when designing a quad rotor.

1. Quad rotors are rigid.

2. The origin of the inertial coordinate system is at the same position as the geometric center of the quad rotor.

3. The resistance and weight of the quad rotor are not affected by flight altitude and other factors.

4. The thrust in all directions is proportional to the square of propeller rotation speed.

As shown in Fig. 3, each variable according to the coordinate system is defined as follows.

3 is the angle between the X axis and the orthogonal axis Ox in the OXY plane and the pitch angle is the distance between the Z axis and the orthogonal Oz in the OXY plane And the roll angle (ψ) is the angle between the Y axis and Oxygen on the OXY plane.

Further, as shown in the equations (1) to (4), the quadrotor coordinate system B can be expressed by the inertia coordinate system E.

4 shows the relationship between the quadrotor and the inertial coordinate system.

R (φ, θ, ψ) is the coordinate system (B) of the transformed quadrotor for the inertial coordinate system (E).

In addition, the control variables and constants can be defined as follows.

That is, [Equation (5)] represents the disturbance received by the quadrotor.

Equation (6) represents the speed of the quadrotor.

Equation (7) represents the mass of the quad rotor.

Equation (8) represents the angular velocity of the center of the quad rotor.

Further, Equation (9) represents the resistance in each rotor.

Wherein the variable D represents a drag force, the variable Cd represents a coefficient of drag, the variable v represents a velocity of the quadrotor, the variable S represents an effective drag area, .

Further, the expression (10) represents the output of each rotor.

Wherein the variable Ct represents a trust coefficient, the variable p represents an air density, the variable A represents a rotor disk region, and the variable R represents a blade radius .

In addition, as shown in [Equation 11] to [Equation 14], elements capable of controlling motion based on each motion element are defined.

The above equations (11) to (14) are solved in the form of a matrix and are summarized as the following equation (15).

In addition, the second law and the dynamics equation of Newton of the quadrotor can be expressed in a vector form as shown in the following Equation (16).

는 외부에서 가해지는 힘, 즉 외란에 의한 힘을 나타낸다. 또한, 상기 변수 m은 쿼드로터의 질량을 나타내고, 상기 변수

는 쿼드로터의 속력을 나타낸다.Here,

Represents the force exerted from outside, that is, the force due to disturbance. Further, the variable m represents the mass of the quadrotor, and the variable < RTI ID = 0.0 >

Represents the speed of the quad rotor.

)와 관성 좌표계에 의한 각운동량(

)은 다음의 [수학식 17]과 같은 형태로 나타낸다.Also, the moment of the quad rotor (

) And angular momentum by the inertial coordinate system (

) Is expressed by the following equation (17).

In addition, the forces on the respective axes are summarized by reflecting the characteristics of the system and the system, as shown in the following equations (18) to (20).

Further, based on the forces shown in the equations (18) to (20), the second law of motion equation of the quadrotor is expressed by the following equations (21) to (23).

은 공기 저항을 나타내며, 네 개의 로터를 가지고 있기 때문에 상기 [수학식 21] 내지 [수학식 23]과 같이 나타낼 수 있다.Here,

Represents the air resistance, and has four rotors, it can be expressed by the following equations (21) to (23).

In addition, the angular velocity relationship between the angle of the inertial coordinate system and the quadrotor coordinate system is expressed by the following equation (24).

In addition, the above equation (24) can be summarized by the following equation (25) and (26).

The following equation (27) can be obtained by summarizing the remainder using the above equation (26).

If the quad rotor has a symmetrical formation, the moment of inertia of the quadrotor can be represented by the following equation (28), which is a diagonal matrix.

Further, the relational expression of the moment, the angular velocity and the angular acceleration can be expressed by the following equations (29) to (31).

The above equations (29) to (31) can be summarized in a matrix form as shown in the following equation (32).

Also, the angular acceleration of the SISO system of the inertial system can be expressed by the following equation (33).

In addition, the equation of motion according to the equations (21) to (33) is summarized as the following equations (34) to (45).

The parameters according to Equations (34) to (45) are used as input values of the quad rotor attitude control system 100.

In addition, the acceleration measured in the hovering state is expressed by the following equation (46).

At this time, the acceleration is not the acceleration in the axial direction. That is, as shown in FIG. 5, since the acceleration is measured in the horizontal direction of the inclination of the sensor (or the inertial navigation unit sensor: IMU), the effect of gravity must be considered.

In addition, in order to consider the acceleration in the axial direction, the acceleration due to the rotation must be considered. However, when the sensor is mounted at the center of gravity, the acceleration in the axial direction is expressed by the following equation (47). However, it is assumed that the sensor calculates a very precise angle preset through filtering for each axis.

Based on this, the displacement caused by the disturbance can be obtained by integrating the acceleration generated in each axis twice.

Therefore, by adding a controller for compensating for the displacement, it is possible to compensate the flow phenomenon due to external force when hovering.

FIG. 6 is a block diagram illustrating a configuration of a quadrotor attitude control system 100 according to an embodiment of the present invention. FIG. 7 is a diagram illustrating a detailed configuration of a quadrotor attitude control system 100 according to an embodiment of the present invention. to be.

6, the quadrotor attitude control system 100 includes a position control unit 110, a posture control unit 120, a quadrotor control unit 130, and a sensor unit 140. Not all of the components of the quad rotor attitude control system 100 shown in Fig. 6 are required, and the quad rotor attitude control system 100 may be implemented by more components than the components shown in Fig. 6 And the quad rotor attitude control system 100 may be implemented by fewer components.

As shown in FIG. 7, in order to compensate displacement due to disturbance in hovering in the quadrotor attitude control system 100, the attitude controller 120 may determine the attitude according to the displacement based on the displacement The control system must be extended.

In other words, when it is pushed backward, it is necessary to input the value to the posture control unit 120 by determining the posture in which the forward acceleration can be obtained by leaning forward so as to go forward.

Accordingly, the quadrotor attitude control system 100 shown in FIG. 7 can be used.

In addition, designing up to the position control system in the attitude control system is not so complicated and intuitive, and it is difficult to know which part of the verification has caused the problem, thereby causing maintenance and repair problems.

However, if the position control unit 110 and the posture control unit 120 are separately designed as shown in FIGS. 6 and 7, the design is simplified and it is easy to analyze the performance and reliability of each control unit. It is also easy to improve performance.

When the correlation between the position control unit 110 and the posture control unit 120 is analyzed, the position control unit 110 corresponds to an upper concept. That is, since the position control unit 110 performs hovering at a desired position using the posture control unit 120, the position control unit 110 can be considered to use the posture control unit 120. Therefore, the quadrotor attitude control system 100 is not planar, but the position control unit 110 may include the attitude control unit 120.

As shown in FIG. 8, in a quadrotor tilted on the ZX plane, if the force received by the four rotors in the X axis direction is F, the magnitude of F is expressed by the following equation (48) .

Here, the angle is an angle to the X axis.

Further, the Newton's two laws represented by the equation (49) can be expressed by the equation (50) by the Laplace transform.

In addition, by substituting the above expression (50) into the expression (48), the expression can be expressed as the following expression (51).

In addition, the above equation (51) is substituted for the angle and is summarized as the following equation (52).

The equation (52) indicates that the angle is determined by the distance to be moved and the output of four rotors. That is, when the distance to be moved is long, the angle becomes large, and when the output of the rotor is large, the angle becomes small.

Further, if the above expression (52) is defined for the X axis and the Y axis, the following expression (53) is obtained.

The above equation (53) is summarized for the two axes. Here, the phi angle is not considered, which should be discussed in terms of attitude control and is not discussed from the viewpoint of the flow phenomenon.

In addition, the altitude control is an important factor because the altitude is controlled by the resultant force of the four rotors and the posture of the aircraft is determined.

Also, in the embodiment of the present invention, the altitude control is performed through the PID control.

Also, there is one problem in the above equation (53), which is meaningless as a stable control system because the angle is 90 degrees when the target distance to move is very long (there is no infinity). In other words, it means that the position control unit must remove the factor that can make the quad rotor unstable, which can be solved by limiting the derived value.

Also, by limiting the angle, the time to reach steady state can be reduced, but the system is able to completely resolve the sudden reaction. The limit value can be obtained by an accumulated experimental value rather than a numerical value.

The implementation and verification of the quadrotor attitude control system 100 is performed using a simulink of matlab. At this time, the quadrotor attitude control system 100 is configured by a block diagram, and the block diagrams are set (or configured) according to the conditions according to the present invention.

The Simulink is one of the internal programs of MATLAB. It is a tool for designing a system based on a GUI (Graphic User Interface), inserting various data into the designed system, and checking the results by graph or data.

9 is a screen on which the simulation is executed.

As shown in FIG. 9, the blocks generate a signal, perform an operation, and display a corresponding result. The performance of the components included in the quadrotor attitude control system 100 can be confirmed by combining the respective blocks to generate control signals and analyzing the generated control signals.

The block diagram of FIG. 10 implements the state equation as shown in Equation (54).

As shown in FIG. 10, two integrators are used and feedback is performed.

In addition, the rightmost block scope shown in FIG. 10 graphically displays the result in the block.

FIG. 11 is a graph showing a system response in a system corresponding to the state equation of Equation (54) that is input with an initial value of 2 as a graph. At this time, we can see that the system reacted to the expectation that it would converge to zero because we put in two spoken feedbacks.

In this way, the quadrotor attitude control system 100 according to the present invention can be designed on a simulink. That is, since the state equations in Equation (54) and the equations to be used for control are expressed by only the blocks provided by Simulink, the design method in each component is not specifically discussed.

FIG. 12 is a block diagram of FIG. 6 or FIG. 7 implemented in a simulink.

As shown in FIG. 12, X, Y, Z and a rotation angle (or a pie value) are input to the position control unit 110 and the output of each motor and X , The displacements of Y and Z, and the slope at each axis.

In addition, an impact (or an acceleration) is inputted in a direction parallel to each axis.

13 is a diagram illustrating the position controller 110 as a simulator.

As shown in FIG. 13, the position controller 110 detects an error for each component, integrates twice, applies a state equation obtained previously, and then detects a limit value and outputs a signal .

FIG. 14 is a diagram illustrating the posture control unit 120 implemented as a simulation.

As shown in FIG. 14, the posture control unit 120 detects an error for each component, performs a PID control function, and outputs a motor control signal.

FIG. 15 is a block diagram of the output and motion of each rotor, and FIG. 16 is a block diagram for determining the motion state.

That is, FIGS. 15 and 16 are block diagrams corresponding to the following equations (55) to (57).

17 shows the reaction of the quadrotor in the absence of disturbance (or the reaction in the case where only displacement is input in the state of no noise). At this time, as shown in FIG. 17, when the desired position is a dotted line, the quad rotor shows convergence to the corresponding portion. That is, it can be confirmed that the position-based control is performed instead of the posture-based control.

Fig. 18 shows a state in which a disturbance is given in the form of a step function (or only noise and the displacement is 0) in the hovering state, that is, when the current position is stuck, and the state is resisting the force.

FIG. 19 shows a state in which the Y-axis resists the force when an irregular disturbance is given when the Y-axis tries to stick to the current position.

FIG. 20 shows a state in which a desired displacement and an irregular disturbance are given to the X axis (or noise and displacement are input together), and the state is moving to a given displacement in resistance to the force.

FIG. 21 shows a state in which a desired displacement and an irregular disturbance are given to the Y-axis (or noise and displacement are input together), and a state is being resolved against the force and moving to a given displacement.

Figs. 22 and 23 show the case where the desired displacement and irregular disturbance are given in all three axes (or noise and displacement input in the three axes), respectively, and the state in which the displacement of the axes converges to a given displacement in response to a given irregular disturbance .

The quadrotor attitude control system 100 ensures reliability of a control unit (for example, the position control unit 110) that has been set in advance, as in the test through the simulation, The posture control unit 120) can be designed to improve reliability.

In addition, it is easy to determine which part of the problem occurred when a problem occurred through a plurality of independent modular control units (for example, the position control unit 110 and the posture control unit 120) have.

The position control unit 110 and the posture control unit 120 complete (or generate) a control formula that can determine the posture with respect to the displacement based on the following Expressions (58) to (67).

That is, [Expression 58] represents a Newton equation on the X axis.

The equation (59) is an equation for the thrust and mass of the gas, and is an equation summarizing the above equation (58).

Here, U ₁ represents the output of the quadrotor.

Equation (60) is an equation summarizing the above-mentioned Equation (59) with respect to theta angle (or an angle to proceed).

Here, the above equation (60) includes the following physical meaning.

That is, Equation (60)

I) A large tilt angle is required to obtain a large acceleration.

Ii) If the mass is large, a large tilt angle is required.

Iii) If the motor output is large, the required angle is reduced.

In addition, when the acceleration is replaced with another variable (for example, when the variable is replaced as in [Equation 62]), the inclination angle for the new variable can be obtained while maintaining the physical characteristics.

That is, when replacing the variable substituted in Equation 62 with the PID control signal value, u (t) can be expressed by Equation (63) and Equation (64).

Further, when the substitution ring parameter is input to the expression (60), the following expression (66) can be completed which can determine the posture with respect to the displacement.

Here, the expression (66) includes the following physical meaning.

That is, Equation (66)

I) If the mass is large, a large tilt angle is required.

Ii) If the motor output is large, the required tilt angle is reduced.

Iii) If the intensity of the signal increases, the tilt angle becomes larger.

The position control unit (or position control system) 110 receives (or receives) the first reference value and the previous displacement value (or the previous displacement value related to the X axis) based on the X axis.

Also, the position controller 110 outputs a first set of reference values for controlling the position of the quadrotor and a set of set values of the previous set through the PID control function based on the received first reference value and the previous set value.

Also, the position controller 110 receives the second reference value and the previous displacement value (or the previous displacement value related to the Y axis) based on the Y axis.

Also, the position controller 110 outputs a first Cy reference value for controlling the position of the quadrotor and a Cy value in the previous loop through the PID control function based on the received second reference value and the previous displacement value.

Also, the position controller 110 receives the third reference value and the previous displacement value (or the previous displacement value related to the Z axis) with reference to the Z axis.

Also, the position controller 110 outputs a first pi reference value for position control of the quadrotor and a pi value in the previous loop through the PID control function based on the received third reference value and the previous displacement value.

The posture control unit (or attitude control system) 120 receives the first theta reference value and the previous theta value output from the position control unit 110.

The posture controller 120 outputs a first motor control signal for controlling the posture of the quadrotor through the PID control function based on the first theta reference value and the previous theta value.

In addition, the posture control unit 120 controls the posture of the quadrotor through the PID control function based on the first Cy reference value output from the position control unit 110 and the Cy value in the previous loop, 2 Output the motor control signal.

In addition, the posture control unit 120 may control the posture of the quad rotor through the PID control function based on the first pie reference value output from the position control unit 110 and the pie value in the previous loop, 3 Output the motor control signal.

The posture controller 120 combines the first motor control signal for the X axis, the second motor control signal for the Y axis, and the third motor control signal for the Z axis to generate one motor control signal Output.

The PID coefficient used in the PID control of the position control unit 110 and the posture control unit 120 according to the present invention may be an optimized value based on physical properties such as a mass moment of inertia.

The quadrotor control unit (or quadrotor system) 130 outputs theta value in the current loop for attitude and position control of the quadrotor based on the control signal output from the attitude control unit 120. [

In addition, the quad-rotor control unit 130 generates a second motor control signal for the Y axis and a third motor control signal for the Z axis, which are respectively output from the posture control unit 120 on the Y axis and the Z axis, The sine value in the loop, the pie value, and the like, respectively.

In addition, the quadrotor control unit 130 generates a motor control signal based on a motor control signal output from the posture control unit 120 as one signal for the X axis, the Y axis, and the Z axis, , And a pie value, respectively.

The sensor unit 140 includes an inertial navigation sensor or an acceleration sensor.

In addition, the sensor unit 140 measures a setta value in the current loop output from the quadrotor control unit 130. At this time, theta value in the measured current loop may include a noise component or the like.

Also, the sensor unit 140 may measure a sway value, a pie value, and the like in the current loop outputted from the quadrotor control unit 130, respectively.

In this way, the sensor unit 140 measures the acceleration output from the quadrotor control unit 130.

In addition, the sensor unit 140 extracts (or calculates) the displacement value at the current loop (or at the present time) by performing a preset integration function on the measured theta value in the measured current loop.

The extracted displacement value is input (or used) to the position control unit 110 in the next step (or the next loop) as theta value calculated in the previous loop.

In addition, the sensor unit 140 performs an integration function on the measured sipe value, the pie value, and the like in the current loop to extract a displacement value in the current loop, and outputs the extracted displacement value to the position control unit 110. [ .

In this way, the sensor unit 140 may perform the integral function on the measured acceleration to extract a displacement value in the current loop, and provide the extracted displacement value to the position control unit 110. [

In this way, based on the quadrotor equipped with four motors, the 9-axis inertial navigation system is used to measure the attitude and motion of the vehicle, and the modularized controller can be controlled to realize a system that is both efficient in performance as well as in performance.

Hereinafter, a control method of the quadrotor attitude control system according to the present invention will be described in detail with reference to FIG. 1 through FIG.

24 is a flowchart illustrating a control method of the quadrotor attitude control system according to the embodiment of the present invention.

First, the position controller 110 receives (or receives) a first reference value and a previous displacement value with respect to the X axis.

Also, the position controller 110 outputs a first set of reference values for controlling the position of the quadrotor and a set of set values of the previous set through the PID control function based on the received first reference value and the previous set value.

For example, as shown in FIG. 25, the position controller 110 performs a PID control function for a first reference value 2511 including noise and a previous displacement value 2512 based on the X axis, And outputs a first theta reference value 2521 according to the execution of the PID control function and a theta value 2522 in the previous loop.

Also, the position controller 110 performs a PID control function based on the second reference value, the third reference value, and the previous displacement values with respect to the Y axis and the Z axis, respectively, to calculate a first pie reference value, A 1-th reference value, a sigh value in the previous loop, and the like (S2410).

Then, the posture control unit 120 receives the first and second theta reference values output from the position control unit 110.

The posture controller 120 outputs a first motor control signal for controlling the posture of the quadrotor through the PID control function based on the first theta reference value and the previous theta value.

25, the posture control unit 120 may include a PID control function for a first theta reference value 2521 output from the position control unit 110 and a theta value 2522 in the previous loop, And outputs a first motor control signal 2531 according to the execution of the PID control function.

In addition, the posture controller 120 may calculate a first pie reference value, a pie value in the previous loop, a first scho reference value, a sie value in the previous loop, and the like, which are respectively output from the position controller 110 on the Y axis and the Z axis, And outputs a second motor control signal for the Y-axis and a third motor control signal for the Z-axis, respectively.

The posture controller 120 combines the first motor control signal for the X axis, the second motor control signal for the Y axis, and the third motor control signal for the Z axis to generate one motor control signal (S2420).

Then, the quad-rotor control unit 130 outputs a setta value in the current loop for attitude and position control of the quadrotor based on the control signal output from the attitude control unit 120. [

25, the quadrotor control unit 130 may control the attitude and position of the quadrotor based on the first motor control signal 2531 output from the attitude control unit 120. For example, And outputs a theta value 2541 in the current loop.

In addition, the quad-rotor control unit 130 generates a second motor control signal for the Y axis and a third motor control signal for the Z axis, which are respectively output from the posture control unit 120 on the Y axis and the Z axis, The sine value in the loop, the pie value, and the like, respectively.

In addition, the quadrotor control unit 130 generates a motor control signal based on a motor control signal output from the posture control unit 120 as one signal for the X axis, the Y axis, and the Z axis, , And a pie value, respectively.

In this manner, the quadrotor control unit 130 outputs the acceleration (S2430).

Then, the sensor unit 140 measures theta value in the current loop output from the quadrotor control unit 130. [ At this time, theta value in the measured current loop may include a noise component or the like.

For example, as shown in FIG. 25, the inertia sensor included in the sensor unit 140 measures a setta value 2541 in the current loop output from the quadrotor control unit 130.

Also, the sensor unit 140 may measure a sway value, a pie value, and the like in the current loop outputted from the quadrotor control unit 130, respectively.

In this manner, the sensor unit 140 measures acceleration output from the quadrotor control unit 130 (S2440).

Then, the sensor unit 140 extracts (or calculates) the displacement value at the current loop (or at the present time point) by performing a preset integration function on the measured theta value in the measured current loop.

The extracted displacement value is input (or used) to the position control unit 110 in the next step (or the next loop) as theta value calculated in the previous loop.

25, the sensor unit 140 performs a preset integration function based on the measured theta value 2541 in the current loop to calculate a displacement value 2551 in the current loop, And transfers the extracted displacement value 2551 to the position control unit 110. [

In addition, the sensor unit 140 performs an integration function on the measured sipe value, the pie value, and the like in the current loop to extract a displacement value in the current loop, and outputs the extracted displacement value to the position control unit 110. [ .

In this way, the sensor unit 140 may perform the integral function on the measured acceleration to extract a displacement value in the current loop, and provide the extracted displacement value to the position control unit 110 ( S2450).

As described above, the embodiment of the present invention measures a posture and a motion of a vehicle using a 9-axis inertial navigation device based on a quadrotor equipped with four motors, controls the modularized controller, By implementing an efficient system, it increases the stability of the quad rotor, which makes it easier to adjust / control the quad rotor, especially the performance of hovering.

It will be apparent to those skilled in the art that various modifications and variations can be made in the present invention without departing from the spirit or essential characteristics thereof. Therefore, the embodiments disclosed in the present invention are intended to illustrate rather than limit the scope of the present invention, and the scope of the technical idea of the present invention is not limited by these embodiments. The scope of protection of the present invention should be construed according to the following claims, and all technical ideas within the scope of equivalents should be construed as falling within the scope of the present invention.

The present invention is based on a quadrotor equipped with four motors, measures the attitude and motion of a vehicle using a 9-axis inertial navigation system, and controls a modular controller to realize a system having a high efficiency as well as a performance, This makes it possible to adjust and control the quadrotor more easily, and in particular, to improve the performance of hovering. It can be widely used in the quadrotor field, the unmanned aerial vehicle (UAV) field, the aviation field, etc. .

Claims (5)

In a quadrotor attitude control system in which the position control function and the attitude control function are modularized,
A reference value and a previous displacement value on the basis of the X-axis, the Y-axis, and the Z-axis, respectively, and a set of theta reference values for the position control of the quadrotor through the PID control function for the received reference value and the previous displacement value, A position control unit for outputting a sine value, a pie reference value, and a pie value in the previous loop, respectively, in the previous loop;
A control for attitude control of the quadrotor through the PID control function for theta reference value output from the position control unit, theta value in the previous loop, the sipe reference value, the sipe value and the pie reference value in the previous loop, A posture control unit for outputting a signal;
A quadrotor control unit for outputting a set value in a current loop, a sigh value in a current loop, and a pie value in a current loop for attitude and position control of the quadrotor, based on a control signal output from the attitude control unit; And
And a sensor unit for measuring theta value in the current loop including the noise component output from the quad rotor control unit, the sipe value in the current loop, and the pie value in the current loop. The method according to claim 1,
The sensor unit includes:
And performing a predetermined integration function on the measured theta value in the current loop, the sity value in the current loop, and the pie value in the current loop to extract the displacement values in the current loop, respectively, And transmits the displacement value of the quad rotor position control unit to the position control unit. A control method of a quadrotor attitude control system in which a position control function and an attitude control function are modularized,
Receiving a reference value and a previous displacement value with respect to the X axis, the Y axis, and the Z axis, respectively, through the position control unit;
The position control unit controls the PID control function for the received reference value and the previous displacement value to determine the setta reference value for the position control of the quadrotor, the theta value in the previous loop, the sipe reference value, the sipe value in the previous loop, And a pie value in a previous loop, respectively;
The PID control function of theta reference value outputted from the position control unit, theta value in the previous loop, the sipe reference value, the sipe value in the previous loop, the pie reference value and the pi value in the previous loop, Outputting a control signal for attitude control;
A set value in the current loop, a sigh value in the current loop, and a pi value in the current loop for attitude and position control of the quadrotor are output through the quad-rotor control unit, respectively, based on the control signal output from the attitude control unit ;
Measuring acceleration including a noise component output from the quad rotor control unit through a sensor unit; And
And a step of extracting a displacement value in a current loop based on the measured acceleration through the sensor unit and transmitting a displacement value in the extracted current loop to an input value of the position control unit, Method of controlling the system. The method of claim 3,
Wherein the measuring the acceleration comprises:
Wherein a set value in a current loop including a noise component output from the quad rotor control unit, a sigh value in a current loop, and a pie value in a current loop are measured. 5. The method of claim 4,
The step of extracting the displacement values in the current loop based on the measured acceleration,
Wherein the quadrotor attitude is obtained by performing a predetermined integration function on the measured theta value in the current loop, the sity value in the current loop, and the pie value in the current loop, Control method of a control system.

Images