(57)【要約】      （修正有）【課題】無人飛行機を自由自在に制御できる飛行制御装置を提供する。【解決手段】位置姿勢算出部が、参照物体１２の三次元座標と、算出された方位角と、参照物体及びＵＡＶ１４のマーカの三次元座標の相対位置関係とに基づいて、目標となる三次元座標とＵＡＶの三次元座標との差分を算出し、ＵＡＶの三次元座標に差分を加算して得られる三次元座標と、ＵＡＶの目標となる三次元座標とに基づいて回転角を算出する。運動制御部が、算出された差分と、回転角とに基づいて、ＵＡＶにおける、Ｘ軸周りの回転角、Ｙ軸周りの回転角、Ｚ軸に沿った速度、及びＺ軸周りの回転速度を飛行指令データとして算出し、算出した飛行指令データに基づいてＵＡＶの運動を制御する。【選択図】図１
2019059314.tif 000002
【特許請求の範囲】
【請求項１】
ＵＡＶ（Unmanned Aerial Vehicle）の飛行を制御するための参照物体に付与され、かつ、マーカ間の距離が既知の複数のマーカと、前記ＵＡＶに付与され、かつ、マーカ間の距離が既知の複数のマーカとの各々の三次元座標を計測する位置計測センサと、
前記位置計測センサによって計測された前記マーカの各々の三次元座標に基づいて、グローバル座標系における基準方向に対する前記参照物体の方位角を算出する方位検出部と、
前記参照物体の前記複数のマーカの各々の三次元座標と、前記算出された方位角と、予め定められた前記参照物体の前記複数のマーカの各々の三次元座標及び前記ＵＡＶの前記複数のマーカの各々の三次元座標の相対位置関係とに基づいて、前記ＵＡＶの前記複数のマーカの各々の目標となる三次元座標を算出し、算出された前記目標となる三次元座標と前記ＵＡＶの前記複数のマーカの各々の三次元座標との差分を算出し、前記ＵＡＶの前記複数のマーカの各々の三次元座標に前記差分を加算して得られる三次元座標と、前記ＵＡＶの前記複数のマーカの各々の前記目標となる三次元座標とに基づいて回転角を算出する位置姿勢算出部と、
算出された前記差分と、前記回転角とに基づいて、前記ＵＡＶにおける、Ｘ軸周りの回転角、Ｙ軸周りの回転角、Ｚ軸に沿った速度、及びＺ軸周りの回転速度を飛行指令データとして算出し、算出した前記飛行指令データに基づいて前記ＵＡＶの運動を制御する運動制御部と、
を含む飛行制御装置。
【請求項２】
前記ＵＡＶに付与された前記複数のマーカは、２つのマーカであり、
前記位置姿勢算出部で算出する前記差分は、前記ＵＡＶの２つのマーカの三次元座標の中点の座標と、算出された前記ＵＡＶの２つのマーカの目標となる三次元座標の中点の座標との差分とする請求項１に記載の飛行制御装置。
【請求項３】
前記ＵＡＶをＮ台のＵＡＶとし、Ｎ−１台目のＵＡＶをＮ台目のＵＡＶに対する前記参照物体とし、
前記方位検出部は、前記参照物体としての前記Ｎ−１台目のＵＡＶについて、方位角を算出し、
前記位置姿勢算出部は、前記Ｎ−１台目のＵＡＶの前記複数のマーカの各々の三次元座標と、前記算出された方位角と、予め定められた前記Ｎ−１台目のＵＡＶの前記複数のマーカの各々の三次元座標及び前記Ｎ台目のＵＡＶの前記複数のマーカの各々の三次元座標の相対位置関係とに基づいて、前記Ｎ台目のＵＡＶの前記複数のマーカの各々の目標となる三次元座標を算出し、算出された前記目標となる三次元座標と前記Ｎ台目のＵＡＶの前記複数のマーカの各々の三次元座標との差分を算出し、前記Ｎ台目のＵＡＶの前記複数のマーカの各々の三次元座標に前記差分を加算して得られる三次元座標と、前記Ｎ台目のＵＡＶの前記複数のマーカの各々の前記目標となる三次元座標とに基づいて回転角を算出し、
前記運動制御部は、算出された前記差分と、前記回転角とに基づいて、前記Ｎ台目のＵＡＶにおける、Ｘ軸周りの回転角、Ｙ軸周りの回転角、Ｚ軸に沿った速度、及びＺ軸周りの回転速度を飛行指令データとして算出し、算出した前記飛行指令データに基づいて前記Ｎ台目のＵＡＶの運動を制御する請求項１又は請求項２に記載の飛行制御装置。
【請求項４】
位置計測センサが、ＵＡＶ（Unmanned Aerial Vehicle）の飛行を制御するための参照物体に付与され、かつ、マーカ間の距離が既知の複数のマーカと、前記ＵＡＶに付与され、かつ、マーカ間の距離が既知の複数のマーカとの各々の三次元座標を計測するステップと、
方位検出部が、前記位置計測センサによって計測された前記マーカの各々の三次元座標に基づいて、グローバル座標系における基準方向に対する前記参照物体の方位角を算出するステップと、
位置姿勢算出部が、前記参照物体の前記複数のマーカの各々の三次元座標と、前記算出された方位角と、予め定められた前記参照物体の前記複数のマーカの各々の三次元座標及び前記ＵＡＶの前記複数のマーカの各々の三次元座標の相対位置関係とに基づいて、前記ＵＡＶの前記複数のマーカの各々の目標となる三次元座標を算出し、算出された前記目標となる三次元座標と前記ＵＡＶの前記複数のマーカの各々の三次元座標との差分を算出し、前記ＵＡＶの前記複数のマーカの各々の三次元座標に前記差分を加算して得られる三次元座標と、前記ＵＡＶの前記複数のマーカの各々の前記目標となる三次元座標とに基づいて回転角を算出するステップと、
運動制御部が、算出された前記差分と、前記回転角とに基づいて、前記ＵＡＶにおける、Ｘ軸周りの回転角、Ｙ軸周りの回転角、Ｚ軸に沿った速度、及びＺ軸周りの回転速度を飛行指令データとして算出し、算出した前記飛行指令データに基づいて前記ＵＡＶの運動を制御するステップと、
を含む飛行制御方法。
【請求項５】
前記ＵＡＶに付与された前記複数のマーカは、２つのマーカであり、
前記位置姿勢算出部が算出するステップにおいて、前記差分は、前記ＵＡＶの２つのマーカの三次元座標の中点の座標と、算出された前記ＵＡＶの２つのマーカの目標となる三次元座標の中点の座標との差分とする請求項４に記載の飛行制御方法。
【請求項６】
前記ＵＡＶをＮ台のＵＡＶとし、Ｎ−１台目のＵＡＶをＮ台目のＵＡＶに対する前記参照物体とし、
前記方位検出部が算出するステップは、前記参照物体としての前記Ｎ−１台目のＵＡＶについて、方位角を算出し、
前記位置姿勢算出部が算出するステップは、前記Ｎ−１台目のＵＡＶの前記複数のマーカの各々の三次元座標と、前記算出された方位角と、予め定められた前記Ｎ−１台目のＵＡＶの前記複数のマーカの各々の三次元座標及び前記Ｎ台目のＵＡＶの前記複数のマーカの各々の三次元座標の相対位置関係とに基づいて、前記Ｎ台目のＵＡＶの前記複数のマーカの各々の目標となる三次元座標を算出し、算出された前記目標となる三次元座標と前記Ｎ台目のＵＡＶの前記複数のマーカの各々の三次元座標との差分を算出し、前記Ｎ台目のＵＡＶの前記複数のマーカの各々の三次元座標に前記差分を加算して得られる三次元座標と、前記Ｎ台目のＵＡＶの前記複数のマーカの各々の前記目標となる三次元座標とに基づいて回転角を算出し、
前記運動制御部が制御するステップは、算出された前記差分と、前記回転角とに基づいて、前記Ｎ台目のＵＡＶにおける、Ｘ軸周りの回転角、Ｙ軸周りの回転角、Ｚ軸に沿った速度、及びＺ軸周りの回転速度を飛行指令データとして算出し、算出した前記飛行指令データに基づいて前記Ｎ台目のＵＡＶの運動を制御する請求項４又は請求項５に記載の飛行制御方法。
【請求項７】
コンピュータを、請求項１〜請求項３のいずれか１項に記載の飛行制御装置の各部として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、飛行制御装置、方法、及びプログラムに係り、特に、無人飛行機を制御する飛行制御装置、方法、及びプログラムに関する。
【背景技術】
【０００２】
クアッドロータ型ＵＡＶ（Unmanned Aerial Vehicle）は４つのプロペラを持ち、それぞれのプロペラに与える揚力を制御することにより飛行操縦する無人飛行機である。ドローンと呼ばれる無人飛行機の多くは、クアッドロータ型ＵＡＶの一種である。以降で扱う無人飛行機をＵＡＶと略称する。
【０００３】
一般的に、ＵＡＶの姿勢を計測するために、図１４に示すように機体にローカル座標系が設定される。ＵＡＶの前進方向をＸ軸、Ｘ軸と垂直な方向をＹ軸、重力とは逆方向をＺ軸とする。
【０００４】
また、ＵＡＶの三次元位置を計測するため、図１５に示すように、ある基準となるグローバル座標系を設定する。ＧＰＳでは世界座標系とした三次元座標となり、モーションキャプチャシステムではその計測座標系がグローバル座標系となる。ＵＡＶの重心すなわちＵＡＶの位置（ローカル座標系の原点）はグローバル座標系の点Ｐ＝（Ｘ，Ｙ，Ｚ）として表現する。また、ＵＡＶの姿勢はグローバル座標系に対するローカル座標系の回転角で表現し、Ｘ軸周りの回転はロール回転（回転角φ）、Ｙ軸周りの回転はピッチ回転（回転角ω）、Ｚ軸周りの回転はヨー回転（回転角θ）と呼ばれる。ＵＡＶの飛行運動は、４つのプロペラに与える揚力を変化させることにより、Ｘ軸周りの回転、Ｙ軸周りの回転、Ｚ軸周りの回転を発生させる。Ｙ軸周りの回転はＸ軸方向の並進運動を生み出し、Ｘ軸周りの回転はＹ軸方向の並進運動を生み出す。Ｚ軸周りの回転は方位の回転を生み出し、同じ揚力が同時に４つのプロペラに与えられたとき、その強弱によってＺ軸方向の並進運動（高度の昇降）を生み出す。
【０００５】
グローバル座標系において所定の座標値（Ｘｄ，Ｙｄ，Ｚｄ）と方位θｄが与えられたとき、ＵＡＶの現在位置Ｐ＝（Ｘ，Ｙ，Ｚ）と現在方位φから、その所定の座標値の位置へ飛行して所定方位に機体を向けるためには、ＵＡＶの姿勢と高度を制御する必要がある。非特許文献１には、ＵＡＶの運動方程式と角運動方程式に基づいたバックステッピング制御が公開されている。非特許文献２では、AR Drone 2.0（市販の低価格なUAV）に関する飛行運動制御法が公開されている。従来技術の多くは、グローバル座標系における飛行の軌跡として離散的な所定位置と所定方位が与えられ、ＵＡＶが各点と各方位を追跡するようにその運動が制御される。
【０００６】
一方、ＵＡＶには空間中を自由自在に飛行する能力があるため、人の代わりとなるロボットとして活躍することができる。非特許文献３では、ＵＡＶは人と協調作業するための道具として使われており、非特許文献４には、複数のＵＡＶを使って空間中に網を張り、人とＵＡＶがボールをやり取りする動作として複数のＵＡＶの運動を制御する方法が公開されている。
【先行技術文献】
【非特許文献】
【０００７】
【非特許文献１】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.
【発明の概要】
【発明が解決しようとする課題】
【０００８】
ＵＡＶの飛行を自由自在に制御する課題は、大まかに３つに大別でき、ホバリング（hovering）、軌跡追跡（trajectory tracking）、パスフォロー（path following）と呼ばれる。ホバリングは、与えられた位置と方位に留まるよう飛行を維持する。軌跡追跡では、与えられた経路（空間中の離散的な三次元位置と方位）に沿ってリアルタイムに追跡するよう制御する。これに対して、パスフォローは、リアルタイム性は問わないが、与えられた空間中の三次元位置と方位において、何らかの空間中の拘束条件を与えて飛行を制御する。非特許文献１と非特許文献２によれば、軌跡追跡の課題に対して、所定の経路に従って飛行するように、ＵＡＶの飛行運動を適切に制御することができる。しかしながら、これらの技術は所定の経路に沿ってＵＡＶが飛行するように制御することが目的であるため、障害物、人あるいは他のＵＡＶとの衝突を回避することは容易ではない。
【０００９】
これに対して、パスフォローは障害物あるいは人との衝突を避けるため、特定の場所を飛行しないように空間中に拘束条件を設定することができる。また、飛行運動中のトラブルを回避するため、ＵＡＶには多様なセンサやカメラが取り付けられている場合がある。例えば、超音波センサは地面あるいは床からの高度を計測し、ジャイロセンサと加速度センサによって機体の速度と姿勢を計測することができる。また、カメラを使えば、移動物体を検出し、空間中の飛行を安定化することに利用できる。これらの機体から計測した空間情報や画像情報を使うことも考えられるが、大半のＵＡＶに内蔵されているセンサの精度はミリ単位での正確な精度で計測できることを保証しておらず、障害物との衝突を避けるためには、別の手段を使って機体の正確な位置と方位を計測する必要がある。
【００１０】
本発明は、上記問題点を解決するために成されたものであり、無人飛行機を自由自在に制御できる飛行制御装置、方法、及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１１】
上記目的を達成するために、本発明に係る飛行制御装置は、ＵＡＶ（Unmanned Aerial Vehicle）の飛行を制御するための参照物体に付与され、かつ、マーカ間の距離が既知の複数のマーカと、前記ＵＡＶに付与され、かつ、マーカ間の距離が既知の複数のマーカとの各々の三次元座標を計測する位置計測センサと、前記位置計測センサによって計測された前記マーカの各々の三次元座標に基づいて、グローバル座標系における基準方向に対する前記参照物体の方位角を算出する方位検出部と、前記参照物体の前記複数のマーカの各々の三次元座標と、前記算出された方位角と、予め定められた前記参照物体の前記複数のマーカの各々の三次元座標及び前記ＵＡＶの前記複数のマーカの各々の三次元座標の相対位置関係とに基づいて、前記ＵＡＶの前記複数のマーカの各々の目標となる三次元座標を算出し、算出された前記目標となる三次元座標と前記ＵＡＶの前記複数のマーカの各々の三次元座標との差分を算出し、前記ＵＡＶの前記複数のマーカの各々の三次元座標に前記差分を加算して得られる三次元座標と、前記ＵＡＶの前記複数のマーカの各々の前記目標となる三次元座標とに基づいて回転角を算出する位置姿勢算出部と、算出された前記差分と、前記回転角とに基づいて、前記ＵＡＶにおける、Ｘ軸周りの回転角、Ｙ軸周りの回転角、Ｚ軸に沿った速度、及びＺ軸周りの回転速度を飛行指令データとして算出し、算出した前記飛行指令データに基づいて前記ＵＡＶの運動を制御する運動制御部と、を含んで構成されている。
【００１２】
また、本発明に係る飛行制御方法は、位置計測センサが、ＵＡＶ（Unmanned Aerial Vehicle）の飛行を制御するための参照物体に付与され、かつ、マーカ間の距離が既知の複数のマーカと、前記ＵＡＶに付与され、かつ、マーカ間の距離が既知の複数のマーカとの各々の三次元座標を計測するステップと、方位検出部が、前記位置計測センサによって計測された前記マーカの各々の三次元座標に基づいて、グローバル座標系における基準方向に対する前記参照物体の方位角を算出するステップと、位置姿勢算出部が、前記参照物体の前記複数のマーカの各々の三次元座標と、前記算出された方位角と、予め定められた前記参照物体の前記複数のマーカの各々の三次元座標及び前記ＵＡＶの前記複数のマーカの各々の三次元座標の相対位置関係とに基づいて、前記ＵＡＶの前記複数のマーカの各々の目標となる三次元座標を算出し、算出された前記目標となる三次元座標と前記ＵＡＶの前記複数のマーカの各々の三次元座標との差分を算出し、前記ＵＡＶの前記複数のマーカの各々の三次元座標に前記差分を加算して得られる三次元座標と、前記ＵＡＶの前記複数のマーカの各々の前記目標となる三次元座標とに基づいて回転角を算出するステップと、運動制御部が、算出された前記差分と、前記回転角とに基づいて、前記ＵＡＶにおける、Ｘ軸周りの回転角、Ｙ軸周りの回転角、Ｚ軸に沿った速度、及びＺ軸周りの回転速度を飛行指令データとして算出し、算出した前記飛行指令データに基づいて前記ＵＡＶの運動を制御するステップと、を含んで実行することを特徴とする。
【００１３】
本発明に係るプログラムは、コンピュータを、本発明に係る飛行制御装置の各部として機能させるためのプログラムである。
【発明の効果】
【００１４】
本発明の飛行制御装置、方法、及びプログラムによれば、参照物体の方位角を算出し、参照物体の三次元座標と、算出された方位角と、参照物体及びＵＡＶのマーカの三次元座標の相対位置関係とに基づいて、目標となる三次元座標とＵＡＶの三次元座標との差分を算出し、ＵＡＶの三次元座標に差分を加算して得られる三次元座標と、ＵＡＶの目標となる三次元座標とに基づいて回転角を算出し、算出された差分と、回転角とに基づいて、ＵＡＶにおける、Ｘ軸周りの回転角、Ｙ軸周りの回転角、Ｚ軸に沿った速度、及びＺ軸周りの回転速度を飛行指令データとして算出し、算出した飛行指令データに基づいてＵＡＶの運動を制御することにより、無人飛行機を自由自在に制御できる、という効果が得られる。
【図面の簡単な説明】
【００１５】
【図１】本発明の実施の形態に係る飛行制御装置の構成を示すブロック図である。
【図２】参照物体を使ってＵＡＶの飛行を制御する状況の一例を示す図である。
【図３】本発明の実施の形態に係る飛行制御装置における飛行制御処理ルーチンを示すフローチャートである。
【図４】飛行制御装置における方向検出部の処理のフローチャートである。
【図５】グローバル座標系において計測された４点の一例を示す図である。
【図６】グローバル座標系において点Ａを原点に平行移動した空間中の各点の配置の一例を示す図である。
【図７】飛行制御装置における位置姿勢算出部の処理のフローチャートである。
【図８】点Ａｓと点Ｂｓと正方形を形成する点Ｃｔと点Ｄｔの空間配置の一例を示す図である。
【図９】中点Ｇｓを中点Ｇｔへ平行移動した空間配置の一例を示す図である。
【図１０】飛行制御装置における運動制御部の処理のフローチャートである。
【図１１】グローバル座標系において計測された４点の一例を示す図である。
【図１２】点Ａｓと点Ｂｓと等脚台形を形成する点Ｃｔと点Ｄｔの空間配置の一例を示す図である。
【図１３】複数のＵＡＶの空間配置の一例を示す図である。
【図１４】ＵＡＶの概観とＵＡＶ固定のローカル座標系の一例を示す図である。
【図１５】グローバル座標系とＵＡＶ固定のローカル座標系の関係の一例を示す図である。
【発明を実施するための形態】
【００１６】
以下、図面を参照して本発明の実施形態を詳細に説明する。本発明の実施の形態に係る手法は、ユーザが参照物体を移動させることによってＵＡＶの飛行を自由自在に操作し、かつ、対象とする物体との衝突を回避する、という課題を解決しようとするものである。
【００１７】
＜本発明の第１の実施形態に係る飛行制御装置の構成＞
【００１８】
本発明の第１の実施形態に係るクアッドロータ型ＵＡＶの飛行制御装置の構成について説明する。本実施形態は１台のＵＡＶの飛行を参照物体で操縦する形態である。図１に示すように、本発明の第１の実施形態に係る飛行制御装置１００は、ＣＰＵと、ＲＡＭと、後述する飛行制御処理ルーチンを実行するためのプログラムや各種データを記憶したＲＯＭと、を含むコンピュータで構成することが出来る。この飛行制御装置１００は、機能的には図１に示すように位置計測センサ１０と、演算部２０と、通信部５０とを備えている。この構成において、位置計測センサ１０は必ずしも構成要素として接続している必要はなく、処理に必要なデータを取得すればよく、演算部２０における方位検出部３０、位置姿勢算出部３２、及び運動制御部３４からそれぞれの矢印へのデータの流れは、ハードディスク、ＲＡＩＤ装置、ＣＤ−ＲＯＭなどの記録媒体を利用する、または、ネットワークを介してリモートなデータ資源を利用する形態でもどちらでも構わない。
【００１９】
位置計測センサ１０は、ＵＡＶ１４の飛行を制御するための参照物体１２に付与され、かつ、マーカ間の距離が既知の複数のマーカ（点Ａ、及び点Ｂとする）と、ＵＡＶ１４に付与され、かつ、マーカ間の距離が既知の複数のマーカ（点Ｃ、及び点Ｄとする）との各々の三次元座標を計測データとして計測する。本実施形態では、図２に示す状況において、参照物体１２を使ってＵＡＶ１４の飛行を制御する。参照物体１２は、障害物との衝突を回避できる移動が可能な物体であればよく、本実施形態では持ち運び可能なＴ字棒を例に説明する。一般的に、モーションキャプチャ装置は、所定のマーカの三次元座標をリアルタイムで高精度に計測することが知られている。本実施形態では、位置計測センサ１０の例として、モーションキャプチャ装置を利用する。図２に示すように、参照物体１２には点Ａと点Ｂの位置にマーカが取り付けられており、ＵＡＶ１４には点Ｃと点Ｄの位置にマーカが取り付けられている。位置計測センサ１０はグローバル座標系において三次元座標を計測するようにセットアップされており、各マーカはその座標系の三次元座標としてある一定間隔で逐次計測される。なお、本実施形態では、点Ａと点Ｂ間の距離はＬであり、点Ｃと点Ｄ間の距離はＬとする。
【００２０】
本実施形態は、参照物体１２の点Ａと点Ｂの三次元位置に応じて、点Ａ、点Ｂ、点Ｃ、及び点Ｄが正方形を形成する相対位置関係となるようにＵＡＶ１４の飛行運動を制御するものであるが、長方形などの他の幾何形状にも同様に応用することができる。なお、本実施形態では、マーカの数は２つとしたが、これに限定されるものではなく、３つ以上として、物体間の相対的な位置関係を求めるようにしてもよい。
【００２１】
演算部２０は、位置計測センサ１０で計測した三次元座標から方位を算出する方位検出部３０と、グローバル座標系におけるＵＡＶ１４の位置及び姿勢を算出する位置姿勢算出部３２と、位置と姿勢を用いて、所定の位置と方位を満たすようにＵＡＶ１４の飛行を制御するための飛行指令データを算出する運動制御部３４と、ＵＡＶ１４に飛行指令データを送信する通信部５０とを含んで構成される。なお、方位検出部３０、位置姿勢算出部３２、及び運動制御部３４の具体的な処理内容は後述の作用の説明において説明する。
【００２２】
方位検出部３０は、位置計測センサ１０によって計測されたマーカの各々の三次元座標に基づいて、グローバル座標系における基準方向に対する参照物体１２の方位角Φを算出する。
【００２３】
位置姿勢算出部３２は、参照物体１２のマーカの貼付位置である点Ａ、及び点Ｂの三次元座標と、算出された方位角Φと、予め定められた参照物体１２のマーカ点Ａ、及び点Ｂの三次元座標及びＵＡＶ１４のマーカ点Ｃ、及び点Ｄの三次元座標の相対位置関係（例えば、正方形を形成する相対位置関係）とに基づいて、ＵＡＶ１４のマーカ点Ｃ、及び点Ｄの目標となる三次元座標を算出し、算出された目標となる三次元座標とＵＡＶ１４のマーカ点Ｃ、及び点Ｄの三次元座標との差分Ｔを算出する。また、位置姿勢算出部３２は、ＵＡＶ１４のマーカ点Ｃ、及び点Ｄの三次元座標に差分を加算して得られる三次元座標と、ＵＡＶ１４のマーカ点Ｃ、及び点Ｄの目標となる三次元座標とに基づいて回転角θを算出する。
【００２４】
運動制御部３４は、算出された差分Ｔと、回転角θとに基づいて、ＵＡＶ１４における、Ｘ軸周りの回転角φ、Ｙ軸周りの回転角ω、Ｚ軸に沿った速度Ｖｚ、及びＺ軸周りの回転速度Ｖθを飛行指令データとして算出し、算出した飛行指令データを通信部５０を介してＵＡＶ１４に送信することで、飛行指令データに基づいてＵＡＶ１４の運動を制御する。
【００２５】
＜本発明の第１の実施の形態に係る飛行制御装置の作用＞
【００２６】
次に、本発明の第１の実施の形態に係る飛行制御装置１００の作用について説明する。位置計測センサ１０によりＵＡＶ１４の飛行を制御するための参照物体１２に付与され、かつ、マーカ間の距離が既知のマーカ点Ａ、及び点Ｂと、ＵＡＶ１４に付与され、かつ、マーカ間の距離が既知のマーカ点Ｃ、及び点Ｄとの各々の三次元座標の計測を開始し、操作者が参照物体１２を持ってＵＡＶ１４の制御を開始すると、飛行制御装置１００は、図３に示す飛行制御処理ルーチンを実行する。
【００２７】
まず、ステップＳ１００では、方位検出部３０が、位置計測センサ１０によって計測されたマーカの各々の三次元座標に基づいて、グローバル座標系における基準方向に対する参照物体１２の方位角Φを算出する。
【００２８】
次に、ステップＳ１０２では、位置姿勢算出部３２が、参照物体１２のマーカの点Ａ、及び点Ｂの三次元座標と、算出された方位角Φと、予め定められた参照物体１２のマーカの点Ａ、及び点Ｂの三次元座標及びＵＡＶ１４のマーカ点Ｃ、及び点Ｄの三次元座標の相対位置関係とに基づいて、ＵＡＶ１４のマーカ点Ｃ、及び点Ｄの目標となる三次元座標を算出し、算出された目標となる三次元座標とＵＡＶ１４のマーカ点Ｃ、及び点Ｄの三次元座標との差分Ｔを算出する。また、ＵＡＶ１４のマーカ点Ｃ、及び点Ｄの三次元座標に差分を加算して得られる三次元座標と、ＵＡＶ１４のマーカ点Ｃ、及び点Ｄの目標となる三次元座標とに基づいて回転角θを算出する。
【００２９】
ステップＳ１０４では、運動制御部３４が、算出された差分Ｔと、回転角θとに基づいて、ＵＡＶ１４における、Ｘ軸周りの回転角φ、Ｙ軸周りの回転角ω、Ｚ軸に沿った速度Ｖｚ、及びＺ軸周りの回転速度Ｖθを飛行指令データとして算出し、算出した飛行指令データをＵＡＶ１４に送信して、飛行指令データに基づいてＵＡＶ１４の運動を制御する。ここで、後述する予め定められた条件を満たすまで、ステップＳ１０２における差分Ｔ及び回転角θの算出、及びステップＳ１０４における飛行指令データに基づくＵＡＶ１４の運動の制御を繰り返す。これにより、操作者が参照物体１２を持って移動することにより、ＵＡＶ１４の運動が制御される。
【００３０】
ステップＳ１００の方位検出部３０の処理の詳細について説明する。
【００３１】
図４は方位検出部３０の処理のフロー図である。方位検出部３０は処理を開始すると、ステップＳ１０００で、位置計測センサ１０で取得された参照物体１２の点Ａ、及び点Ｂの三次元座標値と、ＵＡＶ１４の点Ｃ、及び点Ｄの三次元座標値とを取得する。点Ａの三次元座標値をＡ＝（Ｘａ，Ｙａ，Ｚｈ）、点Ｂの三次元座標値をＢ＝（Ｘｂ，Ｙｂ，Ｚｈ）、点Ｃの三次元座標値をＣ＝（Ｘｃ，Ｙｃ，Ｚｈ）、点Ｄの三次元座標値をＤ＝（Ｘｄ，Ｙｄ，Ｚｈ）とする。説明の簡単のため、本実施形態では、４点の高さは同じものとして、Ｚｗ軸の座標値をＺｈとする。各点の高さが異なる場合については、第２実施形態で説明する。グローバル座標系のＸｗＹｗ面において、計測された４点を図５に示す。図５では、点Ａと点Ｃ間の距離はＬａ（≠Ｌ）、点Ｂと点Ｄ間の距離はＬｂ（≠Ｌ）とする。
【００３２】
ステップＳ１００２では、重心位置の移動が必要かを判定し、必要な場合にはステップＳ１００４へ移行し、必要ない場合にはステップＳ１００８へ移行する。
【００３３】
ステップＳ１００４では、座標値を平行移動変換する。方位検出部３０での座標値の平行移動変換では、点Ａを原点に平行移動させる。平行移動先の三次元座標値として、以下（１）〜（３）式により、点Ａ以外の全ての座標から点Ａの三次元座標値Ａ＝（Ｘａ，Ｙａ，Ｚｈ）を差し引いて、Ｂｓ、Ｃｓ、及びＤｓを算出する。
【００３４】
【数１】
2019059314.tif 000003
・・・（１）
2019059314.tif 000004
・・・（２）
2019059314.tif 000005
・・・（３）
【００３５】
点Ａを原点に平行移動した空間中の各点の配置を図６に示す。
【００３６】
次に、ステップＳ１００６では、参照物体１２の点Ａと点Ｂの方位角を算出するため、点Ｂｂｓから、以下（４）式の計算により、方位角Φを算出する。
【００３７】
【数２】
2019059314.tif 000006
・・・（４）
【００３８】
ステップＳ１００８では、方向検出の処理を停止するかを判定し、処理を停止する場合には方位検出部３０の処理を終了し、終了しない場合にはステップＳ１０００に戻って処理を繰り返す。なお、処理を停止する場合とは、ここでは操作者がＵＡＶ１４の飛行制御を終了する場合とする。
【００３９】
方位検出部３０では、以上の処理により、参照物体１２の点Ａと点Ｂの三次元位置に応じて、方位角Φを算出する。
【００４０】
次に、ステップＳ１０２の位置姿勢算出部３２の処理の詳細について説明する。
【００４１】
図７は位置姿勢算出部３２の処理のフロー図である。ステップＳ１１００では、参照物体１２の点Ａと点Ｂの三次元位置に応じて点Ａ、点Ｂ、点Ｃ、及び点Ｄが正方形を形成するように、点Ｃと点Ｄの目標となる三次元座標である目標座標を算出する。目標座標とは、点Ａと点Ｂを正方形の頂点とした場合の残り２点の頂点座標を指す。方位検出部３０にて、点Ａは原点Ｏに、点Ｂは点Ｂｓへ平行移動されているという前提で、点Ａと点Ｂの三次元位置に応じた方位角Φが算出された後、点Ｃ、点Ｄに対応する目標座標を算出する。図８に、点Ｃ、点Ｄに対応する目標座標の点Ｃｔと点Ｄｔを示す。目標座標の点Ｃｔと点Ｄｔは、以下（５）式、及び（６）式により算出する。
【００４２】
【数３】
2019059314.tif 000007
・・・（５）
2019059314.tif 000008
・・・（６）
【００４３】
次に、ステップＳ１１０２では、平行移動した点Ｃｓと点Ｄｓとの中点の座標Ｇｓ、及び目標座標における点Ｃｔと点Ｄｔとの中点の座標Ｇｔを算出する。先に平行移動した点Ｃｓと点Ｄｓから以下（７）式により中点の座標Ｇｓを算出する。
【００４４】
【数４】
2019059314.tif 000009
・・・（７）
【００４５】
同様に、目標座標の点Ｃｔと点Ｄｔからも、以下（８）式により中点の座標Ｇｔを算出する。
【００４６】
【数５】
2019059314.tif 000010
・・・（８）
【００４７】
次に、ステップＳ１１０４では、位置計測センサ１０で得た現在のＵＡＶ１４の位置と目標座標間の距離の差分Ｔを算出する。差分Ｔは、中点の座標ＧｓとＧｔ間のずれから以下（９）式の計算により検出する。
【００４８】
【数６】
2019059314.tif 000011
・・・（９）
【００４９】
差分Ｔの大きさは、以下（１０）式で得られる。
【００５０】
【数７】
2019059314.tif 000012
・・・（１０）
【００５１】
ステップＳ１１０６では、ΔＴの値が許容範囲（例えば、１０センチメール）を超えているかを判定する。許容範囲を超えていない場合には、ＵＡＶ１４の現在位置が所定位置であると判断してステップＳ１１１０へ移行する。許容範囲を超えている場合には、ＵＡＶ１４の現在位置が所定位置ではないと判断してステップＳ１１０８へ移行する。ステップＳ１１０８では、先に平行移動した点Ｃｓと点Ｄｓに加算して、以下（１１）式、及び（１２）式により目標座標の点Ｃｒと点Ｄｒを求める。
【００５２】
【数８】
2019059314.tif 000013
・・・（１１）
2019059314.tif 000014
・・・（１２）
【００５３】
このとき、ＵＡＶ１４の方位が参照物体１２の点Ａと点Ｂの方位と一致していない場合、点Ｃｒと点Ｄｒは目標座標の点Ｃｔと点Ｄｔに一致していない。図９に、点Ｃｒと点Ｄｒと目標座標の点Ｃｔと点Ｄｔの配置を示す。それぞれが一致していない場合を想定して、点Ｃｒから点ＤｒへのベクトルＥｒと、点Ｃｔから点ＤｔへのベクトルＥｔとを以下（１３）式、及び（１４）式により求める。
【００５４】
【数９】
2019059314.tif 000015
・・・（１３）
2019059314.tif 000016
・・・（１４）
【００５５】
ステップＳ１１１０では、ベクトルの内積の関係を利用して、回転角θを以下（１５）式により算出する。
【００５６】
【数１０】
2019059314.tif 000017
・・・（１５）
【００５７】
ただし、Ｅｒ・ＥｔはベクトルＥｒとベクトルＥｔの内積を表し、||Ｅｒ||と||Ｅｔ||はベクトルＥｒとベクトルＥｔのノルム（大きさ）を表す。
【００５８】
位置姿勢算出部３２では、以上の処理により、ＵＡＶ１４の位置と方位が所定位置と方位となるための平行移動ベクトルＴと回転角θｒを算出する。
【００５９】
次に、ステップＳ１０４の運動制御部３４の処理の詳細について説明する。
【００６０】
図１０は運動制御部３４の処理のフロー図である。運動制御部３４の処理により、位置姿勢算出部３２で得た平行移動ベクトルＴと回転角θｒを使って、ＵＡＶ１４の飛行運動を制御する。使用するＵＡＶ１４によって、ＵＡＶ１４への制御データは様々なデータ形式が存在する。本実施形態では、市販製品のAR Drone 2.0を例にした場合を示すが、それ以外のＵＡＶ１４の飛行を制御する場合にも、本実施形態を利用できる。
【００６１】
上記図２では、参照物体１２とＵＡＶ１４が対面の関係にあることを想定している。ＵＡＶ１４への飛行指令データは、機体に設定されたＸ軸周りの回転角φ、Ｙ軸周りの回転角ω、Ｚ軸に沿った速度Ｖｚ、及びＺ軸周りの回転速度Ｖになる。運動制御部３４の処理を開始すると、ステップＳ１２００では、制御データの読み込みにおいて、平行移動ベクトルＴと回転角θを取り出す。
【００６２】
次に、ステップＳ１２０２では、平行移動ベクトルＴと回転角θｒを、ＵＡＶ１４へ送信するための飛行指令データへ変換する。参照物体１２とＵＡＶ１４が対面の関係にあることを考慮すると、現在のＵＡＶ１４の位置はＴ’＝（Ｔｘ’，Ｔｙ’，Ｔｚ’）＝（−Ｔｘ，−Ｔｙ，Ｔｚ）と与えられ、参照物体１２に対する回転角はθ′ｒ＝−θｒと与えられる。
【００６３】
ＵＡＶ１４では、ロール回転φがＹ軸の並進運動を生み出し、ピッチ回転ωがＸ軸の並進運動を生み出するため、本処理では、ＵＡＶ１４に与える飛行指令データを以下（１６）〜（１９）式により変換する。
【００６４】
【数１１】
2019059314.tif 000018
・・・（１６）
2019059314.tif 000019
・・・（１７）
2019059314.tif 000020
・・・（１８）
2019059314.tif 000021
・・・（１９）
【００６５】
係数αｘ、αｙ、αｚ、αｒはフィードバック制御におけるゲインの役割を担うため、パラメータとしてユーザが決めてよく、例えば、αｘ＝αｙ＝αｚ＝αｒ＝０．１と与える。
【００６６】
ステップＳ１２０４では、通信部５０を介してＷｉＦｉ経由で（１６）〜（１９）式で算出した飛行指令データをＵＡＶ１４へ送信する。
【００６７】
ゲイン係数αｘ、αｙ、αｚ、αｒの設定によっては、１回の飛行指令で所定の位置と方位に到達しない場合がある。
【００６８】
そのため、ステップＳ１２０６では、飛行指令データの送信後に、予め定められた条件を満たすかを判定する。予め定められた条件は、例えば、上記（１０）式で算出される距離ΔＴが許容範囲以内であることとする。条件を満たさない場合には、上記図７の位置姿勢算出部３２の処理フローのステップＳ１１０２の中点の座標の算出に戻り、上記（９）式に従って中点Ｇｓと中点Ｇｔの差分を得る。ステップＳ１２０４でＵＡＶ１４へ飛行指令データが送られると、飛行指令データに基づいてＵＡＶ１４が運動し、先の三次元座標とは異なる三次元座標に変化する。このように、上記（１０）式で算出される距離の差分Ｔの大きさΔＴが許容範囲以内になるまで、位置姿勢算出部３２により平行移動ベクトルＴと回転角θｒを算出し、運動制御部３４により（１６）〜（１９）式で算出した飛行指令データを送信し続ける。条件を満たす場合には、運動制御処理を終了する。
【００６９】
以上により、本実施形態は、参照物体１２の配置に従いＵＡＶ１４の位置と方位を算出し、点Ａ、点Ｂ、点Ｃ、及び点Ｄが正方形を形成するように、ＵＡＶ１４の飛行運動を制御することができる。また、ユーザが参照物体１２を移動させる（点Ａと点Ｂが移動する）たびに、本実施形態は、点Ｃ、及び点Ｄが参照物体１２の点Ａ、及び点Ｂと正方形を形成するようにＵＡＶ１４の飛行を適宜制御することができる。操作者が参照物体１２を連続的に動かすことにより、ＵＡＶ１４の飛行ナビゲーションを実現することができる。
【００７０】
以上説明したように、第１の発明の実施形態に係る飛行制御装置によれば、参照物体の方位角を算出し、参照物体の三次元座標と、算出された方位角と、参照物体１２及びＵＡＶ１４のマーカの三次元座標の相対位置関係とに基づいて、目標となる三次元座標とＵＡＶ１４の三次元座標との差分を算出し、ＵＡＶの三次元座標に差分を加算して得られる三次元座標と、ＵＡＶ１４の目標となる三次元座標とに基づいて回転角を算出し、算出された差分と、回転角とに基づいて、ＵＡＶ１４における、Ｘ軸周りの回転角、Ｙ軸周りの回転角、Ｚ軸に沿った速度、及びＺ軸周りの回転速度を飛行指令データとして算出し、算出した飛行指令データに基づいてＵＡＶ１４の運動を制御することにより、無人飛行機を自由自在に制御できる。
【００７１】
＜本発明の第２の実施の形態に係る飛行制御装置の構成及び作用＞
【００７２】
本発明の第２の実施形態は、上記図２においてＵＡＶ１４に取り付けた点Ｃと点Ｄ間の距離がＭ(≠Ｌ)の場合の例である。図１１に、同じ高さＺｈの４点（点Ａ、点Ｂ、点Ｃ、及び点Ｄ）の配置図を示す。本実施形態では、点Ａと点Ｃ間の距離はＬａ（≠Ｌ)、点Ｂと点Ｄ間の距離はＬｂ（≠Ｌ）とする。
【００７３】
本実施形態は第１の実施形態と同様の構成及び作用において、図４の方位検出部３０の処理フロー、図７の位置姿勢算出部３２の処理フロー、及び図１０の運動制御部３４の処理フローにより、ＵＡＶ１４の飛行運動を制御する。本実施形態は、点Ｃと点Ｄ間の距離が点Ａと点Ｂ間の距離と異なるため、図１２に示すように、点Ａ、点Ｂ、点Ｃ、及び点Ｄが等脚台形を形成する。
【００７４】
＜本発明の第３の実施の形態に係る飛行制御装置の構成及び作用＞
【００７５】
本発明の第３の実施形態は、上記図２において、高さＺｈをもつ点Ａ、及び点Ｂと、高さＺｌをもつ点Ｃ、及び点Ｄの場合の例である。点Ｃと点Ｄ間の距離は点Ａと点Ｂ間の距離Ｌと同じ、あるいはＭ（≠Ｌ）とする。
【００７６】
第３の実施形態では、点Ｃ、及び点Ｄの高さをＺｈ＋Ｎに変更して、４点（点Ａ、点Ｂ、点Ｃ、及び点Ｄ）がＸｗＹｗ面において正方形あるいは等脚台形を形成する場合について説明する。本実施形態は第１の実施形態と同様の構成及び作用において、図７の位置姿勢算出部３２の処理フロー、及び図１０の運動制御部３４の処理フローにより、ＵＡＶ１４の飛行運動を制御する。以下では、第１の実施形態、第２の実施形態と異なる点のみを記載する。
【００７７】
本実施形態の方位検出部３０では、位置計測センサ１０で取得された点Ａ、点Ｂ、点Ｃ、及び点Ｄの三次元座標データを取り出し、平行移動先の三次元座標値として、以下（２０）〜（２２）式により、点Ａ以外の全ての座標から点Ａ三次元座標値をＡ＝（Ｘａ，Ｙａ，Ｚｈ）を差し引いて、Ｂｓ、Ｃｓ、及びＤｓを算出する。
【００７８】
【数１２】
2019059314.tif 000022
・・・（２０）
2019059314.tif 000023
・・・（２１）
2019059314.tif 000024
・・・（２２）
【００７９】
第１の実施形態、及び第２の実施形態と異なる点は、点Ｃｓと点ＤｓのＺ座標がＺｍ≠０となる点である。一方、点Ａを原点に平行移動した空間中の点の配置において、参照物体１２の点Ａと点Ｂの方位角は、第１の実施形態と同様に（４）式の計算で求められる。
【００８０】
位置姿勢算出部３２では、目標座標の点Ｃｔと点Ｄｔについて、以下（２３）式、及び（２４）式の計算により算出する。
【００８１】
【数１３】
2019059314.tif 000025
・・・（２３）
2019059314.tif 000026
・・・（２４）
【００８２】
また、平行移動した点Ｃｓと点Ｄｓとの中点の座標Ｇｓを以下（２５）式により算出する。
【００８３】
【数１４】
2019059314.tif 000027
・・・（２５）
【００８４】
同様に、目標座標の点Ｃｔと点Ｄｔからも、以下（２６）式により中点の座標Ｇｔを算出する。
【００８５】
【数１５】
2019059314.tif 000028
・・・（２６）
【００８６】
中点が得られると、上記（９）式により、平行移動ベクトルＴを算出する。一方、回転角θｒはＸｗＹｗ面において算出するため、上記（１３）式、及び（１４）式において、ベクトルＥｒとベクトルＥｔの算出には、Ｚ値の成分を０と与える。これにより、回転角θｒを上記（１５）式において算出する。
【００８７】
運動制御部３４では、位置姿勢算出部３２で得た平行移動ベクトルＴと回転角θｒを使って、第１の実施形態または第２の実施形態と同様にＵＡＶ１４の飛行運動を制御する。これにより、点Ｃ、及び点Ｄの高さがＺｈ＋Ｎ、点Ａ、及びと点Ｂの高さがＺｈとなり、４点（点Ａ、点Ｂ、点Ｃ、及び点Ｄ）がＸｗＹｗ面において正方形あるいは等脚台形を形成する。なお、Ｎ＝０と設定した場合は、第１の実施形態あるいは第２の実施形態になる。
【００８８】
＜本発明の第４の実施の形態に係る飛行制御装置の構成及び作用＞
【００８９】
本発明の第４の実施形態は、Ｎ台のＵＡＶ１４の飛行を同時に制御する例である。図１３に、ＵＡＶ１４＃１を先頭にしてＮ台のＵＡＶ１４が編成を組んで飛行する状況を示す。それぞれのＵＡＶ１４には位置計測センサ１０で検出可能な２つのマーカが設置されている。ＵＡＶ１４＃１の点Ａ１と点Ｂ１が第１の実施形態の点Ａと点Ｂに対応するものとし、ＵＡＶ１４＃２の点Ａ２と点Ｂ２が第１の実施形態の点Ｃと点Ｄに対応するものとする。この割り当てにより、第１の実施形態と同様に、ＵＡＶ１４＃１の点Ａ１と点Ｂ１の位置と方位に従って、ＵＡＶ１４＃２の点Ａ２と点Ｂ２が水平面において正方形を形成するようにＵＡＶ１４＃２の飛行運動が制御される。同様にして、ＵＡＶ１４＃Ｎの点ＡＮと点ＢＮを第１の実施形態の点Ｃと点Ｄに対応させ、点Ａと点Ｂを１つ前を飛行するＵＡＶ１４＃Ｎ−１の点ＡＮ−１と点ＢＮ−１とすることにより、４つの点ＡＮ−１、点ＢＮ−１、ＡＮ、及び点ＢＮが水平面において正方形を形成するように飛行することができる。
【００９０】
このようにＮ−１台目のＵＡＶ１４をＮ台目のＵＡＶ１４に対する参照物体１２として飛行制御を行う。本実施形態は第１の実施形態と同様の構成及び作用において、Ｎ台のＵＡＶ１４を、第１の実施形態と同様に図７の位置姿勢算出部３２の処理フロー、及び図１０の運動制御部３４の処理フローにより、ＵＡＶ１４の飛行運動を制御する。このとき、１台目のＵＡＶ１４については、第１の実施形態と同様に、操作者が持ち運ぶ参照物体１２に追従するように制御し、２台目以降のＵＡＶ１４については、以下のように、Ｎ−１番のＵＡＶ１４にＮ番目のＵＡＶ１４が順番に追従するように制御する。
【００９１】
本実施形態の方位検出部３０は、参照物体１２について、第１の実施形態と同様に、グローバル座標系における基準方向に対する方位角Φを算出する。
【００９２】
また、方位検出部３０は、２台目以降のＮ台目のＵＡＶ１４の各々に対し、当該Ｎ台目のＵＡＶ１４の参照物体１２としてのＮ−１台目のＵＡＶ１４について、グローバル座標系における基準方向に対する方位角Φを算出する。
【００９３】
位置姿勢算出部３２は、第１の実施形態と同様に、参照物体１２のマーカの点Ａ、及び点Ｂの三次元座標と、算出された方位角Φと、相対位置関係とに基づいて、１台目のＵＡＶ１４のマーカ点Ｃ、及び点Ｄの目標となる三次元座標を算出し、算出された目標となる三次元座標と１台目のＵＡＶ１４のマーカ点Ｃ、及び点Ｄの三次元座標との差分Ｔを算出し、回転角θを算出する。
【００９４】
また、位置姿勢算出部３２は、２台目以降のＮ台目のＵＡＶ１４の各々に対し、Ｎ−１台目のＵＡＶ１４のマーカの点Ａ、及び点Ｂの三次元座標と、算出された方位角Φと、予め定められたＮ−１台目のＵＡＶ１４のマーカの点Ａ、及び点Ｂの三次元座標及びＮ台目のＵＡＶ１４のマーカ点Ｃ、及び点Ｄの三次元座標の相対位置関係とに基づいて、Ｎ台目のＵＡＶ１４のマーカ点Ｃ、及び点Ｄの目標となる三次元座標を算出し、算出された目標となる三次元座標とＮ台目のＵＡＶ１４のマーカ点Ｃ、及び点Ｄの三次元座標との差分Ｔを算出する。また、位置姿勢算出部３２は、Ｎ台目のＵＡＶ１４のマーカ点Ｃ、及び点Ｄの三次元座標に差分を加算して得られる三次元座標と、Ｎ台目のＵＡＶ１４のマーカ点Ｃ、及び点Ｄの目標となる三次元座標とに基づいて回転角θを算出する。
【００９５】
運動制御部３４は、１台目のＵＡＶ１４における、Ｘ軸周りの回転角φ、Ｙ軸周りの回転角ω、Ｚ軸に沿った速度Ｖｚ、及びＺ軸周りの回転速度Ｖθを飛行指令データとして算出し、算出した飛行指令データを通信部５０を介して１台目のＵＡＶ１４に送信することで、飛行指令データに基づいて１台目のＵＡＶ１４の運動を制御する。
【００９６】
また、運動制御部３４は、２台目以降のＮ台目のＵＡＶ１４の各々に対し、算出された差分Ｔと、回転角θとに基づいて、Ｎ台目のＵＡＶ１４における、Ｘ軸周りの回転角φ、Ｙ軸周りの回転角ω、Ｚ軸に沿った速度Ｖｚ、及びＺ軸周りの回転速度Ｖθを飛行指令データとして算出し、算出した飛行指令データに基づいてＮ台目のＵＡＶ１４の運動を制御する。Ｎ台目のＵＡＶ１４の処理が終了したらＮ＝Ｎ＋１として同様に制御を行う。
【００９７】
また、本実施形態において、２点間の距離が異なる場合は第２の実施形態、点の高さが異なる場合は第３の実施形態に従って、複数のＵＡＶの飛行を同時に制御することができる。本実施形態では、ＵＡＶ＃１を先頭にしてＮ台のＵＡＶの編成飛行を可能とする。
【００９８】
以上の各実施形態の手法によれば、ユーザがＵＡＶに対して三次元位置と方位を示す飛行指令を出すことにより、ＵＡＶの飛行を自由自在に操作することを可能とする。複数のＵＡＶに対しても、ユーザがＵＡＶに対して三次元位置と方位を示す飛行指令を出すことにより、複数のＵＡＶ同士を衝突させることなく、空間中を自由自在に操作することを可能とする。さらに、ユーザの意図でＵＡＶを制御することにより、人を取り巻く実環境において、人の単独行動では実現不可能な作業をＵＡＶが支援する、あるいは協調作業することを可能とする。
【００９９】
なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。
【符号の説明】
【０１００】
１０   位置計測センサ
１２   参照物体
１４   ＵＡＶ
２０   演算部
３０   方位検出部
３２   位置姿勢算出部
３４   運動制御部
５０   出力部
１００ 飛行制御装置
【図１】
2019059314.tif 000029
【図２】
2019059314.tif 000030
【図３】
2019059314.tif 000031
【図４】
2019059314.tif 000032
【図５】
2019059314.tif 000033
【図６】
2019059314.tif 000034
【図７】
2019059314.tif 000035
【図８】
2019059314.tif 000036
【図９】
2019059314.tif 000037
【図１０】
2019059314.tif 000038
【図１１】
2019059314.tif 000039
【図１２】
2019059314.tif 000040
【図１３】
2019059314.tif 000041
【図１４】
2019059314.tif 000042
【図１５】
2019059314.tif 000043