(57)【要約】【課題】複数台のＵＡＶが編隊を組んで全方位を監視しながら円滑かつ安定的に飛行させるように制御することができる。【解決手段】一つ以上のフォロワーＵＡＶについて、リーダーＵＡＶの方位角とフォロワーＵＡＶの方位角の角度の差を算出し、算出した角度の差が、予め定められたリーダーＵＡＶに対するフォロワーＵＡＶの角度の許容値を満たさない場合、許容値を満たす角度となるように、フォロワーＵＡＶの方位角を更新し、フォロワーＵＡＶの目標点までのベクトル、及び更新又は算出された方位角から求まる距離に基づいて、第２制御データを更新し、算出した飛行指令データに基づいてリーダーＵＡＶの運動を制御し、第２制御データと、更新、又は算出された方位角とに基づいて、フォロワーＵＡＶにおける、飛行指令データを算出し、算出した飛行指令データに基づいてフォロワーＵＡＶの運動を制御する。【選択図】図１
2020023283.tif 000002
【特許請求の範囲】
【請求項１】
複数のＵＡＶ（Unmanned Aerial Vehicle）の編隊飛行を制御し、前記編隊飛行を先導するリーダーＵＡＶと前記リーダーＵＡＶに従って編隊を形成する一つ以上のフォロワーＵＡＶを制御する飛行制御装置であって、
前記リーダーＵＡＶと前記一つ以上のフォロワーＵＡＶとに付与され、かつ、マーカー間の距離が既知の複数のマーカーの各々の三次元座標を計測する位置計測センサと、
前記位置計測センサによって計測された前記リーダーＵＡＶの前記マーカーの各々の三次元座標と、予め設定された前記リーダーＵＡＶの目標地である第１の目標点とに基づいて、前記リーダーＵＡＶの現在位置と、前記第１の目標点までのベクトルと、前記第１の目標点に対する方位角を算出するリーダー位置検出部と、
前記リーダーＵＡＶの前記第１の目標点までのベクトルに基づいて、前記リーダーＵＡＶの位置を制御するための第１制御データを更新するリーダー位置制御部と、
前記リーダーＵＡＶについて更新された前記第１制御データと、前記リーダーＵＡＶについて算出された前記方位角とに基づいて、前記リーダーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記リーダーＵＡＶの運動を制御する第１の飛行コマンド変換部と、
前記一つ以上のフォロワーＵＡＶについて、前記位置計測センサによって計測された前記フォロワーＵＡＶの前記マーカーの各々の三次元座標と、編隊によって定まる前記フォロワーＵＡＶの第２の目標点とに基づいて、前記フォロワーＵＡＶの現在位置と、前記第２の目標点までのベクトルと、前記第２の目標点に対する方位角とを算出するフォロワー位置検出部と、
前記一つ以上のフォロワーＵＡＶについて、前記リーダーＵＡＶの方位角と前記フォロワーＵＡＶの方位角の差と、前記フォロワーＵＡＶによって定まる所定の角度との差分が、予め定められた許容角度を満たすように、前記フォロワーＵＡＶの方位角を更新し、前記フォロワーＵＡＶの前記第２の目標点までのベクトル、及び前記更新した前記方位角に基づいて、前記フォロワーＵＡＶの位置を制御するための第２制御データを更新するフォーメーション制御部と、
前記一つ以上のフォロワーＵＡＶについて、前記フォロワーＵＡＶについて更新された前記第２制御データと、前記フォロワーＵＡＶについて前記更新した前記方位角とに基づいて、前記フォロワーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記フォロワーＵＡＶの運動を制御する第２の飛行コマンド変換部と、
を含む飛行制御装置。
【請求項２】
前記フォーメーション制御部は、前記リーダーＵＡＶの方位角とすべての前記フォロワーＵＡＶの更新後の方位角とで全周を等分するように、前記フォロワーＵＡＶの方位角を更新する請求項１に記載の飛行制御装置。
【請求項３】
カメラ映像監視部を更に含み、
前記リーダー位置制御部は、前記リーダーＵＡＶについて、前記第１の目標点に対する予め定められた許容距離を満たす場合に、編隊完了通知を前記カメラ映像監視部に送信し、
前記フォーメーション制御部は、前記一つ以上のフォロワーＵＡＶについて、前記第２の目標点に対する予め定められた許容距離を満たす場合に、編隊完了通知を前記カメラ映像監視部に送信し、
前記カメラ映像監視部は、前記リーダーＵＡＶ、及び前記一つ以上のフォロワーＵＡＶの前記編隊完了通知のすべてを受信した場合に、前記リーダーＵＡＶ、及び前記一つ以上のフォロワーＵＡＶのカメラにより映像を取得するように制御する請求項１又は請求項２に記載の飛行制御装置。
【請求項４】
複数のＵＡＶ（Unmanned Aerial Vehicle）の編隊飛行を制御し、前記編隊飛行を先導するリーダーＵＡＶと前記リーダーＵＡＶに従って編隊を形成する一つ以上のフォロワーＵＡＶを制御する飛行制御装置における飛行制御方法であって、
位置計測センサが、前記リーダーＵＡＶと前記一つ以上のフォロワーＵＡＶとに付与され、かつ、マーカー間の距離が既知の複数のマーカーの各々の三次元座標を計測するステップと、
リーダー位置検出部が、前記位置計測センサによって計測された前記リーダーＵＡＶの前記マーカーの各々の三次元座標と、予め設定された前記リーダーＵＡＶの目標地である第１の目標点とに基づいて、前記リーダーＵＡＶの現在位置と、前記第１の目標点までのベクトルと、前記第１の目標点に対する方位角を算出するステップと、
リーダー位置制御部が、前記リーダーＵＡＶの前記第１の目標点までのベクトルに基づいて、前記リーダーＵＡＶの位置を制御するための第１制御データを更新するステップと、
第１の飛行コマンド変換部が、前記リーダーＵＡＶについて更新された前記第１制御データと、前記リーダーＵＡＶについて算出された前記方位角とに基づいて、前記リーダーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記リーダーＵＡＶの運動を制御するステップと、
フォロワー位置検出部が、前記一つ以上のフォロワーＵＡＶについて、前記位置計測センサによって計測された前記フォロワーＵＡＶの前記マーカーの各々の三次元座標と、編隊によって定まる前記フォロワーＵＡＶの第２の目標点とに基づいて、前記フォロワーＵＡＶの現在位置と、前記第２の目標点までのベクトルと、前記第２の目標点に対する方位角とを算出するステップと、
フォーメーション制御部が、前記一つ以上のフォロワーＵＡＶについて、前記リーダーＵＡＶの方位角と前記フォロワーＵＡＶの方位角の差と、前記フォロワーＵＡＶによって定まる所定の角度との差分が、予め定められた許容角度を満たすように、前記フォロワーＵＡＶの方位角を更新し、前記フォロワーＵＡＶの前記第２の目標点までのベクトル、及び前記更新した前記方位角に基づいて、前記フォロワーＵＡＶの位置を制御するための第２制御データを更新するステップと、
第２の飛行コマンド変換部が、前記一つ以上のフォロワーＵＡＶについて、前記フォロワーＵＡＶについて更新した前記第２制御データと、前記フォロワーＵＡＶについて前記更新、又は前記算出された前記方位角とに基づいて、前記フォロワーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記フォロワーＵＡＶの運動を制御するステップと、
を含む飛行制御方法。
【請求項５】
前記フォーメーション制御部は、前記リーダーＵＡＶの方位角とすべての前記フォロワーＵＡＶの更新後の方位角とで全周を等分するように、前記フォロワーＵＡＶの方位角を更新する請求項４に記載の飛行制御方法。
【請求項６】
カメラ映像監視部を更に含み、
前記リーダー位置制御部は、前記リーダーＵＡＶについて、前記第１の目標点に対する予め定められた許容距離を満たす場合に、編隊完了通知を前記カメラ映像監視部に送信し、
前記フォーメーション制御部は、前記一つ以上のフォロワーＵＡＶについて、前記第２の目標点に対する予め定められた許容距離を満たす場合に、編隊完了通知を前記カメラ映像監視部に送信し、
前記カメラ映像監視部は、前記リーダーＵＡＶ、及び前記一つ以上のフォロワーＵＡＶの前記編隊完了通知のすべてを受信した場合に、前記リーダーＵＡＶ、及び前記一つ以上のフォロワーＵＡＶのカメラにより映像を取得するように制御する請求項４又は請求項５に記載の飛行制御方法。
【請求項７】
コンピュータを、請求項１〜請求項３のいずれか１項に記載の飛行制御装置の各部として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、飛行制御装置、方法、及びプログラムに係り、特に、無人飛行機を制御する飛行制御装置、方法、及びプログラムに関する。
【背景技術】
【０００２】
ＵＡＶ（Unmanned Aerial Vehicle）は複数のロータで駆動するプロペラを持ち、それぞれのプロペラに与える揚力を制御することにより飛行操縦する無人飛行機である。ドローンと呼ばれる無人飛行機の多くはＵＡＶの一種である。通常、４つのプロペラを持つクアッドロータ型ＵＡＶが利用されている。以降で扱う無人飛行機をクアッドロータ型ＵＡＶとし、その名称を単にＵＡＶと略称する。
【０００３】
一般的に、ＵＡＶの姿勢を計測するために、図１７に示すように機体にローカル座標系が設定される。ＵＡＶの前進方向をＸ軸、Ｘ軸と垂直な方向をＹ軸、重力とは逆方向をＺ軸とする。
【０００４】
また、ＵＡＶの三次元位置を計測するため、図１８に示すように、ある基準となるグローバル座標系を設定する。ＧＰＳでは世界座標系とした三次元座標となり、モーションキャプチャシステムではその計測座標系がグローバル座標系となる。ＵＡＶの重心すなわちＵＡＶの位置（ローカル座標系の原点）はグローバル座標系の点Ｐ＝（Ｘ，Ｙ，Ｚ）として表現する。また、ＵＡＶの姿勢はグローバル座標系に対するローカル座標系の回転角で表現し、Ｘ軸周りの回転はロール回転（回転角φ）、Ｙ軸周りの回転はピッチ回転（回転角ω）、Ｚ軸周りの回転はヨー回転（回転角θ）と呼ばれる。ＵＡＶの飛行運動は、４つのプロペラに与える揚力を変化させることにより、Ｘ軸周りの回転、Ｙ軸周りの回転、Ｚ軸周りの回転を発生させる。Ｙ軸周りの回転はＸ軸方向の並進運動を生み出し、Ｘ軸周りの回転はＹ軸方向の並進運動を生み出す。Ｚ軸周りの回転は方位の回転を生み出し、同じ揚力が同時に４つのプロペラに与えられたとき、その強弱によってＺ軸方向の並進運動（高度の昇降）を生み出す。
【０００５】
グローバル座標系Ｘｗ，Ｙｗ，Ｚｗにおいて所定の座標値（Ｘｄ，Ｙｄ，Ｚｄ）と方位θｄが与えられたとき、ＵＡＶの現在位置Ｐ＝（Ｘ，Ｙ，Ｚ）と現在方位から、その所定位置へ飛行して所定方位に機体を向けるためには、ＵＡＶの姿勢と位置を制御する必要がある。非特許文献１には、ＵＡＶの運動方程式と角運動方程式に基づいたバックステッピング制御が公開されている。非特許文献２では、AR Drone 2.0（市販の低価格なクアッドロータ型ＵＡＶ）に関する飛行運動制御法が公開されている。従来技術の多くは、グローバル座標系における飛行の軌跡として離散的な所定位置と所定方位が与えられ、ＵＡＶが各点と各方位を追跡するようにその運動が制御される。一方、複数台のＵＡＶを使った編隊飛行はより広域な視点からのパトロール、レスキュー、運搬などを提供することができる。複数台のＵＡＶを使った編隊飛行では、作業ミッションに応じてそれぞれのＵＡＶの位置と姿勢を制御する必要があり、非特許文献３では、線形モデル予測制御（Linear Model Predictive Control）を利用した編隊飛行制御が公開されている。
【０００６】
単体ＵＡＶの飛行運動制御に関する先行技術は非特許文献１と非特許文献２に、複数台のＵＡＶを使った編隊飛行制御は非特許文献３において公知となっている。
【先行技術文献】
【非特許文献】
【０００７】
【非特許文献１】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.
【発明の概要】
【発明が解決しようとする課題】
【０００８】
複数台のＵＡＶを使った編隊飛行の例を図１９に示す。一般的には、リーダー・フォロワー編隊（leader-follower formation）により飛行する。飛行全体を指導するリーダー役のＵＡＶに対して、フォロワー役のＵＡＶが追従して飛行する。トレイルとはリーダーのＵＡＶを先頭にフォロワーのＵＡＶが縦一列に並んで飛行し、アブレストとはリーダーのＵＡＶを中心にフォロワーのＵＡＶが横一列に並んで飛行する。これに対して、デルタではリーダーのＵＡＶを先頭にフォロワーのＵＡＶが三角形を形成するように追従して飛行する。
【０００９】
図１９の例では、各ＵＡＶは進行方向に沿って飛行するため機体の頭（鼻先）は進行方向を向いている。市販のＵＡＶには先頭部分にカメラが取り付けられているものが多く、このカメラを使って前方のシーンを撮像することができる。ＵＡＶを使った遠隔パトロールは、先頭部分のカメラで取得した映像を監視することになる。説明の便宜上、全てのＵＡＶは同じ画角のカメラで映像を取得するものとする。例えば、トレイル型編隊飛行では同一方向のカメラ映像を監視するため、リーダーＵＡＶはオクルージョンのない前方映像を取得し、フォロワーＵＡＶは前方のＵＡＶが映り込んだ前方の映像を取得する。この編隊では一列に飛行するため、前方のリーダーＵＡＶが映り込む以外はほぼ重複した時系列画像を取得する。これに対して、アブレスト型編隊飛行では全てのＵＡＶのカメラ映像には他の機体が映り込まず、隣のＵＡＶとの距離によって画面の左右部分が一部重複した時系列画像が得られる。重複箇所を使って画像を合成すれば前方の広視野画像、すなわち前方のパノラマ画像が得られる。また、デルタ型編隊飛行ではリーダーＵＡＶはオクルージョンのない前方映像を取得する。フォロワーＵＡＶの映像はリーダーＵＡＶが映り込むが、隣のＵＡＶとの距離によって画面の左右部分が一部重複した時系列画像が得られる。リーダーＵＡＶが映り込んだ重複箇所を手掛かりに、それらの画像を合成すれば前方のパノラマ画像が得られる。
【００１０】
図１９の編隊飛行では、全てのＵＡＶは進行方向にカメラを向けて飛行するため、編隊飛行の周囲を一度に監視することができない。例えば、左右方向と後方から障害物、人、並びに別のＵＡＶが接近した場合には時系列画像においてそれらの接近を見落すことになる。非特許文献３では図１９に示した編隊飛行を制御する方法が公開されているが、あくまで現在地から目的地までの編隊飛行を目的としており、全方位監視などのパトロールを課題とする解決策ではない。
【００１１】
本発明は、上記問題点を解決するために成されたものであり、複数台のＵＡＶが編隊を組んで全方位を監視しながら円滑かつ安定的に飛行させるように制御できる飛行制御装置、方法、及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１２】
上記目的を達成するために、第１の発明に係る飛行制御装置は、複数のＵＡＶ（Unmanned Aerial Vehicle）の編隊飛行を制御し、前記編隊飛行を先導するリーダーＵＡＶと前記リーダーＵＡＶに従って編隊を形成する一つ以上のフォロワーＵＡＶを制御する飛行制御装置であって、前記リーダーＵＡＶと前記一つ以上のフォロワーＵＡＶとに付与され、かつ、マーカー間の距離が既知の複数のマーカーの各々の三次元座標を計測する位置計測センサと、前記位置計測センサによって計測された前記リーダーＵＡＶの前記マーカーの各々の三次元座標と、予め設定された前記リーダーＵＡＶの目標地である第１の目標点とに基づいて、前記リーダーＵＡＶの現在位置と、前記第１の目標点までのベクトルと、前記第１の目標点に対する方位角を算出するリーダー位置検出部と、前記リーダーＵＡＶの前記第１の目標点までのベクトルに基づいて、前記リーダーＵＡＶの位置を制御するための第１制御データを更新するリーダー位置制御部と、前記リーダーＵＡＶについて更新された前記第１制御データと、前記リーダーＵＡＶについて算出された前記方位角とに基づいて、前記リーダーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記リーダーＵＡＶの運動を制御する第１の飛行コマンド変換部と、前記一つ以上のフォロワーＵＡＶについて、前記位置計測センサによって計測された前記フォロワーＵＡＶの前記マーカーの各々の三次元座標と、編隊によって定まる前記フォロワーＵＡＶの第２の目標点とに基づいて、前記フォロワーＵＡＶの現在位置と、前記第２の目標点までのベクトルと、前記第２の目標点に対する方位角とを算出するフォロワー位置検出部と、前記一つ以上のフォロワーＵＡＶについて、前記リーダーＵＡＶの方位角と前記フォロワーＵＡＶの方位角の差と、前記フォロワーＵＡＶによって定まる所定の角度との差分が、予め定められた許容角度を満たすように、前記フォロワーＵＡＶの方位角を更新し、前記フォロワーＵＡＶの前記第２の目標点までのベクトル、及び前記更新した前記方位角に基づいて、前記フォロワーＵＡＶの位置を制御するための第２制御データを更新するフォーメーション制御部と、前記一つ以上のフォロワーＵＡＶについて、前記フォロワーＵＡＶについて更新された前記第２制御データと、前記フォロワーＵＡＶについて前記更新した前記方位角とに基づいて、前記フォロワーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記フォロワーＵＡＶの運動を制御する第２の飛行コマンド変換部と、を含んで構成されている。
【００１３】
第２の発明に係る飛行制御方法は、複数のＵＡＶ（Unmanned Aerial Vehicle）の編隊飛行を制御し、前記編隊飛行を先導するリーダーＵＡＶと前記リーダーＵＡＶに従って編隊を形成する一つ以上のフォロワーＵＡＶを制御する飛行制御装置における飛行制御方法であって、位置計測センサが、前記リーダーＵＡＶと前記一つ以上のフォロワーＵＡＶとに付与され、かつ、マーカー間の距離が既知の複数のマーカーの各々の三次元座標を計測するステップと、リーダー位置検出部が、前記位置計測センサによって計測された前記リーダーＵＡＶの前記マーカーの各々の三次元座標と、予め設定された前記リーダーＵＡＶの目標地である第１の目標点とに基づいて、前記リーダーＵＡＶの現在位置と、前記第１の目標点までのベクトルと、前記第１の目標点に対する方位角を算出するステップと、リーダー位置制御部が、前記リーダーＵＡＶの前記第１の目標点までのベクトルに基づいて、前記リーダーＵＡＶの位置を制御するための第１制御データを更新するステップと、第１の飛行コマンド変換部が、前記リーダーＵＡＶについて更新された前記第１制御データと、前記リーダーＵＡＶについて算出された前記方位角とに基づいて、前記リーダーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記リーダーＵＡＶの運動を制御するステップと、フォロワー位置検出部が、前記一つ以上のフォロワーＵＡＶについて、前記位置計測センサによって計測された前記フォロワーＵＡＶの前記マーカーの各々の三次元座標と、編隊によって定まる前記フォロワーＵＡＶの第２の目標点とに基づいて、前記フォロワーＵＡＶの現在位置と、前記第２の目標点までのベクトルと、前記第２の目標点に対する方位角とを算出するステップと、フォーメーション制御部が、前記一つ以上のフォロワーＵＡＶについて、前記リーダーＵＡＶの方位角と前記フォロワーＵＡＶの方位角の差と、前記フォロワーＵＡＶによって定まる所定の角度との差分が、予め定められた許容角度を満たすように、前記フォロワーＵＡＶの方位角を更新し、前記フォロワーＵＡＶの前記第２の目標点までのベクトル、及び前記更新した前記方位角に基づいて、前記フォロワーＵＡＶの位置を制御するための第２制御データを更新するステップと、第２の飛行コマンド変換部が、前記一つ以上のフォロワーＵＡＶについて、前記フォロワーＵＡＶについて更新した前記第２制御データと、前記フォロワーＵＡＶについて前記更新、又は前記算出された前記方位角とに基づいて、前記フォロワーＵＡＶにおける、飛行指令データを算出し、算出した前記飛行指令データに基づいて前記フォロワーＵＡＶの運動を制御するステップと、を含んで実行することを特徴とする。
【００１４】
第３の発明に係るプログラムは、コンピュータを、第１の発明に記載の飛行制御装置の各部として機能させるためのプログラムである。
【発明の効果】
【００１５】
本発明の飛行制御装置、方法、及びプログラムによれば、リーダーＵＡＶの方位角とすべてのフォロワーＵＡＶの方位角とで全周を等分するように制御するため、複数台のＵＡＶが編隊を組んで全方位を監視しながら円滑かつ安定的に飛行させるように制御することができる、という効果が得られる。
【図面の簡単な説明】
【００１６】
【図１】本発明の実施形態に係る飛行制御装置の構成を示すブロック図である。
【図２】ＵＡＶの飛行を制御する状況の一例を示す図である。
【図３】本発明の実施形態に係る飛行制御装置における飛行制御処理ルーチンを示すフローチャートである。
【図４】飛行制御処理ルーチンにおけるリーダー位置検出処理のフローチャートである。
【図５】マーカーの点ａ、点ｂ、及び目標地の点ｐをＺｗ軸方向から視た位置と方位の関係を表す図である。
【図６】飛行制御処理ルーチンにおけるリーダー位置制御処理のフローチャートである。
【図７】飛行コマンド変換処理ルーチンにおける飛行コマンド変換処理のフローチャートである。
【図８】飛行コマンド変換処理ルーチンにおけるフォロワー位置検出処理のフローチャートである。
【図９】第１の実施の形態の初期状態におけるリーダーＵＡＶとフォロワーＵＡＶとの位置関係を示す図である。
【図１０】飛行コマンド変換処理ルーチンにおけるフォーメーション制御処理のフローチャートである。
【図１１】第１の実施の形態の最終状態におけるリーダーＵＡＶとフォロワーＵＡＶとの位置関係を示す図である。
【図１２】飛行コマンド変換処理ルーチンにおけるカメラ映像監視処理のフローチャートである。
【図１３】第２の実施の形態の初期状態におけるリーダーＵＡＶとフォロワーＵＡＶとの位置関係を示す図である。
【図１４】第２の実施の形態の最終状態におけるリーダーＵＡＶとフォロワーＵＡＶとの位置関係を示す図である。
【図１５】第３の実施の形態の初期状態におけるリーダーＵＡＶとフォロワーＵＡＶとの位置関係を示す図である。
【図１６】第３の実施の形態の最終状態におけるリーダーＵＡＶとフォロワーＵＡＶとの位置関係を示す図である。
【図１７】ＵＡＶの概観とＵＡＶ固定のローカル座標系の一例を示す図である。
【図１８】グローバル座標系とＵＡＶ固定のローカル座標系の関係を示す図である。
【図１９】編隊飛行のフォーメーションの例を示す図である。
【発明を実施するための形態】
【００１７】
以下、図面を参照して本発明の実施の形態を詳細に説明する。本発明の実施の形態に係る手法は、互いに衝突しないように各ＵＡＶへ３次元位置と方位を示す飛行指令を適切に出すことにより、編隊を組んだままで周囲のカメラ映像を取得し、全方位の監視を可能とする。さらに、ユーザの意図で各ＵＡＶを制御することにより、人を取り巻く実環境において、人の単独行動では実現不可能な作業を複数のＵＡＶが支援する、あるいは協調作業することを可能とする。
【００１８】
＜本発明の第１の実施の形態に係る飛行制御装置の構成＞
本発明の第１の実施の形態に係るクアッドロータ型ＵＡＶの飛行制御装置の構成について説明する。本実施の形態は複数台のＵＡＶによる編隊飛行を制御して全方位のカメラ映像を取得する形態であり、リーダーＵＡＶとフォロワーＵＡＶとの２台とする場合を例に説明する。
【００１９】
図１に示すように、本発明の第１の実施の形態に係る飛行制御装置１００は、ＣＰＵと、ＲＡＭと、後述する飛行制御処理ルーチンを実行するためのプログラムや各種データを記憶したＲＯＭと、を含むコンピュータで構成することが出来る。この飛行制御装置１００は、機能的には図１に示すように位置計測センサ１０と、演算部２０と、通信部５０とを備えている。この構成において、位置計測センサ１０は必ずしも構成要素として接続している必要はなく、処理に必要なデータを取得すればよく、演算部２０におけるリーダー位置検出部３０、リーダー位置制御部３２、第１の飛行コマンド変換部３４、第２の飛行コマンド変換部３５、フォロワー位置検出部４０、フォーメーション制御部４２、カメラ映像監視部４４、及び映像ＤＢ４６からそれぞれの矢印へのデータの流れは、ハードディスク、ＲＡＩＤ装置、ＣＤ−ＲＯＭなどの記録媒体を利用する、または、ネットワークを介してリモートなデータ資源を利用する形態でもどちらでも構わない。
【００２０】
位置計測センサ１０は、各ＵＡＶに付与され、かつ、マーカー間の距離が既知の複数のマーカーの各々の三次元座標を計測データとして計測する。一般的に、モーションキャプチャ装置は、所定のマーカーの３次元座標をリアルタイムで高精度に計測できることが知られている。本実施の形態では、位置計測センサ１０の例として、モーションキャプチャ装置を利用する。図２に示す状況において、位置計測センサ１０はグローバル座標系において３次元座標を計測するようにセットアップされており、各ＵＡＶに付与された各マーカーの３次元座標が、ある一定間隔で逐次計測される。目標地は点ｐで示されており、その座標値は飛行計画に応じて任意に設定する。以降では、２台のＵＡＶによる編隊飛行制御の例を説明するが、３台以上のＵＡＶを利用する場合にも本発明の実施の形態の手法を容易に拡張することができることは言うまでもない。
【００２１】
リーダーＵＡＶ１４には点ａ、点ｂ、点ｃ、並びに点ｄの位置にセンシング用のマーカーが取り付けられている。点ａと点ｂとを結ぶ第１線分が点ｃと点ｄとを結ぶ第２線分と点ｅの位置で直交しているとする。点ｅの位置は、線分が直交する条件を満たせばよく、各線分の中点である必要はない。リーダーＵＡＶ１４のカメラは、点ｄから点ｃの方向の向きに設置されている。フォロワーＵＡＶ１５＃１には点ａ１、点ｂ１、点ｃ１、及び点ｄ１にセンシング用のマーカーが取り付けられている。点ａ１と点ｂ１とを結ぶ第１線分が点ｃ１と点ｄ１とを結ぶ第２線分と点ｅ１の位置で直交しているとする。フォロワーＵＡＶ１５＃１のカメラは、点ｄ１から点ｃ１の方向の向きに設置されている。なお、フォロワーＵＡＶ１５＃２が存在する場合には、同様に、フォロワーＵＡＶ１５＃２には点ａ２、点ｂ２、点ｃ２、並びに点ｄ２の位置にセンシング用のマーカーが取り付けられているものとする。点ａ２と点ｂ２とを結ぶ第１線分が点ｃ２と点ｄ２とを結ぶ第２線分と点ｅ２の位置で直交しているとする。フォロワーＵＡＶ１５＃２のカメラは、点ｄ２から点ｃ２の方向の向きに設置されている。フォロワーＵＡＶ１５＃１、＃２において、点ｅ１と点ｅ２の位置は、線分が直交する条件を満たせばよく、各線分の中点である必要はない。
【００２２】
演算部２０は、位置計測センサ１０によりグローバル座標系におけるリーダーＵＡＶ１４の位置を検出するリーダー位置検出部３０と、所定の経路に沿ってリーダーＵＡＶ１４の飛行を制御するリーダー位置制御部３２と、リーダーＵＡＶ１４に指令データを送出するための第１の飛行コマンド変換部３４と、フォロワーＵＡＶ１５に指令データを送出するための第２の飛行コマンド変換部３５と、位置計測センサ１０によりグローバル座標系におけるフォロワーＵＡＶ１５の位置を検出するフォロワー位置検出部４０と、リーダーＵＡＶ１４の位置、及びフォロワーＵＡＶ１５の方位角に基づいてフォロワーＵＡＶ１５の飛行を制御するフォーメーション制御部４２と、各ＵＡＶからカメラにより撮影した映像を取得する命令及び映像を映像ＤＢ４６に転送する命令を送出するカメラ映像監視部４４と、映像ＤＢ４６とを含んで構成されている。なお各処理部の具体的な処理内容は後述の作用の説明において説明する。
【００２３】
リーダー位置検出部３０は、位置計測センサ１０によって計測されたリーダーＵＡＶ１４のマーカーの点ａ、点ｂ、点ｃ、及び点ｄの三次元座標と、予め設定されたＵＡＶ１４の目標地１２の目標点ｐとに基づいて、リーダーＵＡＶ１４の現在位置ｅ、現在位置ｅからの目標点ｐまでのベクトルＴ、及び目標点ｐに対する方位角θを算出する。現在位置ｅは、マーカーの点ａ、点ｂの三次元座標を結ぶ第１線分及び点ｃ、点ｄを結ぶ第２線分の交点ｅである。方位角θは、グローバル座標系における目標点ｐに対するマーカーの点ａ、点ｂの三次元座標を結んだ第１線分の方位角である。なお、目標点ｐが第１の目標点の一例であり、方位角θが第１の目標点に対する方位角の一例である。
【００２４】
リーダー位置制御部３２は、リーダーＵＡＶ１４の目標点ｐまでのベクトルＴから求まる距離||Ｔ||に基づいて、リーダーＵＡＶ１４の位置を制御するための第１制御データｕを更新する。また、リーダー位置制御部３２は、リーダーＵＡＶ１４について、目標点ｐに対する予め定められた許容距離ΔＬを満たす場合に、編隊完了通知をカメラ映像監視部４４に送信する。
【００２５】
第１の飛行コマンド変換部３４は、リーダーＵＡＶ１４について、更新された制御データｕと、算出された方位角θとに基づいて、リーダーＵＡＶ１４における、Ｘ軸周りの回転速度Ｖx、Ｙ軸周りの回転速度Ｖy、Ｚ軸に沿った速度Ｖｚ、及びＺ軸周りの回転速度Ｖθを飛行指令データとして算出し、飛行指令データを通信部５０を介してリーダーＵＡＶ１４に送信することで、算出した飛行指令データに基づいてリーダーＵＡＶ１４の運動を制御する。
【００２６】
フォロワー位置検出部４０は、一つ以上のフォロワーＵＡＶ１５＃ｉについて、位置計測センサ１０によって計測されたフォロワーＵＡＶ１５＃ｉのマーカーの各々の三次元座標と、編隊によって定まるフォロワーＵＡＶ１５＃ｉの目標点ｐｉとに基づいて、フォロワーＵＡＶ１５＃ｉについて、フォロワーＵＡＶ１５＃ｉの現在位置ｅｉ、現在位置ｅｉからの目標点ｐｉまでのベクトルＴｉ、及び目標点ｐｉに対する方位角θｉを算出する。現在位置ｅｉは、マーカーの点ａｉ、点ｂｉの三次元座標を結ぶ第１線分及び点ｃｉ、点ｄｉを結ぶ第２線分の交点ｅｉである。方位角θｉは、グローバル座標系における目標点ｐｉに対するマーカーの点ａｉ、点ｂｉの三次元座標を結んだ第１線分の方位角である。なお、目標点ｐｉが第２の目標点の一例であり、方位角θｉが第２の目標点に対する方位角の一例である。
【００２７】
フォーメーション制御部４２は、一つ以上のフォロワーＵＡＶ１５＃ｉについて、フォロワーＵＡＶ１５＃ｉの位置を制御するための第２制御データｕｉを更新するが、条件に応じて処理が異なる。リーダーＵＡＶ１４の方位角とフォロワーＵＡＶ１５＃ｉの方位角の差と、フォロワーＵＡＶ１５＃ｉによって定まる所定の角度との差分を算出し、算出した差分が、予め定められたリーダーＵＡＶ１４に対するフォロワーＵＡＶ１５＃ｉの角度の許容角度Δθを満たすか否かに応じて、以下のように処理する。
【００２８】
差分が、許容角度Δθを満たさない場合には、許容角度Δθを満たす角度となるように、フォロワーＵＡＶ１５＃ｉの方位角θｉを更新し、フォロワーＵＡＶ１５＃ｉの目標点ｐｉまでのベクトルＴｉ’、及び更新された方位角θｉから求まる距離||Ｔｉ’||に基づいて、フォロワーＵＡＶ１５＃ｉの位置を制御するための第２制御データｕｉを更新する。差分が、許容角度Δθを満たす場合には、フォロワーＵＡＶ１５＃ｉの目標点ｐｉまでのベクトルＴｉ’、及び算出された方位角θｉから求まる距離||Ｔｉ’||に基づいて、第２制御データｕｉを更新する。また、フォーメーション制御部４２は、フォロワーＵＡＶ１５＃ｉについて、目標点ｐｉに対する予め定められた許容距離ΔＬを満たす場合に、編隊完了通知をカメラ映像監視部４４に送信する。このように、フォーメーション制御部４２は、リーダーＵＡＶ１４の方位角とすべてのフォロワーＵＡＶ１５＃ｉの更新後の方位角（又は算出した方位角）とで全周を等分するように、フォロワーＵＡＶ＃ｉの方位角を更新する。
【００２９】
第２の飛行コマンド変換部３５は、一つ以上のフォロワーＵＡＶ１５＃ｉ（本実施の形態では、ｉ＝１のみの場合を例に説明する。）について、後述するフォーメーション制御部４２で更新された制御データｕｉと、更新又は算出された方位角θｉとに基づいて、フォロワーＵＡＶ１５＃ｉにおける、Ｘ軸周りの回転速度Ｖx、Ｙ軸周りの回転速度Ｖy、Ｚ軸に沿った速度Ｖｚ、及びＺ軸周りの回転速度Ｖθを飛行指令データとして算出し、飛行指令データを通信部５０を介してフォロワーＵＡＶ１５＃ｉに送信することで、算出した飛行指令データに基づいてフォロワーＵＡＶ１５＃ｉの運動を制御する。
【００３０】
カメラ映像監視部４４は、リーダーＵＡＶ１４、及び一つ以上のフォロワーＵＡＶ１５＃ｉの編隊完了通知のすべてを受信した場合に、リーダーＵＡＶ１４、及び一つ以上のフォロワーＵＡＶ１５＃ｉのカメラにより映像を取得するように制御する。
【００３１】
通信部５０は、飛行指令データをリーダーＵＡＶ１４、及び一つ以上のフォロワーＵＡＶ１５＃ｉに送信する。また、通信部５０は、カメラ映像監視部４４からの取得の命令に応じてリーダーＵＡＶ１４、及び一つ以上のフォロワーＵＡＶ１５＃ｉにカメラの映像を取得する命令を通知する。また、通信部５０は、カメラ映像監視部４４からの転送の命令により各ＵＡＶについて取得した映像を映像ＤＢ４６に格納する。映像ＤＢ４６はオペレータが操作するメインサーバーと接続されている（図示省略）。各ＵＡＶから映像ＤＢ４６への転送は無線を利用して映像を送信する。あるいは各ＵＡＶ内部のメモリに一時保存しておき、適宜、オペレータの指示または一定間隔をおいて無線経由で映像を送信してもよい。
【００３２】
＜本発明の第１の実施の形態に係る飛行制御装置の作用＞
次に、本発明の第１の実施の形態に係る飛行制御装置１００の作用について説明する。
【００３３】
リーダーＵＡＶ１４及び一つ以上のフォロワーＵＡＶ１５の各々の三次元座標の計測を開始すると、飛行制御装置１００は、図３に示す飛行制御処理ルーチンを実行する。なお、飛行制御処理ルーチンは、各ステップの詳細において説明するようにステップごとに独立して処理を繰り返しているものとする。
【００３４】
まず、ステップＳ１００では、リーダー位置検出部３０は、位置計測センサ１０によって計測されたリーダーＵＡＶ１４のマーカーの点ａ、点ｂ、点ｃ、及び点ｄの三次元座標と、予め設定されたリーダーＵＡＶ１４の目標地１２の目標点ｐとに基づいて、マーカーの点ａ、点ｂの三次元座標を結ぶ第１線分及び点ｃ、点ｄを結ぶ第２線分の交点ｅ、交点ｅからの目標点ｐまでのベクトルＴ、及びグローバル座標系における目標点ｐに対するマーカーの点ａ、点ｂの三次元座標を結んだ第１線分の方位角θを算出する。
【００３５】
次に、ステップＳ１０２では、リーダー位置制御部３２は、リーダーＵＡＶ１４の目標点ｐまでのベクトルＴから求まる距離||Ｔ||に基づいて、リーダーＵＡＶ１４の位置を制御するための第１制御データｕを更新する。また、リーダー位置制御部３２は、所定の条件に応じて、編隊完了通知又は編隊未完了通知をカメラ映像監視部４４に送信する。
【００３６】
ステップＳ１０４では、第１の飛行コマンド変換部３４は、リーダーＵＡＶ１４について、ステップＳ１０２で更新された制御データｕと、算出された方位角θとに基づいて、リーダーＵＡＶ１４における、Ｘ軸周りの回転速度Ｖx、Ｙ軸周りの回転速度Ｖy、Ｚ軸に沿った速度Ｖｚ、及びＺ軸周りの回転速度Ｖθを飛行指令データとして算出し、飛行指令データを通信部５０を介してリーダーＵＡＶ１４に送信することで、算出した飛行指令データに基づいてリーダーＵＡＶ１４の運動を制御する。
【００３７】
ステップＳ１０６では、一つ以上のフォロワーＵＡＶ１５を順番に制御するため、フォロワーＵＡＶ１５＃ｉ（ｉ＝１，２，・・・）を選択する。本実施の形態では、ｉ＝１であるため、１台のみ選択される。
【００３８】
ステップＳ１０８では、フォロワー位置検出部４０が、位置計測センサ１０によって計測されたフォロワーＵＡＶ１５＃ｉのマーカーの各々の三次元座標と、リーダーＵＡＶ１４の第１線分及び第２線分の交点ｅから、リーダーＵＡＶ１４の第１線分又は第２線分の延長線上に予め定められた距離だけ離れた座標により定まる、フォロワーＵＡＶ１５＃ｉの目標点ｐｉとに基づいて、フォロワーＵＡＶ１５＃ｉについて、マーカーの点ａｉ、点ｂｉの三次元座標を結ぶ第１線分及び点ｃｉ、点ｄｉを結ぶ第２線分の交点、交点からの目標点ｐｉまでのベクトルＴｉ、及びグローバル座標系における目標点ｐｉに対するマーカーの点ａｉ、点ｂｉの三次元座標を結んだ第１線分の方位角を算出する。
【００３９】
ステップＳ１１０では、フォーメーション制御部４２は、フォロワーＵＡＶ１５＃ｉについて、フォロワーＵＡＶ１５＃ｉの目標点ｐｉまでのベクトルＴｉから求まる距離||Ｔｉ||に基づいて、フォロワーＵＡＶ１５＃ｉの位置を制御するための第２制御データｕｉを更新する。また、フォーメーション制御部４２は、所定の条件に応じて、編隊完了通知又は編隊未完了通知をカメラ映像監視部４４に送信する。
【００４０】
ステップＳ１１２では、第２の飛行コマンド変換部３５は、フォロワーＵＡＶ１５＃ｉについて、ステップＳ１１０で更新された第２制御データｕｉと、算出された方位角θｉとに基づいて、フォロワーＵＡＶ１５＃ｉにおける、Ｘ軸周りの回転速度Ｖx、Ｙ軸周りの回転速度Ｖy、Ｚ軸に沿った速度Ｖｚ、及びＺ軸周りの回転速度Ｖθを飛行指令データとして算出し、飛行指令データを通信部５０を介してフォロワーＵＡＶ１５＃ｉに送信することで、算出した飛行指令データに基づいてフォロワーＵＡＶ１５＃ｉの運動を制御する。
【００４１】
ステップＳ１１４では、全てのフォロワーＵＡＶ１５＃ｉについて制御を終了したかを判定し、終了していなければステップＳ１０６に戻って次のフォロワーＵＡＶ１５＃ｉを選択して処理を繰り返し、終了していればステップＳ１１６へ移行する。
【００４２】
ステップＳ１１６では、カメラ映像監視部４４は、全てのフォロワーＵＡＶ１５＃ｉについて編隊完了通知を受信して編隊が完了したかを判定し、完了していれば、リーダーＵＡＶ１４及び全てのフォロワーＵＡＶ１５＃ｉからカメラの映像を同期させて取得する命令を通信部５０を介して各ＵＡＶに送信し、通信部５０に取得した映像を映像ＤＢ４６に転送する命令を送信する。
【００４３】
次に、ステップＳ１００のリーダー位置検出部３０のリーダー位置検出処理の詳細について説明する。
【００４４】
図４はリーダー位置検出部３０の処理のフロー図である。リーダー位置検出部３０は処理を開始すると、ステップＳ１０００で、グローバル座標系における目標地１２の目標点ｐを設定する。目標点ｐは操作者が決定した任意の三次元座標でよい。リーダーＵＡＶ１４が目的地に到達すると、次の目標地１２の３次元座標ｐが与えられる。目標地１２の３次元座標ｐは編隊飛行の経路に沿って離散的に与えられる。
【００４５】
ステップＳ１００２で、リーダーＵＡＶ１４のマーカーの点ａ、点ｂ、点ｃ、及び点ｄを位置計測センサ１０により検出し、その交点ｅの位置を算出する。交差する位置ｅにセンシング用のマーカーを設置してもよいが、本実施の形態では汎用性のために、点ｅの位置を４つのマーカーの点ａ、点ｂ、点ｃ、及び点ｄから計算で求める。本処理では、点ｅの３次元座標を現在位置と称しており、以降では、点ｅをリーダーＵＡＶ１４の現在位置とする。
【００４６】
ステップＳ１００４で、リーダーＵＡＶ１４の現在位置（点ｅ）から目標地（点ｐ）までのベクトルＴを、Ｔ＝ｐ−ｅの計算により求める。
【００４７】
ステップＳ１００６で、点ａから点ｂへのベクトルａ−ｂがベクトルＴと直交するために必要な回転角θ（方位角）をベクトル内積の関係を利用して算出する。図５は、マーカーの点ａ、マーカーの点ｂ、及び目標地の点ｐをＺｗ軸方向から視た位置と方位の関係を表す。方位角θ＝０のとき、ベクトルａ−ｂがベクトルＴと直交する。
【００４８】
ステップＳ１００８で、位置検出の処理を停止するかを判定し、処理を停止する場合にはリーダー位置検出部３０の処理を終了し、終了しない場合にはステップＳ１００２に戻って処理を繰り返す。なお、処理を停止する場合とは、ここでは操作者がリーダーＵＡＶ１４の飛行制御を終了する場合とし、以下の処理においても同様である。
【００４９】
リーダー位置検出部３０では、以上の処理により、リーダーＵＡＶの現在位置（点ｅ）と、目標点ｐまでのベクトルＴと、方位角θとを算出する。
【００５０】
次に、ステップＳ１０２のリーダー位置制御部３２のリーダー位置制御処理の詳細について説明する。本処理では、リーダー位置検出部３０で得たベクトルＴに基づき、現在位置から目標地までの直線飛行を制御するための処理フローを説明する。
【００５１】
図６はリーダー位置制御部３２の処理のフロー図である。リーダー位置制御部３２は処理を開始すると、ステップＳ１１００で、許容距離を指定する。許容距離とは、リーダーＵＡＶ１４の現在位置ｅが目標地の点ｐに到達したかどうかを判定するための距離ΔＬである。ＵＡＶの飛行には空気抵抗によるゆらぎが影響を与えるため、許容距離ΔＬの例として１０センチメートルとする。
【００５２】
リーダー位置検出部３０において、リーダーＵＡＶ１４の現在位置（点ｅ）が位置計測センサ１０を使って時系列に得られており、現在位置を取得するたびに本処理ルーチンのステップＳ１１０２に与えられる（図４及び図６にはＸと記した）。
【００５３】
ステップＳ１１０２で、与えられた現在位置のデータにより、現在位置ｅから目標地の点ｐまでの距離||Ｔ||を算出する。||・||はベクトルのノルム（大きさ）を表す。
【００５４】
ステップＳ１１０４で、距離||Ｔ||が許容距離ΔＬ以下（||Ｔ||≦ΔＬ）であるか否かを判定する。||Ｔ||≦ΔＬであればステップＳ１１１０へ移行する。||Ｔ||≦ΔＬでなく、||Ｔ||＞ΔＬであれば運動を制御するためステップＳ１１０６へ移行する。
【００５５】
ステップＳ１１０６で、第１制御データを
2020023283.tif 000003
の計算により求める。第１制御データｕは、第１の飛行コマンド変換部３４においてリーダーＵＡＶ１４への飛行指令として使われる。
【００５６】
2020023283.tif 000004
はフィードバック制御におけるゲインパラメータであり、ユーザが状況に応じて設定する。第１制御データｕが計算されると、第１の飛行コマンド変換部３４に渡される（図６にはＹと記した）。
【００５７】
この反復を続けることによりリーダーＵＡＶ１４は目標地の点ｐに近づき、||Ｔ||≦ΔＬを満たすとき、リーダーＵＡＶ１４が目標地に到達したと判定してホバリング状態になる。
【００５８】
ステップＳ１１０７で、リーダーＵＡＶ１４の編隊が未完了であるものとして、カメラ映像監視部４４に編隊未完了通知を送信する（図６にはＷと記した）。
【００５９】
ステップＳ１１０８で、位置制御の処理を停止するかを判定し、処理を停止する場合にはリーダー位置制御部３２の処理を終了し、終了しない場合にはステップＳ１１０２に戻って処理を繰り返す。
【００６０】
ステップＳ１１１０で、ホバリング状態でリーダーＵＡＶ１４を待機させるように制御する。
【００６１】
ステップＳ１１１２で、カメラ映像監視部４４に編隊完了通知を送信し（図６にはＷと記した）、ステップＳ１１０２へ移行する。
【００６２】
以上の処理によって、リーダー位置制御部３２は、リーダーＵＡＶ１４の現在位置ｅから目標地の点ｐまでの距離||Ｔ||に応じて第１制御データｕを出力する。
【００６３】
次に、ステップＳ１０４の第１の飛行コマンド変換部３４の飛行コマンド変換処理の詳細について説明する。
【００６４】
第１の飛行コマンド変換部３４の処理により、リーダーＵＡＶ１４について、リーダー位置制御部３２より与えられる制御データｕとリーダー位置検出部３０で得た方位θを、リーダーＵＡＶ１４に応じて飛行コマンドの飛行指令データへ変換して通信部５０から無線経由で指令する。ＵＡＶ１４への飛行コマンドには様々なデータ形式が存在するが、本実施の形態では、市販製品のAR Drone 2.0を例にした場合を示す。ただし、それ以外のＵＡＶ１４の飛行を制御する場合にも、本実施の形態を利用できることは言うまでもない。また、以下は第１の飛行コマンド変換部３４におけるリーダーＵＡＶ１４の場合を例に説明するが、第２の飛行コマンド変換部３５についても第１の飛行コマンド変換部３４と同様に制御できる。第２の飛行コマンド変換部３５において、一つ以上のフォロワーＵＡＶ１５＃ｉを制御する場合については、第１制御データｕと方位角θとを、第２制御データｕｉと方位角θｉとに置き換えて飛行指令データに変換して制御する。
【００６５】
図７は第１の飛行コマンド変換部３４の飛行コマンド変換処理のフロー図である。第１の飛行コマンド変換部３４は処理を開始すると、ステップＳ１２００で、更新された制御データｕと、算出された方位角θとの入力を受け付ける。
【００６６】
ステップＳ１２０２で、制御データｕと方位角θとの組み合わせを、リーダーＵＡＶ１４へ送信するための飛行指令データへ変換する。リーダーＵＡＶ１４への飛行指令データは、機体に設定されたＸ軸周りの回転速度Ｖｘ、Ｙ軸周りの回転速度Ｖｙ、Ｚ軸に沿った速度Ｖｚと、Ｚ軸周りの回転速度Ｖθになる。制御データｕがｕ＝(ｕｘ,ｕｙ,ｕｚ)と与えられるとする。ＵＡＶ１４では、Ｘ軸周りの回転がＹ軸の並進運動を生み出し、Ｙ軸周りの回転がＸ軸の並進運動を生み出すため、本処理では、リーダーＵＡＶ１４に与える飛行指令データを以下（１）式により変換する。
【００６７】
【数１】
2020023283.tif 000005
・・・（１）
【００６８】
係数αｘ、αｙ、αｚ、αθはAR Drone 2.0が扱うことができる変換係数であり、パラメータとしてユーザが決めてよく、例えば、αｘ＝αｙ＝αｚ＝αθ＝０．１と与える。
【００６９】
ステップＳ１２０４で、通信部５０を介して無線通信により（１）式で算出した飛行指令データをＵＡＶ１４へ送信する。
【００７０】
本処理は、リーダー位置制御部３２で得た制御データｕとリーダー位置検出部３０で得た方位角θが与えられるたびに、（１）式で算出した飛行指令データをリーダーＵＡＶ１４へ送り続ける。図５で示したように、方位角θを送出することにより、目標地の点ｐに対するリーダーＵＡＶ１４の向きはマーカーの点ａとマーカーの点ｂが点Ａと点Ｂとに一致するように補正される。
【００７１】
図８はフォロワー位置検出部４０のフォロワー位置検出処理のフロー図である。図８において、フォロワーＵＡＶ１５＃ｉの位置を検出する。本実施の形態では、フォロワーＵＡＶ１５が１台であるため、ｉ＝１の場合のフォロワー位置検出部４０の処理フローを説明する。
【００７２】
フォロワー位置検出部４０は処理を開始すると、ステップＳ１３００で、フォロワーＵＡＶ１５＃１に対して、２台フォーメーションにおける目標地の３次元座標ｐ１を設定する。点ｐ１はリーダーＵＡＶ１４の現在位置ｅから点ｃと点ｄを結ぶ第２線分の延長線上に距離Ｌ＋Ｌ１だけ離れた座標値とする。Ｌ≠＝Ｌ１の場合も処理内容は変わらないためＬ＝Ｌ１，Ｌ＞０の長さとする。リーダーＵＡＶ１４の運動に従ってフォーメーション飛行させるため、ｐ１＝ｅ＋（−２Ｌ，０，０）と与える。
【００７３】
図９に、リーダーＵＡＶ１４とフォロワーＵＡＶ１５＃１の初期状態での位置関係を示す。初期状態として、フォロワーＵＡＶ１５＃１はリーダーＵＡＶ１４の方向を向いているが、本処理はリーダーＵＡＶ１４とフォロワーＵＡＶ１５＃１の初期位置と初期方向に依存しない。
【００７４】
ステップＳ１３０２で、フォロワーＵＡＶ１５＃１に装着したマーカー点ａ１、点ｂ１、点ｃ１、及び点ｄ１を位置計測センサ１０により検出して、交点ｅ１の位置を算出する。点ｅ１の座標はフォロワーＵＡＶ１５＃１の現在位置となる。現在位置ｅ１が算出されると、フォーメーション制御部４２に渡される（図８にはＺと記した）。
【００７５】
次に、ステップＳ１３０４で、フォロワーＵＡＶ１５＃１の現在位置（点ｅ１）から目標地（点ｐ１）までのベクトルＴ１を、Ｔ１＝ｐ１−ｅ１の計算により求める。
【００７６】
ステップＳ１３０６で、点ａ１から点ｂ１へのベクトルａ１−ｂ１がベクトルＴ１となす回転角θ１（方位角）をベクトル内積の関係を利用して算出する。方位角θ１が算出されると、フォーメーション制御部４２に渡される（図８にはＺと記した）。
【００７７】
ステップＳ１３０８で、位置検出の処理を停止するかを判定し、処理を停止する場合にはフォロワー位置検出部４０の処理を終了し、終了しない場合にはステップＳ１３０２に戻って処理を繰り返す。
【００７８】
以上の処理により、フォロワー位置検出部４０ではフォロワーＵＡＶ１５＃１の現在位置（点ｅ１）と、フォロワーＵＡＶ１５＃１の現在位置（点ｅ１）から目標地（点ｐ１）までのベクトルＴ１と、フォロワーＵＡＶ１５＃１の方位角θ１を算出する。方位角θ１が算出されると、フォーメーション制御部４２に渡される（図８にはＺと記した）。
【００７９】
図１０はフォーメーション制御部４２のフォーメーション制御処理のフロー図である。本実施の形態では、フォロワーＵＡＶ１５が１台であるため、ｉ＝１の場合のフォロワー位置検出部４０の処理フローを説明する。図１１に示すように、リーダーＵＡＶ１４の正反対の位置及び向きのフォーメーションでフォロワーＵＡＶ１５＃１を飛行させるように運動を制御する。図１１において、点ｇはリーダーＵＡＶ１４の位置ｅとフォロワーＵＡＶ１５＃１の位置ｅ１の中点の座標であり、フォロワーＵＡＶ１５＃１は点ｇを中心に１８０度回転した方向を向いた姿勢でリーダーＵＡＶ１４と同じ方向を飛行する。
【００８０】
フォーメーション制御部４２は処理を開始すると、ステップＳ１４００で、許容距離ΔＬを指定する。許容距離ΔＬとは、フォロワーＵＡＶ１５＃ｉの現在位置ｅ１が目標地の点ｐ１に到達したかどうかを判定するための距離である。便宜上、リーダー位置制御部３２で設定した同じ許容距離を設定する。
【００８１】
フォロワー位置検出部４０において、フォロワーＵＡＶ１５＃１の現在位置（点ｅ１）と方位角θ１が位置計測センサ１０を使って時系列に得られており、現在位置と方位角θ１を取得するたびにステップＳ１４０２に与えられる（図１０にはＺと記した）。
【００８２】
ステップＳ１４０２で、与えられた現在位置と方位角θ１のデータから、フォロワーＵＡＶ１５＃１の現在位置ｅ１から目標地の点ｐ１までの距離||Ｔｉ||を算出する。
【００８３】
ステップＳ１４０４で、距離||Ｔ１||が許容距離ΔＬ以下であるか否かを判定する。||Ｔ１||≦ΔＬであればステップＳ１４１８へ移行する。||Ｔ１||≦ΔＬでなく、||Ｔ||＞ΔＬであれば運動を制御するためステップＳ１４０６へ移行する。
【００８４】
ステップＳ１４０６で、フォロワーＵＡＶ１５＃１とリーダーＵＡＶ１４との方位角の差ω１を以下（２）式により算出する。
【００８５】
【数２】
2020023283.tif 000006
・・・（２）
【００８６】
ステップＳ１４０８で、方位角の差分ω１の絶対値｜｜ω１｜｜が許容角度Δθ以下であるか否かを判定する。許容角度Δθ以下であればステップＳ１４１０へ移行する。許容角度Δθ以下でなければステップＳ１４１２へ移行する。ここでは、方位誤差の許容値としてΔθを設定するが、ドリフトを考慮して、例えば、Δθ＝π／１８０（ラジアン）と与える。許容角度Δθ以下ならば、フォロワーＵＡＶ１５＃１の方位角はほぼθ１＝θ＋πとなる。
【００８７】
ステップＳ１４１０で、フォロワーＵＡＶ１５＃１とリーダーＵＡＶ１４との方位が真反対の１８０度をなすようにフォロワーＵＡＶ１５＃１の方位角θ１を以下（３）式により更新する（正の回転を反時計周りの回転とする）。更新した方位角θ１は第２の飛行コマンド変換部３５に渡される（図１０にはＹと記した）。（３）式において、本実施の形態ではｉ＝１である。
【００８８】
【数３】
2020023283.tif 000007
・・・（３）
【００８９】
ステップＳ１４１２で、変化がある現在の方位角θ１に応じて、フォロワーＵＡＶ１５＃１の並進ベクトルＴ１’を以下（４）式により変換する。（４）式において、本実施の形態ではｉ＝１である。ステップＳ１４１０を処理した場合には、ステップＳ１４１０で更新した方位角θ１を用いる。ステップＳ１４１０を処理していない場合には、上記ステップＳ１３０６で算出した方位角θ１を用いる。
【００９０】
【数４】
2020023283.tif 000008
・・・（４）
【００９１】
ステップＳ１４１４で、第２制御データを
2020023283.tif 000009
の計算により求める。第２制御データｕ１は、第２の飛行コマンド変換部３５においてフォロワーＵＡＶ１５＃１への飛行指令として使われる。
【００９２】
2020023283.tif 000010
はフィードバック制御におけるゲインパラメータであり、ユーザが状況に応じて設定する。第２制御データｕ１が計算されると、第２の飛行コマンド変換部３５に渡される（図１０にはＹと記した）。
【００９３】
ステップＳ１４１５で、フォロワーＵＡＶ１５＃１の編隊が未完了であるものとして、カメラ映像監視部４４に編隊未完了通知を送信する（図１０にはＷと記した）。
【００９４】
ステップＳ１４１６で、フォーメーション制御の処理を停止するかを判定し、処理を停止する場合にはフォーメーション制御部４２の処理を終了し、終了しない場合にはステップＳ１４０２に戻って処理を繰り返す。
【００９５】
この反復を続けることによりフォロワーＵＡＶ１５＃１は目標地の点ｐ１に近づくことになり、||Ｔ１’||≦ΔＬを満たすとき図１１のフォーメーションに従って飛行する。
【００９６】
以上の処理によって、フォーメーション制御部４２は、フォロワーＵＡＶ１５＃１の現在位置ｅ１から目標地の点ｐ１までの距離||Ｔ１’||に応じて制御データｕｉを出力する。
【００９７】
ステップＳ１４１８で、ホバリング状態でフォロワーＵＡＶ１５＃１を待機させるように制御する。
【００９８】
ステップＳ１４２０で、カメラ映像監視部４４に編隊完了通知を送信し（図１０にはＷと記した）、ステップＳ１４０２へ移行する。
【００９９】
図１２はカメラ映像監視部４４のカメラ映像監視処理のフロー図である。カメラ映像監視部４４は、リーダーＵＡＶ１４とフォロワーＵＡＶ１５＃１による編隊を組んで飛行している間にカメラ映像を取得して全周囲の環境を監視する機能を有する。本実施の形態では、フォロワーＵＡＶ１５が１台であるため、ｉ＝１の場合のカメラ映像監視部４４の処理フローを説明する。なお、Ｎ台の場合もｉ＝１，２，・・・，Ｎとして同様に処理できる。
【０１００】
カメラ映像監視部４４は、処理を開始すると、ステップＳ１５００で、リーダーＵＡＶ１４からの編隊通知情報を受信する。編隊通知情報とは、リーダーＵＡＶ１４とフォロワーＵＡＶ１５＃１による編隊が完了して飛行しているか否かを知らせる通知である。編隊が完了している場合には、編隊通知情報として編隊完了通知を受信する。編隊が完了してない場合や編隊が崩れた場合には、編隊通知情報として編隊未完通知を受信する。
【０１０１】
ステップＳ１５０２で、全てのＵＡＶから編隊完了通知を受信したかを判定し、受信していればステップＳ１５０４へ移行し、受信していなければステップＳ１５１０へ移行して待機状態に移行する。このように、もし、１台でも編隊が完了していない場合は待機状態になり、まだ通知を受信していないＵＡＶの編隊完了通知を待つ。
【０１０２】
ステップＳ１５０４で、リーダーＵＡＶ１４とフォロワーＵＡＶ１５＃１から同期信号を受信する。同期信号は時間：分：秒：フレームから構成されるタイムコードを利用する。タイムコードを取得した映像フレームと合わせることで、リーダーＵＡＶ１４とフォロワーＵＡＶ１５＃１で取得した映像フレームの同期を合わせることができる。
【０１０３】
ステップＳ１５０６で、同期信号を受信しながら、ＵＡＶの前方のカメラから映像を取得する命令をリーダーＵＡＶ１４とフォロワーＵＡＶ１５＃１に送信する。通信部５０に、リーダーＵＡＶ１４とフォロワーＵＡＶ１５＃１から取得した映像を転送する命令を送信し、取得した映像ＤＢ４６に格納する。
【０１０４】
ステップＳ１５０８で、カメラ映像監視の処理を停止するかを判定し、処理を停止する場合にはカメラ映像監視部４４の処理を終了し、終了しない場合にはステップＳ１５１０に移行して、待機状態となってから処理を繰り返す。
【０１０５】
もし、カメラから映像を取得中に、何らかのトラブルにより編隊が崩れた場合、編隊通知情報において編隊が未完という通知が入る。このとき、カメラ映像の取得を停止させて、編隊が完了するまで待機する。
【０１０６】
以上の処理によって、カメラ映像監視部４４はリーダーＵＡＶ１４とフォロワーＵＡＶ１５＃１による編隊飛行中に全周囲の映像を取得する。
【０１０７】
以上により、本実施の形態は、位置計測センサ１０によってリーダーＵＡＶ１４とフォロワーＵＡＶ１５＃１の位置と方位を算出し、２台のＵＡＶが編隊を組んで飛行し、全周囲の映像を監視することができる。さらに、リーダーＵＡＶ１４の目標地の位置ｐを空間経路に沿って間隔を空けて空間中に設定することで、その軌跡に沿って２台のＵＡＶにより全周囲の映像を監視することができる。
【０１０８】
以上説明したように、第１の実施の形態に係る飛行制御装置によれば、複数台のＵＡＶが編隊を組んで全方位の監視しながら円滑かつ安定的に飛行させるように制御できる。
【０１０９】
＜本発明の第２の実施の形態に係る飛行制御装置の構成及び作用＞
本発明の第２の実施の形態は、上記図１の第１の実施の形態の構成により、リーダーＵＡＶ１４と２台のフォロワーＵＡＶ１５が三角形フォーメーションのリーダー・フォロワー編隊飛行をする制御の実施の形態である。本実施の形態では、正三角形のフォーメーションにした場合を例に説明するが、二等辺三角形、辺の長さが全て異なる三角形にも応用できる。本実施の形態では、正三角形のフォーメーションに応じてフォロワーＵＡＶ１５の飛行を制御する処理が第１の実施の形態と異なるため、以降では第１の実施の形態との差分のみを説明する。
【０１１０】
本実施の形態のフォロワー位置検出部４０は図８のフローに従う。図８において、ｉ＝１の場合がフォロワーＵＡＶ１５＃１に関するフォロワー位置検出部４０の処理フローであり、ｉ＝２の場合がフォロワーＵＡＶ１５＃２に関するフォロワー位置検出部４０の処理フローである。
【０１１１】
フォロワー位置検出部４０は処理を開始すると、ステップＳ１３００で、フォロワーＵＡＶ１５＃ｉに対して、３台フォーメーションにおける目標地の３次元座標ｐｉを設定する。リーダーＵＡＶ１４の現在位置と点ｐｉは正三角形の頂点であり、三角形の重心から各頂点までの距離をＬとする。重心ｇを原点として、リーダーＵＡＶ１４の現在位置がｅであるとき、フォロワーＵＡＶ１５＃１の目標地は３次元座標ｐ１＝ｅ＋（−２Ｌ／３，√３Ｌ／２，０）と与え、フォロワーＵＡＶ１５＃２の目標地は３次元座標ｐ２＝ｅ＋（−２Ｌ／３，−√３Ｌ／２，０）と与える。
【０１１２】
図１３に、リーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉの初期状態での位置関係を示す。初期状態として、フォロワーＵＡＶ１５＃ｉはリーダーＵＡＶ１４の方向を向いているが、本処理はリーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉの初期位置と初期方向に依存しない。
【０１１３】
ステップＳ１３０２で、フォロワーＵＡＶ１５＃ｉに装着したマーカー点ａｉ、点ｂｉ、点ｃｉ、及び点ｄｉを位置計測センサ１０により検出して、交点ｅｉの位置を算出する。点ｅｉの座標はフォロワーＵＡＶ１５＃ｉの現在位置となる。
【０１１４】
次に、ステップＳ１３０４で、フォロワーＵＡＶ１５＃ｉの現在位置（点ｅｉ）から目標地（点ｐｉ）までのベクトルＴｉを、Ｔｉ＝ｐｉ−ｅｉの計算により求める。
【０１１５】
ステップＳ１３０６で、点ａｉから点ｂｉへのベクトルａｉ−ｂｉがベクトルＴｉとなす回転角θｉ（方位角）をベクトル内積の関係を利用して算出する。
【０１１６】
以上の処理により、フォロワー位置検出部４０ではフォロワーＵＡＶ１５＃ｉの現在位置（点ｅｉ）と、フォロワーＵＡＶ１５＃ｉの現在位置（点ｅｉ）から目標地（点ｐｉ）までのベクトルＴｉと、フォロワーＵＡＶ１５＃ｉの方位角θｉを算出する。
【０１１７】
本実施の形態のフォーメーション制御部４２は図１０のフローに従う。図１０において、ｉ＝１の場合がフォロワーＵＡＶ１５＃１に関するフォーメーション制御部４２の処理フローであり、ｉ＝２の場合がフォロワーＵＡＶ１５＃２に関するフォーメーション制御部４２の処理フローである。
【０１１８】
本実施の形態では、リーダーＵＡＶ１４の方位から反時計周りに１２０度回転した方位をフォロワーＵＡＶ１５＃１の方位とし、リーダーＵＡＶ１４の方位から反時計周りに２４０度回転した方位をフォロワーＵＡＶ１５＃２の方位とする。このように、リーダーＵＡＶ１４の方位に対して等分に振り分けた角度の方位を設定する。ステップＳ１４０６で、フォロワーＵＡＶ１５＃ｉとリーダーＵＡＶ１４との方位角の差分ωｉを以下（５）式により算出する。
【０１１９】
【数５】
2020023283.tif 000011
・・・（５）
【０１２０】
ステップＳ１４０８で、方位角の差分ωｉの絶対値｜｜ωｉ｜｜が許容角度Δθ以下であるか否かを判定する。許容角度Δθ以下であればステップＳ１４１０へ移行する。許容角度Δθ以下でなければステップＳ１４１２へ移行する。
【０１２１】
ステップＳ１４１０で、フォロワーＵＡＶ１５＃ｉの方位角θｉを上記（３）式により更新する。更新した方位角θｉは第２の飛行コマンド変換部３５に渡される（図１０にはＹと記した）。
【０１２２】
ステップＳ１４１２で、変化がある現在の方位角θｉに応じて、フォロワーＵＡＶ１５＃ｉの並進ベクトルＴ１’を上記（４）式により変換する。ステップＳ１４１０を処理した場合には、ステップＳ１４１０で更新した方位角θｉを用いる。ステップＳ１４１０を処理していない場合には、上記ステップＳ１３０６で算出した方位角θｉを用いる。
【０１２３】
ステップＳ１４１４で、第２制御データを
2020023283.tif 000012
の計算により求める。第２制御データｕｉは、第２の飛行コマンド変換部３５においてフォロワーＵＡＶ１５＃ｉへの飛行指令として使われる。この反復を続けることによりフォロワーＵＡＶ１５＃ｉは目的地の点ｐｉに近づくことになり、||Ｔｉ’||≦ΔＬを満たすとき図１４のフォーメーションに従って編隊飛行する。
【０１２４】
以上により、本実施の形態は、位置計測センサ１０によってリーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉの位置と方位を算出し、３台のＵＡＶが編隊を組んで飛行し、全周囲の映像を監視することができる。さらに、リーダーＵＡＶ１４の目標地の位置ｐを空間経路に沿って間隔を空けて空間中に設定することで、その軌跡に沿って３台のＵＡＶにより全周囲の映像を監視することができる。
【０１２５】
＜本発明の第３の実施の形態に係る飛行制御装置の構成及び作用＞
【０１２６】
本発明の第３の実施の形態は、上記図１の第１の実施の形態の構成により、リーダーＵＡＶ１４と３台のフォロワーＵＡＶ１５が正方形フォーメーションのリーダー・フォロワー編隊飛行をする制御の実施の形態である。本実施の形態では、正方形のフォーメーションにした場合を例に説明するが、長方形、菱形、四角形にも応用できる。本実施の形態では、正方形のフォーメーションに応じてフォロワーＵＡＶ１５の飛行を制御する処理が第２の実施の形態と異なるため、以降では第２の実施の形態との差分のみを説明する。
【０１２７】
本実施の形態のフォロワー位置検出部４０は図８のフローに従う。図８において、ｉ＝１の場合がフォロワーＵＡＶ１５＃１に関するフォロワー位置検出部４０の処理フローであり、ｉ＝２の場合がフォロワーＵＡＶ１５＃２に関するフォロワー位置検出部４０の処理フローであり、ｉ＝３の場合がフォロワーＵＡＶ１５＃３に関するフォロワー位置検出部４０の処理フローである。
【０１２８】
フォロワー位置検出部４０は処理を開始すると、ステップＳ１３００で、フォロワーＵＡＶ１５＃ｉに対して、３台フォーメーションにおける目標地の３次元座標ｐｉを設定する。リーダーＵＡＶ１４の現在位置と点ｐｉは正方形の頂点であり、正方形の重心から各頂点までの距離をＬとする。重心ｇを原点として、リーダーＵＡＶ１４の現在位置がｅであるとき、フォロワーＵＡＶ１５＃１の目標地は３次元座標ｐ１＝ｅ＋（−Ｌ，Ｌ，０）と与え、フォロワーＵＡＶ１５＃２の目標地は３次元座標ｐ２＝ｅ＋（−２Ｌ，０，０）と与え、フォロワーＵＡＶ１５＃３の目標地は３次元座標ｐ３＝ｅ＋（−Ｌ，−Ｌ，０）と与える。
【０１２９】
図１５に、リーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉの初期状態での位置関係を示す。初期状態として、フォロワーＵＡＶ１５＃ｉはリーダーＵＡＶ１４の方向を向いているが、本処理はリーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉの初期位置と初期方向に依存しない。以降のステップの処理は第２の実施の形態と同様である。
【０１３０】
本実施の形態のフォーメーション制御部４２は図１０のフローに従う。図１０において、ｉ＝１の場合がフォロワーＵＡＶ１５＃１に関するフォーメーション制御部４２の処理フローであり、ｉ＝２の場合がフォロワーＵＡＶ１５＃２に関するフォーメーション制御部４２の処理フローであり、ｉ＝３の場合がフォロワーＵＡＶ１５＃３に関するフォーメーション制御部４２の処理フローである。
【０１３１】
本実施の形態では、リーダーＵＡＶ１４の方位から反時計周りに９０度回転した方位をフォロワーＵＡＶ１５＃１の方位とし、リーダーＵＡＶ１４の方位から反時計周りに１８０度回転した方位をフォロワーＵＡＶ１５＃２の方位とし、リーダーＵＡＶ１４の方位から反時計周りに２７０度回転した方位をフォロワーＵＡＶ１５＃３の方位とする。このように、リーダーＵＡＶ１４の方位に対して等分に振り分けた方位を設定する。ステップＳ１４０６で、フォロワーＵＡＶ１５＃ｉとリーダーＵＡＶ１４との方位角の差ωｉを以下（６）式により算出する。
【０１３２】
【数６】
2020023283.tif 000013
・・・（６）
【０１３３】
以降は、第２の実施の形態のフォーメーション制御部４２の処理と同様であり、フォロワーＵＡＶ１５＃ｉは目的地の点ｐｉに近づくことになり、||Ｔｉ’||≦ΔＬを満たすとき図１６のフォーメーションに従って編隊飛行する。
【０１３４】
以上により、本実施の形態は、位置計測センサ１０によってリーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉの位置と方位を算出し、４台のＵＡＶが編隊を組んで飛行し、全周囲の映像を監視することができる。さらに、リーダーＵＡＶ１４の目標地の位置ｐを空間経路に沿って間隔を空けて空間中に設定することで、その軌跡に沿って４台のＵＡＶにより全周囲の映像を監視することができる。
【０１３５】
＜本発明の第４の実施の形態に係る飛行制御装置の構成及び作用＞
【０１３６】
本発明の第４の実施の形態は、上記図１の第１の実施の形態の構成により、リーダーＵＡＶ１４とＮ台のフォロワーＵＡＶ１５が正多角形フォーメーションのリーダー・フォロワー編隊飛行をする制御の実施の形態である。本実施の形態では、正多角形のフォーメーションにした場合を例に説明する。本実施の形態では、正多角形のフォーメーションに応じてフォロワーＵＡＶ１５の飛行を制御する処理が第３の実施の形態と異なるため、以降では第３の実施の形態との差分のみを説明する。
【０１３７】
本実施の形態のフォロワー位置検出部４０は図８のフローに従う。図８において、ｉ＝１，２，・・・，Ｎの場合が、それぞれフォロワーＵＡＶ１５＃１，フォロワーＵＡＶ１５＃２，・・・，フォロワーＵＡＶ１５＃Ｎのそれぞれに関するフォロワー位置検出部４０の処理フローである。
【０１３８】
フォロワー位置検出部４０は処理を開始すると、ステップＳ１３００で、フォロワーＵＡＶ１５＃ｉに対して、Ｍ＝（Ｎ＋１）台フォーメーションにおける目標地の３次元座標ｐｉを設定する。リーダーＵＡＶ１４の現在位置と点ｐｉは正多角形の頂点であり、正多角形の重心から各頂点までの距離をＬとする。重心ｇを原点として、リーダーＵＡＶ１４の現在位置がｅであるとき、フォロワーＵＡＶ１５＃ｉの目標地は３次元座標ｐ１＝ｅ＋（Ｌ（ｃｏｓ（ｉ×２π／Ｍ）−１），Ｌ（ｓｉｎ（ｉ×２π／Ｍ）），０）と与える。以降のステップの処理は第３の実施の形態と同様である。
【０１３９】
本実施の形態のフォーメーション制御部４２は図１０のフローに従う。図１０において、ｉ＝１，２，・・・，Ｎの場合が、それぞれフォロワーＵＡＶ１５＃１，フォロワーＵＡＶ１５＃２，・・・，フォロワーＵＡＶ１５＃Ｎのそれぞれに関するフォーメーション制御部４２の処理フローである。
【０１４０】
本実施の形態では、リーダーＵＡＶ１４の方位から反時計周りに３６０×ｉ／Ｍ度回転した方位をフォロワーＵＡＶ１５＃ｉの方位とする。ステップＳ１４０６で、フォロワーＵＡＶ１５＃ｉとリーダーＵＡＶ１４との方位角の差ωｉを以下（７）式により算出する。
【０１４１】
【数７】
2020023283.tif 000014
・・・（７）
【０１４２】
以降は、第３の実施の形態のフォーメーション制御部４２の処理と同様であり、フォロワーＵＡＶ１５＃ｉは目的地の点ｐｉに近づくことになり、||Ｔｉ’||≦ΔＬを満たすとき図１６のフォーメーションに従って編隊飛行する。
【０１４３】
以上により、本実施の形態は、位置計測センサ１０によってリーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉの位置と方位を算出し、Ｍ台のＵＡＶが編隊を組んで飛行し、全周囲の映像を監視することができる。さらに、リーダーＵＡＶ１４の目標地の位置ｐを空間経路に沿って間隔を空けて空間中に設定することで、その軌跡に沿ってＭ台のＵＡＶにより全周囲の映像を監視することができる。
【０１４４】
＜本発明の第５の実施の形態に係る飛行制御装置の構成及び作用＞
【０１４５】
本発明の第５の実施の形態は、上記図１の第１の実施の形態の構成により、リーダーＵＡＶ１４とＮ台のフォロワーＵＡＶ１５が多角形フォーメーションのリーダー・フォロワー編隊飛行をする制御の実施の形態である。本実施の形態では、重心からの距離を異ならせた多角形のフォーメーションにした場合を例に説明する。環境における風速などの状況によっては、正多角形ではなく、状況に応じた多角形にした方がよい場合がある。本実施の形態では、多角形のフォーメーションに応じてフォロワーＵＡＶ１５の飛行を制御する処理が第４の実施の形態と異なるため、以降では第４の実施の形態との差分のみを説明する。
【０１４６】
本実施の形態のフォロワー位置検出部４０は図８のフローに従う。図８において、ｉ＝１，２，・・・，Ｎの場合が、それぞれフォロワーＵＡＶ１５＃１，フォロワーＵＡＶ１５＃２，・・・，フォロワーＵＡＶ１５＃Ｎのそれぞれに関するフォロワー位置検出部４０の処理フローである。
【０１４７】
フォロワー位置検出部４０は処理を開始すると、ステップＳ１３００で、フォロワーＵＡＶ１５＃ｉに対して、Ｍ＝（Ｎ＋１）台フォーメーションにおける目標地の３次元座標ｐｉを設定する。リーダーＵＡＶ１４の現在位置と点ｐｉは多角形の頂点であり、多角形の重心からフォロワーＵＡＶ１５＃ｉの各頂点までの距離をＬｉとする。多角形の重心からリーダーＵＡＶ１４までの距離はＬとする。ただし、Ｌｉの距離はフォロワーＵＡＶ１５＃ｉに応じて全て異ならせる。フォロワーＵＡＶ１５＃ｉ同士がぶつかり合わない程度に、Ｌ≠Ｌ１≠Ｌ２≠・・・ＬＮとそれぞれのフォロワーＵＡＶ１５＃ｉに異なる距離を設定する。重心ｇを原点として、リーダーＵＡＶ１４の現在位置がｅであるとき、フォロワーＵＡＶ１５＃１の目標地はｐｉ＝ｅ＋（Ｌｉ（ｃｏｓ（ｉ×２π／Ｍ−Ｌ），Ｌｉ（ｓｉｎ（ｉ×２π／Ｍ）），０）と与える。以降のステップの処理は第４の実施の形態と同様であり、重心からの距離がそれぞれ異なる多角形のフォーメーションに従って編隊飛行する。
【０１４８】
以上により、本実施の形態は、位置計測センサ１０によってリーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉの位置と方位を算出し、Ｍ台のＵＡＶが編隊を組んで飛行し、全周囲の映像を監視することができる。さらに、リーダーＵＡＶ１４の目標地の位置ｐを空間経路に沿って間隔を空けて空間中に設定することで、その軌跡に沿ってＭ台のＵＡＶにより全周囲の映像を監視することができる。
【０１４９】
＜本発明の第６の実施の形態に係る飛行制御装置の構成及び作用＞
【０１５０】
本発明の第６の実施の形態は、上記図１の第１の実施の形態の構成により、リーダーＵＡＶ１４とＮ台のフォロワーＵＡＶ１５が多角形フォーメーションのリーダー・フォロワー編隊飛行をする制御の実施の形態である。本実施の形態では、方位角の間隔を異ならせた多角形のフォーメーションにした場合を例に説明する。本実施の形態では、多角形のフォーメーションに応じてフォロワーＵＡＶ１５の飛行を制御する処理が第４の実施の形態と異なるため、以降では第４の実施の形態との差分のみを説明する。
【０１５１】
本実施の形態のフォーメーション制御部４２は図１０のフローに従う。図１０において、ｉ＝１，２，・・・，Ｎの場合が、それぞれフォロワーＵＡＶ１５＃１，フォロワーＵＡＶ１５＃２，・・・，フォロワーＵＡＶ１５＃Ｎのそれぞれに関するフォーメーション制御部４２の処理フローである。
【０１５２】
本実施の形態では、リーダーＵＡＶ１４の方位から反時計周りにφｉ度回転した方位をフォロワーＵＡＶ１５＃ｉの方位とする。フォロワーＵＡＶ１５＃ｉ同士がぶつかり合わない程度に、φ１≠φ２≠・・・≠φＮとそれぞれのフォロワーＵＡＶ１５＃ｉに異なる方位を設定する。ステップＳ１４０６で、フォロワーＵＡＶ１５＃ｉとリーダーＵＡＶ１４との方位角の差ωｉを以下（８）式により算出する。
【０１５３】
【数８】
2020023283.tif 000015
・・・（８）
【０１５４】
以降は、第４の実施の形態のフォーメーション制御部４２の処理と同様であり、フォロワーＵＡＶ１５＃ｉは目的地の点ｐｉに近づくことになり、||Ｔｉ’||≦ΔＬを満たすとき、方位角の間隔がそれぞれ異なる多角形のフォーメーションに従って編隊飛行する。
【０１５５】
以上により、本実施の形態は、位置計測センサ１０によってリーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉの位置と方位を算出し、Ｍ台のＵＡＶが編隊を組んで飛行し、全周囲の映像を監視することができる。さらに、リーダーＵＡＶ１４の目標地の位置ｐを空間経路に沿って間隔を空けて空間中に設定することで、その軌跡に沿ってＭ台のＵＡＶにより全周囲の映像を監視することができる。
【０１５６】
＜本発明の第７の実施の形態に係る飛行制御装置の構成及び作用＞
【０１５７】
本発明の第７の実施の形態は、上記図１の第１の実施の形態の構成により、リーダーＵＡＶ１４とＮ台のフォロワーＵＡＶ１５が多角形フォーメーションのリーダー・フォロワー編隊飛行をする制御の実施の形態である。本実施の形態では、重心からの距離及び方位を異ならせた多角形のフォーメーションにした場合を例に説明する。本実施の形態では、多角形のフォーメーションに応じてフォロワーＵＡＶ１５の飛行を制御する処理が第４の実施の形態と異なるため、以降では第４の実施の形態との差分のみを説明する。
【０１５８】
本実施の形態のフォロワー位置検出部４０は図８のフローに従う。図８において、ｉ＝１，２，・・・，Ｎの場合が、それぞれフォロワーＵＡＶ１５＃１，フォロワーＵＡＶ１５＃２，・・・，フォロワーＵＡＶ１５＃Ｎのそれぞれに関するフォロワー位置検出部４０の処理フローである。
【０１５９】
フォロワー位置検出部４０は処理を開始すると、ステップＳ１３００で、フォロワーＵＡＶ１５＃ｉに対して、Ｍ＝（Ｎ＋１）台フォーメーションにおける目標地の３次元座標ｐｉを設定する。リーダーＵＡＶ１４の現在位置と点ｐｉは多角形の頂点であり、多角形の重心からフォロワーＵＡＶ１５＃ｉの各頂点までの距離をＬｉとする。多角形の重心からリーダーＵＡＶ１４までの距離はＬとする。ただし、Ｌｉの距離はフォロワーＵＡＶ１５＃ｉに応じて全て異ならせる。フォロワーＵＡＶ１５＃ｉ同士がぶつかり合わない程度に、Ｌ≠Ｌ１≠Ｌ２≠・・・ＬＮとそれぞれのフォロワーＵＡＶ１５＃ｉに異なる距離を設定する。重心ｇを原点として、リーダーＵＡＶ１４の現在位置がｅであるとき、フォロワーＵＡＶ１５＃１の目標地はｐｉ＝ｅ＋（Ｌｉ（ｃｏｓ（ｉ×２π／Ｍ−Ｌ），Ｌｉ（ｓｉｎ（ｉ×２π／Ｍ）），０）と与える。以降のステップの処理は第４の実施の形態と同様である。
【０１６０】
本実施の形態のフォーメーション制御部４２は図１０のフローに従う。図１０において、ｉ＝１，２，・・・，Ｎの場合が、それぞれフォロワーＵＡＶ１５＃１，フォロワーＵＡＶ１５＃２，・・・，フォロワーＵＡＶ１５＃Ｎのそれぞれに関するフォーメーション制御部４２の処理フローである。
【０１６１】
本実施の形態では、リーダーＵＡＶ１４の方位から反時計周りにφｉ度回転した方位をフォロワーＵＡＶ１５＃ｉの方位とする。フォロワーＵＡＶ１５＃ｉ同士がぶつかり合わない程度に、φ１≠φ２≠・・・≠φＮとそれぞれのフォロワーＵＡＶ１５＃ｉに異なる方位を設定する。ステップＳ１４０６で、フォロワーＵＡＶ１５＃ｉとリーダーＵＡＶ１４との方位角の差ωｉを上記（８）式により算出する。
【０１６２】
以降は、第４の実施の形態のフォーメーション制御部４２の処理と同様であり、フォロワーＵＡＶ１５＃ｉは目的地の点ｐｉに近づくことになり、||Ｔｉ’||≦ΔＬを満たすとき重心からの距離及び方位角がそれぞれ異なる多角形のフォーメーションに従って編隊飛行する。
【０１６３】
以上により、本実施の形態は、位置計測センサ１０によってリーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉの位置と方位を算出し、Ｍ台のＵＡＶが編隊を組んで飛行し、全周囲の映像を監視することができる。さらに、リーダーＵＡＶ１４の目標地の位置ｐを空間経路に沿って間隔を空けて空間中に設定することで、その軌跡に沿ってＭ台のＵＡＶにより全周囲の映像を監視することができる。
【０１６４】
なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。
【０１６５】
例えば、上述した各実施形態では、飛行制御装置により、リーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉとを制御する場合を例に説明したが、これに限定されるものではなく、飛行制御システムとして、リーダー飛行制御装置とフォロワー飛行制御装置とを構成し、リーダーＵＡＶ１４とフォロワーＵＡＶ１５＃ｉとをそれぞれの装置によって制御するようにしてもよい。
【符号の説明】
【０１６６】
１０   位置計測センサ
１２   目標地
２０   演算部
３０   リーダー位置検出部
３２   リーダー位置制御部
３４   第１の飛行コマンド変換部
３５   第２の飛行コマンド変換部
４０   フォロワー位置検出部
４２   フォーメーション制御部
４４   カメラ映像監視部
４６   映像ＤＢ
５０   通信部
１００ 飛行制御装置
【図１】
2020023283.tif 000016
【図２】
2020023283.tif 000017
【図３】
2020023283.tif 000018
【図４】
2020023283.tif 000019
【図５】
2020023283.tif 000020
【図６】
2020023283.tif 000021
【図７】
2020023283.tif 000022
【図８】
2020023283.tif 000023
【図９】
2020023283.tif 000024
【図１０】
2020023283.tif 000025
【図１１】
2020023283.tif 000026
【図１２】
2020023283.tif 000027
【図１３】
2020023283.tif 000028
【図１４】
2020023283.tif 000029
【図１５】
2020023283.tif 000030
【図１６】
2020023283.tif 000031
【図１７】
2020023283.tif 000032
【図１８】
2020023283.tif 000033
【図１９】
2020023283.tif 000034