JP6758271B2 - Flight controls, methods, and programs - Google Patents

Description

The present invention relates to flight control devices, methods and programs, and in particular to flight control devices, methods and programs for controlling unmanned aerial vehicles.

A quad-rotor type UAV (Unmanned Aerial Vehicle) is an unmanned aerial vehicle that has four propellers and controls the lift applied to each propeller. Many unmanned aerial vehicles called drones are a type of quad-rotor type UAV. The unmanned aerial vehicle handled below is abbreviated as UAV.

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

Further, in order to measure the three-dimensional position of the UAV, as shown in FIG. 15, a certain reference global coordinate system is set. In GPS, the three-dimensional coordinates are used as the world coordinate system, and in the motion capture system, the measured coordinate system is the global coordinate system. The center of gravity of the UAV, that is, the position of the UAV (origin of the local coordinate system) is expressed as a point P = (X, Y, Z) in the global coordinate system. The posture of UAV 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 the roll rotation (rotation angle φ), the rotation around the Y axis is the pitch rotation (rotation angle ω), and the Z axis. The rotation around is called yaw rotation (rotation angle θ). The UAV's flight motion causes 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 around the Y-axis produces translational motion in the X-axis direction, and rotation around the X-axis produces translational motion in the Y-axis direction. Rotation around the Z-axis produces azimuth rotation, and when the same lift is applied to the four propellers at the same time, its strength produces translational motion (elevation of altitude) in the Z-axis direction.

Given the predetermined coordinate values (X _d , Y _d , Z _d ) and azimuth θ _d in the global coordinate system, the predetermined position P = (X, Y, Z) of the UAV and the current azimuth φ In order to fly to the position of the coordinate values and aim 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 the UAV equation of motion and the angular equation of motion. Non-Patent Document 2 discloses a flight motion control method for AR Drone 2.0 (commercially available low-priced UAV). In many prior arts, discrete predetermined positions and bearings are given as flight trajectories in the global coordinate system, and their motion is controlled so that the UAV tracks each point and bearing.

On the other hand, since UAVs have the ability to fly freely in space, they can play an active role as robots that can replace humans. In Non-Patent Document 3, the UAV is used as a tool for collaborative work with humans, and in Non-Patent Document 4, a plurality of UAVs are used to form a net in the space, and the human and the UAV exchange balls. A method of controlling the movement of a plurality of UAVs as an operation has been published.

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 Robot 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 tasks of freely controlling the flight of a UAV can be roughly divided into three types, which are called hovering, trajectory tracking, and path following. Hovering maintains flight to stay in a given position and bearing. Trajectory tracking controls to track in real time along a given path (discrete three-dimensional position and bearing in space). On the other hand, the path follow controls the flight by giving some kind of constraint condition in the space at the three-dimensional position and the azimuth in the given space, regardless of the real-time property. According to Non-Patent Document 1 and Non-Patent Document 2, the flight motion of the UAV can be appropriately controlled 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 collisions with obstacles, people or other UAVs.

On the other hand, in order to avoid collision with obstacles or people, path follow can set restraint conditions in space so as not to fly in a specific place. Further, in order to avoid troubles during flight movement, various sensors and cameras may be attached to the UAV. For example, an ultrasonic sensor can measure altitude from the ground or floor, and a gyro sensor and an acceleration sensor can measure the speed and attitude of the aircraft. Cameras can also be used to detect moving objects and stabilize flight in space. It is possible to use spatial information and image information measured from these aircraft, but the accuracy of the sensors built into most UAVs does not guarantee that they can be measured with accurate accuracy in millimeters, and obstacles. In order to avoid collision with, it is necessary to measure the accurate position and bearing of the aircraft by another means.

The present invention has been made to solve the above problems, and an object of the present invention is to provide a flight control device, a method, and a program capable of freely controlling an unmanned aerial vehicle.

In order to achieve the above object, the flight control device according to the present invention is attached to a reference object for controlling the flight of a UAV (Unmanned Aerial Vehicle), and a plurality of markers whose distances between the markers are known, and a plurality of markers. The position measurement sensor that measures the three-dimensional coordinates of each of the plurality of markers assigned to the UAV and the distance between the markers is known, and the three-dimensional coordinates of each of the markers measured by the position measurement sensor. Based on this, the orientation detection unit that calculates the orientation angle of the reference object with respect to the reference direction in the global coordinate system, the three-dimensional coordinates of each of the plurality of markers of the reference object, and the calculated orientation angle are predetermined. Each target of the plurality of markers of the UAV is based on the relative positional relationship between the three-dimensional coordinates of each of the plurality of markers of the reference object and the three-dimensional coordinates of each of the plurality of markers of the UAV. The three-dimensional coordinates of the UAV are calculated, the difference between the calculated three-dimensional coordinates of the target and the three-dimensional coordinates of each of the plurality of markers of the UAV is calculated, and each of the plurality of markers of the UAV is calculated. A position / orientation calculation unit that calculates a rotation angle based on the three-dimensional coordinates obtained by adding the difference to the three-dimensional coordinates and the target three-dimensional coordinates of each of the plurality of markers of the UAV. Based on the difference and the rotation angle, the flight command data of the rotation angle around the X axis, the rotation angle around the Y axis, the speed along the Z axis, and the rotation speed around the Z axis in the UAV. It is configured to include a motion control unit that controls the motion of the UAV based on the calculated flight command data.

Further, in the flight control method according to the present invention, 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 whose distances between the markers are known, and the above. The step of measuring the three-dimensional coordinates of each of the plurality of markers assigned to the UAV and the distance between the markers is known, and the orientation detection unit is the three-dimensional of each of the markers measured by the position measurement sensor. The step of calculating the azimuth angle of the reference object with respect to the reference direction in the global coordinate system based on the coordinates, and the position / orientation calculation unit calculated the three-dimensional coordinates of each of the plurality of markers of the reference object. The plurality of UAVs based on the azimuth angle and the relative positional relationship between the three-dimensional coordinates of each of the plurality of markers of the reference object and the three-dimensional coordinates of each of the plurality of markers of the UAV. The target three-dimensional coordinates of each of the markers of the above are calculated, and the difference between the calculated three-dimensional coordinates of the target and the three-dimensional coordinates of each of the plurality of markers of the UAV is calculated. A step of calculating the rotation angle based on the three-dimensional coordinates obtained by adding the difference to the 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. Based on the calculated difference and the rotation angle, the motion control unit determines the rotation angle around the X axis, the rotation angle around the Y axis, the speed along the Z axis, and the Z axis in the UAV. It is characterized in that the rotation speed around the object is calculated as flight command data, and the step of controlling the movement of the UAV based on the calculated flight command data is included and executed.

The 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 device, 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 reference object and the UAV marker. Based on the relative positional relationship, the difference between the target 3D coordinate and the UAV 3D coordinate is calculated, and the 3D coordinate obtained by adding the difference to the UAV 3D coordinate and the UAV target are obtained. The rotation angle is calculated based on the three-dimensional coordinates, and based on the calculated difference and the rotation angle, the rotation angle around the X axis, the rotation angle around the Y axis, and the speed along the Z axis in UAV, And by calculating the rotation speed around the Z axis as flight command data and controlling the movement of the UAV based on the calculated flight command data, it is possible to freely control the unmanned airplane.

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 situation which controls the flight of a UAV using a reference object. 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 process of the direction detection part in a flight control device. It is a figure which shows an example of 4 points measured in a global coordinate system. It is a figure which shows an example of the arrangement of each point in the space which moved the point A parallel to the origin in a global coordinate system. It is a flowchart of the process of the position and attitude calculation part in a flight control device. It is a figure which shows an example of the spatial arrangement of a point C _t and a point D _t which form a square with a point A _s and a point B _s . It is a figure which shows an example of the spatial arrangement which moved the midpoint G _s parallel to the midpoint G _t . It is a flowchart of the process of the motion control unit in a flight control device. It is a figure which shows an example of 4 points measured in a global coordinate system. It is a figure which shows an example of the spatial arrangement of the point C _t and the point D _t which form an isosceles trapezoid with the point A _s and the point B _s . It is a figure which shows an example of the spatial arrangement of a plurality of UAVs. It is a figure which shows the overview of the UAV and an example of the local coordinate system fixed to the UAV. It is a figure which shows an example of the relationship between the global coordinate system and the local coordinate system fixed to UAV.

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

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

The configuration of the flight control device of the quad rotor type UAV according to the first embodiment of the present invention will be described. This embodiment is a mode in which the flight of one UAV is steered by a reference object. As shown in FIG. 1, the flight control device 100 according to the first embodiment of the present invention includes a CPU, a RAM, a ROM that stores a program for executing a flight control processing routine described later, and various data. It can be configured with a computer that includes. The flight control device 100 functionally includes a position measurement sensor 10, a calculation unit 20, and a communication unit 50 as shown in FIG. In this configuration, the position measurement sensor 10 does not necessarily have to be connected as a component, and data necessary for processing may be acquired, and the orientation detection unit 30, the position / orientation calculation unit 32, and the motion control in the calculation 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 attached to the reference object 12 for controlling the flight of the UAV 14, and a plurality of markers (referred to as points A and B) having known distances between the markers, and the position measurement sensor 10 is attached to the UAV 14. In addition, the three-dimensional coordinates of each of the plurality of markers (points C and D) whose distances between the markers are known are measured as measurement data. In the present embodiment, the flight of the UAV 14 is controlled by using the reference object 12 in the situation shown in FIG. 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 rod will be described as an example. In general, it is known that a motion capture device measures three-dimensional coordinates of a predetermined marker with high accuracy in real time. In this embodiment, a motion capture device is used as an example of the position measurement sensor 10. As shown in FIG. 2, the reference object 12 has markers attached at the positions of points A and B, and the UAV 14 has markers attached at the positions of points C and D. 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 regular intervals as the three-dimensional coordinates of the coordinate system. In the present embodiment, the distance between the points A and B is L, and the distance between the points C and D is L.

In the present embodiment, the flight motion of the UAV 14 is such that the points A, B, C, and D have a relative positional relationship forming a square according to the three-dimensional positions of the points A and B of the reference object 12. However, it can be applied to other geometric shapes such as rectangles as well. In the present embodiment, the number of markers is two, but the number is not limited to this, and the relative positional relationship between the objects may be obtained by setting the number to three or more.

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

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 / orientation calculation unit 32 includes the three-dimensional coordinates of the points A and B, which are the positions where the markers of the reference object 12 are attached, the calculated azimuth angle Φ, the marker points A of the reference object 12, and the predetermined marker points A. Based on the relative positional relationship between the three-dimensional coordinates of point B, the marker point C of UAV14, and the three-dimensional coordinates of point D (for example, the relative positional relationship forming a square), the marker points C and D of UAV14 The target three-dimensional coordinates are calculated, and the difference T between the calculated target three-dimensional coordinates and the marker point C of the UAV 14 and the three-dimensional coordinates of the point D is calculated. Further, the position / orientation calculation unit 32 has three-dimensional coordinates obtained by adding a difference to the three-dimensional coordinates of the marker points C and D of the UAV 14, and three-dimensional targets of the marker points C and D of the UAV 14. The rotation angle θ is calculated based on the coordinates.

Based on the calculated difference T and the rotation angle θ, the motion control unit 34 has a rotation angle φ around the X axis, a rotation angle ω around the Y axis, a speed V _z along the Z axis, and a rotation angle V _z in 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 the flight control device according to the first embodiment of the present invention>

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

First, in step S100, 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.

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

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

The details of the processing of the azimuth detection unit 30 in step S100 will be described.

FIG. 4 is a flow chart of processing of the orientation detection unit 30. When the azimuth detection unit 30 starts the process, in step S1000, the three-dimensional coordinate values of the points A and B of the reference object 12 acquired by the position measurement sensor 10 and the three dimensions of the points C and D of the UAV 14 Get the 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 Let C = (X _c , Y _c , Z _h ) and the three-dimensional coordinate values of the point D be D = (X _d , Y _d , Z _h ). For the sake of simplicity, in this embodiment, the heights of the four points are the same, and the coordinate values on the Z _w axis are Z _h . The case where the heights of the points are different will be described in the second embodiment. In X _w Y _w plane in the global coordinate system, showing the four points are measured in FIG. In Figure 5, the distance between point A and point C _{L a} (≠ _L), the distance between the points B and D is the _{L b} (≠ _L).

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

In step S1004, the coordinate values are translated by translation. In the translation of the coordinate values by the orientation detection unit 30, the point A is translated to the origin. As a three-dimensional coordinate values of translation destination, following (1) subtracting to (3), three-dimensional coordinate values _A = the point A from all the coordinates other than the point _{A (X a, Y a,} Z h) the Then, B _s , C _s , and D _s are calculated.

... (1)

... (2)

... (3)

FIG. 6 shows the arrangement of each point in the space in which the point A is translated in parallel with the origin.

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

... (4)

In step S1008, it is determined whether to stop the direction detection process. If the process is stopped, the process of the direction detection unit 30 is ended. If not, the process returns to step S1000 and the process is repeated. The case where the processing is stopped is a case where the operator ends the flight control of the UAV 14.

The azimuth detection unit 30 calculates the azimuth angle Φ according to the three-dimensional positions of the points A and B of the reference object 12 by the above processing.

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

FIG. 7 is a flow chart of processing of the position / posture calculation unit 32. In step S1100, the target tertiary of the points C and D is such that the points A, B, C, and D form a square according to the three-dimensional positions of the points A and B of the reference object 12. Calculate the target coordinates, which are the original coordinates. The target coordinates refer to the coordinates of the remaining two vertices when the points A and B are the vertices of the square. After the azimuth detection unit 30, the azimuth angle Φ corresponding to the three-dimensional position of the points A and B is calculated on the assumption that the point A is translated to the origin O and the point B is translated to the point B _s . , Point C, and the target coordinates corresponding to the point D are calculated. FIG. 8 shows the points C _t and D _t of the target coordinates corresponding to the points C and D. The points C _t and 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 points C _s and D _s that were translated earlier, the coordinates G _s of the midpoint are calculated by the following equation (7).

... (7)

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

... (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 distance between the target coordinates is calculated. The difference T is detected from the deviation between the coordinates G _s and G _t of the midpoint by the calculation of the following equation (9).

... (9)

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

... (10)

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

... (11)

... (12)

In this case, if the orientation of UAV14 does not match the orientation of the points A and B of the reference object 12, point C _r and the point D _r does not match the point target coordinates C _t and the point D _t. FIG. 9 shows the arrangement of the points C _r , D _r, and the points C _t and D _t of the target coordinates. On the assumption that each does not match, the vector _{E r} from the point _{C r} to point _{D r,} and a vector _{E t} from the point _{C t} to point _{D t} or less (13), and (14) Obtained by the 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)

_However, the E r · _{E t} represents the inner product of the vector _{E r} and the vector _{E t,} || _{E t} || a || _{E r} || represents the norm of the vector _{E r} and the vector _{E t} (size).

In the position and orientation calculation unit 32, the above processing, the position and orientation of UAV14 calculates the rotation angle theta _r a translation vector T for a predetermined position and orientation.

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

FIG. 10 is a flow chart 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 UAV14 used, there are various data formats for the control data to the UAV14. In the present embodiment, the case where the commercially available product AR Drone 2.0 is taken as an example is shown, 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 a 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 speed V _z along the Z axis, and the rotation speed V around the Z axis set in the aircraft. When the processing 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 the control data.

Next, in step S1202, the rotation angle theta _r and translation vector T, converts the flight command data to be transmitted to UAV14. 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 as T'= (Tx', Ty', Tz') = (-Tx, -Ty, Tz) and is referred to. The rotation angle with respect to the object 12 is given as θ ′ _r = −θ _r .

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

... (16)

... (17)

... (18)

... (19)

Since the coefficients α _x , α _y , α _z , and α _r play the role of gain in feedback control, they may be determined by the user as parameters. For example, α _x = α _y = α _z = α _r = 0.1 is given. ..

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

Depending on the settings of the gain coefficients α _x , α _y , α _z , and α _r , it may not reach the predetermined position and azimuth with one flight command.

Therefore, in step S1206, after the flight command data is transmitted, it is determined whether or not the predetermined conditions are satisfied. The predetermined condition is that, for example, the distance ΔT calculated by the above equation (10) is within the permissible range. If the condition is not satisfied, the process returns to the calculation of the coordinates of the midpoint in step S1102 of the processing flow of the position / orientation calculation unit 32 in FIG. 7, and the difference between the midpoint G _s and the midpoint G _t according to the above equation (9). To get. When the 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 three-dimensional coordinates different from the previous three-dimensional coordinates. Thus, the up (10) the magnitude ΔT of the difference T of the distance calculated by the equation is within the allowable range, to calculate the rotation angle theta _r and translation vector T by the position and orientation calculation unit 32, motion control The flight command data calculated by the equations (16) to (19) is continuously transmitted by the unit 34. If the condition is satisfied, the motion control process is terminated.

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

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, the three-dimensional coordinates of the reference object, the calculated azimuth angle, the reference object 12, and the reference object 12. Based on the relative positional relationship of the 3D coordinates of the UAV14 marker, the difference between the target 3D coordinates and the 3D coordinates of the UAV14 is calculated, and the difference is added to the 3D coordinates of the UAV to obtain the 3D. The rotation angle is calculated based on the coordinates and the target three-dimensional coordinates of the UAV14, and the rotation angle around the X-axis and the rotation angle around the Y-axis in the UAV14 are based on the calculated difference and the rotation angle. , The speed along the Z-axis and the rotation speed around the Z-axis are calculated as flight command data, and the movement of the UAV 14 is controlled based on the calculated flight command data, so that the unmanned airplane can be freely controlled.

<Structure and operation of the flight control device according to the 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). FIG. 11 shows a layout diagram of four points (point A, point B, point C, and point D) at the same height Z _h . In the present embodiment, the distance L _{a (≠} L) between the points A and C, the distance between the points B and D is the L _{b (≠} L).

In the same configuration and operation as in the first embodiment, the processing flow of the orientation detection unit 30 in FIG. 4, the processing flow of the position / 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 this embodiment, since the distance between the points C and D is different from the distance between the points A and B, as shown in FIG. 12, the points A, B, C, and D have an isosceles trapezoid. Form.

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

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

In the third embodiment, the heights of the points C and D are changed to Z _h + N, and the four points (point A, point B, point C, and point D) are square or square on 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 / attitude calculation unit 32 of FIG. 7 and the processing flow of the motion control unit 34 of FIG. 10 in the same configuration and operation as those of the first embodiment. In the following, only the points different from the first embodiment and the second embodiment will be described.

The orientation detection unit 30 of the present embodiment extracts the three-dimensional coordinate data of the points A, B, C, and D acquired by the position measurement sensor 10, and sets the following as the three-dimensional coordinate values of the translation 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)

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

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

... (23)

... (24)

Further, the coordinates G _s of the midpoint between the points C _s and the point D _s that have moved in parallel are calculated by the following equation (25).

... (25)

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

... (26)

When the midpoint is obtained, the parallel transport vector T is calculated by the above equation (9). Meanwhile, since the rotation angle theta _r is to calculate the _X w _{Y w} plane, the (13), and in (14), the calculation of the vector _{E r} and the vector _{E t,} giving a zero component in the Z-value .. Thus, the rotation angle theta _r is calculated in the above (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, the heights of the points A and B become Z _h , and the four points (point A, point B, point C, and point D) are X. to form a square or isosceles trapezoid in _w Y _w plane. When N = 0 is set, it becomes the first embodiment or the second embodiment.

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

A fourth embodiment of the present invention is an example of simultaneously controlling the flight of N UAV14s. FIG. 13 shows a situation in which N UAV14s fly in a formation with UAV14 # 1 at the head. Two markers that can be detected by the position measurement sensor 10 are installed in each UAV 14. It is assumed that points A ₁ and B _{1 of} UAV 14 # 1 correspond to points A and B of the first embodiment, and points A ₂ and B _{2 of} UAV 14 # 2 correspond to points C of the first embodiment. It shall correspond to the point D. With this assignment, as in the first embodiment, points A ₂ and B _{2 of} UAV 14 # 2 form a square in the horizontal plane according to the position and orientation of points A ₁ and B ₁ of UAV 14 # 1. The flight motion of UAV14 # 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 with a _N-1 and the point _{B N-1,} can be four points _{a N-1,} the point _{B N-1, a N,} and the point _{B N} flies to form a square in the horizontal plane ..

In this way, flight control is performed using the N-1st UAV14 as the reference object 12 with respect to the Nth UAV14. In the present embodiment, in the same configuration and operation as in the first embodiment, N UAV14s are used, the processing flow of the position / posture calculation unit 32 in FIG. 7 and the motion control unit in FIG. 10 as in the first embodiment. The flight motion of the UAV 14 is controlled by the processing flow of 34. At this time, the first UAV 14 is controlled so as to follow the reference object 12 carried by the operator as in the first embodiment, and the second and subsequent UAV 14s are N as follows. The Nth UAV 14 is controlled to follow the -1st UAV 14 in order.

The azimuth detection unit 30 of the present embodiment calculates the azimuth angle Φ with respect to the reference direction in the global coordinate system for the reference object 12 as in the first embodiment.

Further, the azimuth detection unit 30 refers to each of the second and subsequent Nth UAV14s with respect to the N-1th UAV14 as the reference object 12 of the Nth UAV14 in the reference direction in the global coordinate system. Calculate the azimuth angle Φ with respect to.

Similar to the first embodiment, the position / orientation calculation unit 32 is based on the three-dimensional coordinates of the markers A and B of the reference object 12, the calculated azimuth angle Φ, and the relative positional relationship. The target three-dimensional coordinates of the first UAV14 marker point C and point D are calculated, and the calculated target three-dimensional coordinates and the first three-dimensional UAV14 marker point C and point D are calculated. The difference T from the coordinates is calculated, and the rotation angle θ is calculated.

Further, the position / orientation calculation unit 32 sets the three-dimensional coordinates of the markers of the N-1st UAV14 and the three-dimensional coordinates of the points B for each of the second and subsequent Nth UAV14s, and the calculated orientation. Relative positional relationship between the angle Φ and the three-dimensional coordinates of the predetermined N-1th UAV14 marker points A and B and the Nth UAV14 marker points C and D. Based on the above, the marker point C of the Nth UAV14 and the target three-dimensional coordinates of the point D are calculated, and the calculated three-dimensional coordinates of the target and the marker point C of the Nth UAV14 and the marker point C of the Nth unit are calculated. The difference T between the point D and the three-dimensional coordinates is calculated. Further, the position / orientation calculation unit 32 has three-dimensional coordinates obtained by adding a difference to the three-dimensional coordinates of the Nth UAV14, the marker point C, and the point D, and the Nth UAV14 marker point C, and The rotation angle θ is calculated based on the three-dimensional coordinates that are the targets of the point D.

The motion control unit 34 gives a flight command to the rotation angle φ around the X axis, the rotation angle ω around the Y axis, the speed V _z along the Z axis, and the rotation speed V _θ around the Z axis in the first UAV 14. 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.

Further, the motion control unit 34 rotates about the X axis in the Nth UAV 14 based on the calculated difference T and the rotation angle θ for each of the second and subsequent Nth UAV 14s. The angle φ, the rotation angle ω around the Y axis, the speed V _z along the Z axis, and the rotation speed V _θ around the Z axis are calculated as flight command data, and the Nth UAV14 is based on the calculated flight command data. Control the movement of. When the processing of the Nth UAV 14 is completed, the same control is performed with N = N + 1.

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

According to the methods 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 azimuth to the UAV. Even for multiple UAVs, the user can issue flight commands to the UAV to indicate the three-dimensional position and orientation, allowing the user to freely operate in space without colliding with each other. To do. Further, by controlling the UAV with the intention of the user, it is possible for the UAV to support or collaborate on work that cannot be realized by the individual action of the person in the actual environment surrounding the person.

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 Reference object 14 UAV
20 Calculation unit 30 Azimuth detection unit 32 Position / attitude calculation unit 34 Motion control unit 50 Output unit 100 Flight control device

Claims (7)

A plurality of markers assigned to a reference object for controlling the flight of a UAV (Unmanned Aerial Vehicle) and having a known distance between markers, and a plurality of markers assigned to the UAV and having a known distance between markers. A position measurement sensor that measures each three-dimensional coordinate with the marker,
An azimuth detection unit that calculates the azimuth angle of the reference object 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.
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, and the plurality of markers of the UAV. Based on the relative positional relationship of each of the three-dimensional coordinates of the UAV, the target three-dimensional coordinates of each of the plurality of markers of the UAV are calculated, and the calculated three-dimensional coordinates of the target and the UAV are described. The three-dimensional coordinates obtained by calculating the difference between the three-dimensional coordinates of each of the plurality of markers and adding the difference to the three-dimensional coordinates of each of the plurality of markers of the UAV, and the plurality of markers of the UAV. A position / orientation calculation unit that calculates the rotation angle based on the three-dimensional coordinates that are the targets of each of the above.
Based on the calculated difference and the rotation angle, the flight command is given to the rotation angle around the X axis, the rotation angle around the Y axis, the speed along the Z axis, and the rotation speed around the Z axis in the UAV. A motion control unit that is calculated as data and controls the motion of the UAV based on the calculated flight command data.
Flight control device including. The plurality of markers assigned to the UAV are two markers.
The difference calculated by the position / 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 calculated three-dimensional coordinates of the two markers of the UAV. The flight control device according to claim 1, which is a difference from the above. Let the UAV be the N UAV, and let the N-1st UAV be the reference object for the Nth UAV.
The azimuth detection unit calculates the azimuth angle of the N-1th UAV as the reference object.
The position / orientation calculation unit includes the three-dimensional coordinates of each of the plurality of markers of the N-1th UAV, the calculated azimuth angle, and the predetermined N-1th UAV. Based on the relative positional relationship between the three-dimensional coordinates of each of the plurality of markers and the three-dimensional coordinates of each of the plurality of markers of the Nth UAV, each of the plurality of markers of the Nth UAV The target three-dimensional coordinates are calculated, 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, and the Nth unit is calculated. 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. To calculate the rotation angle,
Based on the calculated difference and the rotation angle, the motion control unit determines the rotation angle around the X axis, the rotation angle around the Y axis, and the speed along the Z axis in the Nth UAV. The flight control device according to claim 1 or 2, wherein the rotation speed around the Z axis is calculated as flight command data, and the motion 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 the UAV (Unmanned Aerial Vehicle), and a plurality of markers whose distances between the markers are known, and a distance between the markers attached to the UAV. Steps to measure each three-dimensional coordinate with multiple known markers,
A step in which the azimuth detection unit calculates the azimuth angle of the reference object 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.
The position / orientation calculation unit determines 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, and the predetermined orientation angle. Based on 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 of the UAV are calculated, and the calculated three-dimensional target is the target. The three-dimensional coordinates obtained by calculating the difference between the coordinates and the three-dimensional coordinates of each of the plurality of markers of the UAV and adding the difference to the three-dimensional coordinates of each of the plurality of markers of the UAV and the said A step of calculating the rotation angle based on the target three-dimensional coordinates of each of the plurality of markers of the UAV, and
Based on the calculated difference and the rotation angle, the motion control unit performs the rotation angle around the X axis, the rotation angle around the Y axis, the speed along the Z axis, and the rotation angle around the Z axis in the UAV. A step of calculating the rotation speed as flight command data and controlling the motion of the UAV based on the calculated flight command data.
Flight control methods including. The plurality of markers assigned to the UAV are two markers.
In the step calculated by the position / orientation calculation unit, the difference is within the coordinates of the midpoint of the three-dimensional coordinates of the two markers of the UAV and the target three-dimensional coordinates of the calculated two markers of the UAV. The flight control method according to claim 4, wherein the difference from the coordinates of the point is used. Let the UAV be the N UAV, and let the N-1st UAV be the reference object for the Nth UAV.
In the step calculated by the azimuth detection unit, the azimuth angle is calculated for the N-1th UAV as the reference object.
The steps calculated by the position / orientation calculation unit include the three-dimensional coordinates of each of the plurality of markers of the N-1th UAV, the calculated azimuth angle, and the predetermined N-1th unit. Based on the relative positional relationship between the three-dimensional coordinates of each of the plurality of markers of the UAV and the three-dimensional coordinates of each of the plurality of markers of the Nth UAV, the plurality of UAVs of the Nth unit. The target three-dimensional coordinates of each marker are calculated, and the difference between the calculated three-dimensional coordinates of the target and the three-dimensional coordinates of each of the plurality of markers of the Nth UAV is calculated. The 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 target three-dimensional of each of the plurality of markers of the Nth UAV. Calculate the rotation angle based on the coordinates
The steps controlled by the motion control unit are based on the calculated difference and the rotation angle, and the rotation angle around the X axis, the rotation angle around the Y axis, and the Z axis in the Nth UAV are set. The flight according to claim 4 or 5, wherein the speed along the line and the rotation speed around the Z axis are calculated as flight command data, and the movement of the Nth UAV is controlled based on the calculated flight command data. Control method. 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.