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

Abstract

The present invention makes it possible to perform control that makes a plurality of UAV fly smoothly and stably in formation while monitoring all directions. For at least one follower UAV, the present invention: calculates the difference between the bearing angle of a leader UAV and the bearing angle of the follower UAV and, when the calculated difference does not satisfy a prescribed tolerance for the angle of the follower UAV relative to the leader UAV, updates the bearing angle of the follower UAV such that the tolerance is satisfied; updates second control data on the basis of a distance that is calculated from a vector to a target point for the follower UAV and from the updated or calculated bearing angle; controls the operations of the leader UAV on the basis of calculated flight instruction data; calculates flight instruction data for the follower UAV on the basis of the second control data and of the updated or calculated bearing angle; and controls the operations of the follower UAV on the basis of the calculated flight instruction data.

Description

Flight control device, method, and program

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

A UAV (Unmanned Aerial Vehicle) is an unmanned aerial vehicle that has a propeller driven by a plurality of rotors and controls flight by controlling the lift applied to each propeller. Most unmanned airplanes called drones are a type of UAV. Usually, a quadrotor UAV having four propellers is used. The unmanned airplane handled hereinafter is referred to as a quadrotor UAV, and its name is simply abbreviated as UAV.

Generally, in order to measure the attitude of the UAV, a local coordinate system is set on the body as shown in FIG. The forward direction of the UAV 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.

(4) 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 coordinates are three-dimensional coordinates in the world coordinate system, and in the motion capture system, the measurement coordinate system is the 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 posture of the UAV is represented by the rotation angle of the local coordinate system with respect to the global coordinate system, rotation around the X axis is roll rotation (rotation angle φ), rotation around the Y axis is pitch rotation (rotation angle ω), and Z axis. The surrounding rotation is called yaw rotation (rotation angle θ). The flight motion of the UAV generates rotation around the X axis, rotation around the Y axis, and rotation around the Z axis by changing the lift applied to the four propellers. Rotation about the Y axis produces a translation in the X direction, and rotation about the X axis produces a translation in the Y direction. Rotation around the Z axis produces azimuthal rotation, and when the same lift is simultaneously applied to four propellers, the strength of the propellers produces translational movement in the Z axis direction (elevation elevation).

Global coordinate system _{X w,} Y _w, when the predetermined coordinate value _{(X d, Y d, Z} d) is the orientation theta _d given in _{Z w,} the current position P = the UAV (X, Y, Z) and In order to fly from the current azimuth to the predetermined position and direct the aircraft to the predetermined azimuth, it is necessary to control the attitude and position of the UAV. Non-Patent Document 1 discloses a backstepping control based on the equation of motion and the equation of angular motion of UAV. Non-Patent Document 2 discloses a flight motion control method for AR Drone 2.0 (a commercially available low-cost quad-rotor UAV). In many of the prior art, a discrete predetermined position and a predetermined azimuth are given as a trajectory of a flight in a global coordinate system, and the motion is controlled so that the UAV tracks each point and each azimuth. On the other hand, formation flight using a plurality of UAVs can provide patrol, rescue, and transportation from a broader viewpoint. In formation flight using a plurality of UAVs, it is necessary to control the position and attitude of each UAV according to the work mission, and in Non-Patent Document 3, formation using linear model predictive control (Linear Model Predictive Control) Flight control is public.

先行 Prior art relating to flight motion control of a single UAV is known in Non-Patent Documents 1 and 2, and formation flight control using a plurality of UAVs is known in Non-Patent Document 3.

FIG. 19 shows an example of formation flight using a plurality of UAVs. In general, it flies in a leader-follower formation. The UAV acting as a follower flies following the UAV acting as a leader instructing the entire flight. The trail flies with the UAV of the leader at the head and the UAVs of the followers line up in a vertical line, and the abrest flies with the UAVs of the followers lined up in a horizontal line around the UAV of the leader. On the other hand, in Delta, the UAV of the leader follows the UAV of the follower so as to form a triangle.

In the example of FIG. 19, since each UAV flies along the traveling direction, the head (nose) of the aircraft is oriented in the traveling direction. Many commercially available UAVs have a camera attached to the top, and this camera can be used to capture the scene in front. A remote patrol using a UAV monitors a video acquired by a camera at the head. For convenience of explanation, it is assumed that all UAVs acquire images with cameras having the same angle of view. For example, in a trail-type formation flight, to monitor camera images in the same direction, the leader UAV acquires a front image without occlusion, and the follower UAV acquires a front image in which the front UAV is reflected. Since this flight flies in a row, it acquires substantially overlapping time-series images except that the leader UAV ahead is reflected. On the other hand, in the Abrest type formation flight, the other aircraft is not reflected in the camera images of all UAVs, and a time-series image in which the left and right portions of the screen partially overlap depending on the distance to the adjacent UAV is obtained. If images are synthesized using overlapping portions, a wide-field image in front, that is, a panoramic image in front can be obtained. In a Delta formation flight, the leader UAV acquires a forward image without occlusion. The video of the follower UAV is reflected by the leader UAV, but a time-series image in which the right and left portions of the screen partially overlap depending on the distance to the adjacent UAV is obtained. By synthesizing those images based on the overlapped portions reflected by the leader UAV, a panoramic image ahead can be obtained.

編 In the formation flight shown in FIG. 19, all the UAVs fly with the camera directed in the direction of travel, so that the surroundings of the formation flight cannot be monitored at once. For example, when an obstacle, a person, and another UAV approach from the left and right and from behind, the approach is overlooked in the time-series image. Non-Patent Document 3 discloses a method of controlling the formation flight shown in FIG. 19, but it is intended only for formation flight from the current position to the destination, and there is no solution for patrol such as omnidirectional monitoring. Absent.

SUMMARY OF THE INVENTION The present invention has been made to solve the above problems, and a flight control device and method capable of controlling a plurality of UAVs to form a formation and fly smoothly and stably while monitoring all directions. , And to provide programs.

In order to achieve the above object, a flight control device according to a first invention controls a formation flight of a plurality of UAVs (Unmanned Aerial Vehicles) and forms a formation in accordance with the leader UAV leading the formation flight and the leader UAV. A flight control device for controlling one or more followers UAV to perform the three-dimensional operation of each of a plurality of markers provided to the leader UAV and the one or more followers UAV and having a known distance between the markers. A position measuring sensor for measuring coordinates, three-dimensional coordinates of each of the markers of the reader UAV measured by the position measuring sensor, and a first target point which is a preset destination of the reader UAV. Calculating a current position of the leader UAV, a vector to the first target point, and an azimuth angle with respect to the first target point based on the current position of the leader UAV. A reader position detecting unit that performs updating of first control data for controlling a position of the reader UAV based on a vector of the reader UAV up to the first target point; and a reader UAV. The flight command data in the leader UAV is calculated based on the first control data updated for the leader UAV and the azimuth angle calculated for the leader UAV, and the leader UAV is calculated based on the calculated flight command data. A first flight command converter for controlling the movement of the follower, three-dimensional coordinates of each of the markers of the follower UAV measured by the position measurement sensor for the one or more followers UAV, and the follower determined by formation The current UAV of the follower UAV based on the second target point of the UAV. Position, a vector to the second target point, and a follower position detector that calculates an azimuth angle with respect to the second target point; and, for the one or more followers UAV, an azimuth angle of the reader UAV and the azimuth angle. The azimuth of the follower UAV is updated so that the difference between the azimuth of the follower UAV and a predetermined angle determined by the follower UAV satisfies a predetermined allowable angle. A formation control unit that updates second control data for controlling the position of the follower UAV based on the vector up to the target point and the updated azimuth, and for the one or more followers UAV, The second control data updated for the follower UAV; and the second control data updated for the follower UAV. A second flight command converter configured to calculate flight command data in the follower UAV based on the azimuth and control the movement of the follower UAV based on the calculated flight command data. Have been.

A flight control method according to a second invention controls a formation flight of a plurality of UAVs (Unmanned Aerial Vehicles), and includes a leader UAV leading the formation flight and one or more followers UAV forming a formation according to the leader UAV. A flight control method in a flight control device for controlling, wherein a position measurement sensor is provided to the reader UAV and the one or more followers UAV, and a tertiary of each of a plurality of markers whose distance between the markers is known. Measuring the original coordinates, wherein the leader position detector detects the three-dimensional coordinates of each of the markers of the reader UAV measured by the position measurement sensor and a preset first destination of the leader UAV. , The current position of the leader UAV, the vector to the first target point, and the first Calculating the azimuth angle of the reader UAV with respect to the target point, and the reader position control unit outputs first control data for controlling the position of the reader UAV based on a vector of the reader UAV to the first target point. Updating, and the first flight command conversion unit, based on the first control data updated for the leader UAV and the azimuth calculated for the leader UAV, issues a flight command in the leader UAV. Calculating data and controlling the movement of the leader UAV based on the calculated flight command data, wherein the follower position detector detects the follower measured by the position measurement sensor for the one or more followers UAV. The three-dimensional coordinates of each of the markers on the UAV and the format determined by the formation Calculating a current position of the follower UAV, a vector to the second target point, and an azimuth angle with respect to the second target point based on a second target point of the word UAV; The unit determines, for the one or more followers UAV, a difference between an azimuth of the leader UAV and an azimuth of the follower UAV and a difference between a predetermined angle determined by the follower UAV and a predetermined allowable angle. And updating the azimuth angle of the follower UAV to control the position of the follower UAV based on the vector of the follower UAV to the second target point and the updated azimuth angle. Updating control data; and a second flight command converter for the one or more follower UAVs, Calculating the flight command data in the follower UAV based on the second control data updated for the follower UAV and the updated or calculated azimuth angle for the follower UAV, and calculating the calculated flight command data. Controlling the movement of the follower UAV based on the following.

プ ロ グ ラ ム A program according to a third invention is a program for causing a computer to function as each section of the flight control device according to the first invention.

According to the flight control device, method, and program of the present invention, a plurality of UAVs form a formation in order to control the azimuth of the leader UAV and the azimuths of all followers UAV so as to divide the entire circumference equally. Thus, it is possible to control to fly smoothly and stably while monitoring all directions.

It is a block diagram showing the composition of the flight control device concerning an embodiment of the present invention. It is a figure showing an example of a situation which controls flight of UAV. 4 is a flowchart illustrating a flight control processing routine in the flight control device according to the embodiment of the present invention. It is a flowchart of a leader position detection process in a flight control process routine. A point marker, the point b, and the point p objective point is a diagram representing the position and orientation of the relationship viewed from Z _w axis. It is a flowchart of a leader position control process in a flight control process routine. It is a flowchart of a flight command conversion process in a flight command conversion process routine. It is a flowchart of a follower position detection process in a flight command conversion process routine. FIG. 7 is a diagram illustrating a positional relationship between a leader UAV and a follower UAV in an initial state according to the first embodiment. It is a flow chart of formation control processing in a flight command conversion processing routine. FIG. 7 is a diagram illustrating a positional relationship between a leader UAV and a follower UAV in a final state according to the first embodiment. It is a flowchart of the camera image monitoring processing in the flight command conversion processing routine. FIG. 14 is a diagram illustrating a positional relationship between a leader UAV and a follower UAV in an initial state according to the second embodiment. FIG. 14 is a diagram illustrating a positional relationship between a leader UAV and a follower UAV in a final state according to the second embodiment. FIG. 14 is a diagram illustrating a positional relationship between a leader UAV and a follower UAV in an initial state according to the third embodiment. It is a figure showing the physical relationship of leader UAV and follower UAV in the last state of a 3rd embodiment. FIG. 1 is a diagram illustrating an overview of a UAV and an example of a UAV-fixed local coordinate system. FIG. 3 is a diagram illustrating a relationship between a global coordinate system and a UAV-fixed local coordinate system. It is a figure showing an example of formation of formation flight.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. The method according to the embodiment of the present invention acquires a surrounding camera image while forming a formation by appropriately issuing a flight command indicating a three-dimensional position and a direction to each UAV so as not to collide with each other. Monitoring. Further, by controlling each UAV according to the user's intention, it is possible for a plurality of UAVs to support or cooperate in a work that cannot be realized by a single action of a person in a real environment surrounding the person.

<Configuration of Flight Control Device According to First Embodiment of the Present Invention>
A configuration of a flight control device for a quad rotor UAV according to the first embodiment of the present invention will be described. The present embodiment is a form in which formation flight by a plurality of UAVs is controlled to acquire an omnidirectional camera image, and an example in which a leader UAV and a follower UAV are used will be described.

As shown in FIG. 1, a flight control device 100 according to a first embodiment of the present invention includes a CPU, a RAM, and a ROM storing a program and various data for executing a flight control processing routine described later. , And can be configured by a computer. The flight control device 100 functionally 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 need to be connected as a constituent element, but only needs to acquire data necessary for processing, and the reader position detection unit 30, the leader position control unit 32, the first The flow of data from the flight command conversion unit 34, the second flight command conversion unit 35, the follower position detection unit 40, the formation control unit 42, the camera video monitoring unit 44, and the video DB 46 to the respective arrows is as follows. The recording medium such as an apparatus or a CD-ROM may be used, or a remote data resource may be used via a network.

The position measurement sensor 10 measures, as measurement data, the three-dimensional coordinates of each of a plurality of markers provided to each UAV and having a known distance between the markers. In general, it is known that a motion capture device can measure three-dimensional coordinates of a predetermined marker with high accuracy in real time. In the present embodiment, a motion capture device is used as an example of the position measurement sensor 10. 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 assigned to each UAV are sequentially measured at certain fixed intervals. You. The destination is indicated by a point p, and its coordinate value is arbitrarily set according to the flight plan. Hereinafter, an example of formation flight control using two UAVs will be described. However, it is needless to say that the technique of the embodiment of the present invention can be easily extended even when three or more UAVs are used.

The reader UAV 14 has sensing markers attached at points a, b, c, and d. It is assumed that the first line segment connecting the points a and b is orthogonal to the second line segment connecting the points c and d at the position of the point e. The position of the point e only needs to satisfy the condition that the line segments are orthogonal to each other, and need not be the midpoint of each line segment. The camera of the leader UAV 14 is installed in the direction from the point d to the point c. Followers UAV15 # 1 have sensing markers attached to points a ₁ , b ₁ , c ₁ , and d ₁ . The first line segment connecting the points a ₁ and the point b ₁ are orthogonal with the position of the second line segment and the point e ₁ connecting the point c ₁ and the point d _1. Followers UAV 15 # 1 camera is installed from point _{d 1} in the direction of orientation of the point _{c 1.} In the case where followers UAV 15 # 2 is present are likewise followers UAV 15 # point _{a 2} to 2, point _{b 2,} point _{c 2,} and markers for sensing is attached to the position of the point _{d 2} Shall be. The first line segment connecting the points a ₂ and the point b ₂ are orthogonal with a second position of the line segment and the point e ₂ connecting the point c ₂ and the point d _2. Followers UAV 15 # 2 camera is installed from point _{d 2} in the direction of orientation of the point _{c 2.} In follower UAV 15 # 1, # 2, the position of the point _{e 1} and the point _{e 2} may satisfy the condition that the line segment is perpendicular, not necessarily the midpoint of each line segment.

The calculation unit 20 includes a reader position detection unit 30 that detects the position of the reader UAV 14 in the global coordinate system by the position measurement sensor 10, a reader position control unit 32 that controls the flight of the reader UAV 14 along a predetermined route, and a reader UAV 14 The first flight command conversion unit 34 for sending command data to the follower UAV 15, the second flight command conversion unit 35 for sending command data to the follower UAV 15, and the position measurement sensor 10 A follower position detection unit 40 for detecting the position, a formation control unit 42 for controlling the flight of the follower UAV 15 based on the position of the leader UAV 14 and the azimuth of the follower UAV 15, and a command for acquiring an image captured by a camera from each UAV. And pictures A camera video monitoring unit 44 sends an instruction to transfer the image DB 46, is configured to include a video DB 46. The specific processing contents of each processing unit will be described in the following description of the operation.

The leader position detection unit 30 calculates the three-dimensional coordinates of the points a, b, c, and d of the marker of the reader UAV 14 measured by the position measurement sensor 10 and the preset target point of the destination 12 of the UAV 14. Based on p, a current position e of the leader UAV 14, a vector T from the current position e to the target point p, and an azimuth θ with respect to the target point p are calculated. The current position e is an intersection point e of a first line connecting the three-dimensional coordinates of the points a and b of the marker and a second line connecting the points c and d. The azimuth angle θ is the azimuth angle of the first line connecting the three-dimensional coordinates of the points a and b of the marker with respect to the target point p in the global coordinate system. Note that the target point p is an example of a first target point, and the azimuth θ is an example of an azimuth with respect to the first target point.

The leader position control unit 32 updates the first control data u for controlling the position of the reader UAV 14 based on the distance || T || obtained from the vector T to the target point p of the reader UAV 14. The leader position control unit 32 transmits a formation completion notification to the camera image monitoring unit 44 when the leader UAV 14 satisfies a predetermined allowable distance ΔL with respect to the target point p.

The first flight command converter 34 converts the rotation speed V _x around the X axis and the rotation speed V around the Y axis of the reader UAV 14 based on the updated control data u and the calculated azimuth angle θ for the reader UAV 14. The rotation speed V _y , the speed V _z along the Z-axis, and the rotation speed V _θ around the Z-axis are calculated as flight command data, and the flight command data is transmitted to the reader UAV 14 via the communication unit 50 to calculate. The movement of the leader UAV 14 is controlled based on the obtained flight command data.

The follower position detection unit 40 determines, for one or more followers UAV15 # i, the three-dimensional coordinates of each of the markers of the follower UAV15 # i measured by the position measurement sensor 10, and the target point p of the follower UAV15 # i determined by the formation. based on the _i, calculated for followers UAV 15 # i, the current position _{e i} followers UAV 15 # i, the vector _{T i} to the target point _{p i} from the current position _{e i,} and the azimuth angle theta _i with respect to the target point _{p i} I do. Current position _{e i} is a second line segment intersection _{e i} connecting the first line segment and the point _{c i,} point _{d i} connecting _{a i,} the three-dimensional coordinates of the point _{b i} point markers. The azimuth angle θ _i is the azimuth angle of the first line connecting the three-dimensional coordinates of the marker points a _i and b _i with respect to the target point p _i in the global coordinate system. Note that the target point p _i is an example of a second target point, and the azimuth θ _i is an example of an azimuth with respect to the second target point.

Formation control unit 42, for one or more followers UAV 15 # i, but updates the second control data _{u i} for controlling the position of the follower UAV 15 # i, the process according to the conditions are different. A difference between the difference between the azimuth of the leader UAV14 and the azimuth of the follower UAV15 # i and a predetermined angle determined by the follower UAV15 # i is calculated, and the calculated difference is determined by the follower UAV15 # i with respect to the predetermined leader UAV14. The following processing is performed depending on whether or not the allowable angle Δθ is satisfied.

If the difference does not satisfy the allowable angle Δθ, the azimuth θ _i of the follower UAV15 # i is updated so that the difference satisfies the allowable angle Δθ, and the vector T to the target point p _i of the follower UAV15 # i is updated. _i ', and the distance obtained from the updated azimuth θ _i || T _i' based on ||, and updates the second control data u _i for controlling the position of the follower UAV 15 # i. Difference is acceptable if it meets the angle Δθ based on the || 'distance || T _i obtained from, and the calculated azimuth angle theta _i' the vector T _i to the target point p _i followers UAV 15 # i, updates the second control data _{u i.} In addition, the formation control unit 42 transmits a formation completion notification to the camera video monitoring unit 44 when the follower UAV 15 # i satisfies a predetermined allowable distance ΔL with respect to the target point p _i . In this way, the formation control unit 42 sets the follower UAV # i so that the entire circumference is equally divided by the azimuth of the leader UAV14 and the updated azimuth (or the calculated azimuth) of all the followers UAV15 # i. Update the azimuth of.

The second flight command conversion unit 35 is updated by the formation control unit 42, which will be described later, for one or more follower UAVs 15 # i (in the present embodiment, an example in which only i = 1 is described). Based on the control data u _i and the updated or calculated azimuth angle θ _i , the follower UAV 15 # i has a rotation speed V _x around the X axis, a rotation speed V _y around the Y axis, and a speed along the Z axis. V _z, and calculates the rotational speed V _theta around the Z axis as the flight command data, by sending the flight follower UAV15 command data via the communication unit 50 # i, followers based on the calculated flight command data UAV15 Control the movement of #i.

When the camera image monitoring unit 44 receives all the formation completion notifications of the leader UAV 14 and one or more followers UAV 15 #i, the camera image monitoring unit 44 acquires images by the cameras of the leader UAV 14 and one or more followers UAV 15 #i. Control.

The communication unit 50 transmits the flight command data to the leader UAV14 and one or more followers UAV15 # i. In addition, the communication unit 50 notifies the leader UAV 14 and one or more followers UAV 15 #i of an instruction to acquire a camera image in response to the acquisition instruction from the camera image monitoring unit 44. In addition, the communication unit 50 stores the video acquired for each UAV according to the transfer instruction from the camera video monitoring unit 44 in the video DB 46. The video DB 46 is connected to a main server operated by an operator (not shown). The transfer from each UAV to the video DB 46 transmits the video using wireless. Alternatively, the video may be temporarily stored in a memory inside each UAV, and the video may be transmitted by radio at an instruction of the operator or at a certain interval as appropriate.

<Operation of Flight Control Apparatus According to First Embodiment of 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 each of the leader UAV 14 and one or more followers UAV 15 is started, the flight control device 100 executes a flight control processing routine shown in FIG. It is assumed that the flight control processing routine repeats the processing independently for each step as described in detail of each step.

First, in step S100, the reader position detecting unit 30 determines the three-dimensional coordinates of the points a, b, c, and d of the marker of the reader UAV14 measured by the position measurement sensor 10 and the preset reader UAV14. Of the first line connecting the three-dimensional coordinates of the points a and b of the marker and the second line connecting the points c and d based on the target point p of the destination 12 The vector T to the target point p and the azimuth angle θ of the first line connecting the three-dimensional coordinates of the marker points a and b with respect to the target point p in the global coordinate system are calculated.

Next, in step S102, the leader position control unit 32 determines the first control data u for controlling the position of the reader UAV 14 based on the distance || T || To update. Further, the leader position control unit 32 transmits a formation completion notification or a formation non-completion notification to the camera video monitoring unit 44 according to a predetermined condition.

In step S104, the first flight command conversion unit 34 calculates the rotation speed of the reader UAV 14 about the X axis in the reader UAV 14 based on the control data u updated in step S102 and the calculated azimuth angle θ. V _x, leader via the rotational speed V _y, calculates the speed V _z along the Z _axis, and the rotational speed V _theta around the Z axis as the flight command data, the communication unit 50 the flight command data about the Y-axis UAV14 To control the movement of the leader UAV 14 based on the calculated flight command data.

In step S106, followers UAV15 # i (i = 1, 2,...) Are selected in order to control one or more followers UAV15 in order. In this embodiment, since i = 1, only one device is selected.

In step S108, the follower position detection unit 40 calculates, from the three-dimensional coordinates of each of the markers of the follower UAV15 # i measured by the position measurement sensor 10, and the intersection e of the first line segment and the second line segment of the leader UAV14. Based on the target point p _i of the follower UAV15 # i, which is determined by coordinates at a predetermined distance on an extension of the first or second line segment of the leader UAV14, the marker of the follower UAV15 # i point _{a i,} point _b the first line segment and the point _{c i} connecting the three-dimensional coordinates of the _i, second line of intersection connecting points _{d i,} the vector _{T i} to the target point _{p i} from the intersection, and the global coordinates The azimuth of the first line segment connecting the three-dimensional coordinates of the marker points a _i and b _i with respect to the target point p _i in the system is calculated.

In step S110, formation control unit 42, the follower UAV 15 # i, based on the distance || _{T i} || obtained from the vector _{T i} to the target point _{p i} followers UAV 15 # i, the position of the follower UAV 15 # i updates the second control data u _i to control. Further, the formation control unit 42 transmits a formation completion notification or a formation non-completion notification to the camera video monitoring unit 44 according to a predetermined condition.

In step S112, the second flight command conversion unit 35, the follower UAV 15 # i, based on the second control data _{u i,} is calculated and the azimuth angle theta _i updated in step S110, followers UAV 15 # i in the rotational speed V _x about the X-axis, the rotational speed V _y around the Y-axis, the speed V _z along the Z _axis, and calculates the rotational speed V _theta around the Z axis as the flight command data, the flight command data The motion of the follower UAV 15 # i is controlled based on the calculated flight command data by transmitting the data to the follower UAV 15 # i via the communication unit 50.

In step S114, it is determined whether the control has been completed for all the followers UAV15 # i, and if not completed, the process returns to step S106 to select the next follower UAV15 # i and repeats the processing. The process moves to S116.

In step S116, the camera video monitoring unit 44 receives the formation completion notification for all the followers UAV15 # i, determines whether the formation is completed, and if completed, determines from the leader UAV14 and all the followers UAV15 # i. An instruction to synchronize and acquire the image of the camera is transmitted to each UAV via the communication unit 50, and an instruction to transfer the acquired image to the image DB 46 is transmitted to the communication unit 50.

Next, details of the leader position detection processing of the leader position detection unit 30 in step S100 will be described.

FIG. 4 is a flowchart of the processing of the reader position detection unit 30. Upon starting the process, the leader position detection unit 30 sets a target point p of the destination 12 in the global coordinate system in step S1000. The target point p may be any three-dimensional coordinates determined by the operator. When the leader UAV 14 reaches the destination, three-dimensional coordinates p of the next destination 12 are given. The three-dimensional coordinates p of the destination 12 are discretely given along the formation flight path.

In step S1002, the position measuring sensor 10 detects the points a, b, c, and d of the marker of the reader UAV 14, and calculates the position of the intersection e. Although a marker for sensing may be installed at the intersecting position e, in the present embodiment, the point e is moved from the points a, b, c, and d of the four markers for versatility. Obtain by calculation. In this processing, the three-dimensional coordinates of the point e are referred to as the current position, and hereinafter, the point e is set as the current position of the reader UAV14.

In step S1004, a vector T from the current position (point e) of the reader UAV 14 to the destination (point p) is obtained by calculating T = pe.

In step S1006, a rotation angle θ (azimuth) required for the vector ab from the point a to the point b to be orthogonal to the vector T is calculated using the relationship between vector inner products. Figure 5 represents the point of the marker a, point markers b, and the relationship between the position and orientation as viewed from the Z _w axis point p of the target areas. When the azimuth θ = 0, the vector ab is orthogonal to the vector T.

In step S1008, it is determined whether or not to stop the position detection process. If the process is to be stopped, the process of the reader position detection unit 30 is ended. If not, the process returns to step S1002 to repeat the process. Here, the case where the processing is stopped is defined as a case where the operator ends the flight control of the leader UAV 14, and the same applies to the following processing.

The leader position detection unit 30 calculates the current position (point e) of the leader UAV, the vector T to the target point p, and the azimuth angle θ by the above processing.

Next, details of the leader position control process of the leader position control unit 32 in step S102 will be described. In this processing, a processing flow for controlling a straight flight from the current position to the destination based on the vector T obtained by the leader position detection unit 30 will be described.

FIG. 6 is a flowchart of the processing of the reader position control unit 32. Upon starting the process, the leader position control unit 32 specifies an allowable distance in step S1100. The allowable distance is a distance ΔL for determining whether the current position e of the reader UAV 14 has reached the point p of the destination. Since the fluctuation of the air resistance affects the flight of the UAV, the allowable distance ΔL is set to 10 cm as an example.

In the leader position detection unit 30, the current position (point e) of the reader UAV 14 is obtained in time series using the position measurement sensor 10, and is given to step S1102 of this processing routine every time the current position is acquired (FIG. X in FIG. 4 and FIG. 6).

In step S1102, the distance || T || from the current position e to the point p of the destination is calculated based on the given data of the current position. || · || represents the norm (magnitude) of the vector.

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

In step S1106, the first control data is

Is calculated by the following formula. The first control data u is used by the first flight command converter 34 as a flight command to the reader UAV14.

Is a gain parameter in the feedback control, which is set by the user according to the situation. When the first control data u is calculated, it is passed to the first flight command conversion unit 34 (denoted as Y in FIG. 6).

に よ り By continuing this repetition, the leader UAV 14 approaches the point p of the destination, and when || T || ≦ ΔL is satisfied, the leader UAV 14 determines that the leader UAV 14 has reached the destination and enters the hovering state.

In step S1107, it is determined that the formation of the leader UAV 14 has not been completed, and a formation completion notification is transmitted to the camera video monitoring unit 44 (denoted by W in FIG. 6).

In step S1108, it is determined whether to stop the position control process. If the process is to be stopped, the process of the reader position control unit 32 is ended. If not, the process returns to step S1102 to repeat the process.

In step S1110, control is performed such that the reader UAV 14 is put on standby in a hovering state.

In step S1112, a formation completion notification is transmitted to the camera video monitoring unit 44 (denoted by W in FIG. 6), and the flow shifts to step S1102.

With the above processing, the leader position control unit 32 outputs the first control data u according to the distance || T || from the current position e of the reader UAV 14 to the point p of the destination.

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

By the processing of the first flight command conversion unit 34, the control data u given by the leader position control unit 32 and the azimuth θ obtained by the leader position detection unit 30 are used for the leader UAV 14, and the flight command of the flight command is given according to the leader UAV 14. The data is converted into data and the communication unit 50 issues a command via wireless communication. Although there are various data formats for the flight command to the UAV 14, in the present embodiment, a case is shown in which a commercially available AR Drone 2.0 is used as an example. However, it is needless to say that the present embodiment can be used for controlling the flight of the UAV 14 other than the above. In the following, the case of the leader UAV 14 in the first flight command converter 34 will be described as an example. However, the second flight command converter 35 can be controlled in the same manner as the first flight command converter 34. In the second flight command conversion unit 35, a case of controlling one or more followers UAV 15 # i includes a first control data u and the azimuth angle theta, to the azimuth and theta _i second control data u _i It is converted to flight command data and controlled.

FIG. 7 is a flowchart of the flight command conversion process of the first flight command conversion unit 34. When the first flight command conversion unit 34 starts the process, in step S1200, the input of the updated control data u and the calculated azimuth angle θ is accepted.

In step S1202, the combination of the control data u and the azimuth angle θ is converted into flight command data to be transmitted to the reader UAV14. Flight instruction data to the reader UAV14 the rotational speed V _x around the X axis is set to the _body, the rotational speed V _y around the Y _axis, and the velocity V _z along the Z-axis, the rotational speed V _theta around the Z axis become. Control data u is _{u = (u x, u y} , u z) and is given as. In the UAV 14, since the rotation about the X axis produces a translational movement on the Y axis and the rotation about the Y axis produces a translational movement on the X axis, in this processing, the flight command data given to the reader UAV 14 is expressed by the following equation (1). Convert.

... (1)

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 a parameter. For example, α _x = α _y = α _z = α _θ = 0. Give 1

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

In this process, every time the control data u obtained by the leader position control unit 32 and the azimuth angle θ obtained by the leader position detection unit 30 are given, the flight command data calculated by the equation (1) is continuously sent to the reader UAV14. As shown in FIG. 5, by sending the azimuth angle θ, the direction of the leader UAV 14 with respect to the point p of the destination is corrected such that the points a and b of the marker coincide with the points A and B. Is done.

FIG. 8 is a flowchart of the follower position detection processing of the follower position detection unit 40. In FIG. 8, the position of the follower UAV15 # i is detected. In the present embodiment, since there is one follower UAV 15, a processing flow of the follower position detection unit 40 when i = 1 will be described.

Followers position detection unit 40 starts the process in step S1300, with respect to follower UAV 15 # 1, it sets the three-dimensional coordinates _{p 1} target locations in two formations. Point p ₁ is the coordinate value apart on an extension line of the second line segment connecting the current position e the points c and d by a distance L + L ₁ of the leader UAV14. Also processing the contents case of _L ≠ = L ₁ is the length of _{L = L} 1, _L> 0 for no change. To make the formation fly according to the movement of the leader UAV14, p ₁ = e + (− 2L, 0,0) is given.

FIG. 9 shows the positional relationship between the leader UAV 14 and the follower UAV 15 # 1 in the initial state. As an initial state, the follower UAV 15 # 1 is facing the direction of the leader UAV 14, but this processing does not depend on the initial position and the initial direction of the leader UAV 14 and the follower UAV 15 # 1.

In step S1302, the marker point _{a 1} is mounted on the follower UAV 15 # 1, point _{b 1,} and detects the points _{c 1,} and a point _{d 1} by a position measuring sensor 10, calculates a position of an intersection _{e 1.} The coordinates of the point _{e 1} is the current position of the follower UAV15 # 1. When the current position e ₁ is calculated and passed to formation control unit 42 (marked as Z in Figure 8).

Next, in step S1304, a vector T ₁ from the current position (point e ₁ ) of the follower UAV 15 # 1 to the destination (point p ₁ ) is obtained by calculating T ₁ = p ₁ −e ₁ .

In step S1306, the vector _a 1 -b ₁ from point _{a 1} to point _{b 1} is calculated using the relationship between the rotational angle theta ₁ (the azimuth) vector inner product formed by the vector _{T 1.} When the azimuth angle theta ₁ is calculated, (marked as Z in FIG. 8) is passed to the formation control unit 42.

In step S1308, it is determined whether or not to stop the position detection process. If the process is to be stopped, the process of the follower position detection unit 40 is ended. If not, the process returns to step S1302 to repeat the process.

By the above processing, the current position of the follower position detecting unit 40 in the follower UAV 15 # 1 (point _{e 1),} the vector _{T 1} of the current position of the follower UAV 15 # 1 (point _{e 1)} to the target locations (points _{p 1)} And the azimuth θ1 of the follower UAV15 # ₁ is calculated. When the azimuth angle theta ₁ is calculated, (marked as Z in FIG. 8) is passed to the formation control unit 42.

FIG. 10 is a flowchart of the formation control process of the formation control unit 42. In the present embodiment, since there is one follower UAV 15, a processing flow of the follower position detection unit 40 when i = 1 will be described. As shown in FIG. 11, the movement is controlled so that the follower UAV 15 # 1 flies in the formation of the position and the direction of the leader UAV 14 opposite to the leader UAV 14. In FIG. 11, point g is the coordinates of the middle point of the position e of the leader UAV14 and the position e1 of the follower UAV15 # 1, and the follower UAV15 # 1 is oriented in a direction rotated by 180 degrees about the point g. Fly in the same direction as.

Upon starting the process, the formation control unit 42 specifies an allowable distance ΔL in step S1400. The allowable distance ΔL is the distance for the current position e ₁ follower UAV 15 # i to determine whether the reached point p ₁ of the objective point. For convenience, the same allowable distance set by the reader position control unit 32 is set.

In the follower position detection unit 40, the current position (point e ₁ ) and the azimuth θ ₁ of the follower UAV 15 # 1 are obtained in time series using the position measurement sensor 10, and the current position and the azimuth angle θ ₁ are obtained. Each time is given to step S1402 (denoted by Z in FIG. 10).

In step S1402, from the given current position and azimuth angle theta ₁ of the data to calculate the distance || _{T i} || from the current position _{e 1} follower UAV 15 # 1 to the point _{p 1} of the objective point.

In step S1404, it is determined whether or not the distance || T ₁ || is equal to or smaller than the allowable distance ΔL. If || T ₁ || ≦ ΔL, the flow shifts to step S1418. If || T ₁ || ≦ ΔL and not || T ||> ΔL, the process moves to step S1406 to control the movement.

In step S1406, is calculated by the difference between omega ₁ azimuth followers UAV 15 # 1 and reader UAV14 following equation (2).

... (2)

In step S1408, it is determined whether the absolute value || ω ₁ || of the azimuth angle difference ω ₁ is equal to or smaller than the allowable angle Δθ. If it is equal to or smaller than the allowable angle Δθ, the flow shifts to step S1410. If it is not equal to or smaller than the allowable angle Δθ, the flow shifts to step S1412. Here, Δθ is set as an allowable value of the azimuth error, but in consideration of drift, for example, Δθ = π / 180 (radian) is given. If the angle is equal to or smaller than the allowable angle Δθ, the azimuth of the follower UAV15 # 1 will be substantially θ ₁ = θ + π.

In step S1410, follower UAV 15 # 1 and the azimuth angle theta ₁ follower UAV 15 # 1 as orientation leader UAV14 forms the true opposite 180 degrees or less (3) counterclockwise rotation of updating (positive by formula Rotation). The azimuth angle theta ₁ which was updated and passed to the second flight command conversion unit 35 (in FIG. 10 marked with Y). In the equation (3), i = 1 in the present embodiment.

... (3)

In step S1412, the translation vector T ₁ ′ of the follower UAV 15 # 1 is converted by the following equation (4) according to the current azimuth angle θ _{1 at} which there is a change. In the equation (4), i = 1 in the present embodiment. With by Step S1410, using the azimuth angle theta ₁ updated in step S1410. If not processing step S1410, using the azimuth angle theta ₁ calculated at step S1306.

... (4)

In step S1414, the second control data

Is calculated by the following formula. Second control data _{u 1} is used in the second flight command conversion unit 35 as flight instruction to followers UAV 15 # 1.

Is a gain parameter in the feedback control, which is set by the user according to the situation. When the second control data u ₁ is computed and passed to the second flight command conversion unit 35 (marked as Y in Fig. 10).

In step S1415, a formation incomplete notification is transmitted to the camera video monitoring unit 44 assuming that the formation of the follower UAV15 # 1 has not been completed (denoted by W in FIG. 10).

In step S1416, it is determined whether to stop the formation control process. If the process is to be stopped, the process of the formation control unit 42 is ended. If not, the process returns to step S1402 to repeat the process.

Followers UAV 15 # 1 by continuing the iterations will be closer to the point p ₁ of the objective point, to fly according formation 11 when satisfying || T _{1 '||} ≦ ΔL.

By the above process, the formation control unit 42 outputs the control data _{u i} corresponding to the distance || _{T 1} '|| from the current position _{e 1} follower UAV 15 # 1 to the point _{p 1} of the objective point.

In step S1418, control is performed so that the follower UAV 15 # 1 waits in the hovering state.

In step S1420, a formation completion notification is transmitted to the camera image monitoring unit 44 (denoted by W in FIG. 10), and the flow shifts to step S1402.

FIG. 12 is a flowchart of the camera video monitoring process of the camera video monitoring unit 44. The camera video monitoring unit 44 has a function of acquiring a camera video and monitoring the entire environment while flying in a formation by the leader UAV14 and the follower UAV15 # 1. In the present embodiment, since there is one follower UAV 15, a processing flow of the camera video monitoring unit 44 when i = 1 will be described. .., N can be processed similarly.

When the process starts, the camera video monitoring unit 44 receives formation notification information from the leader UAV 14 in step S1500. The formation notification information is a notification that informs whether or not the formation by the leader UAV14 and the follower UAV15 # 1 has been completed and is flying. When the formation is completed, a formation completion notification is received as formation notification information. If the formation has not been completed or the formation has collapsed, a formation incomplete notification is received as formation notification information.

In step S1502, it is determined whether or not formation completion notifications have been received from all UAVs. If it has been received, the process proceeds to step S1504, and if not received, the process proceeds to step S1510 to enter a standby state. As described above, if the formation is not completed even for one of the units, the system enters a standby state, and waits for a UAV formation completion notification that has not been received.

In step S1504, a synchronization signal is received from the leader UAV14 and the follower UAV15 # 1. The synchronization signal uses a time code composed of hours: minutes: seconds: frames. By combining the time code with the acquired video frame, the synchronization of the video frame acquired by the leader UAV 14 and the follower UAV 15 # 1 can be synchronized.

In step S1506, a command to acquire a video from a camera in front of the UAV is transmitted to the leader UAV14 and the follower UAV15 # 1 while receiving the synchronization signal. A command to transfer the video obtained from the leader UAV14 and the follower UAV15 # 1 is transmitted to the communication unit 50, and stored in the obtained video DB46.

In step S1508, it is determined whether to stop the camera video monitoring process. If the process is to be stopped, the process of the camera video monitoring unit 44 is ended. Then repeat the process.

編 If the formation collapses due to some trouble while acquiring images from the camera, a notification that the formation is incomplete is entered in the formation notification information. At this time, acquisition of camera images is stopped, and the process waits until the formation is completed.

に よ っ て Through the above processing, the camera image monitoring unit 44 acquires images of the entire surroundings during the formation flight by the leader UAV14 and the follower UAV15 # 1.

As described above, according to the present embodiment, the position and orientation of the leader UAV14 and the follower UAV15 # 1 are calculated by the position measurement sensor 10, and the two UAVs fly in a formation and monitor images of the entire surroundings. it can. Further, by setting the position p of the destination of the leader UAV 14 in the space at intervals along the spatial route, the images of the entire surroundings can be monitored by the two UAVs along the trajectory.

As described above, according to the flight control device of the first embodiment, it is possible to control a plurality of UAVs to form a formation and to fly smoothly and stably while monitoring all directions.

<Configuration and Operation of Flight Control Device According to Second Embodiment of the Present Invention>
The second embodiment of the present invention is an embodiment of the control in which the leader UAV 14 and the two followers UAV 15 perform a leader-follower formation flight in a triangle formation by the configuration of the first embodiment of FIG. is there. In the present embodiment, a case where the formation is an equilateral triangle will be described as an example, but the present invention can also be applied to an isosceles triangle and a triangle whose sides are all different in length. In the present embodiment, the process of controlling the flight of the follower UAV 15 according to the formation of an equilateral triangle is different from that of the first embodiment, and hence only the differences from the first embodiment will be described below.

フ ォ ロ ワ ー The follower position detection unit 40 of the present embodiment follows the flow of FIG. In FIG. 8, the case of i = 1 is the processing flow of the follower position detection unit 40 for the follower UAV 15 # 1, and the case of i = 2 is the processing flow of the follower position detection unit 40 for the follower UAV 15 # 2.

Followers position detection unit 40 starts the process in step S1300, with respect to follower UAV 15 # i, sets the three-dimensional coordinates _{p i} of the target locations, in three formations. Current position and the point p _i of the leader UAV14 is a vertex of an equilateral triangle, the distance from the center of gravity of the triangle to each vertex and L. When the current position of the leader UAV 14 is e with the center of gravity g as the origin, the destination of the follower UAV 15 # 1 is given as three-dimensional coordinates p ₁ = e + (− 2L / 3, √3L / 2, 0), and the follower UAV 15 The destination of # 2 is given as three-dimensional coordinates p ₂ = e + (− 2L / 3, −√3L / 2,0).

FIG. 13 shows the positional relationship between the leader UAV 14 and the follower UAV 15 # i in the initial state. As an initial state, the follower UAV15 # i faces the direction of the leader UAV14, but this processing does not depend on the initial position and the initial direction of the leader UAV14 and the follower UAV15 # i.

In step S1302, the marker points attached to followers UAV 15 # i _{a i,} detects a point _{b i,} point _{c i,} and the point _{d i} by a position measuring sensor 10, calculates a position of an intersection _{e i.} Coordinates of the point _{e i} is the current position of the follower UAV15 _{# i.}

Next, in step S1304, a vector T _i from the current position (point e _i ) of the follower UAV 15 # i to the destination (point p _i ) is obtained by calculation of T _i = p _i -e _i .

In step S1306, the vector _a i -b _i from point _{a i} to point _{b i} is calculated vector _{T i} and forms rotation angle theta _i (the azimuth angle) by using the relationship between the vector dot product.

By the above processing, the current position of the follower position detecting unit 40 in the follower UAV 15 # i (point _{e i),} the vector _{T i} of the current position of the follower UAV 15 # i from (point _{e i)} to the target locations (points _{p i)} And the azimuth θ _i of the follower UAV15 # i.

フ ォ ー The formation control unit 42 of the present embodiment follows the flow of FIG. In FIG. 10, the case of i = 1 is the processing flow of the formation control unit 42 regarding the follower UAV15 # 1, and the case of i = 2 is the processing flow of the formation control unit 42 regarding the follower UAV15 # 2.

In the present embodiment, the azimuth rotated 120 degrees counterclockwise from the azimuth of the leader UAV14 is defined as the azimuth of the follower UAV15 # 1, and the azimuth rotated 240 degrees counterclockwise from the azimuth of the leader UAV14 is defined as the azimuth of the follower UAV15 # 2. And In this way, an azimuth of an angle equally divided with respect to the azimuth of the leader UAV 14 is set. In step S1406, it is calculated by the difference between ω _i of the azimuth angle of the followers UAV15 # i and the leader UAV14 following equation (5).

... (5)

In step S1408, it is determined whether the absolute value || ω _i || of the azimuth difference ω _i is equal to or smaller than the allowable angle Δθ. If it is equal to or smaller than the allowable angle Δθ, the flow shifts to step S1410. If it is not equal to or smaller than the allowable angle Δθ, the flow shifts to step S1412.

In step S1410, the azimuth θ _i of the follower UAV15 # i is updated by the above equation (3). The updated azimuth θ _i is passed to the second flight command converter 35 (denoted as Y in FIG. 10).

In step S1412, the translation vector T ₁ ′ of the follower UAV15 # i is converted by the above equation (4) according to the current azimuth angle θ _i having a change. With by step S1410 uses the azimuth angle theta _i updated in step S1410. If step S1410 has not been processed, the azimuth θ _i calculated in step S1306 is used.

In step S1414, the second control data

Is calculated by the following formula. The second control data _{u i,} used in the second flight command conversion unit 35 as flight instruction to followers UAV 15 # i. By continuing this repetition, the follower UAV15 # i approaches the destination point p _i, and flies in formation according to the formation of FIG. 14 when || T _i '|| ≦ ΔL is satisfied.

As described above, according to the present embodiment, the position and orientation of the leader UAV 14 and the follower UAV 15 #i are calculated by the position measurement sensor 10, and three UAVs fly in a formation and monitor images of the entire surroundings. it can. Further, by setting the position p of the destination of the leader UAV 14 in the space at intervals along the spatial route, the images of the entire surroundings can be monitored by three UAVs along the trajectory.

<Configuration and Operation of Flight Control Device According to Third Embodiment of Present Invention>

The third embodiment of the present invention is an embodiment of the control in which the leader UAV 14 and the three followers UAV 15 make a leader-follower formation flight in a square formation by the configuration of the first embodiment of FIG. is there. In this embodiment, a case where the formation is a square will be described as an example, but the present invention can be applied to a rectangle, a rhombus, and a square. In the present embodiment, since the process of controlling the flight of the follower UAV 15 according to the square formation is different from that of the second embodiment, only the differences from the second embodiment will be described below.

フ ォ ロ ワ ー The follower position detection unit 40 of the present embodiment follows the flow of FIG. In FIG. 8, the case of i = 1 is the processing flow of the follower position detecting unit 40 for the follower UAV15 # 1, the case of i = 2 is the processing flow of the follower position detecting unit 40 for the follower UAV15 # 2, and i = The case of No. 3 is a processing flow of the follower position detection unit 40 regarding the follower UAV 15 # 3.

Followers position detection unit 40 starts the process in step S1300, with respect to follower UAV 15 # i, sets the three-dimensional coordinates _{p i} of the target locations, in three formations. Current position and the point p _i leader UAV14 are vertices of a square, the distance from the square center of gravity to each vertex and L. The center of gravity g as the origin, when the current position of the leader UAV14 is e, the target locations followers UAV 15 # 1 is 3-dimensional coordinates _{p 1 = e + (- L} , L, 0) and given, followers UAV 15 # 2 target locations Is given as three-dimensional coordinates p ₂ = e + (− 2L, 0, 0), and the destination of the follower UAV15 # 3 is given as three-dimensional coordinates p ₃ = e + (− L, −L, 0).

FIG. 15 shows the positional relationship between the leader UAV 14 and the follower UAV 15 # i in the initial state. As an initial state, the follower UAV15 # i faces the direction of the leader UAV14, but this processing does not depend on the initial position and the initial direction of the leader UAV14 and the follower UAV15 # i. Subsequent steps are the same as in the second embodiment.

フ ォ ー The formation control unit 42 of the present embodiment follows the flow of FIG. In FIG. 10, the case of i = 1 is the processing flow of the formation control unit 42 for the follower UAV15 # 1, the case of i = 2 is the processing flow of the formation control unit 42 for the follower UAV15 # 2, and the case of i = 3. The case is the processing flow of the formation control unit 42 regarding the follower UAV15 # 3.

In the present embodiment, the azimuth rotated 90 degrees counterclockwise from the azimuth of the leader UAV14 is defined as the azimuth of the follower UAV15 # 1, and the azimuth rotated 180 degrees counterclockwise from the azimuth of the leader UAV14 is defined as the azimuth of the follower UAV15 # 2. The direction rotated 270 degrees counterclockwise from the direction of the leader UAV14 is defined as the direction of the follower UAV15 # 3. In this way, the azimuth equally set to the azimuth of the leader UAV 14 is set. In step S1406, is calculated by the difference between omega _i azimuth followers UAV 15 # i and the reader UAV14 following equation (6).

... (6)

Thereafter, the processing is the same as that of the formation control unit 42 of the second embodiment, and the follower UAV15 # i approaches the destination point p _i and satisfies || T _i '|| ≦ ΔL. The flight flies in accordance with the formation of FIG.

As described above, according to the present embodiment, the position and orientation of the leader UAV14 and the follower UAV15 # i are calculated by the position measurement sensor 10, and four UAVs fly in a formation and monitor images of the entire surroundings. it can. Further, by setting the position p of the destination of the leader UAV 14 in the space at intervals along the spatial route, it is possible to monitor the image of the entire surroundings by four UAVs along the trajectory.

<Configuration and operation of flight control device according to fourth embodiment of the present invention>

According to the fourth embodiment of the present invention, the leader UAV14 and the N followers UAV15 perform the control of the leader / follower formation flight of the regular polygon formation by the configuration of the first embodiment of FIG. It is a form. In the present embodiment, an example in which a regular polygon is formed will be described. In the present embodiment, the process of controlling the flight of the follower UAV 15 according to the formation of the regular polygon is different from that of the third embodiment, and hence only the differences from the third embodiment will be described below.

フ ォ ロ ワ ー The follower position detection unit 40 of the present embodiment follows the flow of FIG. In FIG. 8, the case of i = 1, 2,..., N is the processing flow of the follower position detection unit 40 for each of the follower UAV15 # 1, follower UAV15 # 2,. is there.

Followers position detection unit 40 starts the process in step S1300, with respect to follower UAV 15 # i, sets the three-dimensional coordinate _{p i} of the target areas in M = (N + 1) base formation. The current position of the reader UAV 14 and the point p _i are vertices of a regular polygon, and the distance from the center of gravity of the regular polygon to each vertex is L. When the current position of the leader UAV 14 is e with the center of gravity g as the origin, the destination of the follower UAV 15 #i is three-dimensional coordinates p ₁ = e + (L (cos (i × 2π / M) −1), L (sin (I × 2π / M)), 0). Subsequent steps are the same as in the third embodiment.

フ ォ ー The formation control unit 42 of the present embodiment follows the flow of FIG. In FIG. 10, the case of i = 1, 2,..., N is the processing flow of the formation control unit 42 regarding each of the follower UAV15 # 1, follower UAV15 # 2,. .

In this embodiment, the direction rotated 360 × i / M degrees counterclockwise from the direction of the leader UAV 14 is set as the direction of the follower UAV 15 #i. In step S1406, is calculated by the difference between omega _i azimuth followers UAV 15 # i and the reader UAV14 following equation (7).

... (7)

Thereafter, the process is the same as that of the formation control unit 42 of the third embodiment, and the follower UAV15 # i approaches the destination point p _i and satisfies || T _i '|| ≦ ΔL. The flight flies in accordance with the formation of FIG.

As described above, according to the present embodiment, the position measurement sensor 10 calculates the positions and orientations of the leader UAV 14 and the follower UAV 15 #i, and the M UAVs fly in a formation and monitor images of the entire surroundings. it can. Further, by setting the position p of the destination of the leader UAV 14 in the space at intervals along the spatial route, the images of the entire surroundings can be monitored by M UAVs along the trajectory.

<Structure and operation of the flight control device according to the fifth embodiment of the present invention>

According to the fifth embodiment of the present invention, the leader UAV 14 and the N followers UAV 15 perform a leader-follower formation flight of a polygonal formation by the configuration of the first embodiment of FIG. It is. In the present embodiment, an example will be described in which a polygon is formed at a different distance from the center of gravity. Depending on circumstances such as wind speed in the environment, it may be better to form a polygon according to the situation instead of a regular polygon. In the present embodiment, the process of controlling the flight of the follower UAV 15 in accordance with the formation of the polygon is different from that of the fourth embodiment, and hence only the differences from the fourth embodiment will be described below.

フ ォ ロ ワ ー The follower position detection unit 40 of the present embodiment follows the flow of FIG. In FIG. 8, the case of i = 1, 2,..., N is the processing flow of the follower position detection unit 40 for each of the follower UAV15 # 1, follower UAV15 # 2,. is there.

Followers position detection unit 40 starts the process in step S1300, with respect to follower UAV 15 # i, it sets the three-dimensional coordinates _{p i} of the target areas in M = (N + 1) base formation. Current position and the point _{p i} leader UAV14 is vertices of a polygon, the distance from the polygon center of gravity to each vertex followers UAV 15 # i and _{L i.} The distance from the center of gravity of the polygon to the reader UAV 14 is L. However, the distance _{L i} is varied all according to the followers UAV 15 # i. To the extent that followers UAV 15 # i each other not Awa hit, to set different distances _{L ≠ L 1 ≠ L 2 ≠} ··· L N and respective followers UAV 15 # i. When the current position of the leader UAV 14 is e with the center of gravity g as the origin, the destination of the follower UAV 15 # 1 is p _i = e + (L _i (cos (i × 2π / ML), L _i (sin (i × 2π / M)), 0) The processing in the subsequent steps is the same as in the fourth embodiment, and the formation flies in accordance with the formation of polygons having different distances from the center of gravity.

As described above, according to the present embodiment, the position measurement sensor 10 calculates the positions and orientations of the leader UAV 14 and the follower UAV 15 #i, and the M UAVs fly in a formation and monitor images of the entire surroundings. it can. Further, by setting the position p of the destination of the leader UAV 14 in the space at intervals along the spatial route, the images of the entire surroundings can be monitored by M UAVs along the trajectory.

<Structure and operation of the flight control device according to the sixth embodiment of the present invention>

The sixth embodiment of the present invention is an embodiment of the control in which the leader UAV 14 and the N followers UAV 15 perform the leader-follower formation flight of the polygon formation by the configuration of the first embodiment of FIG. It is. In the present embodiment, a case where a polygon is formed with different azimuth intervals will be described as an example. In the present embodiment, the process of controlling the flight of the follower UAV 15 in accordance with the formation of the polygon is different from that of the fourth embodiment, and hence only the differences from the fourth embodiment will be described below.

フ ォ ー The formation control unit 42 of the present embodiment follows the flow of FIG. In FIG. 10, the case of i = 1, 2,..., N is the processing flow of the formation control unit 42 regarding each of the follower UAV15 # 1, follower UAV15 # 2,. .

In this embodiment, the orientation rotated phi _i of the orientation of the leader UAV14 counterclockwise and orientation followers UAV 15 # i. A degree that followers UAV 15 # i each other not Awa hit, sets different orientations to _{φ 1 ≠ φ 2 ≠ ··· ≠} φ N and respective followers UAV 15 # i. In step S1406, is calculated by the difference between omega _i azimuth followers UAV 15 # i and the reader UAV14 following equation (8).

... (8)

Thereafter, the processing is the same as that of the formation control unit 42 of the fourth embodiment, and the follower UAV15 # i approaches the destination point p _i and satisfies || T _i '|| ≦ ΔL. Fly in formation according to polygonal formations with different azimuth intervals.

As described above, according to the present embodiment, the position measurement sensor 10 calculates the positions and orientations of the leader UAV 14 and the follower UAV 15 #i, and the M UAVs fly in a formation and monitor images of the entire surroundings. it can. Further, by setting the position p of the destination of the leader UAV 14 in the space at intervals along the spatial route, the images of the entire surroundings can be monitored by M UAVs along the trajectory.

<Structure and operation of the flight control device according to the seventh embodiment of the present invention>

The seventh embodiment of the present invention is an embodiment of the control in which the leader UAV 14 and the N followers UAV 15 perform the leader-follower formation flight of the polygon formation by the configuration of the first embodiment of FIG. It is. In the present embodiment, a case will be described as an example where a polygon is formed with different distances and orientations from the center of gravity. In the present embodiment, the process of controlling the flight of the follower UAV 15 in accordance with the formation of the polygon is different from that of the fourth embodiment, and hence only the differences from the fourth embodiment will be described below.

フ ォ ロ ワ ー The follower position detection unit 40 of the present embodiment follows the flow of FIG. In FIG. 8, the case of i = 1, 2,..., N is the processing flow of the follower position detection unit 40 for each of the follower UAV15 # 1, follower UAV15 # 2,. is there.

Followers position detection unit 40 starts the process in step S1300, with respect to follower UAV 15 # i, it sets the three-dimensional coordinates _{p i} of the target areas in M = (N + 1) base formation. Current position and the point _{p i} leader UAV14 are vertices of a polygon, the distance from the polygon center of gravity to each vertex followers UAV 15 # i and _{L i.} The distance from the center of gravity of the polygon to the reader UAV 14 is L. However, the distance _{L i} is varied all according to the followers UAV 15 # i. To the extent that followers UAV 15 # i each other not Awa hit, to set different distances _{L ≠ L 1 ≠ L 2 ≠} ··· L N and respective followers UAV 15 # i. When the current position of the leader UAV 14 is e with the center of gravity g as the origin, the destination of the follower UAV 15 # 1 is p _i = e + (L _i (cos (i × 2π / ML), L _i (sin (i × 2π / M)), 0) The processing in the subsequent steps is the same as in the fourth embodiment.

フ ォ ー The formation control unit 42 of the present embodiment follows the flow of FIG. In FIG. 10, the case of i = 1, 2,..., N is the processing flow of the formation control unit 42 regarding each of the follower UAV15 # 1, follower UAV15 # 2,. .

In this embodiment, the orientation rotated phi _i of the orientation of the leader UAV14 counterclockwise and orientation followers UAV 15 # i. To the extent that followers UAV 15 # i each other not Awa hit, sets different orientations to _{φ 1 ≠ φ 2 ≠ ··· ≠} φ N and respective followers UAV 15 # i. In step S1406, the difference omega _i azimuth followers UAV 15 # i and the reader UAV14 is calculated by equation (8).

Thereafter, the processing is the same as that of the formation control unit 42 of the fourth embodiment, and the follower UAV15 # i approaches the destination point p _i and satisfies || T _i '|| ≦ ΔL. The formation flies in a polygonal formation with different distances and azimuths from the center of gravity.

As described above, according to the present embodiment, the position measurement sensor 10 calculates the positions and orientations of the leader UAV 14 and the follower UAV 15 #i, and the M UAVs fly in a formation and monitor images of the entire surroundings. it can. Further, by setting the position p of the destination of the leader UAV 14 in the space at intervals along the spatial route, the images of the entire surroundings can be monitored by M UAVs along the trajectory.

The present invention is not limited to the above-described embodiment, and various modifications and applications can be made without departing from the spirit of the present invention.

For example, in each of the embodiments described above, the case where the leader UAV 14 and the follower UAV 15 #i are controlled by the flight control device has been described as an example. However, the present invention is not limited to this. A device and a follower flight control device may be configured, and the leader UAV 14 and the follower UAV 15 # i may be controlled by the respective devices.

DESCRIPTION OF SYMBOLS 10 Position measurement sensor 12 Destination 20 Calculation part 30 Leader position detection part 32 Leader position control part 34 First flight command conversion part 35 Second flight command conversion part 40 Follower position detection part 42 Formation control part 44 Camera image monitoring part 46 Video DB
50 communication unit 100 flight control device

Claims (7)

1. A flight control device that controls formation flight of a plurality of UAVs (Unmanned Aerial Vehicles) and controls a leader UAV that leads the formation flight and one or more followers UAV that form a formation in accordance with the leader UAV.
   A position measurement sensor that is provided to the reader UAV and the one or more followers UAV, and that measures the three-dimensional coordinates of each of the plurality of markers whose distance between the markers is known,
   The current position of the reader UAV based on the three-dimensional coordinates of each of the markers of the reader UAV measured by the position measurement sensor and a preset first target point of the reader UAV. A reader position detecting unit that calculates a vector to the first target point and an azimuth angle with respect to the first target point;
   A reader position control unit that updates first control data for controlling the position of the reader UAV based on a vector of the reader UAV to the first target point;
   Based on the first control data updated for the leader UAV and the azimuth calculated for the leader UAV, flight command data in the leader UAV is calculated, and based on the calculated flight command data. A first flight command converter for controlling the movement of the leader UAV;
   For the one or more follower UAVs, the follower UAV based on three-dimensional coordinates of each of the markers of the follower UAV measured by the position measurement sensor and a second target point of the follower UAV determined by formation. A follower position detection unit that calculates a current position of the UAV, a vector to the second target point, and an azimuth angle with respect to the second target point;
   For the one or more followers UAV, the difference between the azimuth of the leader UAV and the azimuth of the follower UAV, and the difference between a predetermined angle determined by the follower UAV, satisfy a predetermined allowable angle, The azimuth of the follower UAV is updated, and a second control data for controlling the position of the follower UAV is updated based on the vector of the follower UAV to the second target point and the updated azimuth. A formation control unit to be updated,
   For the one or more follower UAVs, based on the updated second control data for the follower UAV and the updated azimuth for the follower UAV, calculate flight command data for the follower UAV, A second flight command conversion unit that controls the movement of the follower UAV based on the calculated flight command data;
   Flight control device including.
2. 2. The azimuth of the follower UAV according to claim 1, wherein the formation control unit updates the azimuth of the follower UAV so as to divide the entire circumference by the azimuth of the leader UAV and the updated azimuth of all the followers UAV. Flight control device.
3. Further including a camera image monitoring unit,
   The leader position control unit transmits a formation completion notification to the camera image monitoring unit when the leader UAV satisfies a predetermined allowable distance with respect to the first target point,
   The formation control unit transmits a formation completion notification to the camera image monitoring unit when the one or more followers UAV satisfy a predetermined allowable distance to the second target point,
   The camera image monitoring unit obtains an image from the camera of the leader UAV and the one or more follower UAVs when receiving all of the formation completion notification of the leader UAV and the one or more followers UAV. The flight control device according to claim 1, wherein the flight control device controls the flight.
4. A flight control method for a flight control device that controls formation flight of a plurality of UAVs (Unmanned Aerial Vehicles) and controls a leader UAV that leads the formation flight and one or more followers UAV that form a formation according to the leader UAV. hand,
   A position measurement sensor is provided to the reader UAV and the one or more followers UAV, and measures the three-dimensional coordinates of each of the plurality of markers whose distance between the markers is known,
   A leader position detecting unit, based on three-dimensional coordinates of each of the markers of the reader UAV measured by the position measurement sensor, and a first target point that is a preset destination of the reader UAV, Calculating a current position of the reader UAV, a vector to the first target point, and an azimuth angle with respect to the first target point;
   A step of the reader position control unit updating first control data for controlling the position of the reader UAV based on a vector of the reader UAV up to the first target point;
   A first flight command conversion unit, based on the first control data updated for the leader UAV and the azimuth calculated for the leader UAV, calculates flight command data for the leader UAV; Controlling the movement of the leader UAV based on the calculated flight command data;
   A follower position detector detects, for the one or more followers UAV, three-dimensional coordinates of each of the markers of the follower UAV measured by the position measurement sensor, and a second target point of the follower UAV determined by formation. Calculating a current position of the follower UAV, a vector to the second target point, and an azimuth angle with respect to the second target point, based on
   The formation control unit determines, for the one or more followers UAV, a difference between an azimuth of the leader UAV and an azimuth of the follower UAV and a predetermined angle determined by the follower UAV, a predetermined allowable angle. To update the azimuth of the follower UAV so as to satisfy the following, and to control the position of the follower UAV based on the vector of the follower UAV to the second target point and the updated azimuth. Updating the second control data;
   A second flight command conversion unit, for the one or more followers UAV, based on the second control data updated for the follower UAV and the updated or calculated azimuth angle for the follower UAV. Calculating flight command data in the follower UAV, and controlling the movement of the follower UAV based on the calculated flight command data;
   A flight control method including:
5. The azimuth of the follower UAV is updated such that the formation control unit updates the azimuth of the follower UAV so as to divide the entire circumference by the azimuth of the leader UAV and the updated azimuth of all the followers UAV. Flight control method.
6. Further including a camera image monitoring unit,
   The leader position control unit transmits a formation completion notification to the camera image monitoring unit when the leader UAV satisfies a predetermined allowable distance with respect to the first target point,
   The formation control unit transmits a formation completion notification to the camera image monitoring unit when the one or more followers UAV satisfy a predetermined allowable distance to the second target point,
   The camera image monitoring unit obtains an image from the camera of the leader UAV and the one or more follower UAVs when receiving all of the formation completion notification of the leader UAV and the one or more followers UAV. The flight control method according to claim 4, wherein the control is performed to perform the flight control.
7. A program for causing a computer to function as each section of the flight control device according to any one of claims 1 to 3.

Images