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

Abstract

To provide a flight control device capable of freely controlling an unmanned aircraft.SOLUTION: A position attitude calculation part calculates a difference between targeted three-dimensional coordinates and three-dimensional coordinates of a UAV 14 on the basis of three-dimensional coordinates of a reference object 12, a calculated azimuth angle, and a relative position relation between three-dimensional coordinates of the reference object and a marker of the UAV, and calculates a rotation angle on the basis of three-dimensional coordinates obtained by adding the difference to the three-dimensional coordinates of the UAV, and targeted three-dimensional coordinates of the UAV. A motion control part calculates a rotation angle around an X-axis, a rotation angle around a Y-axis, a speed along a Z-axis, and a rotation speed around the Z-axis in the UAV as flight command data on the basis of the calculated difference and rotation angle, and controls the motion of the UAV on the basis of the calculated flight command data.SELECTED DRAWING: Figure 1

Description

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

  The Quadrotor UAV (Unmanned Aerial Vehicle) is an unmanned airplane that has four propellers and is controlled by controlling the lift applied to each propeller. Many of the unmanned airplanes called drone are a kind of quadrotor type UAV. The unmanned aircraft handled in the following is abbreviated as UAV.

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

  Also, in order to measure the three-dimensional position of the UAV, as shown in FIG. 15, a global coordinate system as a reference is set. In GPS, it is a three-dimensional coordinate in the world coordinate system, and in a motion capture system, its 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) of the global coordinate system. Also, the attitude of the UAV is expressed 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 ω), Z axis The rotation around it 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 translational motion in the X axis direction, and rotation about the X axis produces translational motion in the Y axis direction. The rotation around the Z-axis produces an azimuthal rotation, and when the same lift is simultaneously applied to the four propellers, the strength produces a translational motion (high elevation) in the Z-axis direction.

Given a predetermined coordinate value (X _d , Y _d , Z _d ) and an orientation θ _d in the global coordinate system, the current position P of the UAV P = (X, Y, Z) and the current orientation φ It is necessary to control the attitude and altitude of the UAV in order to fly to the position of the coordinate value and to turn the airframe to a predetermined direction. Non-Patent Document 1 discloses backstepping control based on UAV equations of motion and angular equations of motion. Non-Patent Document 2 discloses a flight motion control method for AR Drone 2.0 (commercially available low-priced UAV). In many of the prior art, discrete predetermined positions and orientations are given as flight trajectories in the global coordinate system, and their motion is controlled so that the UAV tracks each point and each orientation.

  On the other hand, UAV has the ability to fly freely in space, so it can play an active role as a robot to replace human beings. In Non-Patent Document 3, UAVs are used as a tool for collaborating with people, and in Non-Patent Document 4, a plurality of UAVs are used to construct a network in space, and the people and UAVs exchange balls. A method of controlling the motion of a plurality of UAVs as an operation is disclosed.

T. Madani and A. Benallegue: "Backstepping Control for a Quadrotor Helicopter", IEEE Conference on Intelligent Robots and Systems, 2006. L. V. Santana, A. S. Brandao, M. S-Filho, and R. Carelli: "A Trajectory Tracking and 3D Positioning Controller for the AR. Drone Quadrotor", International Conference on Unmanned Aircraft Systems (ICUAS) May 27-30, 2014. W. S. Ng and E. Sharlin: "Collocated Interaction with Flying Robots", IEEE International Symposium on Robots and Human Interactive Communication, 2011. R. Ritz, M. W. Muller, M. Hehn, and R. D'Andrea: "Cooperative Quadrocopter Ball Throwing and Catching", IEEE / RSJ International Conference on Intelligent Robots and Systems, 2012.

  The task that freely controls the UAV flight can be roughly divided into three groups, which are called hovering, trajectory tracking, and path following. Hover maintains flight to stay at a given position and orientation. Trajectory tracking is controlled to track in real time along a given path (discrete three-dimensional position and orientation in space). On the other hand, path follow controls flight by giving some constraint condition in space at given three-dimensional position and orientation in space, regardless of real time property. 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 for the task of trajectory tracking. However, since these techniques aim to control the UAV to fly along a predetermined path, it is not easy to avoid collision with an obstacle, a person or another UAV.

  On the other hand, path following can set constraints in the space so as not to fly in a specific place in order to avoid collision with an obstacle or a person. In addition, various sensors and cameras may be attached to the UAV to avoid problems during flight movements. For example, an ultrasonic sensor can measure the altitude from the ground or floor, and can measure the velocity and attitude of the vehicle with a gyro sensor and an acceleration sensor. Cameras can also be used to detect moving objects and stabilize flight in space. Although it is conceivable to use spatial information and image information measured from these aircraft, the accuracy of the sensors built in most UAVs does not guarantee that they can be measured with an accuracy in millimeters, and obstacles In order to avoid collisions with other vehicles, it is necessary to measure the exact position and orientation of the vehicle using other means.

  The present invention has been made to solve the above problems, and it is an object of the present invention to provide a flight control device, method, and program that can freely control an unmanned airplane.

  In order to achieve the above object, a flight control device according to the present invention is provided with a reference object for controlling the flight of a UAV (Unmanned Aerial Vehicle), and a plurality of markers whose distances between markers are known, In the three-dimensional coordinates of each of the markers measured by the position measurement sensor, the position measurement sensor measuring the three-dimensional coordinates of each of the plurality of markers given to the UAV and whose distances between the markers are known An azimuth detecting unit which calculates an azimuth angle of the reference object with respect to a reference direction in a global coordinate system, three-dimensional coordinates of each of the plurality of markers of the reference object, the calculated azimuth angle, and In front of the UAV based on the three-dimensional coordinates of each of the plurality of markers of the reference object and the relative positional relationship of the three-dimensional coordinates of each of the plurality of markers of the UAV The target three-dimensional coordinates of each of the plurality of markers are calculated, and the difference between the calculated target three-dimensional coordinates and each of the plurality of markers of the plurality of markers of the UAV is calculated. The rotation angle is calculated based on three-dimensional coordinates obtained by adding the difference to three-dimensional coordinates of each of the plurality of markers, and the target three-dimensional coordinates of each of the plurality of markers of the UAV. A rotation angle around the X axis, a rotation angle around the Y axis, a velocity along the Z axis, and a rotation around the Z axis in the UAV based on the position and orientation calculation unit, the calculated difference, and the rotation angle. And a motion control unit for controlling the motion of the UAV based on the calculated flight command data.

  Further, in the flight control method according to the present invention, the position measurement sensor is attached to a reference object for controlling the flight of a UAV (Unmanned Aerial Vehicle), and a plurality of markers with known distances between markers; Measuring three-dimensional coordinates of each of a plurality of markers given to the UAV and having a known distance between the markers; and an orientation detection unit measuring the three-dimensional coordinates of each of the markers measured by the position measurement sensor. Calculating an azimuth angle of the reference object with respect to a reference direction in a global coordinate system based on coordinates; and a position and orientation calculation unit calculating three-dimensional coordinates of each of the plurality of markers of the reference object; Based on an azimuth angle, relative positional relationship between predetermined three-dimensional coordinates of each of the plurality of markers of the reference object, and three-dimensional coordinates of each of the plurality of markers of the UAV. Calculating the target three-dimensional coordinates of each of the plurality of markers of the UAV, and calculating the difference between the calculated three-dimensional coordinates of the target and the respective three-dimensional coordinates of the plurality of markers of the UAV Based on the calculated and obtained three-dimensional coordinates obtained by adding the difference to the three-dimensional coordinates of each of the plurality of markers of the UAV, and the target three-dimensional coordinates of each of the plurality of markers of the UAV Calculating the rotation angle, the motion control unit using the calculated difference and the rotation angle, the rotation angle around the X axis, the rotation angle around the Y axis, and the Z axis in the UAV. Calculating the speed along the axis and the rotational speed around the Z axis as flight command data, and controlling the movement of the UAV based on the calculated flight command data.

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

  According to the flight control apparatus, method, and program of the present invention, the azimuth angle of the reference object is calculated, and the three-dimensional coordinates of the reference object, the calculated azimuth angle, and the three-dimensional coordinates of the markers of the reference object and the UAV The difference between the target three-dimensional coordinates and the three-dimensional coordinates of the UAV is calculated based on the relative positional relationship, and the three-dimensional coordinates obtained by adding the differences to the three-dimensional coordinates of the UAV are the target of the UAV The rotation angle is calculated based on the three-dimensional coordinates, and the rotation angle around the X axis, the rotation angle around the Y axis, the velocity along the Z axis, in the UAV based on the calculated difference and the rotation angle The rotation speed around Z and Z axes is calculated as flight command data, and the movement of the UAV is controlled based on the calculated flight command data, so that the unmanned airplane can be controlled freely.

It is a block diagram showing composition of a flight control device concerning an embodiment of the invention. It is a figure which shows an example of the condition which controls a flight of UAV using a reference object. It is a flow chart which shows flight control processing routine in a flight control device concerning an embodiment of the invention. It is a flowchart of a process of the direction detection part in a flight control apparatus. It is a figure which shows an example of four points measured in the global coordinate system. It is a figure which shows an example of arrangement | positioning of each point in the space which moved the point A in parallel with the origin in a global coordinate system. It is a flowchart of a process of the position and orientation calculation unit in the flight control device. It is a figure which shows an example of the space arrangement | positioning of point C _t and point D _t which form a square with point A _s and point B _s . It is a figure which shows an example of the space arrangement | positioning which translated the midpoint G _s to the midpoint G _t in parallel. It is a flowchart of a process of the movement control part in a flight control device. It is a figure which shows an example of four points measured in the global coordinate system. Is a diagram illustrating an example of a spatial arrangement of the points A _s and the point B _s and isosceles C _t points forming a trapezoidal and the point D _t. It is a figure which shows an example of space arrangement | positioning of several UAV. It is a figure which shows the outline of UAV, and an example of a local coordinate system fixed to UAV. It is a figure which shows an example of the relationship of a global coordinate system and the local coordinate system of UAV fixation.

  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 tries to solve the problem that the user freely manipulates the flight of the UAV by moving the reference object and avoids the collision with the target object. It is a thing.

<Configuration of Flight Control Device According to First Embodiment of the Present Invention>

  The configuration of a flight control apparatus of a quad rotor type UAV according to a first embodiment of the present invention will be described. In the present embodiment, the flight of one UAV is steered with a reference object. As shown in FIG. 1, the flight control apparatus 100 according to the first embodiment of the present invention includes a CPU, a RAM, and a ROM storing a program for executing a flight control processing routine described later and various data. Can be configured with a computer including The flight control device 100 functionally includes a position measurement sensor 10, an arithmetic 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 data necessary for processing may be acquired, and the azimuth detection unit 30, the position and orientation calculation unit 32, and the motion control in the arithmetic unit 20 The flow of data from the unit 34 to each arrow may be in the form of using a recording medium such as a hard disk, a RAID device, or a CD-ROM, or using a remote data resource via a network.

  The position measurement sensor 10 is applied to the reference object 12 for controlling the flight of the UAV 14, and is applied to the UAV 14 as a plurality of markers (referred to as points A and B) with known distances between markers. In addition, three-dimensional coordinates of each of a plurality of markers (referred to as points C and D) whose distances between markers are known are measured as measurement data. In the present embodiment, in the situation shown in FIG. 2, the reference object 12 is used to control the flight of the UAV 14. The reference object 12 may be any movable object that can avoid collision with an obstacle, and in the present embodiment, a portable T-shaped bar will be described as an example. Generally, a motion capture device is known to 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. As shown in FIG. 2, markers are attached to the reference object 12 at the positions of point A and point B, and markers are attached to the position of point C and point D on the UAV 14. The position measurement sensor 10 is set up to measure three-dimensional coordinates in the global coordinate system, and each marker is sequentially measured at certain intervals as three-dimensional coordinates of the coordinate system. In the present embodiment, the distance between the point A and the point B is L, and the distance between the point C and the point D is L.

  In this embodiment, the flying motion of the UAV 14 is made such that the points A, B, C, and D have a relative positional relationship to form a square according to the three-dimensional position of the point A and the point B of the reference object 12 , But can be applied to other geometric shapes such as rectangles as well. Although the number of markers is two in the present embodiment, the number of markers is not limited to two, and three or more markers may be used to determine the relative positional relationship between objects.

  The operation unit 20 uses an orientation detection unit 30 that calculates an orientation from three-dimensional coordinates measured by the position measurement sensor 10, a position and orientation calculation unit 32 that calculates the position and orientation of the UAV 14 in the global coordinate system, and the position and orientation. The motion control unit 34 calculates flight command data for controlling the flight of the UAV 14 so as to satisfy predetermined positions and orientations, and the communication unit 50 transmits flight command data to the UAV 14. The specific processing contents of the azimuth detection unit 30, the position and orientation calculation unit 32, and the motion control unit 34 will be described in the description of the operation described later.

  The azimuth detection unit 30 calculates the azimuth angle Φ of the reference object 12 with respect to the reference direction in the global coordinate system, based on the three-dimensional coordinates of each of the markers measured by the position measurement sensor 10.

  The position and orientation calculation unit 32 calculates three-dimensional coordinates of points A and B, which are the marker attachment positions of the reference object 12, the calculated azimuth angle と, the marker point A of the reference object 12 determined in advance, Based on the relative positional relationship of the three-dimensional coordinates of the point B and the marker point C of the UAV 14 and the three-dimensional coordinates of the point D (for example, the relative positional relationship forming a square) The target three-dimensional coordinates are calculated, and the difference T between the calculated target three-dimensional coordinates and the three-dimensional coordinates of the marker point C and the point D of the UAV 14 is calculated. Further, the position and orientation calculation unit 32 calculates three-dimensional coordinates obtained by adding differences to the three-dimensional coordinates of the marker point C and the point D of the UAV 14, and the three-dimensional target of the marker point C and the point D of the UAV 14. The rotation angle θ is calculated based on the coordinates.

The motion control unit 34, based on the calculated difference T and the rotation angle θ, the rotation angle φ around the X axis, the rotation angle ω around the Y axis, the velocity V _z along the Z axis, and the UAV 14 The rotation speed V _θ around the Z axis is calculated as flight command data, and the calculated flight command data is transmitted to the UAV 14 via the communication unit 50 to control the motion of the UAV 14 based on the flight command data.

<Operation of Flight Control Device According to First Embodiment of the Present Invention>

  Next, the operation of the flight control device 100 according to the first embodiment of the present invention will be described. The position measuring sensor 10 applies to the reference object 12 for controlling the flight of the UAV 14, and the distance between the markers is given to the marker points A and B and to the UAV 14, and the distance between the markers is When measurement of three-dimensional coordinates of each of the known marker points C and D is started, and the operator starts control of the UAV 14 with the reference object 12, the flight control device 100 controls the flight control shown in FIG. Execute processing routine.

  First, in step S100, based on the three-dimensional coordinates of each of the markers measured by the position measurement sensor 10, the azimuth detection unit 30 calculates the azimuth angle Φ of the reference object 12 with respect to the reference direction in the global coordinate system.

  Next, in step S102, the position and orientation calculation unit 32 calculates the three-dimensional coordinates of the points A and B of the marker of the reference object 12, the calculated azimuth angle と, and the markers of the reference object 12 determined in advance. Based on the relative positional relationship between the point A and the three-dimensional coordinates of the point B and the marker point C of the UAV 14 and the three-dimensional coordinates of the point D, the target three-dimensional coordinates of the marker point C and the point D of the UAV 14 are The difference T between the calculated target three-dimensional coordinates and the three-dimensional coordinates of the marker point C and the point D of the UAV 14 is calculated. In addition, the rotation angle is based on the three-dimensional coordinates obtained by adding the difference to the three-dimensional coordinates of the marker point C and the point D of the UAV 14 and the target three-dimensional coordinates of the marker point C and the point D of the UAV 14 Calculate θ.

In step S104, the motion control unit 34 determines the rotation angle φ around the X axis, the rotation angle ω around the Y axis, and the velocity along the Z axis in the UAV 14 based on the calculated difference T and the rotation angle θ. V _z, and calculates the rotational speed V _theta around the Z axis as the flight command data, and transmits the calculated flight command data to UAV14, to control the movement of UAV14 based on flight command data. Here, the calculation of the difference T and the rotation angle θ in step S102 and the control of the motion of the UAV 14 based on the flight command data in step S104 are repeated until a predetermined condition described later is satisfied. Thereby, the movement of the UAV 14 is controlled by the operator moving with the reference object 12.

  Details of the processing of the direction detection unit 30 in step S100 will be described.

FIG. 4 is a flowchart of processing of the direction detection unit 30. When the direction detection unit 30 starts processing, in step S1000, the three-dimensional coordinate values of the point A and point B of the reference object 12 acquired by the position measurement sensor 10, the point C of the UAV 14, and three dimensional of the point D Get coordinate values. The three-dimensional coordinate values _A = the point _{A (X a, Y a,} Z h), = three-dimensional coordinates of the point _{B B (X b, Y b} , Z h), the 3D coordinates of the point C C = (X _c , Y _c , Z _h ), and let three-dimensional coordinate values of the point D be D = (X _d , Y _d , Z _h ). For simplicity of explanation, in the present embodiment, the height of the four points as the same, the coordinate value of Z _w axis and Z _h. The case where the heights of the points are different will be described in the second embodiment. The four measured points are shown in FIG. 5 in the X _w Y _w plane of the global coordinate system. In FIG. 5, the distance between the point A and the point C is L _a (≠ L), and the distance between the point B and the point D is L _b (≠ L).

  In step S1002, it is determined whether it is necessary to move the position of the center of gravity. If it is necessary, the process proceeds to step S1004, and if it is not necessary, the process proceeds to step S1008.

In step S1004, the coordinate values are translated and converted. In parallel conversion conversion of coordinate values in the direction detection unit 30, the point A is moved parallel to the origin. Subtract three-dimensional coordinate value A = (X _a , Y _a , Z _h ) of point A from all coordinates except point A as the three-dimensional coordinate values of parallel movement destination by the following equations (1) to (3) Then, calculate B _s , C _s , and D _s .

... (1)

... (2)

... (3)

  The arrangement of each point in the space in which the point A is moved parallel to the origin is shown in FIG.

Next, in step S1006, in order to calculate the azimuth angles of the point A and the point B of the reference object 12, the azimuth angle Φ is calculated from the point B _bs by calculation of the following equation (4).

... (4)

  In step S1008, it is determined whether or not the direction detection process is to be stopped. If the process is to be stopped, the process of the azimuth detecting unit 30 is ended. If it is not ended, the process is repeated by returning to step S1000. In the case where the process is stopped, it is assumed here that the operator ends the flight control of the UAV 14.

  The azimuth detecting unit 30 calculates the azimuth angle Φ in accordance with the three-dimensional positions of the point A and the point B of the reference object 12 by the above processing.

  Next, details of the processing of the position and orientation calculation unit 32 in step S102 will be described.

FIG. 7 is a flowchart of processing of the position and orientation calculation unit 32. In step S1100, a third-order target of points C and D such that points A, B, C and D form a square according to the three-dimensional position of points A and B of reference object 12 Calculate target coordinates that are original coordinates. The target coordinates indicate the vertex coordinates of the remaining two points when the point A and the point B are the vertices of a square. After the azimuth angle Φ is calculated according to the three-dimensional positions of the point A and the point B on the assumption that the point A is moved in parallel to the origin O and the point B to the point B _{s in the} azimuth detecting unit 30 The target coordinates corresponding to point C and point D are calculated. FIG. 8 shows points C _t and D _t of target coordinates corresponding to the point C and the point D. The point C _t and the point D _t of the target coordinates are calculated by the following equations (5) and (6).

... (5)

... (6)

Next, in step S1102, calculates coordinates _{G s,} and the coordinates _{G t} of the midpoint between the point _{C t} and the point _{D t} in the target coordinates of the midpoint of the _{C s} and the point _{D s} point translated. From the point C _s and the point D _s moved in parallel in advance, the coordinates G _s of the middle point are calculated by the following equation (7).

... (7)

Similarly, the coordinates G _t of the middle point are calculated by the following equation (8) also from the point C _t and the point D _t of the target coordinates.

... (8)

Next, in step S1104, the difference T between the current position of the UAV 14 obtained by the position measurement sensor 10 and the target coordinates is calculated. The difference T is detected from the deviation between the coordinates G _s and G _t of the middle point by calculation of the following equation (9).

... (9)

  The magnitude of the difference T is obtained by the following equation (10).

... (10)

In step S1106, it is determined whether the value of ΔT exceeds an allowable range (for example, 10 cm mail). If the allowable range is not exceeded, it is determined that the current position of the UAV 14 is the predetermined position, and the process proceeds to step S1110. If it exceeds the allowable range, it is determined that the current position of the UAV 14 is not the predetermined position, and the process proceeds to step S1108. In step S1108, points C _r and D _r of the target coordinates are determined by the following equations (11) and (12) by adding points C _s and D _s that have been translated in advance.

... (11)

... (12)

At this time, if the orientation of the UAV 14 does not match the orientations of the points A and B of the reference object 12, the points C _r and D _r do not match the points C _t and D _t of the target coordinates. FIG. 9 shows the arrangement of the point C _r and the point D _r and the point C _t and the point D _t of the target coordinates. Assuming that they do not match each other, the vector E _r from the point C _r to the point D _r and the vector E _t from the point C _t to the point D _t can be _expressed by the following equations (13) and (14) Calculated by a formula.

... (13)

... (14)

  In step S1110, the rotation angle θ is calculated by the following equation (15) using the relationship of the inner product of the vectors.

... (15)

Here, E _r · E _t represents the inner product of the vector E _r and the vector E _t , and || E _r || and || E _t || represent the norm (magnitude) of the vector E _r and the vector E _t .

The position and orientation calculation unit 32 calculates the parallel movement vector T and the rotation angle θ _r so that the position and orientation of the UAV 14 become the predetermined position and orientation by the above processing.

  Next, details of the processing of the motion control unit 34 in step S104 will be described.

FIG. 10 is a flowchart of processing of the motion control unit 34. The process of the motion control unit 34, by using the rotation angle theta _r and translation vector T obtained by the position and orientation calculation unit 32, controls the flight movement of UAV14. Depending on the UAV 14 used, there are various data formats for control data to the UAV 14. In the present embodiment, the case of using the commercially available product AR Drone 2.0 is taken as an example, but the present embodiment can also be used when controlling the flight of the UAV 14 other than that.

In FIG. 2 above, it is assumed that the reference object 12 and the UAV 14 are in face-to-face relationship. The flight command data to the UAV 14 are the rotation angle φ around the X axis, the rotation angle ω around the Y axis, the velocity V _z along the Z axis, and the rotation velocity V around the Z axis set for the airframe. When the process of the motion control unit 34 is started, in step S1200, the parallel movement vector T and the rotation angle θ are taken out in reading control data.

Next, in step S 1202, the translation vector T and the rotation angle θ _r are converted into flight command data for transmission to the UAV 14. Considering that the reference object 12 and the UAV 14 are in a face-to-face relationship, the current position of the UAV 14 is given by T '= (Tx', Ty ', Tz') = (-Tx, -Ty, Tz) The rotation angle for the object 12 is given by θ ′ _r = −θ _r .

  In UAV14, roll rotation φ produces Y-axis translational motion and pitch rotation ω produces X-axis translational motion. In this processing, the flight command data given to UAV 14 is given by the following equations (16) to (19) Convert.

... (16)

... (17)

... (18)

... (19)

The coefficients α _x , α _y , α _z and α _r take on the role of gain in feedback control and may be determined by the user as parameters, for example, given as α _x = α _y = α _z = α _r = 0.1 .

  In step S 1204, flight command data calculated by the equations (16) to (19) is transmitted to the UAV 14 via WiFi via the communication unit 50.

Depending on the settings of the gain coefficients α _x , α _y , α _z and α _r , a given flight command may not reach a predetermined position and orientation.

Therefore, in step S1206, after transmission of the flight command data, it is determined whether a predetermined condition is satisfied. The predetermined condition is, for example, that the distance ΔT calculated by the above equation (10) is within the allowable range. If the condition is not satisfied, the process returns to the calculation of the coordinates of the middle point in step S1102 of the processing flow of the position and orientation calculation unit 32 of FIG. 7 and the difference between the middle point G _s and the middle point G _t according to the equation (9). Get When flight command data is sent to the UAV 14 in step S1204, the UAV 14 moves based on the flight command data, and changes to a three-dimensional coordinate different from the previous three-dimensional coordinate. Thus, until the magnitude ΔT of the difference T of the distance calculated by the above equation (10) falls within the allowable range, the translation control vector T and the rotation angle θ _r are calculated by the position and orientation calculation unit 32, and the motion control is performed. The unit 34 continues to transmit flight command data calculated by the equations (16) to (19). If the condition is satisfied, the motion control process is ended.

  As described above, the present embodiment calculates the position and orientation of the UAV 14 according to the arrangement of the reference object 12, and controls the flight motion of the UAV 14 such that the points A, B, C, and D form a square. be able to. Also, each time the user moves the reference object 12 (the point A and the point B move), in the present embodiment, the point C and the point D form a square with the point A and the point B of the reference object 12 As such, the flight of the UAV 14 can be controlled accordingly. The flight navigation of the UAV 14 can be realized by the operator moving the reference object 12 continuously.

  As described above, according to the flight control device according to the embodiment of the first invention, the azimuth angle of the reference object is calculated, and the three-dimensional coordinates of the reference object, the calculated azimuth angle, the reference object 12 and The difference between the target three-dimensional coordinates and the three-dimensional coordinates of the UAV 14 is calculated based on the relative positional relationship between the three-dimensional coordinates of the markers of the UAV 14, and the difference is added to the three-dimensional coordinates of the UAV. The rotation angle is calculated based on the coordinates and the target three-dimensional coordinates of the UAV 14, and the rotation angle around the X axis and the rotation angle around the Y axis in the UAV 14 based on the calculated difference and the rotation angle By calculating the velocity along the Z axis and the rotation velocity around the Z axis as flight command data and controlling the motion of the UAV 14 based on the calculated flight command data, the unmanned airplane can be freely controlled.

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

The second embodiment of the present invention is an example in the case where the distance between the point C and the point D attached to the UAV 14 in FIG. 2 is M (≠ L). Figure 11 shows four points of the same height _{Z h} (Point A, Point B, Point C, and Point D) the layout of. In this embodiment, the distance between the point A and the point C is L _a (≠ L), and the distance between the point B and the point D is L _b (≠ L).

  The present embodiment is the same as the first embodiment in configuration and operation, except that the processing flow of the orientation detection unit 30 in FIG. 4, the processing flow of the position and orientation calculation unit 32 in FIG. 7 and the processing of the motion control unit 34 in FIG. The flow controls the flight motion of the UAV 14. In the present embodiment, since the distance between the point C and the point D is different from the distance between the point A and the point B, the points A, B, C, and D have an isosceles trapezoidal shape as shown in FIG. Form.

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

The third embodiment of the present invention is an example in the case of point A and point B having height Z _h and point C and point D having height Z _l in FIG. 2 described above. The distance between the point C and the point D is the same as the distance L between the point A and the point B, or M (≠ L).

In the third embodiment, the heights of point C and point D are changed to Z _h + N, and four points (point A, point B, point C, and point D) are square or in the X _w Y _w plane. The case of forming an isosceles trapezoid will be described. The present embodiment controls the flight motion of the UAV 14 by the processing flow of the position and orientation calculation unit 32 of FIG. 7 and the processing flow of the motion control unit 34 of FIG. 10 with the same configuration and operation as the first embodiment. Hereinafter, only points different from the first embodiment and the second embodiment will be described.

In the azimuth detecting unit 30 of this embodiment, the three-dimensional coordinate data of the point A, the point B, the point C, and the point D acquired by the position measurement sensor 10 is taken out and used as the three-dimensional coordinate value of the parallel movement destination. by 20) - (22), all the three-dimensional coordinates point a from the coordinates _a = (X _a other than point _a, Y a, by subtracting the _{Z h), B s, C} s, and _{D s} calculate.

... (20)

... (21)

... (22)

A difference from the first embodiment and the second embodiment is that the Z coordinates of the point C _s and the point D _s are such that Z _m ≠ 0. On the other hand, in the arrangement of points in the space in which the point A is moved in parallel to the origin, the azimuth angles of the points A and B of the reference object 12 can be obtained by calculation of equation (4) as in the first embodiment.

The position and orientation calculation unit 32 calculates the point C _t and the point D _t of the target coordinates by the following equations (23) and (24).

... (23)

... (24)

Further, coordinates G _s of the middle point between the parallel moved point C _s and the point D _s are calculated by the following equation (25).

... (25)

Similarly, the coordinates G _t of the middle point are calculated from the points C _t and D _t of the target coordinates by the following equation (26).

... (26)

When the middle point is obtained, the translation vector T is calculated by the above equation (9). On the other hand, since the rotation angle θ _r is calculated in the X _w Y _w plane, the component of the Z value is given as 0 in the calculation of the vectors E _r and E _{t in} the above equations (13) and (14). . Thus, the rotation angle θ _r is calculated in the above equation (15).

The motion control unit 34, with the translation vector T obtained by the position and orientation calculation unit 32 a rotation angle theta _r, controls the flight movement similarly UAV14 the first or second embodiment. As a result, the heights of the points C and D become Z _h + N, and the heights of the points A and B become Z _h , and four points (point A, point B, point C, and point D) are X _w Y _w Form a square or isosceles trapezoid in the plane. In addition, when it sets to N = 0, it becomes 1st Embodiment or 2nd Embodiment.

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

The fourth embodiment of the present invention is an example of simultaneously controlling the flight of N UAVs 14. FIG. 13 shows a situation in which N UAVs 14 form and fly with UAV 14 # 1 at the top. Each UAV 14 is provided with two markers that can be detected by the position measurement sensor 10. UAV14 # point _{A 1} and the point _{B 1} of 1 is assumed to correspond to the points A and B of the first embodiment, UAV14 # point _{A 2} and point _{B 2} of 2 and a point C of the first embodiment It corresponds to point D. This assignment, as in the first embodiment, UAV14 according to the position and orientation of the _{A 1} and the point _{B 1} point # 1, as UAV14 # point _{A 2} and point _{B 2} of 2 to form a square in the horizontal plane The flight motion of UAV 14 # 2 is controlled. Similarly, UAV14 # points _{A N} and the point _{B N} the N to correspond to C and the point D from that of the first embodiment, UAV14 # point N-1 to fly before one points A and B By setting A _N-1 and B _{N -1} , four points A _{N -1} , B _{N -1} , A _N , and B _N can fly so as to form a square in the horizontal plane. .

  Thus, flight control is performed with the (N-1) th UAV 14 as the reference object 12 for the Nth UAV 14. This embodiment has the same configuration and operation as the first embodiment, and the N UAVs 14 are processed in the same manner as in the first embodiment, with the process flow of the position and orientation calculation unit 32 of FIG. 7 and the motion control unit of FIG. The process flow of 34 controls the flight motion of the UAV 14. At this time, the first UAV 14 is controlled to follow the reference object 12 carried by the operator, as in the first embodiment, and the second and subsequent UAVs 14 are N, as described below. Control is performed so that the Nth UAV 14 follows the UAV 14 of No. -1 in order.

  The azimuth detecting unit 30 of the present embodiment calculates, for the reference object 12, an azimuth angle に 対 す る with respect to the reference direction in the global coordinate system, as in the first embodiment.

  Further, for each of the second and subsequent N-th UAVs 14, the direction detection unit 30 determines the reference direction in the global coordinate system for the N−1-th UAV 14 as the reference object 12 of the N-th UAV 14. Calculate the azimuth angle に 対 す る for.

  As in the first embodiment, the position and orientation calculation unit 32 is based on the three-dimensional coordinates of the points A and B of the marker of the reference object 12, the calculated azimuth angle Φ, and the relative positional relationship. The target three-dimensional coordinates of the marker point C and point D of the first UAV 14 are calculated, and the calculated target three-dimensional coordinates and the marker point C of the first UAV 14 and three-dimensional of the point D The difference T with the coordinates is calculated, and the rotation angle θ is calculated.

  In addition, the position and orientation calculation unit 32 calculates the three-dimensional coordinates of the point A and the point B of the marker of the N-1st UAV 14 and the calculated azimuth with respect to each of the 2nd and subsequent N UAVs 14. Relative positional relationship between angle Φ, three-dimensional coordinates of marker point A of N-1 first UAV 14 predetermined, and point B of marker point C and N-th UAV 14 and three-dimensional coordinates of point D To calculate the target three-dimensional coordinates of the marker point C of the Nth UAV 14 and the point D, and the calculated three-dimensional coordinates of the target and the marker point C of the Nth UAV 14, and The difference T with the three-dimensional coordinates of the point D is calculated. Further, the position and orientation calculation unit 32 calculates three-dimensional coordinates obtained by adding differences to the three-dimensional coordinates of the marker point C and the point D of the Nth UAV 14, the marker point C of the Nth UAV 14, and The rotation angle θ is calculated based on the target three-dimensional coordinates of the point D.

The motion control unit 34 instructs the first UAV 14 to fly about the rotation angle φ around the X axis, the rotation angle ω around the Y axis, the velocity V _z along the Z axis, and the rotation velocity V _θ around the Z axis By calculating as data and transmitting the calculated flight command data to the first UAV 14 via the communication unit 50, the motion of the first UAV 14 is controlled based on the flight command data.

In addition, the motion control unit 34 rotates the X U-axis in the Nth UAV 14 based on the calculated difference T and the rotation angle θ with respect to each of the second UAV 14 and subsequent N UAVs 14. The Nth UAV 14 is calculated based on the calculated flight command data by calculating the angle φ, the rotation angle ω around the Y axis, the velocity V _z along the Z axis, and the rotation velocity V _θ around the Z axis as flight command data. Control the movement of When the processing of the Nth UAV 14 is completed, control is similarly performed with N = N + 1.

  Further, in the present embodiment, the flight of a plurality of UAVs can be simultaneously controlled according to the second embodiment when the distance between the two points is different, and according to the third embodiment when the heights of the points are different. In the present embodiment, formation of N UAVs is enabled, with UAV # 1 at the top.

  According to the method of each of the above embodiments, the user can freely operate the flight of the UAV by issuing a flight command indicating the three-dimensional position and orientation to the UAV. Even for multiple UAVs, it is possible for the user to freely operate in the space without causing the multiple UAVs to collide with each other by issuing a flight command indicating the three-dimensional position and orientation to the UAVs. Do. Furthermore, by controlling the UAV with the user's intention, the UAV can support or cooperate with work that can not be realized by a single person action in a real environment surrounding the person.

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

10 Position measurement sensor 12 Reference object 14 UAV
Reference Signs List 20 operation unit 30 azimuth detection unit 32 position and attitude calculation unit 34 motion control unit 50 output unit 100 flight control device

Claims (7)

A plurality of markers attached to a reference object for controlling UAV (Unmanned Aerial Vehicle) flight and having a known distance between markers, and a plurality of markers applied to the UAV and a known distance between markers A position measurement sensor that measures three-dimensional coordinates of each of the markers and
An azimuth detection unit that calculates an azimuth angle of the reference object with respect to a reference direction in a global coordinate system based on three-dimensional coordinates of each of the markers measured by the position measurement sensor;
The three-dimensional coordinates of each of the plurality of markers of the reference object, the calculated azimuth angle, the three-dimensional coordinates of each of the plurality of markers of the reference object determined in advance, and the plurality of markers of the UAV Calculating a target three-dimensional coordinate of each of the plurality of markers of the UAV based on the relative positional relationship between each of the three-dimensional coordinates, and the calculated target three-dimensional coordinates and the target of the UAV Three-dimensional coordinates obtained by calculating differences with three-dimensional coordinates of each of a plurality of markers, adding the differences to three-dimensional coordinates of each of the plurality of markers of the UAV, and the plurality of markers of the UAV A position and orientation calculation unit that calculates a rotation angle based on the target three-dimensional coordinates of each of
Based on the calculated difference and the rotation angle, the rotation angle around the X axis, the rotation angle around the Y axis, the velocity along the Z axis, and the rotation velocity around the Z axis in the UAV are commanded to fly A motion control unit that calculates the data as the data and controls the motion of the UAV based on the calculated flight command data;
Flight control equipment. The plurality of markers attached to the UAV are two markers,
The difference calculated by the position and orientation calculation unit is the coordinates of the midpoint of the three-dimensional coordinates of the two markers of the UAV and the coordinates of the midpoint of the three-dimensional coordinates of the two markers of the UAV calculated. The flight control device according to claim 1, wherein the difference is Let the UAV be N UAVs, let the N-1 UAV be the reference object for the N UAV,
The azimuth detection unit calculates an azimuth angle of the N-1 first UAV as the reference object,
The position and orientation calculation unit may calculate three-dimensional coordinates of each of the plurality of markers of the N-1st UAV, the calculated azimuth angle, and the predetermined value of the N-1th UAV. Each of the plurality of markers of the Nth UAV is based on the three-dimensional coordinates of each of the plurality of markers and the relative positional relationship of the three-dimensional coordinates of each of the plurality of markers of the Nth UAV The target three-dimensional coordinates are calculated, and the difference between the calculated target three-dimensional coordinates and the respective three-dimensional coordinates of the plurality of markers of the Nth UAV is calculated, and the Nth Based on the three-dimensional coordinates obtained by adding the difference to the three-dimensional coordinates of each of the plurality of markers of the UAV, and the target three-dimensional coordinates of each of the plurality of markers of the Nth UAV Calculate the rotation angle,
The motion control unit is configured to calculate a rotation angle around the X axis, a rotation angle around the Y axis, a velocity along the Z axis, in the Nth UAV based on the calculated difference and the rotation angle. The flight control device according to claim 1 or 2, wherein the rotation speed around Z and Z axes is calculated as flight command data, and movement of the Nth UAV is controlled based on the calculated flight command data. A position measurement sensor is attached to a reference object for controlling the flight of a UAV (Unmanned Aerial Vehicle), and a plurality of markers with known distances between markers, and the UAV applied, and a distance between markers Measuring the three-dimensional coordinates of each of a plurality of markers known by
An azimuth detecting unit calculating an azimuth angle of the reference object with respect to a reference direction in a global coordinate system based on three-dimensional coordinates of each of the markers measured by the position measurement sensor;
The position and orientation calculation unit determines three-dimensional coordinates of each of the plurality of markers of the reference object, the calculated azimuth angle, and three-dimensional coordinates of each of the plurality of markers of the reference object determined in advance. The target three-dimensional coordinates of each of the plurality of markers of the UAV are calculated based on the relative positional relationship of the three-dimensional coordinates of each of the plurality of markers of the UAV, and the calculated three-dimensional targets are calculated. Three-dimensional coordinates obtained by calculating a difference between coordinates and three-dimensional coordinates of each of the plurality of markers of the UAV, and adding the difference to three-dimensional coordinates of each of the plurality of markers of the UAV; Calculating a rotation angle based on the target three-dimensional coordinates of each of the plurality of markers of the UAV;
A motion control unit generates a rotation angle around the X axis, a rotation angle around the Y axis, a velocity along the Z axis, and a rotation around the Z axis in the UAV based on the calculated difference and the rotation angle. Calculating the rotational speed as flight command data, and controlling the motion of the UAV based on the calculated flight command data;
Flight control method including: The plurality of markers attached to the UAV are two markers,
In the step calculated by the position and orientation calculation unit, the difference is a coordinate of a middle point of three-dimensional coordinates of two markers of the UAV and a three-dimensional coordinate which is a target of the two markers of the calculated UAV. The flight control method according to claim 4, wherein the difference is a difference from the coordinates of a point. Let the UAV be N UAVs, let the N-1 UAV be the reference object for the N UAV,
The step of calculation by the azimuth detection unit calculates an azimuth angle for the N-1 first UAV as the reference object,
The step of calculation by the position and orientation calculation unit includes three-dimensional coordinates of each of the plurality of markers of the N-1th UAV, the calculated azimuth angle, and the N-1th predetermined antenna. The plurality of markers of the Nth UAV based on the three-dimensional coordinates of each of the plurality of markers of the UAV and the relative positional relationship of the three-dimensional coordinates of each of the plurality of markers of the Nth UAV; The target three-dimensional coordinates of each marker are calculated, and the difference between the calculated target three-dimensional coordinates and the three-dimensional coordinates of each of the plurality of markers of the Nth UAV is calculated. Three-dimensional coordinates obtained by adding the difference to the three-dimensional coordinates of each of the plurality of markers of the Nth UAV, and the three-dimensional target of each of the plurality of markers of the Nth UAV Calculate the rotation angle based on the coordinates,
In the step of controlling by the motion control unit, a rotation angle around the X axis, a rotation angle around the Y axis, and a Z axis in the Nth UAV based on the calculated difference and the rotation angle. The flight according to claim 4 or 5, wherein the velocity along the axis and the rotational velocity around the Z axis are calculated as flight command data, and the motion of the Nth UAV is controlled based on the calculated flight command data. Control method.   The program for functioning a computer as each part of the flight control apparatus of any one of Claims 1-3.