(57)【要約】【課題】複数台のＵＡＶが編隊を組んで円滑かつ安定的に飛行させるように制御できる。【解決手段】飛行コマンド変換部が、リーダーＵＡＶについて更新された第１制御データと、リーダーＵＡＶについて算出された方位角とに基づいて、リーダーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいてリーダーＵＡＶの運動を制御し、複数のフォロワーＵＡＶの各々について、フォロワーＵＡＶについて更新された第２制御データと、フォロワーＵＡＶについて算出された方位角とに基づいて、フォロワーＵＡＶにおける、飛行指令データを算出し、算出した飛行指令データに基づいてフォロワーＵＡＶの運動を制御する。【選択図】図１
2019185603.tif 000002
【特許請求の範囲】
【請求項１】
複数のＵＡＶ（Unmanned Aerial Vehicle）の編隊飛行を制御し、前記編隊飛行を先導するリーダーＵＡＶと前記リーダーＵＡＶに従って編隊を形成する複数のフォロワーＵＡＶの各々を制御する飛行制御装置であって、
前記リーダーＵＡＶと前記複数のフォロワーＵＡＶの各々とに付与され、かつ、マーカー間の距離が既知の複数のマーカーの各々の三次元座標を計測する位置計測センサと、
前記位置計測センサによって計測された前記リーダーＵＡＶの前記マーカーの各々の三次元座標と、予め設定された前記リーダーＵＡＶの目標地の目標点とに基づいて、前記リーダーＵＡＶについて、前記マーカーの各々の三次元座標を結ぶ第１線分及び第２線分の交点、前記交点からの前記目標点までのベクトル、及びグローバル座標系における前記目標点に対する前記マーカーの各々の三次元座標を結んだ前記第１線分の方位角を算出するリーダー位置検出部と、
前記リーダーＵＡＶの前記目標点までのベクトルから求まる距離に基づいて、前記リーダーＵＡＶの位置を制御するための第１制御データを更新するリーダー位置制御部と、
前記複数のフォロワーＵＡＶの各々について、前記位置計測センサによって計測された前記フォロワーＵＡＶの前記マーカーの各々の三次元座標と、前記リーダーＵＡＶの第１線分及び第２線分の交点から、前記第１線分又は前記第２線分の延長線上に予め定められた距離だけ離れた座標により定まる、前記フォロワーＵＡＶの目標点とに基づいて、前記フォロワーＵＡＶについて、前記マーカーの各々の三次元座標を結ぶ第１線分及び第２線分の交点、前記交点からの前記目標点までのベクトル、及びグローバル座標系における前記目標点に対する前記マーカーの各々の三次元座標を結んだ前記第１線分の方位角を算出するフォロワー位置検出部と、
前記複数のフォロワーＵＡＶの各々について、前記フォロワーＵＡＶの前記目標点までのベクトルから求まる距離に基づいて、前記フォロワーＵＡＶの位置を制御するための第２制御データを更新するフォーメーション制御部と、
前記リーダーＵＡＶについて更新された前記第１制御データと、前記リーダーＵＡＶについて算出された前記方位角とに基づいて、前記リーダーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記リーダーＵＡＶの運動を制御し、前記複数のフォロワーＵＡＶの各々について、前記フォロワーＵＡＶについて更新された前記第２制御データと、前記フォロワーＵＡＶについて算出された前記方位角とに基づいて、前記フォロワーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記フォロワーＵＡＶの運動を制御する飛行コマンド変換部と、
を含む飛行制御装置。
【請求項２】
ｉ台目の前記フォロワーＵＡＶについて予め定められた距離は、ｉ−１台目の前記フォロワーＵＡＶの距離に対し所定の関係となるように定められる請求項１に記載の飛行制御装置。
【請求項３】
前記フォロワーＵＡＶの目標点は、
前記リーダーＵＡＶの第１線分及び第２線分の交点から、前記第１線分の延長線上に予め定められた距離だけ離れた座標により定まるか、
前記リーダーＵＡＶの第１線分及び第２線分の交点から、前記第２線分の延長線上に予め定められた距離だけ離れた座標により定まるか、又は
前記リーダーＵＡＶの第１線分及び第２線分の交点から、前記第１線分の延長線上に予め定められた距離だけ離れた座標と、前記第２線分の延長線上に予め定められた距離だけ離れた座標とにより定まる請求項１または請求項２に記載の飛行制御装置。
【請求項４】
複数のＵＡＶ（Unmanned Aerial Vehicle）の編隊飛行を制御し、前記編隊飛行を先導するリーダーＵＡＶと前記リーダーＵＡＶに従って編隊を形成する複数のフォロワーＵＡＶの各々を制御する飛行制御装置における飛行制御方法であって、
位置計測センサが、前記リーダーＵＡＶと前記複数のフォロワーＵＡＶの各々とに付与され、かつ、マーカー間の距離が既知の複数のマーカーの各々の三次元座標を計測するステップと、
リーダー位置検出部が、前記位置計測センサによって計測された前記リーダーＵＡＶの前記マーカーの各々の三次元座標と、予め設定された前記リーダーＵＡＶの目標地の目標点とに基づいて、前記リーダーＵＡＶについて、前記マーカーの各々の三次元座標を結ぶ第１線分及び第２線分の交点、前記交点からの前記目標点までのベクトル、及びグローバル座標系における前記目標点に対する前記マーカーの各々の三次元座標を結んだ前記第１線分の方位角を算出するステップと、
リーダー位置制御部が、前記リーダーＵＡＶの前記目標点までのベクトルから求まる距離に基づいて、前記リーダーＵＡＶの位置を制御するための第１制御データを更新するステップと、
飛行コマンド変換部が、前記リーダーＵＡＶについて更新された前記第１制御データと、前記リーダーＵＡＶについて算出された前記方位角とに基づいて、前記リーダーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記リーダーＵＡＶの運動を制御するステップと、
フォロワー位置検出部が、前記複数のフォロワーＵＡＶの各々について、前記位置計測センサによって計測された前記フォロワーＵＡＶの前記マーカーの各々の三次元座標と、前記リーダーＵＡＶの第１線分及び第２線分の交点から、前記第１線分又は前記第２線分の延長線上に予め定められた距離だけ離れた座標により定まる、前記フォロワーＵＡＶの目標点とに基づいて、前記フォロワーＵＡＶについて、前記マーカーの各々の三次元座標を結ぶ第１線分及び第２線分の交点、前記交点からの前記目標点までのベクトル、及びグローバル座標系における前記目標点に対する前記マーカーの各々の三次元座標を結んだ前記第１線分の方位角を算出するステップと、
フォーメーション制御部が、前記複数のフォロワーＵＡＶの各々について、前記フォロワーＵＡＶの前記目標点までのベクトルから求まる距離に基づいて、前記フォロワーＵＡＶの位置を制御するための第２制御データを更新するステップと、
前記飛行コマンド変換部が、前記複数のフォロワーＵＡＶの各々について、前記フォロワーＵＡＶについて更新された前記第２制御データと、前記フォロワーＵＡＶについて算出された前記方位角とに基づいて、前記フォロワーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記フォロワーＵＡＶの運動を制御するステップと、
を含む飛行制御方法。
【請求項５】
ｉ台目の前記フォロワーＵＡＶについて予め定められた距離は、ｉ−１台目の前記フォロワーＵＡＶの距離に対し所定の関係となるように定められる請求項４に記載の飛行制御方法。
【請求項６】
前記フォロワーＵＡＶの目標点は、
前記リーダーＵＡＶの第１線分及び第２線分の交点から、前記第１線分の延長線上に予め定められた距離だけ離れた座標により定まるか、
前記リーダーＵＡＶの第１線分及び第２線分の交点から、前記第２線分の延長線上に予め定められた距離だけ離れた座標により定まるか、又は
前記リーダーＵＡＶの第１線分及び第２線分の交点から、前記第１線分の延長線上に予め定められた距離だけ離れた座標と、前記第２線分の延長線上に予め定められた距離だけ離れた座標とにより定まる請求項４または請求項５に記載の飛行制御方法。
【請求項７】
コンピュータを、請求項１〜請求項３のいずれか１項に記載の飛行制御装置の各部として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、飛行制御装置、方法、及びプログラムに係り、特に、無人飛行機を制御する飛行制御装置、方法、及びプログラムに関する。
【背景技術】
【０００２】
クアッドロータ型ＵＡＶ（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.
【非特許文献３】M. Iskandarani, S. N. Givigi, G. Fusina, and A. Beaulieu: "Unmanned Aerial Vehicle Formation Flying using Linear Model Predictive Control", 2014 8th Annual IEEE Systems Conference(SysCon), 2014.
【発明の概要】
【発明が解決しようとする課題】
【０００８】
複数台のＵＡＶを使った編隊飛行の例を図１７に示す。一般的に、編隊飛行では、リーダー・フォロワー編隊（ｌｅａｄｅｒ−ｆｏｌｌｏｗｅｒ ｆｏｒｍａｔｉｏｎ）により飛行する。飛行全体を指導するリーダー役のＵＡＶに対して、フォロワー役のＵＡＶが追従して飛行する。トレイルとは、リーダーのＵＡＶを先頭に、フォロワーのＵＡＶが縦一列に並んで飛行し、アブレストとは、リーダーのＵＡＶを中心に、フォロワーのＵＡＶが横一列に並んで飛行する。これに対して、デルタでは、リーダーのＵＡＶを先頭に、フォロワーのＵＡＶが三角形を形成するように追従して飛行する。
【０００９】
非特許文献１と非特許文献２によれば、所定の経路に従って飛行するように、ＵＡＶの運動を適切に制御することができる。しかしながら、これらの技術は単体のＵＡＶを所定の経路に沿って目標地に到達することを目的とした制御であるが、編隊飛行においてはＵＡＶ間で衝突しないようにそれぞれのＵＡＶの運動を制御することが保証されていない。
【００１０】
一方、障害物あるいは人との衝突を避けるため、特定の場所を飛行しないように空間中に拘束条件を設定することができる。また、飛行運動中の飛行トラブルを回避するため、ＵＡＶには多様なセンサやカメラが取り付けられている場合がある。例えば、超音波センサにより地面あるいは床からの高度を計測し、ジャイロセンサと加速度センサによって機体の速度と姿勢を計測することができる。また、移動物体を検出しながら、空間中を自律的に飛行させることにカメラが利用されている。これらの機体から計測した空間情報や画像情報を使うことも考えられるが、大半のＵＡＶに内蔵されているセンサの精度はミリ単位での正確な精度で計測できることを保証しておらず、別の手段を使って機体の正確な位置と方位を計測する必要がある。
【００１１】
非特許文献３では図１７に示した編隊飛行を制御する方法が公開されているが、その方法は、ＵＡＶに与える入力データと出力（運動）データの関係が数式モデルで与えられる場合に限定される。しかしながら、市販のクアッドロータ型ＵＡＶは入出力特性が不明のブラックボックスであるため、各機体の正確な位置と方位を計測しながら編隊飛行を制御する必要がある。
【００１２】
本発明は、上記問題点を解決するために成されたものであり、複数台のＵＡＶが編隊を組んで円滑かつ安定的に飛行させるように制御できる飛行制御装置、方法、及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１３】
上記目的を達成するために、第１の発明に係る飛行制御装置は、複数のＵＡＶ（Unmanned Aerial Vehicle）の編隊飛行を制御し、前記編隊飛行を先導するリーダーＵＡＶと前記リーダーＵＡＶに従って編隊を形成する複数のフォロワーＵＡＶの各々を制御する飛行制御装置であって、前記リーダーＵＡＶと前記複数のフォロワーＵＡＶの各々とに付与され、かつ、マーカー間の距離が既知の複数のマーカーの各々の三次元座標を計測する位置計測センサと、前記位置計測センサによって計測された前記リーダーＵＡＶの前記マーカーの各々の三次元座標と、予め設定された前記リーダーＵＡＶの目標地の目標点とに基づいて、前記リーダーＵＡＶについて、前記マーカーの各々の三次元座標を結ぶ第１線分及び第２線分の交点、前記交点からの前記目標点までのベクトル、及びグローバル座標系における前記目標点に対する前記マーカーの各々の三次元座標を結んだ前記第１線分の方位角を算出するリーダー位置検出部と、前記リーダーＵＡＶの前記目標点までのベクトルから求まる距離に基づいて、前記リーダーＵＡＶの位置を制御するための第１制御データを更新するリーダー位置制御部と、前記複数のフォロワーＵＡＶの各々について、前記位置計測センサによって計測された前記フォロワーＵＡＶの前記マーカーの各々の三次元座標と、前記リーダーＵＡＶの第１線分及び第２線分の交点から、前記第１線分又は前記第２線分の延長線上に予め定められた距離だけ離れた座標により定まる、前記フォロワーＵＡＶの目標点とに基づいて、前記フォロワーＵＡＶについて、前記マーカーの各々の三次元座標を結ぶ第１線分及び第２線分の交点、前記交点からの前記目標点までのベクトル、及びグローバル座標系における前記目標点に対する前記マーカーの各々の三次元座標を結んだ前記第１線分の方位角を算出するフォロワー位置検出部と、前記複数のフォロワーＵＡＶの各々について、前記フォロワーＵＡＶの前記目標点までのベクトルから求まる距離に基づいて、前記フォロワーＵＡＶの位置を制御するための第２制御データを更新するフォーメーション制御部と、前記リーダーＵＡＶについて更新された前記第１制御データと、前記リーダーＵＡＶについて算出された前記方位角とに基づいて、前記リーダーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記リーダーＵＡＶの運動を制御し、前記複数のフォロワーＵＡＶの各々について、前記フォロワーＵＡＶについて更新された前記第２制御データと、前記フォロワーＵＡＶについて算出された前記方位角とに基づいて、前記フォロワーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記フォロワーＵＡＶの運動を制御する飛行コマンド変換部と、を含んで構成されている。
【００１４】
第２の発明に係る飛行制御方法は、複数のＵＡＶ（Unmanned Aerial Vehicle）の編隊飛行を制御し、前記編隊飛行を先導するリーダーＵＡＶと前記リーダーＵＡＶに従って編隊を形成する複数のフォロワーＵＡＶの各々を制御する飛行制御装置における飛行制御方法であって、位置計測センサが、前記リーダーＵＡＶと前記複数のフォロワーＵＡＶの各々とに付与され、かつ、マーカー間の距離が既知の複数のマーカーの各々の三次元座標を計測するステップと、リーダー位置検出部が、前記位置計測センサによって計測された前記リーダーＵＡＶの前記マーカーの各々の三次元座標と、予め設定された前記リーダーＵＡＶの目標地の目標点とに基づいて、前記リーダーＵＡＶについて、前記マーカーの各々の三次元座標を結ぶ第１線分及び第２線分の交点、前記交点からの前記目標点までのベクトル、及びグローバル座標系における前記目標点に対する前記マーカーの各々の三次元座標を結んだ前記第１線分の方位角を算出するステップと、リーダー位置制御部が、前記リーダーＵＡＶの前記目標点までのベクトルから求まる距離に基づいて、前記リーダーＵＡＶの位置を制御するための第１制御データを更新するステップと、飛行コマンド変換部が、前記リーダーＵＡＶについて更新された前記第１制御データと、前記リーダーＵＡＶについて算出された前記方位角とに基づいて、前記リーダーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記リーダーＵＡＶの運動を制御するステップと、フォロワー位置検出部が、前記複数のフォロワーＵＡＶの各々について、前記位置計測センサによって計測された前記フォロワーＵＡＶの前記マーカーの各々の三次元座標と、前記リーダーＵＡＶの第１線分及び第２線分の交点から、前記第１線分又は前記第２線分の延長線上に予め定められた距離だけ離れた座標により定まる、前記フォロワーＵＡＶの目標点とに基づいて、前記フォロワーＵＡＶについて、前記マーカーの各々の三次元座標を結ぶ第１線分及び第２線分の交点、前記交点からの前記目標点までのベクトル、及びグローバル座標系における前記目標点に対する前記マーカーの各々の三次元座標を結んだ前記第１線分の方位角を算出するステップと、フォーメーション制御部が、前記複数のフォロワーＵＡＶの各々について、前記フォロワーＵＡＶの前記目標点までのベクトルから求まる距離に基づいて、前記フォロワーＵＡＶの位置を制御するための第２制御データを更新するステップと、前記飛行コマンド変換部が、前記複数のフォロワーＵＡＶの各々について、前記フォロワーＵＡＶについて更新された前記第２制御データと、前記フォロワーＵＡＶについて算出された前記方位角とに基づいて、前記フォロワーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記フォロワーＵＡＶの運動を制御するステップと、を含んで実行することを特徴とする。
【００１５】
第３の発明に係るプログラムは、コンピュータを、第１の発明に係る飛行制御装置の各部として機能させるためのプログラムである。
【発明の効果】
【００１６】
本発明の飛行制御装置、方法、及びプログラムによれば、リーダーＵＡＶについて更新された第１制御データと、リーダーＵＡＶについて算出された方位角とに基づいて、リーダーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいてリーダーＵＡＶの運動を制御し、複数のフォロワーＵＡＶの各々について、フォロワーＵＡＶについて更新された第２制御データと、フォロワーＵＡＶについて算出された方位角とに基づいて、フォロワーＵＡＶにおける、飛行指令データを算出し、算出した飛行指令データに基づいてフォロワーＵＡＶの運動を制御することにより、複数台のＵＡＶが編隊を組んで円滑かつ安定的に飛行させるように制御できる、という効果が得られる。
【図面の簡単な説明】
【００１７】
【図１】本発明の実施形態に係る飛行制御装置の構成を示すブロック図である。
【図２】ＵＡＶの飛行を制御する状況の一例を示す図である。
【図３】本発明の実施形態に係る飛行制御装置における飛行制御処理ルーチンを示すフローチャートである。
【図４】飛行制御処理ルーチンにおけるリーダー位置検出処理のフローチャートである。
【図５】マーカーの点ａ、点ｂ、及び目標地の点ｐをＺｗ軸方向から視た位置と方位の関係を表す図である。
【図６】飛行制御処理ルーチンにおけるリーダー位置制御処理のフローチャートである。
【図７】飛行コマンド変換処理ルーチンにおける飛行コマンド変換処理のフローチャートである。
【図８】飛行コマンド変換処理ルーチンにおけるフォロワー位置検出処理のフローチャートである。
【図９】トレイル・フォーメーションのリーダーＵＡＶとフォロワーＵＡＶとの位置関係を示す図である。
【図１０】飛行コマンド変換処理ルーチンにおけるフォーメーション制御処理のフローチャートである。
【図１１】アブレスト・フォーメーションのリーダーＵＡＶとフォロワーＵＡＶとの位置関係を示す図である。
【図１２】デルタ・フォーメーションのリーダーＵＡＶとフォロワーＵＡＶとの位置関係を示す図である。
【図１３】デルタ・フォーメーションの制御後のリーダーＵＡＶとフォロワーＵＡＶとの位置関係を示す図である。
【図１４】幾何パターンに基づいたＵＡＶの飛行経路の例を示す図である。
【図１５】ＵＡＶの概観とＵＡＶ固定のローカル座標系の一例を示す図である。
【図１６】グローバル座標系とＵＡＶ固定のローカル座標系の関係の一例を示す図である。
【図１７】編隊飛行のフォーメーションの例を示す図である。
【発明を実施するための形態】
【００１８】
以下、図面を参照して本発明の実施形態を詳細に説明する。本発明の実施形態に係る手法は、各ＵＡＶに対して３次元位置と方位を示す飛行指令を適切に出すことにより、複数のＵＡＶ同士を衝突させることなく、編隊を組んで自由自在に自律飛行することを可能とする。さらに、ユーザの意図で各ＵＡＶを制御することにより、人を取り巻く実環境において、人の単独行動では実現不可能な作業を複数のＵＡＶが支援する、あるいは協調作業することを可能とする。
【００１９】
＜本発明の第１の実施形態に係る飛行制御装置の構成＞
本発明の第１の実施形態に係るクアッドロータ型ＵＡＶの飛行制御装置の構成について説明する。本実施形態は複数台のＵＡＶによる編隊飛行を制御する形態である。
【００２０】
初期状態において、各ＵＡＶは任意の位置で飛行しているとする。本実施形態では、図１７（Ａ）に示したように、リーダー・フォロワー編隊においてリーダーＵＡＶ１４を先頭に、フォロワーＵＡＶ１５が縦一列に並んで飛行するトレイルをフォーメーションとして説明する。
【００２１】
図１に示すように、本発明の第１の実施形態に係る飛行制御装置１００は、ＣＰＵと、ＲＡＭと、後述する飛行制御処理ルーチンを実行するためのプログラムや各種データを記憶したＲＯＭと、を含むコンピュータで構成することが出来る。この飛行制御装置１００は、機能的には図１に示すように位置計測センサ１０と、演算部２０と、通信部５０とを備えている。この構成において、位置計測センサ１０は必ずしも構成要素として接続している必要はなく、処理に必要なデータを取得すればよく、演算部２０におけるリーダー位置検出部３０、リーダー位置制御部３２、飛行コマンド変換部３４、フォロワー位置検出部４０、フォーメーション制御部４２のからそれぞれの矢印へのデータの流れは、ハードディスク、ＲＡＩＤ装置、ＣＤ−ＲＯＭなどの記録媒体を利用する、または、ネットワークを介してリモートなデータ資源を利用する形態でもどちらでも構わない。
【００２２】
位置計測センサ１０は、各ＵＡＶに付与され、かつ、マーカー間の距離が既知の複数のマーカーの各々の三次元座標を計測データとして計測する。一般的に、モーションキャプチャ装置は、所定のマーカーの３次元座標をリアルタイムで高精度に計測できることが知られている。本実施形態では、位置計測センサ１０の例として、モーションキャプチャ装置を利用する。図２に示す状況において、位置計測センサ１０はグローバル座標系において３次元座標を計測するようにセットアップされており、各ＵＡＶに付与された各マーカーの３次元座標が、ある一定間隔で逐次計測される。目標地は点ｐで示されており、その座標値は飛行計画に応じて任意に設定する。以降では、３台のＵＡＶによる編隊飛行制御の例を説明するが、４台以上のＵＡＶを利用する場合にも本発明の実施形態の手法を容易に拡張することができることは言うまでもない。
【００２３】
リーダーＵＡＶ１４には点ａ、点ｂ、点ｃ、並びに点ｄの位置にセンシング用のマーカーが取り付けられている。点ａと点ｂとを結ぶ第１線分が点ｃと点ｄとを結ぶ第２線分と点ｅの位置で直交しているとする。点ｅの位置は、線分が直交する条件を満たせばよく、各線分の中点である必要はない。フォロワーＵＡＶ１５＃１には点ａ１、点ｂ１、点ｃ１、及び点ｄ１にセンシング用のマーカーが取り付けられている。点ａ１と点ｂ１とを結ぶ第１線分が点ｃ１と点ｄ１とを結ぶ第２線分と点ｅ１の位置で直交しているとする。同様に、フォロワーＵＡＶ１５＃２には点ａ２、点ｂ２、点ｃ２、並びに点ｄ２の位置にセンシング用のマーカーが取り付けられている。点ａ２と点ｂ２とを結ぶ第１線分が点ｃ２と点ｄ２とを結ぶ第２線分と点ｅ２の位置で直交しているとする。フォロワーＵＡＶ１５において、点ｅ１と点ｅ２の位置は、線分が直交する条件を満たせばよく、各線分の中点である必要はない。
【００２４】
演算部２０は、位置計測センサ１０によりグローバル座標系におけるリーダーＵＡＶ１４の位置を検出するリーダー位置検出部３０と、所定の経路に沿ってリーダーＵＡＶ１４の飛行を制御するリーダー位置制御部３２と、リーダーＵＡＶ１４及びフォロワーＵＡＶ１５に指令データを送出するための飛行コマンド変換部３４と、位置計測センサ１０によりグローバル座標系におけるフォロワーＵＡＶ１５の位置を検出するフォロワー位置検出部４０と、リーダーＵＡＶ１４の位置に基づいてフォロワーＵＡＶ１５の飛行を制御するフォーメーション制御部４２とを含んで構成されている。なお各処理部の具体的な処理内容は後述の作用の説明において説明する。
【００２５】
リーダー位置検出部３０は、位置計測センサ１０によって計測されたリーダーＵＡＶ１４のマーカーの点ａ、点ｂ、点ｃ、及び点ｄの三次元座標と、予め設定されたＵＡＶ１４の目標地１２の目標点ｐとに基づいて、マーカーの点ａ、点ｂの三次元座標を結ぶ第１線分及び点ｃ、点ｄを結ぶ第２線分の交点ｅ、交点ｅからの目標点ｐまでのベクトルＴ、及びグローバル座標系における目標点ｐに対するマーカーの点ａ、点ｂの三次元座標を結んだ第１線分の方位角θを算出する。
【００２６】
リーダー位置制御部３２は、リーダーＵＡＶ１４の目標点ｐまでのベクトルＴから求まる距離||Ｔ||に基づいて、リーダーＵＡＶ１４の位置を制御するための第１制御データｕを更新する。
【００２７】
飛行コマンド変換部３４は、リーダーＵＡＶ１４について、更新された制御データｕと、算出された方位角θとに基づいて、リーダーＵＡＶ１４における、Ｘ軸周りの回転速度Ｖx、Ｙ軸周りの回転速度Ｖy、Ｚ軸に沿った速度Ｖｚ、及びＺ軸周りの回転速度Ｖθを飛行指令データとして算出し、飛行指令データを通信部５０を介してリーダーＵＡＶ１４に送信することで、算出した飛行指令データに基づいてリーダーＵＡＶ１４の運動を制御する。また、複数のフォロワーＵＡＶ１５の各々についても同様に、フォロワーＵＡＶ１５について、後述するフォーメーション制御部４２で更新された制御データｕｉと、算出された方位角θｉとに基づいて、フォロワーＵＡＶ１５における、Ｘ軸周りの回転速度Ｖx、Ｙ軸周りの回転速度Ｖy、Ｚ軸に沿った速度Ｖｚ、及びＺ軸周りの回転速度Ｖθを飛行指令データとして算出し、飛行指令データを通信部５０を介してフォロワーＵＡＶ１５に送信することで、算出した飛行指令データに基づいてフォロワーＵＡＶ１５の運動を制御する。
【００２８】
フォロワー位置検出部４０は、複数のフォロワーＵＡＶ１５の各々について、位置計測センサ１０によって計測されたフォロワーＵＡＶ１５のマーカーの各々の三次元座標と、リーダーＵＡＶ１４の第１線分及び第２線分の交点ｅから、第１線分又は第２線分の延長線上に予め定められた距離だけ離れた座標により定まる、フォロワーＵＡＶ１５の目標点ｐｉとに基づいて、フォロワーＵＡＶ１５について、マーカーの点ａｉ、点ｂｉの三次元座標を結ぶ第１線分及び点ｃｉ、点ｄｉを結ぶ第２線分の交点、交点からの目標点ｐｉまでのベクトルＴｉ、及びグローバル座標系における目標点ｐｉに対するマーカーの点ａｉ、点ｂｉの三次元座標を結んだ第１線分の方位角を算出する。
【００２９】
フォーメーション制御部４２は、複数のフォロワーＵＡＶ１５の各々について、フォロワーＵＡＶ１５の目標点ｐｉまでのベクトルＴｉから求まる距離||Ｔｉ||に基づいて、フォロワーＵＡＶ１５の位置を制御するための第２制御データｕｉを更新する。
【００３０】
＜本発明の第１の実施形態に係る飛行制御装置の作用＞
次に、本発明の第１の実施形態に係る飛行制御装置１００の作用について説明する。
【００３１】
リーダーＵＡＶ１４及び複数のフォロワーＵＡＶ１５の各々の三次元座標の計測を開始すると、飛行制御装置１００は、図３に示す飛行制御処理ルーチンを実行する。
【００３２】
まず、ステップＳ１００では、リーダー位置検出部３０が、位置計測センサ１０によって計測されたリーダーＵＡＶ１４のマーカーの点ａ、点ｂ、点ｃ、及び点ｄの三次元座標と、予め設定されたリーダーＵＡＶ１４の目標地１２の目標点ｐとに基づいて、マーカーの点ａ、点ｂの三次元座標を結ぶ第１線分及び点ｃ、点ｄを結ぶ第２線分の交点ｅ、交点ｅからの目標点ｐまでのベクトルＴ、及びグローバル座標系における目標点ｐに対するマーカーの点ａ、点ｂの三次元座標を結んだ第１線分の方位角θを算出する。
【００３３】
次に、ステップＳ１０２では、リーダー位置制御部３２が、リーダーＵＡＶ１４の目標点ｐまでのベクトルＴから求まる距離||Ｔ||に基づいて、リーダーＵＡＶ１４の位置を制御するための第１制御データｕを更新する。
【００３４】
ステップＳ１０４では、飛行コマンド変換部３４が、リーダーＵＡＶ１４について、ステップＳ１０２で更新された制御データｕと、算出された方位角θとに基づいて、リーダーＵＡＶ１４における、Ｘ軸周りの回転速度Ｖx、Ｙ軸周りの回転速度Ｖy、Ｚ軸に沿った速度Ｖｚ、及びＺ軸周りの回転速度Ｖθを飛行指令データとして算出し、飛行指令データを通信部５０を介してリーダーＵＡＶ１４に送信することで、算出した飛行指令データに基づいてリーダーＵＡＶ１４の運動を制御する。
【００３５】
ステップＳ１０６では、複数のフォロワーＵＡＶ１５を順番に制御するため、フォロワーＵＡＶ１５＃ｉ（ｉ＝１，２，・・・）を選択する。
【００３６】
ステップＳ１０８では、フォロワー位置検出部４０が、位置計測センサ１０によって計測されたフォロワーＵＡＶ１５＃ｉのマーカーの各々の三次元座標と、リーダーＵＡＶ１４の第１線分及び第２線分の交点ｅから、リーダーＵＡＶ１４の第１線分又は第２線分の延長線上に予め定められた距離だけ離れた座標により定まる、フォロワーＵＡＶ１５の目標点ｐｉとに基づいて、フォロワーＵＡＶ１５＃ｉについて、マーカーの点ａｉ、点ｂｉの三次元座標を結ぶ第１線分及び点ｃｉ、点ｄｉを結ぶ第２線分の交点、交点からの目標点ｐｉまでのベクトルＴｉ、及びグローバル座標系における目標点ｐｉに対するマーカーの点ａｉ、点ｂｉの三次元座標を結んだ第１線分の方位角を算出する。
【００３７】
ステップＳ１１０では、フォーメーション制御部４２は、フォロワーＵＡＶ１５＃ｉについて、フォロワーＵＡＶ１５＃ｉの目標点ｐｉまでのベクトルＴｉから求まる距離||Ｔｉ||に基づいて、フォロワーＵＡＶ１５＃ｉの位置を制御するための第２制御データｕｉを更新する。
【００３８】
ステップＳ１１２では、飛行コマンド変換部３４が、フォロワーＵＡＶ１５＃ｉについて、ステップＳ１１０で更新された第２制御データｕｉと、算出された方位角θｉとに基づいて、フォロワーＵＡＶ１５＃ｉにおける、Ｘ軸周りの回転速度Ｖx、Ｙ軸周りの回転速度Ｖy、Ｚ軸に沿った速度Ｖｚ、及びＺ軸周りの回転速度Ｖθを飛行指令データとして算出し、飛行指令データを通信部５０を介してフォロワーＵＡＶ１５＃ｉに送信することで、算出した飛行指令データに基づいてフォロワーＵＡＶ１５＃ｉの運動を制御する。
【００３９】
ステップＳ１１４では、全てのフォロワーＵＡＶ１５＃ｉについて制御を終了したかを判定し、終了していなければステップＳ１０６に戻って次のフォロワーＵＡＶ１５＃ｉを選択して処理を繰り返し、終了していれば飛行制御処理ルーチンを終了する。
【００４０】
次に、ステップＳ１００のリーダー位置検出部３０の位置検出処理の詳細について説明する。
【００４１】
図４はリーダー位置検出部３０の処理のフロー図である。リーダー位置検出部３０は処理を開始すると、ステップＳ１０００で、グローバル座標系における目標地１２の目標点ｐを設定する。目標点ｐは操作者が決定した任意の三次元座標でよい。
【００４２】
ステップＳ１００２で、リーダーＵＡＶ１４のマーカーの点ａ、点ｂ、点ｃ、及び点ｄを位置計測センサ１０により検出し、その交点ｅの位置を算出する。交差する位置ｅにセンシング用のマーカーを設置してもよいが、本実施形態では汎用性のために、点ｅの位置を４つのマーカーの点ａ、点ｂ、点ｃ、及び点ｄから計算で求める。本処理では、点ｅの３次元座標を現在位置と称しており、以降では、点ｅをリーダーＵＡＶ１４の現在位置とする。
【００４３】
ステップＳ１００４で、リーダーＵＡＶ１４の現在位置（点ｅ）から目標地（点ｐ）までのベクトルＴを、Ｔ＝ｐ−ｅの計算により求める。
【００４４】
ステップＳ１００６で、点ａから点ｂへのベクトルａ−ｂがベクトルＴと直交するために必要な回転角（方位角）をベクトル内積の関係を利用して算出する。図５は、マーカーの点ａ、マーカーの点ｂ、及び目標地の点ｐをＺｗ軸方向から視た位置と方位の関係を表す。方位角θ＝０のとき、ベクトルａ−ｂがベクトルＴと直交する。
【００４５】
ステップＳ１００８で、位置検出の処理を停止するかを判定し、処理を停止する場合にはリーダー位置検出部３０の処理を終了し、終了しない場合にはステップＳ１００２に戻って処理を繰り返す。なお、処理を停止する場合とは、ここでは操作者がリーダーＵＡＶ１４の飛行制御を終了する場合とし、以下の処理においても同様である。
【００４６】
リーダー位置検出部３０では、以上の処理により、リーダーＵＡＶの現在位置（点ｅ）と、目標点ｐまでのベクトルＴと、方位角θとを算出する。
【００４７】
次に、ステップＳ１０２のリーダー位置制御部３２の制御処理の詳細について説明する。本処理では、リーダー位置検出部３０で得たベクトルＴに基づき、現在位置から目標地までの直線飛行を制御するための処理フローを説明する。
【００４８】
図６はリーダー位置制御部３２の処理のフロー図である。リーダー位置制御部３２は処理を開始すると、ステップＳ１１００で、許容距離を指定する。許容距離とは、リーダーＵＡＶ１４の現在位置ｅが目標地の点ｐに到達したかどうかを判定するための距離ΔＬである。ＵＡＶの飛行には空気抵抗によるゆらぎが影響を与えるため、許容距離ΔＬの例として１０センチメートルとする。
【００４９】
リーダー位置検出部３０において、リーダーＵＡＶ１４の現在位置（点ｅ）が位置計測センサ１０を使って時系列に得られており、現在位置を取得するたびに本処理ルーチンのステップＳ１１０２に与えられる（図４及び図６にはＸと記した）。
【００５０】
ステップＳ１１０２で、与えられた現在位置のデータにより、現在位置ｅから目標地の点ｐまでの距離||Ｔ||を算出する。||・||はベクトルのノルム（大きさ）を表す。
【００５１】
ステップＳ１１０４で、距離||Ｔ||が許容距離ΔＬ以下（||Ｔ||≦ΔＬ）であるか否かを判定する。||Ｔ||≦ΔＬであればステップＳ１１０６へ移行する。||Ｔ||≦ΔＬでなく、||Ｔ||＞ΔＬであれば運動を制御するためステップＳ１１０８へ移行する。
【００５２】
ステップＳ１１０６で、ホバリング状態でリーダーＵＡＶ１４を待機させるように制御し、ステップＳ１１０２へ移行する。
【００５３】
ステップＳ１１０８で、第１制御データを
2019185603.tif 000003
の計算により求める。第１制御データｕは、飛行コマンド変換部３４においてリーダーＵＡＶ１４への飛行指令として使われる。
【００５４】
2019185603.tif 000004
はフィードバック制御におけるゲインパラメータであり、ユーザが状況に応じて設定する。第１制御データｕが計算されると、飛行コマンド変換部３４に渡される（図６にはＹと記した）。
【００５５】
この反復を続けることによりリーダーＵＡＶ１４は目標地の点ｐに近づき、||Ｔ||≦ΔＬを満たすとき、リーダーＵＡＶ１４が目標地に到達したと判定してホバリング状態になる。
【００５６】
ステップＳ１１００で、位置制御の処理を停止するかを判定し、処理を停止する場合にはリーダー位置制御部３２の処理を終了し、終了しない場合にはステップＳ１１０２に戻って処理を繰り返す。
【００５７】
以上の処理によって、リーダー位置制御部３２は、リーダーＵＡＶ１４の現在位置ｅから目標地の点ｐまでの距離||Ｔ||に応じて第１制御データｕを出力する。
【００５８】
次に、ステップＳ１０４の飛行コマンド変換部３４の飛行コマンド変換処理の詳細について説明する。
【００５９】
飛行コマンド変換部３４の処理により、リーダーＵＡＶ１４について、リーダー位置制御部３２より与えられる制御データｕとリーダー位置検出部３０で得た方位θを、リーダーＵＡＶ１４に応じて飛行コマンドの飛行指令データへ変換して通信部５０から無線経由で指令する。ＵＡＶ１４への飛行コマンドには様々なデータ形式が存在するが、本実施形態では、市販製品のAR Drone 2.0を例にした場合を示す。ただし、それ以外のＵＡＶ１４の飛行を制御する場合にも、本実施形態を利用できることは言うまでもない。また、以下はリーダーＵＡＶ１４の場合を例に説明するが、リーダーＵＡＶ１４と同様に、複数のフォロワーＵＡＶ１５の各々についても、第１制御データｕｉと方位角θｉとを飛行指令データに変換して制御する。
【００６０】
図７は飛行コマンド変換部３４の処理のフロー図である。飛行コマンド変換部３４は処理を開始すると、ステップＳ１２００で、更新された制御データｕと、算出された方位角θとの入力を受け付ける。
【００６１】
ステップＳ１２０２で、制御データｕと方位角θとの組み合わせを、リーダーＵＡＶ１４へ送信するための飛行指令データへ変換する。リーダーＵＡＶ１４への飛行指令データは、機体に設定されたＸ軸周りの回転速度Ｖｘ、Ｙ軸周りの回転速度Ｖｙ、Ｚ軸に沿った速度Ｖｚと、Ｚ軸周りの回転速度Ｖθになる。制御データｕがｕ＝(ｕｘ,ｕｙ,ｕｚ)と与えられるとする。ＵＡＶ１４では、Ｘ軸周りの回転がＹ軸の並進運動を生み出し、Ｙ軸周りの回転がＸ軸の並進運動を生み出するため、本処理では、リーダーＵＡＶ１４に与える飛行指令データを以下（１）式により変換する。
【００６２】
【数１】
2019185603.tif 000005
・・・（１）
【００６３】
係数αｘ、αｙ、αｚ、αθはAR Drone 2.0が扱うことができる変換係数であり、パラメータとしてユーザが決めてよく、例えば、αｘ＝αｙ＝αｚ＝αθ＝０．１と与える。
【００６４】
ステップＳ１２０４で、通信部５０を介して無線通信により（１）式で算出した飛行指令データをＵＡＶ１４へ送信する。
【００６５】
本処理は、リーダー位置制御部３２で得た制御データｕとリーダー位置検出部３０で得た方位角θが与えられるたびに、（１）式で算出した飛行指令データをリーダーＵＡＶ１４へ送り続ける。図５で示したように、方位角θを送出することにより、目標地の点ｐに対するリーダーＵＡＶ１４の向きはマーカーの点ａとマーカーの点ｂは点Ａと点Ｂとに一致するように補正される。
【００６６】
図８はフォロワー位置検出部４０の処理のフロー図である。図８において、フォロワーＵＡＶ１５＃ｉの位置を検出する。ｉ＝１の場合がフォロワーＵＡＶ１５＃１に関するフォロワー位置検出部４０の処理フローであり、ｉ＝２の場合がフォロワーＵＡＶ１５＃２に関するフォロワー位置検出部４０の処理フローである。
【００６７】
フォロワー位置検出部４０は処理を開始すると、ステップＳ１３００で、フォロワーＵＡＶ１５＃ｉに対して、トレイル・フォーメーションにおける目標地の３次元座標ｐｉを設定する。点ｐｉはリーダーＵＡＶ１４の現在位置ｅから点ｃと点ｄを結ぶ第２線分の延長線上に距離Ｌｉだけ離れた座標値とする。つまり、リーダーＵＡＶ１４の第１線分及び第２線分の交点ｅから、第２線分の延長線上に距離Ｌｉだけ離れた座標により定まる。リーダーＵＡＶ１４の運動に従ってフォーメーション飛行するため機体座標系が世界座標系と考えると、ｐｉ＝ｅ＋（−Ｌｉ，０，０），Ｌｉ＞０と与える。図１７（Ａ）のトレイル・フォーメーションで示したように、リーダーＵＡＶ１４を先頭にして飛行する場合、フォロワーＵＡＶ１５＃１についてはｐ１＝ｅ＋（−Ｌ１，０，０）と設定し、フォロワーＵＡＶ１５＃２についてはｐ２＝ｅ＋（−Ｌ２，０，０）と設定する（リーダーＵＡＶ１４の進行方向をＸ軸の正の方向とするため、後方から追従して飛行するフォロワーＵＡＶ１５の位置はＸ軸の負の値になる）。ただし、衝突防止のため、フォロワーＵＡＶ１５＃１とフォロワーＵＡＶ１５＃２が十分に離れてフォーメーションを形成するようにＬ２＞＞Ｌ１とする。このようにフォロワーＵＡＶ１５＃ｉの距離を所定の関係となるように設定する。図９左に、リーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉの初期状態での位置関係を示す。
【００６８】
ステップＳ１３０２で、フォロワーＵＡＶ１５＃ｉに装着したマーカー点ａｉ、点ｂｉ、点ｃｉ、及び点ｄｉを位置計測センサ１０により検出して、交点ｅｉの位置を算出する。点ｅｉの座標はフォロワーＵＡＶ１５＃ｉの現在位置となる。
【００６９】
次に、ステップＳ１３０４で、図９左に示すように、フォロワーＵＡＶ１５＃ｉの現在位置（点ｅｉ）から目標地（点ｐｉ）までのベクトルＴｉを、Ｔｉ＝ｐｉ−ｅｉの計算により求める。
【００７０】
ステップＳ１３０６で、点ａｉから点ｂｉへのベクトルａｉ−ｂｉがベクトルＴｉとなす回転角θｉ（方位角）をベクトル内積の関係を利用して算出する。
【００７１】
ステップＳ１３０８で、位置検出の処理を停止するかを判定し、処理を停止する場合にはフォロワー位置検出部４０の処理を終了し、終了しない場合にはステップＳ１３０２に戻って処理を繰り返す。
【００７２】
以上の処理により、フォロワー位置検出部４０ではフォロワーＵＡＶ１５＃ｉの現在位置（点ｅｉ）と、フォロワーＵＡＶ１５＃ｉの現在位置（点ｅｉ）から目標地（点ｐｉ）までのベクトルＴｉと、フォロワーＵＡＶ１５＃ｉの方位角θｉを算出する。
【００７３】
図１０はフォーメーション制御部４２の処理のフロー図である。図１０において、フォロワーＵＡＶ１５＃ｉの図９右のフォーメーションとなるように制御する。ｉ＝１の場合がフォロワーＵＡＶ１５＃１に関するフォーメーション制御部４２の処理フローであり、ｉ＝２の場合がフォロワーＵＡＶ１５＃２に関するフォーメーション制御部４２の処理フローである。
【００７４】
フォーメーション制御部４２は処理を開始すると、ステップＳ１４００で、許容距離を指定する。許容距離とは、フォロワーＵＡＶ１５＃ｉの現在位置ｅｉが目標地の点ｐｉに到達したかどうかを判定するための距離ΔＬｉである。ＵＡＶの飛行には空気抵抗によるゆらぎが影響を与えるため、許容距離ΔＬｉの例として１０センチメートルとする。
【００７５】
フォロワー位置検出部４０において、フォロワーＵＡＶ１５＃ｉの現在位置（点ｅｉ）が位置計測センサ１０を使って時系列に得られており、現在位置を取得するたびにステップＳ１４０２に与えられる（図９にはＺと記した）。
【００７６】
ステップＳ１４０２で、与えられた現在位置のデータから、フォロワーＵＡＶ１５＃ｉの現在位置ｅｉから目標地の点ｐｉまでの距離||Ｔｉ||を算出する。
【００７７】
ステップＳ１４０４で、距離||Ｔｉ||が許容距離ΔＬ以下（||Ｔｉ||≦ΔＬ）であるか否かを判定する。||Ｔｉ||≦ΔＬであればステップＳ１４０６へ移行する。||Ｔｉ||≦ΔＬでなく、||Ｔ||＞ΔＬであれば運動を制御するためステップＳ１４０８へ移行する。
【００７８】
ステップＳ１４０６で、ホバリング状態でフォロワーＵＡＶ１５＃ｉを待機させるように制御し、ステップＳ１４０２へ移行する。
【００７９】
ステップＳ１４０８で、第２制御データを
2019185603.tif 000006
の計算により求める。第２制御データｕｉは、飛行コマンド変換部３４においてフォロワーＵＡＶ１５＃ｉへの飛行指令として使われる。
【００８０】
2019185603.tif 000007
はフィードバック制御におけるゲインパラメータであり、ユーザが状況に応じて設定する。第２制御データｕｉが計算されると、飛行コマンド変換部３４に渡される（図１０にはＹと記した）。
【００８１】
この反復を続けることによりフォロワーＵＡＶ１５＃ｉは目標地の点ｐｉに近づくことになり、||Ｔｉ||≦ΔＬを満たすとき図９右のフォーメーションに従って飛行する。
【００８２】
以上の処理によって、フォーメーション制御部４２は、フォロワーＵＡＶ１５＃ｉの現在位置ｅｉから目標地の点ｐｉまでの距離||Ｔｉ||に応じて制御データｕｉを出力する。
【００８３】
以上により、本実施形態は、位置計測センサ１０によってリーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉの位置と方位を算出し、トレイル・フォーメーションの編隊で飛行することができる。さらに、リーダーＵＡＶ１４の目標地の位置ｐを空間経路に沿って間隔を空けて空間中に設定することで、トレイル・フォーメーションの編隊がその軌跡に沿って飛行することができる。
【００８４】
以上説明したように、第１の実施形態に係る飛行制御装置によれば、複数台のＵＡＶが編隊を組んで円滑かつ安定的に飛行させるように制御できる。
【００８５】
＜本発明の第２の実施形態に係る飛行制御装置の構成及び作用＞
本発明の第２の実施形態は、上記図１の第１の実施形態の構成により、図１７（Ｂ）のリーダー・フォロワー編隊において、リーダーＵＡＶ１４を中心にフォロワーＵＡＶ１５が横一列に並んで飛行するアブレストをフォーメーションとした編隊飛行の制御の実施形態である．本実施形態では、アブレスト・フォーメーションに応じてフォロワーＵＡＶ１５の飛行を制御する処理が第１の実施形態と異なる。以降では、第１の実施形態との差分のみを説明する。
【００８６】
本実施形態のフォロワー位置検出部４０は図８のフローに従う。
【００８７】
フォロワー位置検出部４０は処理を開始すると、ステップＳ１３００で、アブレスト・フォーメーションにおけるフォロワーＵＡＶ１５＃ｉの目標地の３次元座標ｐｉを設定する。点ｐｉはリーダーＵＡＶ１４の現在位置ｅから点ａと点ｂを結ぶ第１線分の延長線上に距離Ｍｉだけ離れた座標値とする。つまり、リーダーＵＡＶ１４の第１線分及び第２線分の交点ｅから、第１線分の延長線上に距離Ｍｉだけ離れた座標により定まる。リーダーＵＡＶ１４の運動に従ってフォーメーション飛行するため機体座標系が世界座標系と考えると、ｐｉ＝ｅ＋（０，Ｍｉ，０），Ｌｉ＞０と与える。図１７（Ｂ）のアブレスト・フォーメーションで示したように、リーダーＵＡＶ１４を中心にして飛行する場合、フォロワーＵＡＶ１５＃１についてはｐ１＝ｅ＋（０，Ｍ１，０）と設定し、フォロワーＵＡＶ１５＃２についてはｐ２＝ｅ＋（０，−Ｍ２，０），Ｍ２＞０と設定する。
【００８８】
一般的には、リーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉの位置関係は、図１１上で示される。ステップＳ１３０２で、フォロワーＵＡＶ１５＃ｉに装着したマーカー点ａｉ、点ｂｉ、点ｃｉ、及び点ｄｉを位置計測センサ１０により検出して、交点ｅｉの位置を算出する。点ｅｉの座標はフォロワーＵＡＶ１５＃ｉの現在位置となる。
【００８９】
ステップＳ１３０４で、図１１上に示すように、フォロワーＵＡＶ１５＃ｉの現在位置（点ｅｉ）から目標地（点ｐｉ）までのベクトルＴｉを、Ｔｉ＝ｐｉ−ｅｉの計算により求める。
【００９０】
ステップＳ１３０６で、点ｃｉから点ｄｉへのベクトルｃｉ−ｄｉがベクトルＴｉとなす回転角θｉ（方位角）をベクトル内積の関係を利用して算出する。
【００９１】
以上の処理により、フォロワー位置検出部４０ではフォロワーＵＡＶ１５＃ｉの現在位置（点ｅｉ）と、フォロワーＵＡＶ１５＃ｉの現在位置（点ｅｉ）から目標地（点ｐｉ）までのベクトルＴｉと、フォロワーＵＡＶ１５＃ｉの方位角θｉを算出し、第１の実施形態と同様にフォーメーション制御部４２の処理と飛行コマンド変換部３４の処理を反復することにより、図１１下に示すアブレスト・フォーメーションの編隊で飛行することができる。さらに、リーダーＵＡＶ１４の目標地の位置ｐを空間経路に沿って間隔を空けて空間中に設定することで、アブレスト・フォーメーションの編隊がその軌跡に沿って飛行することができる。
【００９２】
＜本発明の第３の実施形態に係る飛行制御装置の構成及び作用＞
本発明の第３の実施形態は、上記図１の第１の実施形態の構成により、図１７（Ｃ）のリーダー・フォロワー編隊において、リーダーＵＡＶ１４を先頭にフォロワーＵＡＶ１５が三角形を形成するように飛行するデルタをフォーメーションとした編隊飛行の制御の実施形態である．本実施形態では、デルタ・フォーメーションに応じてフォロワーＵＡＶ１５の飛行を制御する処理が第１の実施形態と異なる。以降では、第１の実施形態との差分のみを説明する。
【００９３】
本実施形態のフォロワー位置検出部４０は図８のフローに従う。
【００９４】
フォロワー位置検出部４０は処理を開始すると、ステップＳ１３００で、アブレスト・フォーメーションにおけるフォロワーＵＡＶ１５＃ｉの目標地の３次元座標ｐｉを設定する。点ｐｉは、リーダーＵＡＶ１４の現在位置ｅから点ｃと点ｄを結ぶ第２線分の延長線上に距離Ｌｉだけ離れた座標と、点ａと点ｂを結ぶ第１線分の延長線上に距離Ｍｉだけ離れた座標とから定まる座標値とする。つまり、リーダーＵＡＶ１４の第１線分及び第２線分の交点から、第１線分の延長線上に距離Ｌｉだけ離れた座標と第２線分の延長線上に距離Ｍｉだけ離れた座標とにより定まる。リーダーＵＡＶ１４の運動に従ってフォーメーション飛行するため機体座標系が世界座標系と考えると、ｐｉ＝ｅ＋（−Ｌｉ，Ｍｉ，０），Ｌｉ＞０と与える。図１７（Ｃ）のデルタ・フォーメーションで示したように、リーダーＵＡＶ１４を先頭にして飛行する場合、フォロワーＵＡＶ１５＃１についてはｐ１＝ｅ＋（−Ｌ１，Ｍ１，０），Ｍ１＞０と設定し、フォロワーＵＡＶ１５＃２についてはｐ２＝ｅ＋（−Ｌ２，−Ｍ２，０），Ｍ２＞０，Ｌ１＝Ｌ２と設定する。
【００９５】
一般的には、リーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉの位置関係は、図１２上で示される。ステップＳ１３０２で、フォロワーＵＡＶ１５＃ｉに装着したマーカー点ａｉ、点ｂｉ、点ｃｉ、及び点ｄｉを位置計測センサ１０により検出して、交点ｅｉの位置を算出する。点ｅｉの座標はフォロワーＵＡＶ１５＃ｉの現在位置となる。
【００９６】
ステップＳ１３０４で、図１２上に示すように、フォロワーＵＡＶ１５＃ｉの現在位置（点ｅｉ）から目標地（点ｐｉ）までのベクトルＴｉを、Ｔｉ＝ｐｉ−ｅｉの計算により求める。
【００９７】
ステップＳ１３０６で、点ｃｉから点ｄｉへのベクトルｃｉ−ｄｉがベクトルＴｉとなす回転角θｉ（方位角）をベクトル内積の関係を利用して算出する。
【００９８】
以上の処理により、フォロワー位置検出部４０ではフォロワーＵＡＶ１５＃ｉの現在位置（点ｅｉ）と、フォロワーＵＡＶ１５＃ｉの現在位置（点ｅｉ）から目標地（点ｐｉ）までのベクトルＴｉと、フォロワーＵＡＶ１５＃ｉの方位角θｉを算出し、第１の実施形態と同様にフォーメーション制御部４２の処理と飛行コマンド変換部３４の処理を反復することにより、図１３に示すデルタ・フォーメーションの編隊で飛行することができる。さらに、リーダーＵＡＶ１４の目標地の位置ｐを空間経路に沿って間隔を空けて空間中に設定することで、デルタ・フォーメーションの編隊がその軌跡に沿って飛行することができる。
【００９９】
＜本発明の第４の実施形態に係る飛行制御装置の構成及び作用＞
本発明の第４の実施形態は、上記図１の第１〜３の実施形態の構成において、空間中において多様な幾何パターンを描くように編隊飛行を制御する例である。図１４に幾何パターンに基づいたリーダーＵＡＶ１４の飛行経路の例を示す。図１４（Ａ）では、上記図１の第１〜３の実施形態の構成において、Ａ地点からＢ地点への直線飛行とＢ地点からＡ地点への直線飛行を連続するようにリーダーＵＡＶ１４の目標地ｐを設定して飛行制御することにより、リーダーＵＡＶ１４及びフォロワーＵＡＶ１５＃ｉはＡ地点とＢ地点間の往復路を飛行する。
【０１００】
図１４（Ｂ）では、上記図１の第１〜３の実施形態の構成において、Ａ地点からＢ地点への直線飛行、Ｂ地点からＣ地点への直線飛行、並びにＣ地点からＡ地点への直線飛行を連続するようにリーダーＵＡＶ１４の目標地ｐを設定して飛行制御することにより、リーダーＵＡＶ１４及びフォロワーＵＡＶ１５＃ｉはＡ地点、Ｂ地点、Ｃ地点で形成される三角形状の周回を飛行する。
【０１０１】
図１４（Ｃ）では、上記図１の第１〜３の実施形態の構成において、Ａ地点からＢ地点への直線飛行、Ｂ地点からＣ地点への直線飛行、Ｃ地点からＤ地点への直線飛行、並びにＤ地点からＡ地点への直線飛行を連続するようにリーダーＵＡＶ１４の目標地ｐを設定して飛行制御することにより、リーダーＵＡＶ１４及びフォロワーＵＡＶ１５＃ｉはＡ地点、Ｂ地点、Ｃ地点、Ｄ地点で形成される四角形状の周回を飛行する。
【０１０２】
図１４（Ｄ）では、上記図１の第１〜３の実施形態の構成において、Ａ地点からＢ地点への直線飛行、Ｂ地点からＣ地点への直線飛行、Ｃ地点からＤ地点への直線飛行、Ｄ地点からＥ地点への直線飛行、並びにＥ地点からＡ地点への直線飛行を連続するようにリーダーＵＡＶ１４の目標地ｐを設定して飛行制御することにより、リーダーＵＡＶ１４及びフォロワーＵＡＶ１５＃ｉはＡ地点、Ｂ地点、Ｃ地点、Ｄ地点、Ｅ地点で形成される五角形状を描くように飛行する。あるいは、Ａ地点からＤ地点への直線飛行、Ｄ地点からＢ地点への直線飛行、Ｂ地点からＥ地点への直線飛行、Ｅ地点からＣ地点への直線飛行、並びにＣ地点からＡ地点への直線飛行を連続するようにリーダーＵＡＶ１４の目標地ｐを設定して飛行制御することにより、リーダーＵＡＶ１４及びフォロワーＵＡＶ１５＃ｉはＡ地点、Ｄ地点、Ｂ地点、Ｅ地点、Ｃ地点で形成される五角形状の周回を飛行する。第１〜３の実施形態は直線飛行を基本にして加速、等速、減速の制御が行われるため、直線飛行で描く幾何パターンであれば任意の幾何形状の経路に沿って飛行することができる。
【０１０３】
なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。
【０１０４】
例えば、上述した各実施形態では、飛行制御装置により、リーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉとを制御する場合を例に説明したが、これに限定されるものではなく、飛行制御システムとして、リーダー飛行制御装置とフォロワー飛行制御装置とを構成し、リーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉとをそれぞれの装置によって制御するようにしてもよい。
【符号の説明】
【０１０５】
１０  位置計測センサ
１２  目標地
２０  演算部
３０  リーダー位置検出部
３２  リーダー位置制御部
３４  飛行コマンド変換部
４０  フォロワー位置検出部
４２  フォーメーション制御部
５０  通信部
１００飛行制御装置
【図１】
2019185603.tif 000008
【図２】
2019185603.tif 000009
【図３】
2019185603.tif 000010
【図４】
2019185603.tif 000011
【図５】
2019185603.tif 000012
【図６】
2019185603.tif 000013
【図７】
2019185603.tif 000014
【図８】
2019185603.tif 000015
【図９】
2019185603.tif 000016
【図１０】
2019185603.tif 000017
【図１１】
2019185603.tif 000018
【図１２】
2019185603.tif 000019
【図１３】
2019185603.tif 000020
【図１４】
2019185603.tif 000021
【図１５】
2019185603.tif 000022
【図１６】
2019185603.tif 000023
【図１７】
2019185603.tif 000024