WO2019194011A1 - 飛行制御装置、方法、及びプログラム - Google Patents

飛行制御装置、方法、及びプログラム Download PDF

Info

Publication number
WO2019194011A1
WO2019194011A1 PCT/JP2019/012540 JP2019012540W WO2019194011A1 WO 2019194011 A1 WO2019194011 A1 WO 2019194011A1 JP 2019012540 W JP2019012540 W JP 2019012540W WO 2019194011 A1 WO2019194011 A1 WO 2019194011A1
Authority
WO
WIPO (PCT)
Prior art keywords
uav
target point
flight
point
midpoint
Prior art date
Application number
PCT/JP2019/012540
Other languages
English (en)
French (fr)
Inventor
宮川　勲
杵渕　哲也
Original Assignee
日本電信電話株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 日本電信電話株式会社 filed Critical 日本電信電話株式会社
Publication of WO2019194011A1 publication Critical patent/WO2019194011A1/ja

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64C—AEROPLANES; HELICOPTERS
    • B64C13/00—Control systems or transmitting systems for actuating flying-control surfaces, lift-increasing flaps, air brakes, or spoilers
    • B64C13/02—Initiating means
    • B64C13/16—Initiating means actuated automatically, e.g. responsive to gust detectors
    • B64C13/18—Initiating means actuated automatically, e.g. responsive to gust detectors using automatic pilot
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions

Definitions

  • the present invention relates to a flight control device, method, and program, and more particularly, to a flight control device, method, and program for controlling an unmanned airplane.
  • the quad-rotor type UAV (Unmanned Aerial Vehicle) is an unmanned airplane that has four propellers and controls the lift applied to each propeller. Many unmanned airplanes called drones are a kind of quadrotor type UAV. The unmanned airplane that will be handled hereinafter is abbreviated as UAV.
  • a local coordinate system is set on the aircraft as shown in FIG.
  • the UAV forward direction is the X axis
  • the direction perpendicular to the X axis is the Y axis
  • the direction opposite to gravity is the Z axis.
  • a certain global coordinate system is set.
  • the coordinate system is a three-dimensional coordinate system
  • the measurement coordinate system is a global coordinate system.
  • the UAV posture is expressed by the rotation angle of the local coordinate system with respect to the global coordinate system.
  • the rotation around the X axis is roll rotation (rotation angle ⁇ )
  • the rotation around the Y axis is pitch rotation (rotation angle ⁇ )
  • the Z axis is set in order to measure the three-dimensional position of the UAV.
  • the rotation around is called yaw rotation (rotation angle ⁇ ).
  • the flight movement of the UAV generates rotation about the X axis, rotation about the Y axis, and rotation about the Z axis by changing the lift applied to the four propellers.
  • Rotation around the Y axis produces a translational motion in the X axis direction
  • rotation around the X axis produces a translational motion in the Y axis direction.
  • Rotation around the Z-axis produces azimuthal rotation, and when the same lift is applied to the four propellers simultaneously, it produces a translational movement (high elevation) in the Z-axis direction.
  • Non-Patent Document 1 discloses backstepping control based on UAV equations of motion and angular equations of motion.
  • Non-Patent Document 2 discloses a flight motion control method for AR Drone 2.0 (a commercially available low-cost UAV). In many of the conventional technologies, discrete predetermined positions and predetermined directions are given as flight trajectories in the global coordinate system, and the movement is controlled so that the UAV tracks each point and each direction.
  • the tasks for freely controlling UAV flight can be roughly divided into three categories, hovering, trajectory tracking, and path following.
  • Hovering controls flight to stay in a given position and orientation.
  • trajectory tracking control is performed so that tracking is performed in real time along a given route (a discrete three-dimensional position and direction in space).
  • the path follow does not matter in real time, the flight is controlled by giving some constraint condition in the space at the specified three-dimensional position and orientation in the space.
  • Non-Patent Document 1 and Non-Patent Document 2 it is possible to appropriately control the flight motion of the UAV so as to fly according to a predetermined route with respect to the problem of trajectory tracking.
  • these techniques are controls aimed at reaching a target location along a predetermined route, stable flight of UAV is not guaranteed.
  • a constraint condition can be set in the space so as not to fly in a specific place.
  • various sensors and cameras may be attached to the UAV.
  • the ultrasonic sensor can measure the altitude from the ground or the floor, and can measure the speed and posture of the aircraft by using a gyro sensor and an acceleration sensor. Cameras are used to fly autonomously in space while detecting moving objects.
  • the accuracy of sensors built into most UAVs is not guaranteed to be measured with accurate accuracy in millimeters. It is necessary to measure the exact position and orientation of the aircraft using the means.
  • these techniques are also controlled for reaching the target location, it is not guaranteed that the UAV can fly smoothly and stably.
  • the present invention has been made to solve the above-described problems, and provides a flight control device, method, and program capable of controlling an unmanned airplane to freely operate and to smoothly and stably fly.
  • the purpose is to provide.
  • a flight control apparatus measures the three-dimensional coordinates of each of a plurality of markers provided to a UAV (UnmannedmannAerial Vehicle) and the distance between the markers is known. Based on the position measurement sensor, the three-dimensional coordinates of each of the markers measured by the position measurement sensor, and the preset target point of the UAV target location, A position and direction detection unit that calculates a point, a distance from the midpoint to the target point, and an azimuth angle of a line segment connecting each three-dimensional coordinate of the marker with respect to the target point in a global coordinate system; and the target A speed control unit that updates control data for controlling the speed vector of the UAV determined from the middle point and the target point so as to accelerate or decelerate according to the distance to the point; A flight command conversion unit that calculates flight command data in the UAV based on the control data and the calculated azimuth angle, and controls movement of the UAV based on the calculated flight command data; It is comprised including
  • the flight control method includes a step of measuring a three-dimensional coordinate of each of a plurality of markers provided with a position measurement sensor in a UAV (Unmanned Aerial Vehicle) and having a known distance between the markers;
  • the position / orientation detection unit is configured to determine the three-dimensional coordinates of each of the markers based on the three-dimensional coordinates of each of the markers measured by the position measurement sensor and the preset target point of the target location of the UAV.
  • the flight command conversion unit calculates flight command data in the UAV based on the updated control data and the calculated azimuth angle, and the UAV motion based on the calculated flight command data And a step of controlling.
  • the program according to the third invention is a program for causing a computer to function as each part of the flight control device according to the first invention.
  • each of the markers is based on the three-dimensional coordinates of each of the markers measured by the position measurement sensor and the preset target point of the UAV target location. Calculate the azimuth of the line segment connecting the 3D coordinates of each marker with respect to the target point in the global coordinate system by calculating the midpoint of 3D coordinates, the distance from the midpoint to the target point, and the target point in the global coordinate system.
  • the control data for controlling the velocity vector of the UAV determined from the middle point and the target point is updated so as to accelerate or decelerate according to the control point, and based on the updated control data and the calculated azimuth angle
  • the flight control apparatus 100 includes a CPU, a RAM, a ROM for storing a program and various data for executing a flight control processing routine to be described later, Can be configured with a computer including Functionally, the flight control apparatus 100 includes a position measurement sensor 10, a calculation unit 20, and a communication unit 50 as shown in FIG. In this configuration, the position measurement sensor 10 does not necessarily have to be connected as a component, and only needs to acquire data necessary for processing.
  • the position / direction detection unit 30, the speed control unit 32, and the flight command in the calculation unit 20 may be in the form of using a recording medium such as a hard disk, a RAID device, a CD-ROM, or using remote data resources via a network.
  • the position measurement sensor 10 measures three-dimensional coordinates of each of a plurality of markers (points q 1 and q 2 ) that are given to the UAV 14 and have known distances between the markers as measurement data.
  • a motion capture device is used as an example of the position measurement sensor 10.
  • the position of the target point p of the target location 12 of the UAV 14 is arbitrarily set according to the flight plan, and a sensing marker is attached to the position of the points q 1 and q 2 on the UAV 14.
  • the position measurement sensor 10 is set up to measure three-dimensional coordinates in a global coordinate system, and the three-dimensional coordinates of each marker are sequentially measured at a certain interval.
  • the marker point q 1 attached to the UAV 14, the midpoint q of the marker point q 2 , and the target point p of the target location 12 are sufficiently separated.
  • FIG. 3 shows an example of a straight flight with the initial position of the midpoint q between the marker point q 1 and the marker point q 2 attached to the UAV 14 as the A point and the target point p of the target location 12 as the B point.
  • the UAV 14 speed is set to 0 at points A and B. In other words, it stays in the air due to hovering.
  • a graph of speed v and time t is shown. This graph shows the speed change of UAV14.
  • the movement of the UAV 14 is controlled in order to stably reach the B point from the A point in the straight flight shown in FIG.
  • the number of markers is two.
  • the number of markers is not limited to this.
  • the number of markers may be three or more, and the relative positional relationship between objects may be obtained.
  • the calculation unit 20 includes a position / orientation detection unit 30 that detects the position and orientation of the UAV 14 in the global coordinate system by the position measurement sensor 10, a speed control unit 32 that controls the flight speed of the UAV 14 based on the position, and the flight speed and direction. And a flight command conversion unit 34 for sending a command in accordance with the UAV 14 to be used.
  • the specific processing contents of the position / orientation detection unit 30, the speed control unit 32, and the flight command conversion unit 34 will be described in the description of the operation described later.
  • the position / orientation detection unit 30 is based on the three-dimensional coordinates of the marker points q 1 and q 2 of the UAV 14 measured by the position measurement sensor 10 and the target point p of the target location 12 of the UAV 14 set in advance. , Marker point q 1 , and midpoint q of the three-dimensional coordinates of point q 2 , distance
  • the speed control unit 32 updates the control data u for controlling the speed vector of the UAV 14 so as to achieve the speed according to the flight plan shown in FIG. 3 based on the distance to the target point p.
  • the flight command conversion unit 34 Based on the updated control data u and the calculated azimuth angle ⁇ , the flight command conversion unit 34 converts the rotation speed V x around the X axis, the rotation speed V y around the Y axis, and the Z axis in the UAV 14.
  • the speed V z along and the rotation speed V ⁇ around the Z-axis are calculated as flight command data, and the flight command data is transmitted to the UAV 14 via the communication unit 50, so that the UAV 14 can be controlled based on the calculated flight command data. Control movement.
  • step S100 the position / orientation detection unit 30 uses the three-dimensional coordinates of the marker points q 1 and q 2 of the UAV 14 measured by the position measurement sensor 10 and the target of the target location 12 of the UAV 14 set in advance.
  • step S102 the control data for the speed control unit 32 to control the speed vector of the UAV 14 so as to achieve the speed according to the flight plan shown in FIG. 3 based on the distance to the target point p. Update u.
  • step S104 the flight command conversion unit 34, based on the updated control data u and the calculated azimuth angle ⁇ , in the UAV 14, the rotational speed V x around the X axis and the rotational speed V y around the Y axis.
  • the speed V z along the Z axis and the rotational speed V ⁇ around the Z axis are calculated as flight command data, and the flight command data is transmitted to the UAV 14 via the communication unit 50, whereby the calculated flight command data Based on this, the movement of the UAV 14 is controlled.
  • step S100 details of the position / orientation detection processing of the position / orientation detection unit 30 in step S100 will be described.
  • FIG. 5 is a flowchart of the processing of the position / orientation detection unit 30.
  • the position / orientation detection unit 30 sets a target point p of the target location 12 in the global coordinate system in step S1000.
  • the target point p may be any three-dimensional coordinate determined by the operator.
  • step S1002 the three-dimensional coordinates of the marker points q 1 and q 2 of the UAV 14 measured by the position measurement sensor 10 are acquired.
  • the coordinate of the midpoint q is the current position of the UAV 14.
  • step S1004 the calculated distance from the midpoint q point markers q 1 and point q 2 to the target point p.
  • the distance between the current position (midpoint q) of the UAV 14 obtained by the position measurement sensor 10 and the target point p of the target location 12 is obtained from the vector T by calculating
  • represents the norm (size) of the vector.
  • step S1006 the azimuth angle ⁇ of the line segment connecting the point q 1 of the marker with respect to the target point p in the global coordinate system and the midpoint q of the point q 2 by three-dimensional coordinates is calculated.
  • a point q 1 marker, and point q 2 shows the position and orientation relationship between the target point p of the target areas 12 as viewed from the Z w axis.
  • the rotation angle (azimuth angle ⁇ ) is set so that the three-dimensional coordinate line segments of the marker points q 1 and q 2 are perpendicular to the line segment of the target point p. Calculated using the vector dot product relationship.
  • step S1008 it is determined whether to stop the position / orientation detection process. If the process is stopped, the process of the position / orientation detection unit 30 is terminated. If not, the process returns to step S1000 to repeat the process.
  • the case where the process is stopped is a case where the operator ends the flight control of the UAV 14, and the same applies to the following processes.
  • the position / orientation detection unit 30 calculates the distance
  • FIG. 3 shows a straight flight from point A (departure point) to point B (arrival point).
  • the coordinate value of the point B corresponds to the point p in FIG. It is assumed that the point p and the UAV 14 are in a face-to-face relationship, and that the UAV 14 is flying in space by hovering at a point A as an initial state.
  • FIG. 7 is a flowchart of processing of the speed control unit 32.
  • a limiting parameter is input in step S1100.
  • the limiting parameters are the constant-velocity flight speed V 0 shown in FIG. 3, the allowable error ⁇ v for satisfying a constant speed, the distance L 0 from the point to start deceleration to the point B, and the point B is reached. This is an allowable distance ⁇ L for determining whether or not.
  • the allowable error ⁇ v and the allowable distance ⁇ L which are threshold values for the determination process, are set to values close to zero.
  • step S1101 the distance
  • represents the norm (size) of the vector.
  • step S1102 it is determined whether the distance
  • step S1104 control is performed so that the flight is maintained by hovering.
  • step S1106 the velocity vector v of the UAV 14 is calculated.
  • step S1108 the distance
  • ⁇ ⁇ v, the control data is given as u 0.
  • control is performed to accelerate to a constant speed V 0 .
  • Control data u is output and passed to the flight command converter 34.
  • the UAV 14 accelerates to a constant speed V 0, and when it reaches the speed limit, it continues to fly at the constant speed V 0 .
  • the relationship from the position of the UAV 14 to the point B eventually becomes
  • step S1110 the motion is controlled so as to decelerate from the flight at a constant speed in the deceleration control.
  • of the current speed v is obtained.
  • the control data is I ask.
  • step S1112 it is determined whether to stop the speed control process. If the process is stopped, the process of the speed control unit 32 is terminated. If not, the process returns to step S1101 to repeat the process.
  • the speed control unit 32 outputs the control data u according to acceleration, constant speed, and deceleration of the UAV 14 according to the flight plan of FIG.
  • the control data u given by the speed controller 32 and the azimuth ⁇ obtained by the position / orientation detector 30 are converted into flight command flight command data according to the UAV 14 to be used.
  • Command from 50 via wireless There are various data formats for the flight command to the UAV 14, but in the present embodiment, a case where a commercially available product AR Drone 2.0 is taken as an example is shown. However, it goes without saying that this embodiment can also be used when controlling the flight of other UAVs 14.
  • FIG. 8 is a flowchart of processing of the flight command conversion unit 34.
  • the flight command conversion unit 34 receives input of the updated control data u and the calculated azimuth angle ⁇ in step S1200.
  • step S1202 the combination of the control data u and the azimuth angle ⁇ is converted into flight command data for transmission to the UAV 14.
  • the flight command data to the UAV 14 includes the rotation speed V x around the X axis, the rotation speed V y around the Y axis, the speed V z along the Z axis, and the rotation speed V ⁇ around the Z axis set in the aircraft.
  • rotation around the X-axis produces Y-axis translation
  • rotation around the Y-axis produces X-axis translation. Therefore, in this process, flight command data given to UAV14 is expressed as (1) to ( 4) Convert according to equation.
  • step S1204 the flight command data calculated by the equations (1) to (4) is transmitted to the UAV 14 by wireless communication via the communication unit 50.
  • the flight command data calculated by the equations (1) to (4) is sent to the UAV 14.
  • the azimuth angle ⁇ the orientation of the UAV 14 with respect to the target point p is corrected so as to be orthogonal to the vector T (middle point q to point p), as shown in FIG.
  • the marker point q 1 and the marker point q 2 coincide with the points p 1 and p 2 .
  • step S1206 it is determined whether to stop the flight command conversion process. If the process is stopped, the process of the flight command conversion unit 34 is terminated. If not, the process returns to step S1200 to repeat the process.
  • the three-dimensional coordinates of each marker measured by the position measurement sensor and the preset target point of the target location 12 of the UAV 14 are set. Based on this, the midpoint of each 3D coordinate of the marker, the distance from the midpoint to the target point, and the azimuth of the line connecting each 3D coordinate of the marker to the target point in the global coordinate system are calculated.
  • the control data for controlling the velocity vector of the UAV determined from the middle point and the target point is updated so as to accelerate or decelerate according to the distance to the target point, and the updated control data and the calculated azimuth angle Based on the above, the flight command data in the UAV is calculated, and the movement of the UAV is controlled based on the calculated flight command data, so that the unmanned airplane can be operated freely and smoothly.
  • the second embodiment of the present invention is an example of controlling the flight so as to draw various geometric patterns in the space in the configuration of the first embodiment of FIG.
  • FIG. 9 shows an example of the flight path of the UAV 14 based on the geometric pattern.
  • the UAV in the configuration of the first embodiment of FIG. 2, the UAV is controlled by continuously controlling the straight flight from the point A to the point B and the straight flight from the point B to the point A. Flies in a round trip between point A and point B.
  • the straight flight from the point A to the point B the straight flight from the point B to the point C, and the straight flight from the point C to the point A.
  • the UAV 14 flies around a triangular circuit formed by points A, B, and C.
  • the straight flight from the point A to the point B, the straight flight from the point B to the point C, the straight flight from the point C to the point D In addition, the UAV 14 flies through a square circuit formed by the points A, B, C, and D by continuously controlling the straight flight from the point D to the point A.
  • the straight flight from the point A to the point B, the straight flight from the point B to the point C, the straight flight from the point C to the point D By continuously controlling straight flight from D point to E point and straight flight from E point to A point, UAV 14 is formed at A point, B point, C point, D point and E point. Fly like drawing a pentagon. Or, straight flight from point A to point D, straight flight from point D to point B, straight flight from point B to point E, straight flight from point E to point C, and from point C to point A
  • the UAV 14 flies in a pentagonal circle formed by point A, point D, point B, point E, and point C by continuously controlling straight flight.
  • acceleration, constant speed, and deceleration are controlled on the basis of linear flight, and therefore, a geometric pattern drawn by linear flight can fly along a path of any geometric shape.
  • the third embodiment of the present invention is an example in which the flight of N UAVs 14 is simultaneously controlled in the configuration of the first embodiment of FIG.
  • FIG. 10 shows a situation in which N UAVs 14 form a flight starting from UAV 14 # 1.
  • Each UAV 14 is provided with two markers that can be detected by the position measurement sensor 10.
  • UAV14 # midpoint of A 1 and point B 1 point 1 is assumed to correspond to the target point p of the first embodiment
  • a sufficient interval is set as the interval D so that the UAV 14 does not collide.
  • the distance may be individually set to a safe distance so that the UAV 14 does not collide.
  • the point A 2 of the UAV 14 # 2 approaches the midpoint coordinate of the point A 1 of the UAV 14 # 1 and the point D of the point B 1 , that is, the distance D to the target point. position and orientation of the point B 2, and UAV14 # 2 speed is controlled.
  • the points A N and B N of the UAV 14 # N are made to correspond to the points q 1 and q 2 of the first embodiment, and the target point p is the UAV 14 # N-1 that is flying one before.
  • the midpoint between the points A N-1 and B N-1 as the target point p the position, orientation, and speed of the UAV 14 # N are controlled.
  • the movements of a plurality of UAVs 14 can be controlled simultaneously, and the formation flight of N UAVs 14 is possible with UAV 14 # 1 at the head along the straight line.
  • the position / orientation detection unit 30, the speed control unit 32, and the flight command conversion unit 34 of the present embodiment perform the same process for the first UAV 14 as in the first embodiment. Thereafter, the following processing is performed for the second and subsequent UAVs 14.
  • the position / orientation detection unit 30 of this embodiment is based on the three-dimensional coordinates of each of the Nth markers measured by the position measurement sensor 10 and the midpoint of the (N ⁇ 1) th UAV 14 obtained in advance. , The midpoint of the three-dimensional coordinates of each of the Nth UAV14 markers, the distance
  • the speed control unit 32 is set to a speed according to the flight plan shown in FIG.
  • the control data u for the Nth UAV 14 is updated.
  • the flight command conversion unit 34 calculates flight command data in the Nth UAV 14 based on the control data u updated for the Nth UAV 14 and the azimuth angle ⁇ calculated for the Nth UAV 14. The movement of the Nth UAV 14 is controlled based on the calculated flight command data.
  • a plurality of UAVs can be obtained by issuing a flight command indicating a three-dimensional position and direction to each UAV. It is possible to freely operate in the space without colliding each other. Furthermore, by controlling the UAV for the user's intention, it becomes possible for the UAV to support work that cannot be realized by a single action of a person or to perform cooperative work in an actual environment surrounding the person.
  • the fourth embodiment of the present invention is an example of controlling formation flight so as to draw various geometric patterns shown in FIG. 9 in the space in the configuration of the third embodiment. Alternatively, if it is a geometric pattern drawn by straight flight, it is possible to fly in a formation along a path having an arbitrary geometric shape.
  • Position measurement sensor 12 Target location 14
  • UAV 20 arithmetic unit 30 position and direction detection unit 32 speed control unit 34 flight command conversion unit 50 communication unit 100 flight control device

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

無人飛行機を自由自在に操作し、かつ、円滑かつ安定的に飛行させるように制御できる。　位置方位検出部が、位置計測センサによって計測されたマーカの各々の三次元座標と、予め設定されたＵＡＶの目標地の目標点とに基づいて、マーカの各々の三次元座標の中点、中点からの目標点までの距離、及びグローバル座標系における目標点に対するマーカの各々の三次元座標を結んだ線分の方位角を算出する。速度制御部が、目標点までの距離に応じて加速又は減速するように、前記中点及び前記目標点から定まる前記ＵＡＶの速度ベクトルを制御するための制御データを更新する。飛行コマンド変換部が、更新された制御データと、算出された方位角とに基づいて、ＵＡＶにおける、飛行指令データを算出し、算出した飛行指令データに基づいてＵＡＶの運動を制御する。

Description

飛行制御装置、方法、及びプログラム
　本発明は、飛行制御装置、方法、及びプログラムに係り、特に、無人飛行機を制御する飛行制御装置、方法、及びプログラムに関する。
　クアッドロータ型ＵＡＶ（Unmanned Aerial Vehicle）は４つのプロペラを持ち、それぞれのプロペラに与える揚力を制御することにより飛行操縦する無人飛行機である。ドローンと呼ばれる無人飛行機の多くは、クアッドロータ型ＵＡＶの一種である。以降で扱う無人飛行機をＵＡＶと略称する。
　一般的に、ＵＡＶの姿勢を計測するために、図１１に示すように機体にローカル座標系が設定される。ＵＡＶの前進方向をＸ軸、Ｘ軸と垂直な方向をＹ軸、重力とは逆方向をＺ軸とする。
　また、ＵＡＶの三次元位置を計測するため、図１２に示すように、ある基準となるグローバル座標系を設定する。ＧＰＳでは世界座標系とした三次元座標となり、モーションキャプチャシステムではその計測座標系がグローバル座標系となる。ＵＡＶの重心すなわちＵＡＶの位置（ローカル座標系の原点）はグローバル座標系の点Ｐ＝（Ｘ，Ｙ，Ｚ）として表現する。また、ＵＡＶの姿勢はグローバル座標系に対するローカル座標系の回転角で表現し、Ｘ軸周りの回転はロール回転（回転角φ）、Ｙ軸周りの回転はピッチ回転（回転角ω）、Ｚ軸周りの回転はヨー回転（回転角θ）と呼ばれる。ＵＡＶの飛行運動は、４つのプロペラに与える揚力を変化させることにより、Ｘ軸周りの回転、Ｙ軸周りの回転、Ｚ軸周りの回転を発生させる。Ｙ軸周りの回転はＸ軸方向の並進運動を生み出し、Ｘ軸周りの回転はＹ軸方向の並進運動を生み出す。Ｚ軸周りの回転は方位の回転を生み出し、同じ揚力が同時に４つのプロペラに与えられたとき、その強弱によってＺ軸方向の並進運動（高度の昇降）を生み出す。
　グローバル座標系において所定の座標値（Ｘｄ，Ｙｄ，Ｚｄ）と方位θｄが与えられたとき、ＵＡＶの現在位置Ｐ＝（Ｘ，Ｙ，Ｚ）と現在方位から、その所定位置へ飛行して所定方位に機体を向けるためには、ＵＡＶの姿勢と位置を制御する必要がある。非特許文献１には、ＵＡＶの運動方程式と角運動方程式に基づいたバックステッピング制御が公開されている。非特許文献２では、AR Drone 2.0（市販の低価格なＵＡＶ）に関する飛行運動制御法が公開されている。従来技術の多くは、グローバル座標系における飛行の軌跡として離散的な所定位置と所定方位が与えられ、ＵＡＶが各点と各方位を追跡するようにその運動が制御される。
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.
　ＵＡＶの飛行を自由自在に制御する課題は、大まかに３つに大別でき、ホバリング（hovering）、軌跡追跡（trajectory tracking）、パスフォロー（path following）と呼ばれる。ホバリングは、与えられた位置と方位に留まるよう飛行を制御する。軌跡追跡では、与えられた経路（空間中の離散的な三次元位置と方位）に沿ってリアルタイムに追跡するよう制御する。これに対して、パスフォローは、リアルタイム性は問わないが、指定された空間中の三次元位置と方位において、何らかの空間中の拘束条件を与えて飛行を制御する。非特許文献１と非特許文献２によれば、軌跡追跡の課題に対して、所定の経路に従って飛行するように、ＵＡＶの飛行運動を適切に制御することができる。しかしながら、これらの技術は所定の経路に沿って目標地に到達することを目的とした制御であるため、ＵＡＶの安定した飛行が保証されていない。
　これに対して、パスフォローは障害物あるいは人との衝突を避けるため、特定の場所を飛行しないように空間中に拘束条件を設定することができる。また、飛行運動中のトラブルを回避するため、ＵＡＶには多様なセンサやカメラが取り付けられている場合がある。例えば、超音波センサは地面あるいは床からの高度を計測し、ジャイロセンサと加速度センサによって機体の速度と姿勢を計測することができる。また、移動物体を検出しながら、空間中を自律的に飛行させることにカメラが利用されている。これらの機体から計測した空間情報や画像情報を使うことも考えられるが、大半のＵＡＶに内蔵されているセンサの精度はミリ単位での正確な精度で計測できることを保証しておらず、別の手段を使って機体の正確な位置と方位を計測する必要がある。さらに、これらの技術も目標地に到達することを目的とした制御であるため、ＵＡＶを円滑かつ安定して飛行させることは保証されていない。
　本発明は、上記問題点を解決するために成されたものであり、無人飛行機を自由自在に操作し、かつ、円滑かつ安定的に飛行させるように制御できる飛行制御装置、方法、及びプログラムを提供することを目的とする。
　上記目的を達成するために、第１の発明に係る飛行制御装置は、ＵＡＶ（Unmanned Aerial Vehicle）に付与され、かつ、マーカ間の距離が既知の複数のマーカの各々の三次元座標を計測する位置計測センサと、前記位置計測センサによって計測された前記マーカの各々の三次元座標と、予め設定された前記ＵＡＶの目標地の目標点とに基づいて、前記マーカの各々の三次元座標の中点、前記中点からの前記目標点までの距離、及びグローバル座標系における前記目標点に対する前記マーカの各々の三次元座標を結んだ線分の方位角を算出する位置方位検出部と、前記目標点までの距離に応じて加速又は減速するように、前記中点及び前記目標点から定まる前記ＵＡＶの速度ベクトルを制御するための制御データを更新する速度制御部と、更新された前記制御データと、算出された前記方位角とに基づいて、前記ＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記ＵＡＶの運動を制御する飛行コマンド変換部と、を含んで構成されている。
　第２の発明に係る飛行制御方法は、位置計測センサが、ＵＡＶ（Unmanned Aerial Vehicle）に付与され、かつ、マーカ間の距離が既知の複数のマーカの各々の三次元座標を計測するステップと、位置方位検出部が、前記位置計測センサによって計測された前記マーカの各々の三次元座標と、予め設定された前記ＵＡＶの目標地の目標点とに基づいて、前記マーカの各々の三次元座標の中点、前記中点からの前記目標点までの距離、及びグローバル座標系における前記目標点に対する前記マーカの各々の三次元座標を結んだ線分の方位角を算出するステップと、速度制御部が、前記目標点までの距離に応じて加速又は減速するように、前記中点及び前記目標点から定まる前記ＵＡＶの速度ベクトルを制御するための制御データを更新するステップと、飛行コマンド変換部が、更新された前記制御データと、算出された前記方位角とに基づいて、前記ＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記ＵＡＶの運動を制御するステップと、を含んで実行することを特徴とする。
　第３の発明に係るプログラムは、コンピュータを、第１の発明に係る飛行制御装置の各部として機能させるためのプログラムである。
　本発明の飛行制御装置、方法、及びプログラムによれば、位置計測センサによって計測されたマーカの各々の三次元座標と、予め設定されたＵＡＶの目標地の目標点とに基づいて、マーカの各々の三次元座標の中点、中点からの目標点までの距離、及びグローバル座標系における目標点に対するマーカの各々の三次元座標を結んだ線分の方位角を算出し、目標点までの距離に応じて加速又は減速するように、前記中点及び前記目標点から定まる前記ＵＡＶの速度ベクトルを制御するための制御データを更新し、更新された制御データと、算出された方位角とに基づいて、ＵＡＶにおける、飛行指令データを算出し、算出した飛行指令データに基づいてＵＡＶの運動を制御することにより、無人飛行機を自由自在に操作し、かつ、円滑かつ安定的に飛行させるように制御できる、という効果が得られる。
本発明の実施形態に係る飛行制御装置の構成を示すブロック図である。 ＵＡＶの飛行を制御する状況の一例を示す図である。 直線飛行におけるＵＡＶの速度変化を示す図である。 本発明の実施形態に係る飛行制御装置における飛行制御処理ルーチンを示すフローチャートである。 飛行制御処理ルーチンにおける位置方位検出処理のフローチャートである。 点ｑ１、及び点ｑ２、並びに、目標点ｐをＺｗ軸方向から視た位置と方位の関係を示す図である。 飛行制御処理ルーチンにおける速度制御処理のフローチャートである。 飛行コマンド変換処理ルーチンにおける飛行コマンド変換処理のフローチャートである。 幾何パターンに基づいたＵＡＶの飛行経路の例を示す図である。 複数のＵＡＶの空間配置の一例を示す図である。 ＵＡＶの概観とＵＡＶ固定のローカル座標系の一例を示す図である。 グローバル座標系とＵＡＶ固定のローカル座標系の関係の一例を示す図である。
　以下、図面を参照して本発明の実施形態を詳細に説明する。本発明の実施形態に係る手法は、ＵＡＶの飛行を自由自在に操作し、かつ、円滑かつ安定的にＵＡＶを飛行させる、という課題を解決するものである。
＜本発明の第１の実施形態に係る飛行制御装置の構成＞
　本発明の第１の実施形態に係るクアッドロータ型ＵＡＶの飛行制御装置の構成について説明する。本実施形態は１台のＵＡＶの飛行速度を制御する形態である。図１に示すように、本発明の第１の実施形態に係る飛行制御装置１００は、ＣＰＵと、ＲＡＭと、後述する飛行制御処理ルーチンを実行するためのプログラムや各種データを記憶したＲＯＭと、を含むコンピュータで構成することが出来る。この飛行制御装置１００は、機能的には図１に示すように位置計測センサ１０と、演算部２０と、通信部５０とを備えている。この構成において、位置計測センサ１０は必ずしも構成要素として接続している必要はなく、処理に必要なデータを取得すればよく、演算部２０における位置方位検出部３０、速度制御部３２、及び飛行コマンド変換部３４からそれぞれの矢印へのデータの流れは、ハードディスク、ＲＡＩＤ装置、ＣＤ－ＲＯＭなどの記録媒体を利用する、または、ネットワークを介してリモートなデータ資源を利用する形態でもどちらでも構わない。
　位置計測センサ１０は、ＵＡＶ１４に付与され、かつ、マーカ間の距離が既知の複数のマーカ（点ｑ１、及び点ｑ２とする）の各々の三次元座標を計測データとして計測する。本実施形態では、位置計測センサ１０の例として、モーションキャプチャ装置を利用する。ＵＡＶ１４の目標地１２の目標点ｐの位置は飛行計画に応じて任意に設定し、ＵＡＶ１４には点ｑ１、及び点ｑ２の位置にセンシング用のマーカが取り付けられている。図２に示す状況において、位置計測センサ１０はグローバル座標系において３次元座標を計測するようにセットアップされており、各マーカの３次元座標が、ある一定間隔で逐次計測される。初期状態において、ＵＡＶ１４に取り付けたマーカ点ｑ1とマーカ点ｑ2の中点ｑと目標地１２の目標点ｐが十分に離れているとする。
　図３は、ＵＡＶ１４に取り付けたマーカ点ｑ1とマーカ点ｑ2の中点ｑの初期位置をＡ地点、目標地１２の目標点ｐをＢ地点とした直線飛行の例を示す。Ａ地点とＢ地点ではＵＡＶ１４の速度を0とする。すなわち、ホバリングにて空中に停滞する。図３の下には、速度ｖと時刻ｔのグラフが示されている。このグラフはＵＡＶ１４の速度変化を示す。初期状態ではホバリングにて飛行を行っており、時刻ｔ1において加速し始める（その途中の速度をｖ=Ｖaとする）。速度がＶ0に達した時点、すなわち、時刻ｔ2で、その速度を維持して飛行する。さらに、Ｘ地点を通過した時点すなわち時刻ｔ3で減速を始める（その途中の速度をｖ=Ｖbとする）。このとき、Ｘ地点からＢ点までの距離をＬ0とする。やがて、時刻ｔ4において速度0になりホバリング状態となる。
　本実施形態では、図３に示した直線飛行においてＡ地点からＢ地点に安定的に到達するためにＵＡＶ１４の運動を制御する。なお、本実施形態では、マーカの数は２つとしたが、これに限定されるものではなく、３つ以上として、物体間の相対的な位置関係を求めるようにしてもよい。
　演算部２０は、位置計測センサ１０によりグローバル座標系におけるＵＡＶ１４の位置と方位を検出する位置方位検出部３０と、位置に基づき、ＵＡＶ１４の飛行速度を制御する速度制御部３２と、飛行速度と方位に基づき、使用するＵＡＶ１４に応じて指令を送出するための飛行コマンド変換部３４とを含んで構成されている。なお、位置方位検出部３０、速度制御部３２、及び飛行コマンド変換部３４の具体的な処理内容は後述の作用の説明において説明する。
　位置方位検出部３０は、位置計測センサ１０によって計測されたＵＡＶ１４のマーカの点ｑ１、及び点ｑ２の三次元座標と、予め設定されたＵＡＶ１４の目標地１２の目標点ｐとに基づいて、マーカの点ｑ１、及び点ｑ２の三次元座標の中点ｑ、中点ｑからの目標点ｐまでのベクトルＴから求まる距離||Ｔ||、及びグローバル座標系における目標点ｐに対するマーカの点ｑ１、及び点ｑ２の中点ｑを三次元座標で結んだ線分の方位角θを算出する。
　速度制御部３２は、目標点ｐまでの距離に基づいて、上記図３に示す飛行計画に従った速度となるように、ＵＡＶ１４の速度ベクトルを制御するための制御データｕを更新する。
　飛行コマンド変換部３４は、更新された制御データｕと、算出された方位角θとに基づいて、ＵＡＶ１４における、Ｘ軸周りの回転速度Ｖx、Ｙ軸周りの回転速度Ｖy、Ｚ軸に沿った速度Ｖｚ、及びＺ軸周りの回転速度Ｖθを飛行指令データとして算出し、飛行指令データを通信部５０を介してＵＡＶ１４に送信することで、算出した飛行指令データに基づいてＵＡＶ１４の運動を制御する。
＜本発明の第１の実施形態に係る飛行制御装置の作用＞
　次に、本発明の第１の実施形態に係る飛行制御装置１００の作用について説明する。ＵＡＶ１４のマーカの点ｑ１、及び点ｑ２の三次元座標の計測を開始すると、飛行制御装置１００は、図４に示す飛行制御処理ルーチンを実行する。
　まず、ステップＳ１００では、位置方位検出部３０が、位置計測センサ１０によって計測されたＵＡＶ１４のマーカの点ｑ１、及び点ｑ２の三次元座標と、予め設定されたＵＡＶ１４の目標地１２の目標点ｐとに基づいて、マーカの点ｑ１、及び点ｑ２の三次元座標の中点ｑ、中点ｑからの目標点ｐまでのベクトルＴから求まる距離||Ｔ||、及びグローバル座標系における目標点ｐに対するマーカの点ｑ１、及び点ｑ２の中点ｑを三次元座標で結んだ線分の方位角θを算出する。
　次に、ステップＳ１０２では、速度制御部３２が、目標点ｐまでの距離に基づいて、上記図３に示す飛行計画に従った速度となるように、ＵＡＶ１４の速度ベクトルを制御するための制御データｕを更新する。
　ステップＳ１０４では、飛行コマンド変換部３４が、更新された制御データｕと、算出された方位角θとに基づいて、ＵＡＶ１４における、Ｘ軸周りの回転速度Ｖx、Ｙ軸周りの回転速度Ｖy、Ｚ軸に沿った速度Ｖｚ、及びＺ軸周りの回転速度Ｖθを飛行指令データとして算出し、飛行指令データを通信部５０を介してＵＡＶ１４に送信することで、算出した飛行指令データに基づいてＵＡＶ１４の運動を制御する。
　次に、ステップＳ１００の位置方位検出部３０の位置方位検出処理の詳細について説明する。
　図５は位置方位検出部３０の処理のフロー図である。位置方位検出部３０は処理を開始すると、ステップＳ１０００で、グローバル座標系における目標地１２の目標点ｐを設定する。目標点ｐは操作者が決定した任意の三次元座標でよい。
　ステップＳ１００２で、位置計測センサ１０によって計測されたＵＡＶ１４のマーカの点ｑ１、及び点ｑ２の三次元座標を取得する。
　ステップＳ１００３で、位置計測センサ１０によって計測されたＵＡＶ１４のマーカの点ｑ１、及び点ｑ２の三次元座標に基づいて、中点ｑの座標を、ｑ＝(ｑ１＋ｑ２)/２により計算する。以下では、中点ｑの座標を、ＵＡＶ１４の現在位置とする。
　ステップＳ１００４で、マーカの点ｑ１及び点ｑ２の中点ｑから目標点ｐまでの距離を算出する。位置計測センサ１０で得た現在のＵＡＶ１４の位置（中点ｑ）と目標地１２の目標点ｐ間の距離を、ベクトルＴから||Ｔ||＝||ｐ－ｑ||の計算により求める。||・||はベクトルのノルム（大きさ）を表す。
　ステップＳ１００６で、グローバル座標系における目標点ｐに対するマーカの点ｑ１、及び点ｑ２の中点ｑを三次元座標で結んだ線分の方位角θを算出する。図６に、マーカの点ｑ１、及び点ｑ２、並びに、目標地１２の目標点ｐをＺｗ軸方向から視た位置と方位の関係を示す。位置方位検出部３０では、マーカの点ｑ１及び点ｑ２の三次元座標の線分が、目標点ｐの線分に対して直交する線分となるように回転角（方位角θ）をベクトル内積の関係を用いて算出する。
　ステップＳ１００８では、位置方位検出の処理を停止するかを判定し、処理を停止する場合には位置方位検出部３０の処理を終了し、終了しない場合にはステップＳ１０００に戻って処理を繰り返す。なお、処理を停止する場合とは、ここでは操作者がＵＡＶ１４の飛行制御を終了する場合とし、以下の処理においても同様である。
　位置方位検出部３０では、以上の処理により、中点ｑから目標点ｐまでの距離||Ｔ||、及び方位角θを算出する。
　次に、ステップＳ１０２の速度制御部３２の速度制御処理の詳細について説明する。以下では、図３で示した直線飛行を例にして詳細な処理を説明する。図３には、Ａ地点（出発地点）からＢ地点（到着地点）までの直線飛行が示されている。Ｂ地点の座標値は、図２の点ｐに対応する。点ｐとＵＡＶ１４が対面の関係にあるとし、ＵＡＶ１４の初期状態としてＡ地点においてホバリングにより空間中を飛行しているとする。
　図７は速度制御部３２の処理のフロー図である。速度制御部３２は処理を開始すると、ステップＳ１１００で、制限パラメータが入力される。制限パラメータとは、図３に示す等速飛行の速度Ｖ0、一定速度を満たすための許容誤差Δｖ、減速を開始するための地点からＢ地点までの距離Ｌ0、並びに、Ｂ地点に到達したかどうかを判定するための許容距離ΔＬである。本実施形態では、判定処理の閾値である許容誤差Δｖと許容距離ΔＬをほぼ0に近い値に設定する。
　そして、ステップＳ１１０１で、中点ｑから目標点ｐまでの距離||Ｔ||を取得する。具体的には、位置方位検出部３０において、ＵＡＶ１４の位置（点ｑ）が位置計測センサ１０を使って時系列に得られているので、現在時点でのＵＡＶ１４の位置とΔｔ秒前（1つ前のステップ）に計測したＵＡＶ１４の位置を取り出し、現在位置からＢ地点までの距離||Ｔ||を算出する。||・||はベクトルのノルム（大きさ）を表す。
　ステップＳ１１０２で、距離||Ｔ||が許容距離ΔＬ以下（||Ｔ||≦ΔＬ）であるか否かを判定する。||Ｔ||≦ΔＬであればステップＳ１１０４へ移行する。||Ｔ||≦ΔＬでなく、||Ｔ||＞ΔＬであれば運動を制御するためステップＳ１１０６へ移行する。
　ステップＳ１１０４で、ホバリングにより飛行を維持するように制御する。
　ステップＳ１１０６で、ＵＡＶ１４の速度ベクトルｖを算出する。本実施形態では、位置計測センサ１０によって点ｑ１から点ｑ２の三次元位置が入力として与えられるため、逐次、中点ｑが入力として与えられる。中点ｑを用いて、時間間隔Δｔの間に中点ｑがベクトルｍだけ移動したときの速度ベクトルｖ＝ｍ/Δｔを算出する。初期状態のＵＡＶ１４については、ホバリング状態から開始するので速度はｖ=0であり、B地点までの距離||Ｔ||はＬ0より十分に大きいとする。
　ステップＳ１１０８で、現在位置からＢ地点までの距離||Ｔ||が、Ｌ0以下であるか否かを判定する。現在位置からＢ地点までの距離||Ｔ||が、Ｌ0より大きい場合には、ステップＳ１１０９へ移行し、加速制御において上記図３の飛行計画に従い、ホバリング状態のｖ=0から一定速度Ｖ0まで加速させる。具体的には、加速制御では、現在の速度ｖと制限速度Ｖ0間の絶対差分||Ｖ0－ｖ||を求める。このとき、||Ｖ0－ｖ||＞Δｖのとき、制御データを
Figure JPOXMLDOC01-appb-I000001

 
と求める。もし、||Ｖ0－ｖ||≦Δｖならば、制御データをｕ=0と与える。
Figure JPOXMLDOC01-appb-I000002

 
はフィードバック制御におけるゲインパラメータであり、ユーザが状況に応じて設定する。このフィードバックにより、ｖ＜Ｖ0のとき、一定速度Ｖ0まで加速するように制御する。制御データｕが出力されて、飛行コマンド変換部３４に渡される。この反復を続けることにより、ＵＡＶ１４は一定速度Ｖ0まで加速し、その制限速度に達したとき、等速Ｖ0で飛行を続けることになる。一定速度で飛行を続けることにより、やがてＵＡＶ１４の位置からＢ地点までの関係は||Ｔ||≦Ｌ0となる。すなわち、ＵＡＶ１４の現在位置から目標点ｐまでの距離がＬ0以下になる。このとき、ステップＳ１１１０へ移行し、減速制御において一定速度の飛行から減速するように運動を制御する。減速制御では、現在の速度ｖの絶対差分||ｖ||を求める。このとき、||ｖ||＞Δｖのとき、制御データを
Figure JPOXMLDOC01-appb-I000003

 
と求める。||ｖ||≦Δｖならば、制御データｕ=0と与える。λはフィードバック制御におけるゲインパラメータであり、ユーザが状況に応じて設定する。例えば、
Figure JPOXMLDOC01-appb-I000004

 
と与えてもよいが、Ｂ地点でホバリング状態となるように、加速時のゲインパラメータ値よりやや大きめの値を設定してもよい。制御データｕが出力されて、飛行コマンド変換部３４に渡される。このフィードバックを反復することにより、ｖ＞0のとき、ｖ＝０になるまでＵＡＶ１４を減速する。
　ステップＳ１１１２では、速度制御の処理を停止するかを判定し、処理を停止する場合には速度制御部３２の処理を終了し、終了しない場合にはステップＳ１１０１に戻って処理を繰り返す。
　以上の処理によって、速度制御部３２は、上記図３の飛行計画に従い、ＵＡＶ１４を加速、等速、減速に応じて制御データｕを出力する。
　次に、ステップＳ１０４の飛行コマンド変換部３４の飛行コマンド変換処理の詳細について説明する。
　飛行コマンド変換部３４の処理により、速度制御部３２より与えられる制御データｕと位置方位検出部３０で得た方位θを、使用するＵＡＶ１４に応じて飛行コマンドの飛行指令データへ変換して通信部５０から無線経由で指令する。ＵＡＶ１４への飛行コマンドには様々なデータ形式が存在するが、本実施形態では、市販製品のAR Drone 2.0を例にした場合を示す。ただし、それ以外のＵＡＶ１４の飛行を制御する場合にも、本実施形態を利用できることは言うまでもない。
　図８は飛行コマンド変換部３４の処理のフロー図である。飛行コマンド変換部３４は処理を開始すると、ステップＳ１２００で、更新された制御データｕと、算出された方位角θとの入力を受け付ける。
　ステップＳ１２０２で、制御データｕと方位角θとの組み合わせを、ＵＡＶ１４へ送信するための飛行指令データへ変換する。ＵＡＶ１４への飛行指令データは、機体に設定されたＸ軸周りの回転速度Ｖｘ、Ｙ軸周りの回転速度Ｖｙ、Ｚ軸に沿った速度Ｖｚと、Ｚ軸周りの回転速度Ｖθになる。制御データｕがｕ＝(ｕｘ,ｕｙ,ｕｚ)と与えられるとする。ＵＡＶ１４では、Ｘ軸周りの回転がＹ軸の並進運動を生み出し、Ｙ軸周りの回転がＸ軸の並進運動を生み出するため、本処理では、ＵＡＶ１４に与える飛行指令データを以下（１）～（４）式により変換する。
Figure JPOXMLDOC01-appb-M000005
 
　係数αｘ、αｙ、αｚ、αθはAR Drone 2.0が扱うことができる変換係数であり、パラメータとしてユーザが決めてよく、例えば、αｘ＝αｙ＝αｚ＝αθ＝０．１と与える。
　ステップＳ１２０４で、通信部５０を介して無線通信により（１）～（４）式で算出した飛行指令データをＵＡＶ１４へ送信する。
　本処理は、速度制御部３２で得た制御データｕと位置方位検出部３０で得た方位角θが与えられるたびに、式(1)～式(4)で算出した飛行指令データをＵＡＶ１４へ送り続ける。なお、方位角θを送出することにより、目標地の点ｐに対するＵＡＶ１４の向きは、図６で示したように、ベクトルＴ（中点ｑから点ｐ）と直交するように補正される。正確に直交するときは、マーカ点ｑ1とマーカ点ｑ2は点ｐ1と点ｐ2に一致する。
　ステップＳ１２０６で、飛行コマンド変換の処理を停止するかを判定し、処理を停止する場合には飛行コマンド変換部３４の処理を終了し、終了しない場合にはステップＳ１２００に戻って処理を繰り返す。
　以上により、本実施形態は、位置計測センサ１０によってＵＡＶ１４の位置と方位を算出し、目標地に向かって上記図３に示す飛行計画に従った速度で飛行するように、ＵＡＶ１４の飛行運動を制御することができる。さらに、目標地では速度ｖ=0でホバリングすることを拘束条件として制御するため、経路を逸脱することなく、飛行経路に沿って忠実に制御するため、安定的かつ正確にＵＡＶ１４の飛行ナビゲーションを実現することができる。
　以上説明したように、第１の実施形態に係る飛行制御装置によれば、位置計測センサによって計測されたマーカの各々の三次元座標と、予め設定されたＵＡＶ１４の目標地１２の目標点とに基づいて、マーカの各々の三次元座標の中点、中点からの目標点までの距離、及びグローバル座標系における目標点に対するマーカの各々の三次元座標を結んだ線分の方位角を算出し、目標点までの距離に応じて加速又は減速するように、中点及び目標点から定まるＵＡＶの速度ベクトルを制御するための制御データを更新し、更新された制御データと、算出された方位角とに基づいて、ＵＡＶにおける、飛行指令データを算出し、算出した飛行指令データに基づいてＵＡＶの運動を制御することにより、無人飛行機を自由自在に操作し、かつ、円滑かつ安定的に飛行させるように制御できる。
＜本発明の第２の実施形態に係る飛行制御装置の構成及び作用＞
　本発明の第２の実施形態は、上記図２の第１の実施形態の構成において、空間中において多様な幾何パターンを描くように飛行を制御する例である。図９に幾何パターンに基づいたＵＡＶ１４の飛行経路の例を示す。図９（Ａ）では、上記図２の第１の実施形態の構成において、Ａ地点からＢ地点への直線飛行とＢ地点からＡ地点への直線飛行を連続して飛行制御することにより、ＵＡＶはＡ地点とＢ地点間の往復路を飛行する。
　図９（Ｂ）では、上記図２の第１の実施形態の構成において、Ａ地点からＢ地点への直線飛行、Ｂ地点からＣ地点への直線飛行、並びにＣ地点からＡ地点への直線飛行を連続して飛行制御することにより、ＵＡＶ１４はＡ地点、Ｂ地点、Ｃ地点で形成される三角形状の周回を飛行する。
　図９（Ｃ）では、上記図２の第１の実施形態の構成において、Ａ地点からＢ地点への直線飛行、Ｂ地点からＣ地点への直線飛行、Ｃ地点からＤ地点への直線飛行、並びにＤ地点からＡ地点への直線飛行を連続して飛行制御することにより、ＵＡＶ１４はＡ地点、Ｂ地点、Ｃ地点、Ｄ地点で形成される四角形状の周回を飛行する。
　図９（Ｄ）では、上記図２の第１の実施形態の構成において、Ａ地点からＢ地点への直線飛行、Ｂ地点からＣ地点への直線飛行、Ｃ地点からＤ地点への直線飛行、Ｄ地点からＥ地点への直線飛行、並びにＥ地点からＡ地点への直線飛行を連続して飛行制御することにより、ＵＡＶ１４はＡ地点、Ｂ地点、Ｃ地点、Ｄ地点、Ｅ地点で形成される五角形状を描くように飛行する。あるいは、Ａ地点からＤ地点への直線飛行、Ｄ地点からＢ地点への直線飛行、Ｂ地点からＥ地点への直線飛行、Ｅ地点からＣ地点への直線飛行、並びにＣ地点からＡ地点への直線飛行を連続して飛行制御することにより、ＵＡＶ１４はＡ地点、Ｄ地点、Ｂ地点、Ｅ地点、Ｃ地点で形成される五角形状の周回を飛行する。第１の実施形態は直線飛行を基本にして加速、等速、減速の制御が行われるため、直線飛行で描く幾何パターンであれば任意の幾何形状の経路に沿って飛行することができる。
＜本発明の第３の実施形態に係る飛行制御装置の構成及び作用＞
　本発明の第３の実施形態は、上記図２の第１の実施形態の構成において、Ｎ台のＵＡＶ１４の飛行を同時に制御する例である。図１０に、ＵＡＶ１４＃１を先頭にしてＮ台のＵＡＶ１４が編成を組んで飛行する状況を示す。それぞれのＵＡＶ１４には位置計測センサ１０で検出可能な２つのマーカが設置されている。ＵＡＶ１４＃１の点Ａ１と点Ｂ１の中点が第１の実施形態の目標点ｐに対応するものとし、ＵＡＶ１４＃２の点Ａ２と点Ｂ２の中点が第１の実施形態の点ｑ１と点ｑ２に対応するものとする。以下、Ｎ台目のＵＡＶ１４の中点をｑ、Ｎ－１台目のＵＡＶ１４の目標点をｐとして説明する。各ＵＡＶ１４が互いに衝突しないように、第１の実施形態と同様に速度制御部３２において、ΔＬ＝Ｄと与えて、ＵＡＶ１４間の距離をＤとするようにＵＡＶ１４＃２の運動を制御する。ただし、間隔ＤとしてはＵＡＶ１４が衝突しない程度に十分な間隔が設定される。本実施形態では、等間隔Ｄを設定した場合を例に説明するが、ＵＡＶ１４が衝突しない程度の安全な距離に個別に設定してもよい。
　この割り当てにより、第１の実施形態１と同様に、ＵＡＶ１４＃１の点Ａ１と点Ｂ１の中点座標、すなわち目標点までの距離Ｄに近づくように、ＵＡＶ１４＃２の点Ａ２と点Ｂ２の位置と方位、並びにＵＡＶ１４＃２の速度が制御される。
　同様にして、ＵＡＶ１４＃Ｎの点ＡＮと点ＢＮを第１の実施形態の点ｑ１と点ｑ２に対応させ、目標点ｐについては、１つ前を飛行するＵＡＶ１４＃Ｎ－１の点ＡＮ－１と点ＢＮ－１の中点を目標点ｐとすることにより、ＵＡＶ１４＃Ｎの位置、方位、並びに速度を制御する。
　本実施形態において、複数のＵＡＶ１４の運動を同時に制御することができ、直線に沿ってＵＡＶ１４＃１を先頭にしてＮ台のＵＡＶ１４の編隊飛行を可能とする。
　本実施形態の位置方位検出部３０、速度制御部３２、及び飛行コマンド変換部３４は１台目のＵＡＶ１４について上記第１の実施形態と同様に処理を行う。以下、２台目以降のＵＡＶ１４について以下の処理を行う。
　本実施形態の位置方位検出部３０は、位置計測センサ１０によって計測されたＮ台目のマーカの各々の三次元座標と、予め求められたＮ－１台目のＵＡＶ１４の中点とに基づいて、Ｎ台目のＵＡＶ１４のマーカの各々の三次元座標の中点、中点からＮ－１台目のＵＡＶ１４の中点である目標点ｐまでの距離||Ｔ||、及びグローバル座標系におけるＮ－１台目のＵＡＶ１４の中点である目標点ｐに対するＮ台目のＵＡＶ１４のマーカの各々の三次元座標を結んだ線分の方位角θを算出する。
　速度制御部３２は、Ｎ台目のＵＡＶ１４の中点ｑに対するＮ－１台目のＵＡＶ１４の目標点ｐまでの距離に基づいて、上記図３に示す飛行計画に従った速度となるように、Ｎ台目のＵＡＶ１４に対する制御データｕを更新する。
　飛行コマンド変換部３４は、Ｎ台目のＵＡＶ１４について更新された制御データｕと、Ｎ台目のＵＡＶ１４について算出された方位角θとに基づいて、Ｎ台目のＵＡＶ１４における、飛行指令データを算出し、算出した飛行指令データに基づいてＮ台目のＵＡＶ１４の運動を制御する。
　以上説明したように、第３の実施形態に係る飛行制御装置によれば、複数のＵＡＶに対しても、各ＵＡＶに対して三次元位置と方位を示す飛行指令を出すことにより、複数のＵＡＶ同士を衝突させることなく、空間中を自由自在に操作することが可能となる。さらに、ユーザの意図でＵＡＶを制御することにより、人を取り巻く実環境において、人の単独行動では実現不可能な作業をＵＡＶが支援する、あるいは協調作業することが可能となる。
＜本発明の第４の実施形態に係る飛行制御装置の構成及び作用＞
　本発明の第４の実施形態は、上記第３の実施形態の構成において、空間中において上記図９に示す多様な幾何パターンを描くように編隊飛行を制御する例である。あるいは、直線飛行で描く幾何パターンであれば、任意の幾何形状の経路に沿って編隊飛行することができる。
　なお、本発明は、上述した実施形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。
１０ 位置計測センサ
１２ 目標地
１４ ＵＡＶ
２０ 演算部
３０ 位置方位検出部
３２ 速度制御部
３４ 飛行コマンド変換部
５０ 通信部
１００ 飛行制御装置

Claims (7)

  1. 　ＵＡＶ（Unmanned Aerial Vehicle）に付与され、かつ、マーカ間の距離が既知の複数のマーカの各々の三次元座標を計測する位置計測センサと、
    　前記位置計測センサによって計測された前記マーカの各々の三次元座標と、予め設定された前記ＵＡＶの目標地の目標点とに基づいて、前記マーカの各々の三次元座標の中点、前記中点からの前記目標点までの距離、及びグローバル座標系における前記目標点に対する前記マーカの各々の三次元座標を結んだ線分の方位角を算出する位置方位検出部と、
    　前記目標点までの距離に応じて加速又は減速するように、前記中点及び前記目標点から定まる前記ＵＡＶの速度ベクトルを制御するための制御データを更新する速度制御部と、
    　更新された前記制御データと、算出された前記方位角とに基づいて、前記ＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記ＵＡＶの運動を制御する飛行コマンド変換部と、
    　を含む飛行制御装置。
  2. 　前記速度制御部は、前記目標点までの距離が、予め定められた距離以下である場合には、前記速度ベクトルを減速させた任意の速度ベクトルとして、前記制御データを更新し、
    　前記目標点までの距離が、前記予め定められた距離より長い場合には、前記速度ベクトルを加速させた、または一定の速度とした任意の速度ベクトルとして、前記制御データを更新する請求項１に記載の飛行制御装置。
  3. 　前記ＵＡＶをＮ台のＵＡＶとし、Ｎ－１台目のＵＡＶの中点をＮ台目のＵＡＶの前記目標点とし、
    　前記位置方位検出部は、前記位置計測センサによって計測された前記Ｎ台目の前記マーカの各々の三次元座標と、予め求められた前記Ｎ－１台目の前記ＵＡＶの中点とに基づいて、前記Ｎ台目の前記ＵＡＶの前記マーカの各々の三次元座標の中点、前記中点からＮ－１台目のＵＡＶの中点である前記目標点までの距離、及びグローバル座標系におけるＮ－１台目のＵＡＶの中点である前記目標点に対する前記Ｎ台目の前記ＵＡＶの前記マーカの各々の三次元座標を結んだ線分の方位角を算出し、
    　前記速度制御部は、前記Ｎ台目の前記ＵＡＶの前記中点に対する前記目標点までの距離に基づいて、前記Ｎ台目の前記ＵＡＶに対する前記制御データを更新し、
    　前記飛行コマンド変換部は、前記Ｎ台目のＵＡＶについて更新された前記制御データと、前記Ｎ台目のＵＡＶについて算出された前記方位角とに基づいて、前記Ｎ台目の前記ＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記Ｎ台目の前記ＵＡＶの運動を制御する請求項１又は請求項２に記載の飛行制御装置。
  4. 　位置計測センサが、ＵＡＶ（Unmanned Aerial Vehicle）に付与され、かつ、マーカ間の距離が既知の複数のマーカの各々の三次元座標を計測するステップと、
    　位置方位検出部が、前記位置計測センサによって計測された前記マーカの各々の三次元座標と、予め設定された前記ＵＡＶの目標地の目標点とに基づいて、前記マーカの各々の三次元座標の中点、前記中点からの前記目標点までの距離、及びグローバル座標系における前記目標点に対する前記マーカの各々の三次元座標を結んだ線分の方位角を算出するステップと、
    　速度制御部が、前記目標点までの距離に応じて加速又は減速するように、前記中点及び前記目標点から定まる前記ＵＡＶの速度ベクトルを制御するための制御データを更新するステップと、
    　飛行コマンド変換部が、更新された前記制御データと、算出された前記方位角とに基づいて、前記ＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記ＵＡＶの運動を制御するステップと、
    　を含む飛行制御方法。
  5. 　前記速度制御部が更新するステップでは、前記目標点までの距離が、予め定められた距離以下である場合には、前記速度ベクトルを減速させた任意の速度ベクトルとして、前記制御データを更新し、
    　前記目標点までの距離が、前記予め定められた距離より長い場合には、前記速度ベクトルを加速させた、または一定の速度とした任意の速度ベクトルとして、前記制御データを更新する請求項４に記載の飛行制御方法。
  6. 　前記ＵＡＶをＮ台のＵＡＶとし、Ｎ－１台目のＵＡＶの中点をＮ台目のＵＡＶの前記目標点とし、
    　前記位置方位検出部が算出するステップでは、前記位置計測センサによって計測された前記Ｎ台目の前記マーカの各々の三次元座標と、予め求められた前記Ｎ－１台目の前記ＵＡＶの中点とに基づいて、前記Ｎ台目の前記ＵＡＶの前記マーカの各々の三次元座標の中点、前記中点からＮ－１台目のＵＡＶの中点である前記目標点までの距離、及びグローバル座標系におけるＮ－１台目のＵＡＶの中点である前記目標点に対する前記Ｎ台目の前記ＵＡＶの前記マーカの各々の三次元座標を結んだ線分の方位角を算出し、
    　前記速度制御部が更新するステップでは、前記Ｎ台目の前記ＵＡＶの前記中点に対する前記目標点までの距離に基づいて、前記Ｎ台目の前記ＵＡＶに対する前記制御データを更新し、
    　前記飛行コマンド変換部が制御するステップでは、前記Ｎ台目のＵＡＶについて更新された前記制御データと、前記Ｎ台目のＵＡＶについて算出された前記方位角とに基づいて、前記Ｎ台目の前記ＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記Ｎ台目の前記ＵＡＶの運動を制御する請求項４又は請求項５に記載の飛行制御方法。
  7. 　コンピュータを、請求項１～請求項３のいずれか１項に記載の飛行制御装置の各部として機能させるためのプログラム。
PCT/JP2019/012540 2018-04-06 2019-03-25 飛行制御装置、方法、及びプログラム WO2019194011A1 (ja)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2018073930A JP2019182131A (ja) 2018-04-06 2018-04-06 飛行制御装置、方法、及びプログラム
JP2018-073930 2018-04-06

Publications (1)

Publication Number Publication Date
WO2019194011A1 true WO2019194011A1 (ja) 2019-10-10

Family

ID=68100589

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2019/012540 WO2019194011A1 (ja) 2018-04-06 2019-03-25 飛行制御装置、方法、及びプログラム

Country Status (2)

Country Link
JP (1) JP2019182131A (ja)
WO (1) WO2019194011A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111422373A (zh) * 2020-06-10 2020-07-17 之江实验室 面向运输任务的纯电驱多旋翼飞行器动力系统选型方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH11139396A (ja) * 1997-11-10 1999-05-25 Makoto Toyama 編隊飛行制御装置
JP2004017743A (ja) * 2002-06-14 2004-01-22 Kumamoto Technology & Industry Foundation 自律飛行カイトプレーンシステムおよびカイトプレーン制御装置
JP2008168852A (ja) * 2007-01-15 2008-07-24 Mitsubishi Electric Corp 画像センサ搭載型航空機用航法計算プログラム
JP2012041004A (ja) * 2010-08-23 2012-03-01 Nippon Telegr & Teleph Corp <Ntt> 自律移動ロボットの動作計画方法、自律移動ロボットの動作計画方法を利用した自律移動ロボットの制御方法、自律移動ロボットの動作計画装置、自律移動ロボットの動作制御装置、自律移動ロボットの動作計画プログラム、自律移動ロボットの制御プログラム
CN105717489A (zh) * 2016-03-11 2016-06-29 歌尔声学股份有限公司 一种无人飞行器的追随方法、装置以及可穿戴设备
US20170131726A1 (en) * 2015-11-06 2017-05-11 SySense Incorporated Automated operation of aircraft systems in inverted-v formations
US20170269612A1 (en) * 2016-03-18 2017-09-21 Sunlight Photonics Inc. Flight control methods for operating close formation flight
JP2018040786A (ja) * 2016-09-08 2018-03-15 博泰科技有限公司 飛翔体の位置決め制御方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH11139396A (ja) * 1997-11-10 1999-05-25 Makoto Toyama 編隊飛行制御装置
JP2004017743A (ja) * 2002-06-14 2004-01-22 Kumamoto Technology & Industry Foundation 自律飛行カイトプレーンシステムおよびカイトプレーン制御装置
JP2008168852A (ja) * 2007-01-15 2008-07-24 Mitsubishi Electric Corp 画像センサ搭載型航空機用航法計算プログラム
JP2012041004A (ja) * 2010-08-23 2012-03-01 Nippon Telegr & Teleph Corp <Ntt> 自律移動ロボットの動作計画方法、自律移動ロボットの動作計画方法を利用した自律移動ロボットの制御方法、自律移動ロボットの動作計画装置、自律移動ロボットの動作制御装置、自律移動ロボットの動作計画プログラム、自律移動ロボットの制御プログラム
US20170131726A1 (en) * 2015-11-06 2017-05-11 SySense Incorporated Automated operation of aircraft systems in inverted-v formations
CN105717489A (zh) * 2016-03-11 2016-06-29 歌尔声学股份有限公司 一种无人飞行器的追随方法、装置以及可穿戴设备
US20170269612A1 (en) * 2016-03-18 2017-09-21 Sunlight Photonics Inc. Flight control methods for operating close formation flight
JP2018040786A (ja) * 2016-09-08 2018-03-15 博泰科技有限公司 飛翔体の位置決め制御方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111422373A (zh) * 2020-06-10 2020-07-17 之江实验室 面向运输任务的纯电驱多旋翼飞行器动力系统选型方法

Also Published As

Publication number Publication date
JP2019182131A (ja) 2019-10-24

Similar Documents

Publication Publication Date Title
JP6758271B2 (ja) 飛行制御装置、方法、及びプログラム
US11801938B2 (en) Aircraft control apparatus, control system and control method
AU2017386252B2 (en) Rotor units having asymmetric rotor blades
Thomas et al. Toward image based visual servoing for aerial grasping and perching
Mueller et al. Fusing ultra-wideband range measurements with accelerometers and rate gyroscopes for quadrocopter state estimation
US10717525B2 (en) Aircraft control apparatus, control system and control method
WO2019203166A1 (ja) 飛行制御装置、方法、及びプログラム
JP4685866B2 (ja) 動的システムを制御するためのシステムおよび方法
WO2020032058A1 (ja) 飛行制御装置、方法、及びプログラム
Mai et al. Local positioning system using UWB range measurements for an unmanned blimp
Chowdhary et al. Self-contained autonomous indoor flight with ranging sensor navigation
Wang et al. Guidance, navigation and control of an unmanned helicopter for automatic cargo transportation
WO2019172335A1 (ja) 飛行制御装置、方法、プログラム、及び記憶媒体
WO2019194011A1 (ja) 飛行制御装置、方法、及びプログラム
Douthwaite et al. An interval approach to multiple unmanned aerial vehicle collision avoidance
WO2022126396A1 (zh) 状态信息确定方法、装置、平台及计算机可读存储介质
Wang et al. Vision-aided tracking of a moving ground vehicle with a hybrid uav
Pestana et al. A general purpose configurable navigation controller for micro aerial multirotor vehicles
Chowdhary et al. Integrated guidance navigation and control for a fully autonomous indoor uas
Rossouw et al. An open-source autopilot and source localisation for bio-inspired miniature blimps
He et al. Vision based autonomous landing of the quadrotor using fuzzy logic control
US20230107289A1 (en) Information processing method, information processor, and program
Blouin et al. Optimal control for the trajectory planning of micro airships
Midhun et al. Safe Traversal Guidance for Quadrotors using Gap Bearing Information
Nagata et al. Flexible flight navigation for quadcopters based on geometric formation using motion sensing

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 19781373

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 19781373

Country of ref document: EP

Kind code of ref document: A1