WO2019172335A1

WO2019172335A1 - Flight control device, method, program, and storage medium

Info

Publication number: WO2019172335A1
Application number: PCT/JP2019/008939
Authority: WO
Inventors: 宮川　勲; 杵渕　哲也
Original assignee: 日本電信電話株式会社
Priority date: 2018-03-08
Filing date: 2019-03-06
Publication date: 2019-09-12
Also published as: JP2019159488A

Abstract

The objective of the present invention is to enable an unmanned aerial vehicle to be operated freely and controlled to fly stably. On the basis of three-dimensional coordinates of each marker, measured by means of a position measuring sensor, and on the basis of a preset target point at a target location of a UAV, a position and azimuth detecting unit calculates a midpoint of the three-dimensional coordinates of each marker, the distance from the midpoint to the target point, and the azimuth angle of a line segment joining the three-dimensional coordinates of each marker, relative to the target point in a global coordinate system. On the basis of the distance to the target point, a velocity control unit updates control data for controlling a velocity vector of the UAV, in such a way as to achieve a velocity vector defined from the midpoint and the target point. A flight command converting unit calculates flight instruction data for the UAV on the basis of the updated control data and the calculated azimuth angle, and controls the operation of the UAV on the basis of the calculated flight instruction data.

Description

Flight control apparatus, method, program, and storage medium

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

The quad-rotor type UAV (Unmanned Aerial Vehicle) is an unmanned airplane that has four propellers and controls 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 UAV attitude, 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.

In addition, in order to measure the three-dimensional position of the UAV, as shown in FIG. 12, a certain global coordinate system is set. 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 θ are In order to fly to the position of the coordinate value and point the aircraft in a predetermined direction, it is necessary to control the attitude and altitude 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 related to AR Drone 2.0 (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.

Furthermore, since UAV has the ability to fly freely in space, it can play an active role as a robot that replaces people. In Non-Patent Document 3, UAV is used as a tool for collaborative work with humans. In Non-Patent Document 4, a plurality of UAVs are used to set up a network in the space, and humans and UAVs exchange balls. A method for controlling movements of a plurality of UAVs as an operation is disclosed.

The tasks for freely controlling UAV flight can be roughly divided into three categories, hovering, trajectory tracking, and path following. Hovering keeps flying 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 a constraint condition in some space at a given 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 techniques are controls aimed at reaching a target location along a predetermined route, stable flight of UAV is not guaranteed.

On the other hand, in order to avoid a collision with an obstacle or a person in the path follow, 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 technologies are also controlled for reaching the target location, stable flight of UAV has not been guaranteed.

The present invention has been made to solve the above-described problems, and is a flight control device, method, program, and storage medium that can control an unmanned airplane to operate freely 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 that are assigned to a UAV (Unmanned Aerial Vehicle) and the distance between the markers is known. 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 for updating control data for controlling the speed vector of the UAV based on a distance to a point so as to become a speed vector determined from the middle point and the target point; Flight command conversion for calculating flight command data in the UAV based on the updated control data and the calculated azimuth, and controlling movement of the UAV based on the calculated flight command data Part.

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 provided with a position measurement sensor in 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. 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; Based on the distance to the target point, the control data for controlling the speed vector of the UAV is updated so as to be a speed vector determined from the midpoint and the target point. And a flight command conversion unit calculates flight command data in the UAV based on the updated control data and the calculated azimuth angle, and the UAV based on the calculated flight command data And controlling the movement of the robot.

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

A storage medium according to a fourth invention is a storage medium for storing 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, program, and storage medium of the present invention, based on the three-dimensional coordinates of each marker measured by the position measurement sensor and the target point of the preset UAV target location, Calculate the midpoint of each 3D coordinate of the marker, the distance from the midpoint to the target point, and the azimuth of the line segment connecting each 3D coordinate of the marker to the target point in the global coordinate system. The control data for controlling the speed vector of the UAV is updated based on the distance up to the speed vector determined from the midpoint and 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 UAV motion is controlled based on the calculated flight command data. To 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 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 an example in the case of accelerating or decelerating flight speed. It is a flowchart of the speed control process in the flight control process routine of 2nd Embodiment. 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 stably.

A configuration of a quadrotor 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, a CD-ROM, or using remote data resources via a network.

The position measurement sensor 10 measures, as measurement data, three-dimensional coordinates of each of a plurality of markers (points q ₁ and q ₂ ) given to the UAV 14 and having known distances between the markers. 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 present embodiment, the movement is controlled so that the UAV 14 flies at a constant speed until the midpoint between the marker point q ₁ and the marker point q ₂ attached to the UAV 14 reaches the target point p. 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 uses 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, and a speed control unit 32 that controls the flight speed of the UAV 14 based on the position and orientation. And a flight command conversion unit 34 for sending a command in accordance with the UAV 14. 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 it becomes a speed vector V ₀ determined from the midpoint q and the target point p. To do.

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.

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 the global coordinates The azimuth angle θ of the line segment connecting the marker point q ₁ 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 speed control unit 32 obtains control data u for controlling the speed vector of the UAV 14 based on the distance to the target point p. The speed vector is determined from the midpoint q and the target point p. updated so that the V _0.

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 processing of the position / orientation detection unit 30 in step S100 will be described.

FIG. 4 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 S1004, the calculated distance from the midpoint q point markers _{q 1} and point _{q 2} to the target point p. The coordinates of the midpoint q are calculated by q = (q ₁ + q ₂ ) / 2. 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. 5, 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 stopped, the process of the position / orientation detection unit 30 is terminated. 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 θ by the above processing.

Next, details of the speed control processing of the speed control unit 32 in step S102 will be described.

FIG. 6 is a process flow diagram of the speed control unit 32. When starting the process, the speed control unit 32 acquires a distance || T || from the midpoint q to the target point p in step S1100.

In step S1102, it is determined whether the distance || T || is less than the allowable distance ΔL (|| T || <ΔL). If || T || <ΔL, the process proceeds 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 so that the flight is maintained 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.

In step S1108, it determines whether the UAV14, updates the control data u order to fly at a speed vector V ₀ which constant velocity obtained from the vector T from the midpoint q to the target point p. An absolute difference || V ₀ −v || between the constant velocity vector V _o obtained from the vector T from the midpoint q to the target point p and the current velocity vector v is obtained, and an allowable error for satisfying the constant velocity is obtained. Let Δv. At this time, it is determined whether or not || V ₀ −v || Δv. If the condition is satisfied, the process proceeds to step S1110. If the condition is not satisfied, the process proceeds to step S1100.

In step S1110, the control data u is updated. The control data u is

Update with

Is a gain parameter in feedback control and is set by the user according to the situation. The updated control data u is output to the flight command converter 34.

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 S1100 to repeat the process.

With the above processing, the speed control unit 32 updates the control data u by feedback control until || T || <ΔL and || V ₀ −v || ≦ Δv.

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 the present embodiment, a case where a commercially available product AR Drone 2.0 is taken as an example is shown. However, it is needless to say that this embodiment can also be used when controlling the flight of other UAVs 14.

FIG. 7 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 control data u and the azimuth angle θ are converted into flight command data for transmission to the UAV 14. Flight command data to the UAV 14 includes a rotation speed V _x around the x axis, a rotation speed V y around the _y axis, a speed V _z along the Z axis, and a 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 in the y-axis, and rotation around the y-axis produces a translational movement in 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 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, according to the present embodiment, the position measurement sensor 10 calculates the position and orientation of the UAV 14 and can control the flight movement of the UAV 14 so as to fly at a predetermined speed toward the destination. Note that the flight navigation of the UAV 14 can be realized by continuously setting the coordinates of the destination.

As described above, according to the flight control device according to the embodiment of the first invention, the three-dimensional coordinates of each of the markers measured by the position measurement sensor and the preset target point of the target location 12 of the UAV 14 are set. Based on the above, the midpoint of each 3D coordinate of the marker, the distance from the midpoint to the target point, and the azimuth angle of the line segment connecting each 3D coordinate of the marker to the target point in the global coordinate system Calculating and updating the control data for controlling the velocity vector of the UAV based on the distance to the target point so as to become a velocity vector determined from the middle point and the target point, and the updated control data; Based on the calculated azimuth angle, UAV flight command data is calculated, and the UAV motion is controlled based on the calculated flight command data, so that the unmanned airplane can be operated freely. And, and it can be controlled so as to stably fly.

The second embodiment of the present invention does not fix the speed limit of the UAV 14 to V ₀ in the configuration of FIG. 2 described above, but allows the UAV 14 to fly at a predetermined speed when an arbitrary speed limit is given. It is an example in the case of controlling by accelerating or decelerating. In addition, the same code | symbol is attached | subjected to the location similar to 1st Embodiment, and description is abbreviate | omitted.

FIG. 8 shows the flight speed of the UAV 14 controlled by the method of this embodiment. The horizontal axis is time t, and the vertical axis is speed v. The left diagram in FIG. 8 shows the case of acceleration (|| V ₀ || <|| V ₁ ||), and the right diagram in FIG. 8 shows the case of deceleration (|| V ₀ ||> || V ₁ ||). Note that the speed limit may be given in time series as the speed limit input. Or according to the magnitude | size of distance || T ||, you may make it accelerate when it leaves | separates to a certain fixed distance, and you may make it decelerate when it comes within a certain fixed distance.

FIG. 9 is a flowchart of the speed control process of the speed control unit 32 of the present embodiment.

If it is determined in step S1102 that || T || ΔL, it is determined in step S2100 whether there is an input of a predetermined speed limit. If there is an input of the speed limit, the process proceeds to step S2102. If there is an input of the speed limit, the process proceeds to step S2104.

In step S2102, based on the input of the speed limit, a speed vector V _t determined from the speed limit and the vector T from the midpoint q to the target point p so as to accelerate or decelerate with respect to the speed of V _t−1. Set. This sets the V _t to accelerate if Hayakere speed limit to the speed of the V _t-1, to set the V _t to decelerate as late speed limit to the speed of the V _t-1.

In step S2104, the velocity vector v of the UAV 14 is calculated. The calculation method is the same as that in step S1106.

In step S2106, UAV14 and updates the control data u order to fly at a velocity vector _{V t} of the speed limit set in step S2102. An absolute difference || V _t −v || between an arbitrary velocity vector V _t and a current velocity vector v is obtained, and an allowable error for satisfying the arbitrary velocity is Δv, || V _t −v || It is determined whether or not> Δv. If the condition is satisfied, the process proceeds to step S2108. If the condition is not satisfied, the process proceeds to step S1104.

In step S2108, the control data u is updated. The control data u is

Update with

As described above, in this embodiment, the flight of the UAV 14 is controlled so as to have an arbitrary speed according to the input of the speed limit for acceleration or deceleration.

The third embodiment of the present invention is an example of simultaneously controlling the flight of N UAVs 14 in the configuration 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 middle point of the Nth UAV 14 is q, and the target point of the (N−1) th UAV 14 is p. In order to prevent the UAVs 14 from colliding with each other, similarly to the first embodiment, the speed control unit 32 gives ΔL = L and controls the movement of the UAV 14 # 2 so that the distance between the UAVs 14 is L.

As a result of this assignment, as in the first embodiment, the positions of the points A ₂ and B ₂ of the UAV 14 # 2 are set according to the settings of the midpoint coordinates of the points A ₁ and B ₁ of the UAV 14 # 1 and the distance L. The azimuth and UAV # 2 speed are controlled.

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

In this embodiment, the movements of a plurality of UAVs 14 can be simultaneously controlled so as to fly at a constant speed. According to the present embodiment, it is possible to organize N UAVs 14 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 the same process for the first UAV 14 as in the first embodiment. Thereafter, the following processing is performed for the second and subsequent UUAVs 14.

The position / orientation detection unit 30 of this 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 the three-dimensional coordinates of each of the Nth UAV14 markers, the distance || T || 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 obtains the control data u for the Nth UAV 14 from the midpoint q and the target Update so that the velocity vector V ₀ is determined from the point p.

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.

In the speed control unit 32 of the present embodiment, the speed vector V ₀ is not set, but the speed vector V _t is controlled by giving a speed limit by acceleration or deceleration as in the second embodiment. Also good.

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

A position measuring 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 based on a distance to the target point so as to be a speed vector determined from the middle point and 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.
The speed control unit makes the speed vector determined from the midpoint and the target point constant, or as an arbitrary speed vector obtained by accelerating or decelerating the speed vector determined from the midpoint and the target point, The flight control apparatus according to claim 1, wherein the control data is updated.
The UAV is the 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 the three-dimensional coordinates of each of the Nth markers measured by the position measurement sensor and the 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 the azimuth angle of the 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 first UAV;
The speed control unit determines the control data for the Nth UAV from the midpoint and the target point based on a distance from the midpoint of the Nth UAV to the target point. Updated to the given velocity vector,
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;
A speed control unit updating control data for controlling the speed vector of the UAV based on a distance to the target point so as to become a speed vector determined from the middle point and 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. Controlling step;
Including flight control method.
The step of updating by the speed control unit is an arbitrary one in which the speed vector determined from the midpoint and the target point is constant, or the speed vector determined from the midpoint and the target point is accelerated or decelerated. The flight control method according to claim 4, wherein the control data is updated as a velocity vector.
The UAV is the N UAVs, the midpoint of the (N-1) th UAV is the target point of the Nth UAV,
The step of calculating the position / orientation detection unit includes the three-dimensional coordinates of each of the Nth markers measured by the position measurement sensor, and the midpoint of the N−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 middle point of the N-1st UAV in the coordinate system;
The step of updating by the speed control unit is based on the distance from the middle point of the Nth UAV to the target point to the target point, the control data for the Nth UAV, the middle point and the middle point Update to the speed vector determined from the target point,
The step of controlling the flight command conversion unit is based on the control data updated for the Nth UAV and the azimuth calculated for 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.
A program for causing a computer to function as each part of the flight control device according to any one of claims 1 to 3.
A storage medium for storing a program for causing a computer to function as each part of the flight control device according to any one of claims 1 to 3.