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