JP2019182131A - Flight control device, method and program - Google Patents

Abstract

An unmanned airplane can be operated freely and controlled to fly smoothly and stably. A position and orientation detector detects three-dimensional coordinates of each marker based on three-dimensional coordinates of each marker measured by a position measurement sensor and a preset target point of a UAV destination. The midpoint, the distance from the midpoint to the target point, and the azimuth of a line connecting the three-dimensional coordinates of each of the markers with respect to the target point in the global coordinate system are calculated. The speed control unit updates control data for controlling a speed vector of the UAV determined from the middle point and the target point so as to accelerate or decelerate according to the distance to the target point. The flight command conversion unit calculates flight command data in the UAV based on the updated control data and the calculated azimuth, and controls the movement of the UAV based on the calculated flight command data. [Selection diagram] Fig. 1

Description

  The present invention relates to a flight control apparatus, method, and program, and more particularly, to a flight control apparatus, method, and program for controlling an unmanned airplane.

  A quad-rotor type UAV (Unmanned Aerial Vehicle) is an unmanned airplane that has four propellers and controls flight by controlling the lift applied to each propeller. Many unmanned airplanes called drones are a kind of quadrotor type UAV. The unmanned airplane that will be handled hereinafter is abbreviated as UAV.

  Generally, in order to measure the posture of the UAV, a local coordinate system is set on the aircraft as shown in FIG. The UAV forward direction is the X axis, the direction perpendicular to the X axis is the Y axis, and the direction opposite to gravity is the Z axis.

  Further, in order to measure the three-dimensional position of the UAV, a global coordinate system serving as a reference is set as shown in FIG. In GPS, the coordinate system is a three-dimensional coordinate system, and in the motion capture system, the measurement coordinate system is a global coordinate system. The center of gravity of the UAV, that is, the position of the UAV (the origin of the local coordinate system) is expressed as a point P = (X, Y, Z) in the global coordinate system. The UAV posture is expressed by the rotation angle of the local coordinate system with respect to the global coordinate system. The rotation around the X axis is roll rotation (rotation angle φ), the rotation around the Y axis is pitch rotation (rotation angle ω), and the Z axis. The rotation around is called yaw rotation (rotation angle θ). The flight movement of the UAV generates rotation about the X axis, rotation about the Y axis, and rotation about the Z axis by changing the lift applied to the four propellers. Rotation around the Y axis produces a translational motion in the X axis direction, and rotation around the X axis produces a translational motion in the Y axis direction. Rotation around the Z-axis produces azimuthal rotation, and when the same lift is applied to the four propellers simultaneously, it produces a translational movement (high elevation) in the Z-axis direction.

When given coordinate values (X _d , Y _d , Z _d ) and azimuth θ _d are given in the global coordinate system, the UAV current position P = (X, Y, Z) and the current azimuth to the predetermined position In order to fly and point the aircraft in a predetermined direction, it is necessary to control the attitude and position of the UAV. Non-Patent Document 1 discloses backstepping control based on UAV equations of motion and angular equations of motion. Non-Patent Document 2 discloses a flight motion control method for AR Drone 2.0 (a commercially available low-cost UAV). In many of the conventional technologies, discrete predetermined positions and predetermined directions are given as flight trajectories in the global coordinate system, and the movement is controlled so that the UAV tracks each point and each direction.

T. Madani and A. Benallegue: "Backstepping Control for a Quadrotor Helicopter", IEEE Conference on Intelligent Robots and Systems, 2006. L. V. Santana, A. S. Brandao, M. S-Filho, and R. Carelli: "A Trajectory Tracking and 3D Positioning Controller for the AR. Drone Quadrotor", International Conference on Unmanned Aircraft Systems (ICUAS) May 27-30, 2014.

  The problems of freely controlling the flight of UAV can be roughly divided into three, which are called hovering, trajectory tracking, and path following. Hovering controls the flight to stay in a given position and orientation. In trajectory tracking, control is performed so that tracking is performed in real time along a given route (a discrete three-dimensional position and direction in space). On the other hand, although the path follow does not matter in real time, the flight is controlled by giving some constraint condition in the space at the specified three-dimensional position and orientation in the space. According to Non-Patent Document 1 and Non-Patent Document 2, it is possible to appropriately control the flight motion of the UAV so as to fly according to a predetermined route with respect to the problem of trajectory tracking. However, since these technologies are controls aimed at reaching the target location along a predetermined route, stable flight of the UAV is not guaranteed.

  On the other hand, in the path follow, in order to avoid collision with an obstacle or a person, a constraint condition can be set in the space so as not to fly in a specific place. In addition, in order to avoid troubles during flight movement, various sensors and cameras may be attached to the UAV. For example, the ultrasonic sensor can measure the altitude from the ground or the floor, and can measure the speed and posture of the aircraft by using a gyro sensor and an acceleration sensor. Cameras are used to fly autonomously in space while detecting moving objects. Although it is conceivable to use spatial information and image information measured from these aircraft, the accuracy of sensors built into most UAVs is not guaranteed to be measured with accurate accuracy in millimeters. It is necessary to measure the exact position and orientation of the aircraft using the means. Furthermore, since these techniques are also controlled for reaching the target location, it is not guaranteed that the UAV can fly smoothly and stably.

  The present invention has been made to solve the above-described problems, and provides a flight control device, method, and program capable of controlling an unmanned airplane to freely operate and to smoothly and stably fly. The purpose is to provide.

  In order to achieve the above object, a flight control apparatus according to a first aspect of the present invention measures the three-dimensional coordinates of each of a plurality of markers provided to a UAV (Unmanned Aerial Vehicle) and having a known distance between the markers. Based on the position measurement sensor, the three-dimensional coordinates of each of the markers measured by the position measurement sensor, and the preset target point of the UAV target location, A position and direction detection unit that calculates a point, a distance from the midpoint to the target point, and an azimuth angle of a line segment connecting each three-dimensional coordinate of the marker with respect to the target point in a global coordinate system; and the target A speed control unit that updates control data for controlling the speed vector of the UAV determined from the middle point and the target point so as to accelerate or decelerate according to the distance to the point; A flight command conversion unit that calculates flight command data in the UAV based on the control data and the calculated azimuth angle, and controls movement of the UAV based on the calculated flight command data; It is configured to include.

  The flight control method according to the second invention includes a step of measuring a three-dimensional coordinate of each of a plurality of markers having a position measurement sensor attached to a UAV (Unmanned Aerial Vehicle) and having a known distance between the markers; The position / orientation detection unit, based on the three-dimensional coordinates of each of the markers measured by the position measurement sensor and the target point of the target location of the UAV set in advance, A step of calculating a midpoint, a distance from the midpoint to the target point, and an azimuth angle of a line segment connecting each three-dimensional coordinate of the marker with respect to the target point in a global coordinate system; Updating control data for controlling the velocity vector of the UAV determined from the middle point and the target point so as to accelerate or decelerate according to the distance to the target point; A line command conversion unit calculates flight command data in the UAV based on the updated control data and the calculated azimuth angle, and moves the UAV based on the calculated flight command data. And the step of controlling.

  A program according to a third invention is a program for causing a computer to function as each part of the flight control device according to the first invention.

  According to the flight control device, method, and program of the present invention, each of the markers is based on the three-dimensional coordinates of each of the markers measured by the position measurement sensor and the preset target point of the UAV target location. Calculate the azimuth of the line segment connecting the 3D coordinates of each marker with respect to the target point in the global coordinate system by calculating the midpoint of 3D coordinates, the distance from the midpoint to the target point, and the target point in the global coordinate system. The control data for controlling the velocity vector of the UAV determined from the middle point and the target point is updated so as to accelerate or decelerate according to the control point, and based on the updated control data and the calculated azimuth angle By calculating flight command data in UAV, and controlling UAV movement based on the calculated flight command data, the unmanned airplane can be operated freely and smoothly and stably. Can be controlled so as to fly, the effect is obtained that.

It is a block diagram which shows the structure of the flight control apparatus which concerns on embodiment of this invention. It is a figure which shows an example of the condition which controls the flight of UAV. It is a figure which shows the speed change of UAV in a straight flight. It is a flowchart which shows the flight control processing routine in the flight control apparatus which concerns on embodiment of this invention. It is a flowchart of the position direction detection process in a flight control processing routine. Point q _1, and point q _2, and a view of the target point p indicating the position and orientation of the relationship viewed from Z _w axis. It is a flow chart of speed control processing in a flight control processing routine. It is a flowchart of the flight command conversion process in a flight command conversion process routine. It is a figure which shows the example of the flight path | route of UAV based on a geometric pattern. It is a figure which shows an example of the space arrangement | positioning of several UAV. It is a figure which shows an example of a UAV external appearance, and a UAV fixed local coordinate system. It is a figure which shows an example of the relationship between a global coordinate system and a UAV fixed local coordinate system.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. The technique according to the embodiment of the present invention solves the problem of freely operating UAV flight and allowing the UAV to fly smoothly and stably.

<Configuration of Flight Control Device According to First Embodiment of the Present Invention>
The configuration of the quadrotor type UAV flight control apparatus according to the first embodiment of the present invention will be described. In this embodiment, the flight speed of one UAV is controlled. As shown in FIG. 1, the flight control apparatus 100 according to the first embodiment of the present invention includes a CPU, a RAM, a ROM for storing a program and various data for executing a flight control processing routine to be described later, Can be configured with a computer including Functionally, the flight control apparatus 100 includes a position measurement sensor 10, a calculation unit 20, and a communication unit 50 as shown in FIG. In this configuration, the position measurement sensor 10 does not necessarily have to be connected as a component, and only needs to acquire data necessary for processing. The position / direction detection unit 30, the speed control unit 32, and the flight command in the calculation unit 20 The flow of data from the conversion unit 34 to each arrow may be in the form of using a recording medium such as a hard disk, a RAID device, or a CD-ROM, or using a remote data resource via a network.

The position measurement sensor 10 measures three-dimensional coordinates of each of a plurality of markers (points q ₁ and q ₂ ) that are given to the UAV 14 and have known distances between the markers as measurement data. In this embodiment, a motion capture device is used as an example of the position measurement sensor 10. The position of the target point p of the target location 12 of the UAV 14 is arbitrarily set according to the flight plan, and a sensing marker is attached to the position of the points q ₁ and q ₂ on the UAV 14. In the situation shown in FIG. 2, the position measurement sensor 10 is set up to measure three-dimensional coordinates in a global coordinate system, and the three-dimensional coordinates of each marker are sequentially measured at a certain interval. In the initial state, it is assumed that the marker point q ₁ attached to the UAV 14, the midpoint q of the marker point q ₂ , and the target point p of the target location 12 are sufficiently separated.

FIG. 3 shows an example of a straight flight in which the initial position of the midpoint q between the marker point q ₁ and the marker point q ₂ attached to the UAV 14 is the A point and the target point p of the target location 12 is the B point. The UAV 14 speed is set to 0 at points A and B. In other words, it stays in the air due to hovering. In the lower part of FIG. 3, a graph of speed v and time t is shown. This graph shows the speed change of UAV14. In the initial state, the aircraft flies by hovering and starts accelerating at time t ₁ (the speed on the way is assumed to be v = V _a ). When the velocity reaches V ₀ , that is, at time t ₂ , the aircraft keeps the velocity and flies. Further, the vehicle starts decelerating at the point of passing through point X, that is, at time t ₃ (the speed in the middle is assumed to be v = V _b ). At this time, the distance from the point X to the point B is L ₀ . Eventually, at time t ₄ , the speed becomes 0 and the hovering state is established.

  In this embodiment, the movement of the UAV 14 is controlled in order to stably reach the point B from the point A in the straight flight shown in FIG. In the present embodiment, the number of markers is two. However, the number of markers is not limited to this. The number of markers may be three or more, and the relative positional relationship between objects may be obtained.

  The calculation unit 20 includes a position / orientation detection unit 30 that detects the position and orientation of the UAV 14 in the global coordinate system by the position measurement sensor 10, a speed control unit 32 that controls the flight speed of the UAV 14 based on the position, and the flight speed and direction. And a flight command conversion unit 34 for sending a command in accordance with the UAV 14 to be used. The specific processing contents of the position / orientation detection unit 30, the speed control unit 32, and the flight command conversion unit 34 will be described in the description of the operation described later.

The position / orientation detection unit 30 is based on the three-dimensional coordinates of the marker points q ₁ and q ₂ of the UAV 14 measured by the position measurement sensor 10 and the target point p of the target location 12 of the UAV 14 set in advance. , Marker point q ₁ , and midpoint q of the three-dimensional coordinates of point q ₂ , distance || T || determined from vector T from midpoint q to target point p, and target point p in the global coordinate system The azimuth angle θ of the line segment connecting the marker point q ₁ and the midpoint q ₂ of the point q ₂ with three-dimensional coordinates is calculated.

  Based on the distance to the target point p, the speed control unit 32 updates the control data u for controlling the speed vector of the UAV 14 so that the speed according to the flight plan shown in FIG.

Based on the updated control data u and the calculated azimuth angle θ, the flight command conversion unit 34 converts the rotation speed V _x around the X axis, the rotation speed V _y around the Y axis, and the Z axis in the UAV 14. The speed V _z along and the rotation speed V _θ around the Z axis are calculated as flight command data, and the flight command data is transmitted to the UAV 14 via the communication unit 50, so that the UAV 14 Control movement.

<Operation of the flight control apparatus according to the first embodiment of the present invention>
Next, the operation of the flight control device 100 according to the first embodiment of the present invention will be described. When the measurement of the three-dimensional coordinates of the marker points q ₁ and q ₂ of the UAV 14 is started, the flight control device 100 executes a flight control processing routine shown in FIG.
First, in step S100, the position / orientation detection unit 30 uses the three-dimensional coordinates of the marker points q ₁ and q ₂ of the UAV 14 measured by the position measurement sensor 10 and the target of the target location 12 of the UAV 14 set in advance. Based on the point p, the point q _{1 of} the marker and the center point q of the three-dimensional coordinates of the point q ₂ , the distance || T || obtained from the vector T from the center point q to the target point p, and global coordinates The azimuth angle θ of the line segment connecting the point q ₁ of the marker with respect to the target point p in the system and the midpoint q of the point q ₂ by three-dimensional coordinates is calculated.

  Next, in step S102, the control data for the speed control unit 32 to control the speed vector of the UAV 14 so as to achieve the speed according to the flight plan shown in FIG. 3 based on the distance to the target point p. Update u.

In step S104, the flight command conversion unit 34, based on the updated control data u and the calculated azimuth angle θ, in the UAV 14, the rotational speed V _x around the X axis and the rotational speed V _y around the Y axis. The speed V _z along the Z axis and the rotational speed V _θ around the Z axis are calculated as flight command data, and the flight command data is transmitted to the UAV 14 via the communication unit 50, whereby the calculated flight command data Based on this, the movement of the UAV 14 is controlled.

  Next, details of the position / orientation detection process of the position / orientation detection unit 30 in step S100 will be described.

  FIG. 5 is a flowchart of the processing of the position / orientation detection unit 30. When the processing starts, the position / orientation detection unit 30 sets a target point p of the target location 12 in the global coordinate system in step S1000. The target point p may be any three-dimensional coordinate determined by the operator.

In step S1002, the three-dimensional coordinates of the marker points q ₁ and q ₂ of the UAV 14 measured by the position measurement sensor 10 are acquired.

In step S1003, based on the three-dimensional coordinates of the marker point q ₁ and the point q ₂ of the UAV 14 measured by the position measurement sensor 10, the coordinate of the middle point q is expressed by q = (q ₁ + q ₂ ) / 2. calculate. Hereinafter, the coordinate of the midpoint q is the current position of the UAV 14.

In step S1004, the calculated distance from the midpoint q point markers _{q 1} and point _{q 2} to the target point p. The distance between the current position (midpoint q) of the UAV 14 obtained by the position measurement sensor 10 and the target point p of the target location 12 is obtained from the vector T by calculating || T || = || p−q ||. . || · || represents the norm (size) of the vector.

In step S1006, the azimuth angle θ of the line segment connecting the point q ₁ of the marker with respect to the target point p in the global coordinate system and the midpoint q of the point q ₂ by three-dimensional coordinates is calculated. 6, a point q _{1 marker,} and point q _2, and shows the position and orientation relationship between the target point p of the target areas 12 as viewed from the Z _w axis. In the position / orientation detection unit 30, the rotation angle (azimuth angle θ) is set so that the three-dimensional coordinate line segments of the marker points q ₁ and q ₂ are perpendicular to the line segment of the target point p. Calculated using the vector dot product relationship.

  In step S1008, it is determined whether to stop the position / orientation detection process. If the process is to be stopped, the process of the position / orientation detection unit 30 ends. If not, the process returns to step S1000 to repeat the process. Here, the case where the process is stopped is a case where the operator ends the flight control of the UAV 14, and the same applies to the following processes.

  The position / orientation detection unit 30 calculates the distance || T || from the midpoint q to the target point p and the azimuth angle θ through the above processing.

  Next, details of the speed control process of the speed control unit 32 in step S102 will be described. Hereinafter, detailed processing will be described using the straight flight shown in FIG. 3 as an example. FIG. 3 shows a straight flight from point A (departure point) to point B (arrival point). The coordinate value of the point B corresponds to the point p in FIG. It is assumed that the point p and the UAV 14 are in a face-to-face relationship, and that the UAV 14 is flying in space by hovering at a point A as an initial state.

FIG. 7 is a flowchart of processing of the speed control unit 32. When the speed control unit 32 starts the process, a limiting parameter is input in step S1100. The limiting parameters are the constant-velocity flight speed V ₀ shown in FIG. 3, the allowable error Δv for satisfying a constant speed, the distance L ₀ from the point to start deceleration to the point B, and the point B is reached. This is an allowable distance ΔL for determining whether or not. In the present embodiment, the allowable error Δv and the allowable distance ΔL, which are threshold values for the determination process, are set to values close to zero.

  In step S1101, the distance || T || from the midpoint q to the target point p is acquired. Specifically, since the position (point q) of the UAV 14 is obtained in time series using the position measurement sensor 10 in the position / orientation detection unit 30, the position of the UAV 14 at the present time and Δt seconds ago (one The position of the UAV 14 measured in the previous step) is taken out, and the distance || T || from the current position to the point B is calculated. || · || represents the norm (size) of the vector.

  In step S1102, it is determined whether the distance || T || is equal to or smaller than the allowable distance ΔL (|| T || ≦ ΔL). If || T || ≦ ΔL, the process shifts to step S1104. If || T ||> ΔL instead of || T || ΔL, the process proceeds to step S1106 to control the motion.

  In step S1104, control is performed to maintain the flight by hovering.

In step S1106, the velocity vector v of the UAV 14 is calculated. In the present embodiment, since the three-dimensional position of the point q ₂ from the point q ₁ by the position measuring sensor 10 is provided as an input, sequentially, given midpoint q as input. Using the midpoint q, the velocity vector v = m / Δt when the midpoint q moves by the vector m during the time interval Δt is calculated. Since the UAV 14 in the initial state starts from the hovering state, the speed is v = 0, and the distance to the point B || T || is sufficiently larger than L ₀ .

In step S1108, the distance || T || from the current position to the point B, it is determined whether the L ₀ or less. If the distance || T || from the current position to the point B is larger than L ₀ , the process proceeds to step S1109, and in acceleration control, according to the flight plan of FIG. Accelerate to _zero . Specifically, in the acceleration control, an absolute difference || V ₀ −v || between the current speed v and the speed limit V ₀ is obtained. At this time, when || V ₀ −v ||> Δv, the control data is

I ask. If || V ₀ −v || ≦ Δv, the control data is given as u = 0.

Is a gain parameter in feedback control and is set by the user according to the situation. By this feedback, when v <V ₀ , control is performed to accelerate to a constant speed V ₀ . Control data u is output and passed to the flight command converter 34. By continuing this repetition, the UAV 14 accelerates to a constant speed V _0, and when it reaches the speed limit, it continues to fly at the constant speed V ₀ . By continuing to fly at a constant speed, the relationship from the position of the UAV 14 to the point B eventually becomes || T || ≦ L ₀ . That is, the distance from the current position of the UAV 14 to the target point p is L ₀ or less. At this time, the process proceeds to step S1110, and the motion is controlled so as to decelerate from the flight at a constant speed in the deceleration control. In the deceleration control, an absolute difference || v || of the current speed v is obtained. At this time, if || v ||> Δv, the control data is

I ask. If || v || ≦ Δv, control data u = 0 is given. λ is a gain parameter in feedback control, and is set by the user according to the situation. For example,

However, a slightly larger value than the gain parameter value at the time of acceleration may be set so as to be in a hovering state at point B. Control data u is output and passed to the flight command converter 34. By repeating this feedback, when v> 0, the UAV 14 is decelerated until v = 0.

  In step S1112, it is determined whether to stop the speed control process. If the process is stopped, the process of the speed control unit 32 is terminated. If not, the process returns to step S1101 to repeat the process.

  Through the above processing, the speed control unit 32 outputs the control data u according to acceleration, constant speed, and deceleration of the UAV 14 in accordance with the flight plan of FIG.

  Next, details of the flight command conversion process of the flight command conversion unit 34 in step S104 will be described.

  By the processing of the flight command converter 34, the control data u given by the speed controller 32 and the azimuth θ obtained by the position / orientation detector 30 are converted into flight command flight command data according to the UAV 14 to be used. Command from 50 via wireless. There are various data formats for the flight command to the UAV 14, but in this embodiment, a case where a commercially available product AR Drone 2.0 is taken as an example is shown. However, it goes without saying that this embodiment can also be used when controlling the flight of other UAVs 14.

  FIG. 8 is a flowchart of processing of the flight command conversion unit 34. When the process is started, the flight command conversion unit 34 receives input of the updated control data u and the calculated azimuth angle θ in step S1200.

In step S1202, the combination of the control data u and the azimuth angle θ is converted into flight command data for transmission to the UAV 14. The flight command data to the UAV 14 includes the rotation speed V _x around the X axis, the rotation speed V _y around the Y axis, the speed V _z along the Z axis, and the rotation speed V _θ around the Z axis set in the aircraft. Become. It is assumed that the control data u is given as u = (u _x , u _y , u _z ). In UAV14, rotation around the X axis produces a translational movement of the Y axis, and rotation around the Y axis produces a translational movement of the X axis. 4) Convert according to equation.

The coefficients α _x , α _y , α _z , and α _θ are conversion coefficients that can be handled by the AR Drone 2.0, and may be determined by the user as parameters, for example, α _x = α _y = α _z = α _θ = 0. Give it one.

  In step S1204, the flight command data calculated by the equations (1) to (4) is transmitted to the UAV 14 by wireless communication via the communication unit 50.

In this process, whenever the control data u obtained by the speed control unit 32 and the azimuth angle θ obtained by the position / orientation detection unit 30 are given, the flight command data calculated by the equations (1) to (4) is sent to the UAV 14. Keep sending. By sending the azimuth angle θ, the orientation of the UAV 14 with respect to the target point p is corrected so as to be orthogonal to the vector T (middle point q to point p), as shown in FIG. When they are exactly orthogonal, the marker point q ₁ and the marker point q ₂ coincide with the points p ₁ and p ₂ .

  In step S1206, it is determined whether to stop the flight command conversion process. If the process is stopped, the process of the flight command conversion unit 34 is terminated. If not, the process returns to step S1200 to repeat the process.

  As described above, in the present embodiment, the position measurement sensor 10 calculates the position and orientation of the UAV 14 and controls the flight movement of the UAV 14 so as to fly toward the destination at a speed according to the flight plan shown in FIG. can do. Furthermore, since hovering at a speed of v = 0 is controlled as a constraint condition at the target location, the flight navigation of the UAV14 is realized stably and accurately because it is faithfully controlled along the flight route without departing from the route. can do.

  As described above, according to the flight control device according to the first embodiment, the three-dimensional coordinates of each marker measured by the position measurement sensor and the preset target point of the target location 12 of the UAV 14 are set. Based on this, the midpoint of each 3D coordinate of the marker, the distance from the midpoint to the target point, and the azimuth of the line connecting each 3D coordinate of the marker to the target point in the global coordinate system are calculated. The control data for controlling the velocity vector of the UAV determined from the middle point and the target point is updated so as to accelerate or decelerate according to the distance to the target point, and the updated control data and the calculated azimuth angle Based on the above, the flight command data in the UAV is calculated, and the movement of the UAV is controlled based on the calculated flight command data. It can be controlled so as to stably fly.

<Configuration and Action of Flight Control Device According to Second Embodiment of the Present Invention>
The second embodiment of the present invention is an example of controlling the flight so as to draw various geometric patterns in the space in the configuration of the first embodiment of FIG. FIG. 9 shows an example of the flight path of the UAV 14 based on the geometric pattern. In FIG. 9A, in the configuration of the first embodiment of FIG. 2 described above, the UAV is controlled by continuously controlling the straight flight from the point A to the point B and the straight flight from the point B to the point A. Flies in a round trip between point A and point B.

  In FIG. 9B, in the configuration of the first embodiment of FIG. 2, the straight flight from the point A to the point B, the straight flight from the point B to the point C, and the straight flight from the point C to the point A. By continuously controlling the flight, the UAV 14 flies around a triangular circuit formed by points A, B, and C.

  In FIG. 9C, in the configuration of the first embodiment of FIG. 2, the straight flight from the point A to the point B, the straight flight from the point B to the point C, the straight flight from the point C to the point D, In addition, the UAV 14 flies through a square circuit formed by the points A, B, C, and D by continuously controlling the straight flight from the point D to the point A.

  In FIG. 9D, in the configuration of the first embodiment of FIG. 2, the straight flight from the point A to the point B, the straight flight from the point B to the point C, the straight flight from the point C to the point D, By continuously controlling straight flight from D point to E point and straight flight from E point to A point, UAV 14 is formed at A point, B point, C point, D point and E point. Fly like drawing a pentagon. Or, straight flight from point A to point D, straight flight from point D to point B, straight flight from point B to point E, straight flight from point E to point C, and from point C to point A The UAV 14 flies in a pentagonal circle formed by point A, point D, point B, point E, and point C by continuously controlling straight flight. In the first embodiment, acceleration, constant speed, and deceleration are controlled on the basis of linear flight, and therefore, a geometric pattern drawn by linear flight can fly along a path of any geometric shape.

<Configuration and Action of Flight Control Device According to Third Embodiment of the Present Invention>
The third embodiment of the present invention is an example in which the flight of N UAVs 14 is simultaneously controlled in the configuration of the first embodiment of FIG. FIG. 10 shows a situation in which N UAVs 14 form a flight starting from UAV 14 # 1. Each UAV 14 is provided with two markers that can be detected by the position measurement sensor 10. UAV14 # midpoint of _{A 1} and point _{B 1} point 1 is assumed to correspond to the target point p of the first embodiment, UAV14 # midpoint of _{A 2} and point _{B 2} points 2 the first embodiment It corresponds to a _{q 1} and the point _{q 2} points. In the following description, it is assumed that the midpoint of the Nth UAV14 is q and the target point of the (N-1) th UAV14 is p. In order to prevent the UAVs 14 from colliding with each other, the speed control unit 32 controls the movement of the UAV 14 # 2 so that ΔL = D is given and the distance between the UAVs 14 is D, as in the first embodiment. However, a sufficient interval is set as the interval D so that the UAV 14 does not collide. In this embodiment, the case where the equal interval D is set will be described as an example. However, the distance may be individually set to a safe distance so that the UAV 14 does not collide.

By this assignment, as in the first embodiment, the point A ₂ of the UAV 14 # 2 approaches the midpoint coordinate of the point A ₁ of the UAV 14 # 1 and the point D of the point B ₁ , that is, the distance D to the target point. the position and orientation of the point _{B 2,} and UAV14 # 2 speed is controlled.

Similarly, the point A _N and the point B _N of the UAV 14 # N are made to correspond to the point q ₁ and the point q ₂ of the _first embodiment, and the target point p is UAV 14 # N-1 which is flying one before. By setting the midpoint of the points A _N-1 and B _N-1 as the target point p, the position, orientation, and speed of the UAV 14 # N are controlled.

  In the present embodiment, the movements of a plurality of UAVs 14 can be controlled simultaneously, and N UAVs 14 can fly in a flight along the straight line with UAV 14 # 1 at the head.

  The position / orientation detection unit 30, the speed control unit 32, and the flight command conversion unit 34 of the present embodiment perform processing on the first UAV 14 in the same manner as in the first embodiment. Thereafter, the following processing is performed for the second and subsequent UAVs 14.

  The position / orientation detection unit 30 according to the present embodiment is based on the three-dimensional coordinates of each of the Nth markers measured by the position measurement sensor 10 and the midpoint of the (N−1) th UAV 14 obtained in advance. , The midpoint of each of the Nth UAV14 markers, the distance from the midpoint to the target point p which is the midpoint of the (N-1) th UAV14, and the global coordinate system The azimuth angle θ of the line segment connecting the three-dimensional coordinates of each marker of the Nth UAV 14 with respect to the target point p, which is the middle point of the (N−1) th UAV 14, is calculated.

  Based on the distance from the midpoint q of the Nth UAV 14 to the target point p of the (N-1) th UAV 14, the speed control unit 32 is set to a speed according to the flight plan shown in FIG. The control data u for the Nth UAV 14 is updated.

  The flight command conversion unit 34 calculates flight command data in the Nth UAV 14 based on the control data u updated for the Nth UAV 14 and the azimuth angle θ calculated for the Nth UAV 14. The movement of the Nth UAV 14 is controlled based on the calculated flight command data.

  As described above, according to the flight control device according to the third embodiment, a plurality of UAVs can be obtained by issuing a flight command indicating a three-dimensional position and direction to each UAV. It is possible to freely operate in the space without colliding each other. Furthermore, by controlling the UAV for the user's intention, it becomes possible for the UAV to support work that cannot be realized by a single action of a person or to perform cooperative work in an actual environment surrounding the person.

<Configuration and Action of Flight Control Device According to Fourth Embodiment of the Present Invention>
The fourth embodiment of the present invention is an example of controlling formation flight so as to draw various geometric patterns shown in FIG. 9 in the space in the configuration of the third embodiment. Alternatively, if it is a geometric pattern drawn by straight flight, it is possible to fly in a formation along a path having an arbitrary geometric shape.

  Note that the present invention is not limited to the above-described embodiment, and various modifications and applications are possible without departing from the gist of the present invention.

10 Position measurement sensor 12 Target location 14 UAV
20 arithmetic unit 30 position and direction detection unit 32 speed control unit 34 flight command conversion unit 50 communication unit 100 flight control device

Claims (7)

A position measurement sensor that is provided to a UAV (Unmanned Aerial Vehicle) and that measures the three-dimensional coordinates of each of a plurality of markers whose distances between the markers are known;
Based on the three-dimensional coordinates of each of the markers measured by the position measurement sensor and the target point of the target location of the UAV set in advance, the midpoint, the midpoint of each of the markers A position and orientation detection unit that calculates a distance from the target point to the target point, and an azimuth angle of a line segment connecting each three-dimensional coordinate of the marker with respect to the target point in a global coordinate system;
A speed control unit that updates control data for controlling the speed vector of the UAV determined from the middle point and the target point so as to accelerate or decelerate according to the distance to the target point;
A flight command conversion unit that calculates flight command data in the UAV based on the updated control data and the calculated azimuth angle, and controls movement of the UAV based on the calculated flight command data When,
Including flight control device. When the distance to the target point is equal to or less than a predetermined distance, the speed control unit updates the control data as an arbitrary speed vector obtained by decelerating the speed vector,
The control data is updated as an arbitrary speed vector obtained by accelerating the speed vector or setting a constant speed when the distance to the target point is longer than the predetermined distance. The flight control device described. The UAV is N UAVs, the midpoint of the (N-1) th UAV is the target point of the Nth UAV,
The position / orientation detection unit is based on a three-dimensional coordinate of each of the Nth markers measured by the position measurement sensor and a midpoint of the N−1th UAV obtained in advance. , The midpoint of each of the markers of the Nth UAV, the distance from the midpoint to the target point which is the midpoint of the (N-1) th UAV, and N in the global coordinate system Calculating an azimuth angle of a line segment connecting the three-dimensional coordinates of the markers of the Nth UAV with respect to the target point which is the midpoint of the first UAV;
The speed control unit updates the control data for the Nth UAV based on a distance from the midpoint of the Nth UAV to the target point,
The flight command conversion unit is configured to perform a flight in the Nth UAV based on the control data updated for the Nth UAV and the azimuth calculated for the Nth UAV. The flight control device according to claim 1, wherein command data is calculated, and the movement of the Nth UAV is controlled based on the calculated flight command data. A step of measuring a three-dimensional coordinate of each of a plurality of markers provided with a position measurement sensor on a UAV (Unmanned Aerial Vehicle) and having a known distance between the markers;
The position / orientation detection unit is configured to determine the three-dimensional coordinates of each of the markers based on the three-dimensional coordinates of each of the markers measured by the position measurement sensor and the preset target point of the target location of the UAV. Calculating a midpoint, a distance from the midpoint to the target point, and an azimuth of a line segment connecting each three-dimensional coordinate of the marker with respect to the target point in a global coordinate system;
Updating a control data for controlling a velocity vector of the UAV determined from the middle point and the target point so that the speed control unit accelerates or decelerates according to a distance to the target point;
A flight command conversion unit calculates flight command data in the UAV based on the updated control data and the calculated azimuth angle, and moves the UAV based on the calculated flight command data. A controlling step;
Including flight control method. In the step of updating the speed control unit, when the distance to the target point is equal to or less than a predetermined distance, the control data is updated as an arbitrary speed vector obtained by decelerating the speed vector,
The control data is updated as an arbitrary speed vector obtained by accelerating the speed vector or setting a constant speed when the distance to the target point is longer than the predetermined distance. The flight control method described. The UAV is N UAVs, the midpoint of the (N-1) th UAV is the target point of the Nth UAV,
In the step of calculating by the position / orientation detection unit, the three-dimensional coordinates of each of the Nth markers measured by the position measurement sensor and the midpoint of the U-1th UAV obtained in advance Based on the three-dimensional coordinates of each of the markers of the Nth UAV, the distance from the midpoint to the target point which is the midpoint of the (N-1) th UAV, and global Calculating an azimuth angle of a line segment connecting the three-dimensional coordinates of each of the markers of the Nth UAV with respect to the target point which is the midpoint of the N-1st UAV in the coordinate system;
The step of updating by the speed control unit updates the control data for the Nth UAV based on a distance from the middle point of the Nth UAV to the target point,
In the step of controlling by the flight command conversion unit, based on the control data updated for the Nth UAV and the azimuth calculated for the Nth UAV, the Nth UAV 6. The flight control method according to claim 4, wherein flight command data in a UAV is calculated, and a motion of the Nth UAV is controlled based on the calculated flight command data. 7.   The program for functioning a computer as each part of the flight control apparatus of any one of Claims 1-3.