JP7351609B2 - 経路探索装置及びプログラム - Google Patents

Description

本発明は、経路探索装置及びプログラムに係り、特に、三次元の移動可能空間内における開始位置から目標位置に至る移動経路を求める経路探索装置及びプログラムに関する。

従来、現在位置から所定の目標位置に至る飛行経路が最短となるよう飛行経路を算出し、当該飛行経路に沿って自律的に飛行可能な自律飛行ロボットが提案されている。

例えば、特許文献１には、建物や樹木などの障害物を除いた空間（飛行可能空間）を所定サイズのボクセルにて分割し、各ボクセルの重心位置にノードを置いたグラフ構造を求め、当該グラフ構造にＡ＊（エースター）経路探索法などの経路探索アルゴリズムを適用することで、開始位置から目的位置までに至る最短経路を探索し、飛行経路を求める技術が記載されている。

特開2014-119901号公報

しかしながら、上記の従来技術では、飛行可能空間が広大となるに従ってボクセル数が増大するため、経路探索のために要する計算コストが膨大なものとなり、リアルタイムで飛行経路を求めることが困難となっていた。

そこで、本発明は、移動可能空間が広くなっても膨大な計算コストとなることなく移動経路を生成することができる経路探索装置及びプログラムを提供することを目的とする。

上記の目的を達成するために本発明に係る経路探索装置は、三次元の移動可能空間内における開始位置から目標位置に至る移動経路を求める経路探索装置であって、前記移動可能空間を表す立体の辺上の各点又は各頂点をノードとし、各ノードについて、当該ノードから前記移動可能空間内の線分にて接続可能な他のノードをエッジにて連結した可視グラフを記憶した記憶部と、前記可視グラフに前記開始位置および前記目標位置から前記移動可能空間内の線分にて接続可能な前記ノードへのエッジを加えたグラフにおいて、前記開始位置から前記目標位置に至る最短経路上の前記ノードを前記エッジで連結した経路グラフを求める経路グラフ算出手段と、前記経路グラフを用いて前記移動経路を求める経路算出手段とを備えて構成されている。

本発明に係る経路探索装置によれば、経路グラフ算出手段によって、前記可視グラフに前記開始位置および前記目標位置から前記移動可能空間内の線分にて接続可能な前記ノードへのエッジを加えたグラフにおいて、前記開始位置から前記目標位置に至る最短経路上の前記ノードを前記エッジで連結した経路グラフを求める。そして、経路算出手段によって、前記経路グラフを用いて前記移動経路を求める。

また、前記経路算出手段は、前記経路グラフを構成するノードの位置を、前記移動可能空間を表す立体の辺上に沿って変位させることによって前記エッジで表される経路を最短化させ前記移動経路を求めることが好適である。

また、前記経路算出手段は、変位対象のノードに接続された他の２つのノードまでの距離の和が最小となる位置まで当該変位対象のノードを変位させることが好適である。

また、前記経路算出手段は、変位対象のノードに接続された他の２つのノードに至る線分が前記移動可能空間内から外れない方向に当該変位対象のノードの位置を変位させることが好適である。

また、前記経路算出手段は、変位対象のノードに接続された他の２つのノードに至る線分のなす角度が減少する方向に当該変位対象のノードの位置を変位させることが好適である。

また、前記経路算出手段は、変位対象のノードの各々の変位量が閾値以下となるまで前記変位を繰り返すことにより前記移動経路を求めることが好適である。

また、前記移動可能空間は、複数の平面によって規定される空間であることが好適である。

本発明に係るプログラムは、三次元の移動可能空間内における開始位置から目標位置に至る移動経路を求めるためのプログラムであって、前記移動可能空間を表す立体の辺上の各点又は各頂点をノードとし、各ノードについて、当該ノードから前記移動可能空間内の線分にて接続可能な他のノードをエッジにて連結した可視グラフについての可視グラフ情報を記憶した記憶部を含むコンピュータを、前記可視グラフに前記開始位置および前記目標位置から前記移動可能空間内の線分にて接続可能な前記ノードへのエッジを加えたグラフにおいて、前記開始位置から前記目標位置に至る最短経路上の前記ノードを前記エッジで連結した経路グラフを求める経路グラフ算出手段、及び前記経路グラフを用いて前記移動経路を求める経路算出手段として機能させるためのプログラムである。

以上説明したように、本発明の経路探索装置及びプログラムによれば、前記可視グラフに前記開始位置および前記目標位置から前記移動可能空間内の線分にて接続可能な前記ノードへのエッジを加えたグラフにおいて、前記開始位置から前記目標位置に至る最短経路上の前記ノードを前記エッジで連結した経路グラフを求め、前記経路グラフを用いて前記移動経路を求めることにより、移動可能空間が広くなっても膨大な計算コストとなることなく移動経路を生成することができる、という効果が得られる。

本発明の実施の形態に係る移動制御システムの構成を示す概略図である。 本発明の実施の形態に係る自律飛行ロボットの構成を示す概略図である。 本発明の実施の形態に係る管理装置の構成を示す概略図である。 ノード位置情報の構成を示す図である。 可視グラフ情報の構成を示す図である。 経路テーブルの構成を示す図である。 （ａ）飛行空間を上方から見た場合の図、（ｂ）飛行可能空間の例を示す図、及び（ｃ）飛行可能空間を表す立体の各頂点に設定されたノードを示す図である。 （ａ）ノードＮ１の可視グラフを示す図、（ｂ）経路テーブルにおける、ノードＮ１からノードＮ１６に至る最短経路を説明するための図、及び（ｃ）経路テーブルにおける、ノードＮ４からノードＮ１３に至る最短経路を説明するための図である。 （ａ）開始ノードＳの可視グラフ及び目標ノードＧの可視グラフを示す図、及び（ｂ）開始ノードＳから目標ノードＧに至る経路グラフを説明するための図である。 （ａ）中間ノードの変位方向を決定する方法を説明するための図、（ｂ）中間ノードを変位させた結果を示す図、及び（ｃ）中間ノードを変位させた結果を示す図である。 本発明の実施の形態に係る管理装置による経路テーブル生成処理の動作を示すフローチャートである。 経路テーブルを算出する際に用いられるツリー構造の例を示す図である。 本発明の実施の形態に係る管理装置による経路探索処理の動作を示すフローチャートである。 本発明の実施の形態に係る自律飛行ロボットの他の例の構成を示す概略図である。

以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、複数のドローンの移動を制御する移動制御システムに本発明を適用した場合を例に説明する。

＜システム構成＞
以下、本発明を適用した移動制御システム１０００の概略構成を示した図１を参照し、本発明の実施の形態の構成を説明する。

（移動制御システム１０００）
移動制御システム１０００は、管理装置１００、及び複数台の自律飛行ロボット２００を有する。管理装置１００と自律飛行ロボット２００の間は所定の閉域網（無線ＬＡＮ等）を介して通信する。なお、管理装置１００が、経路探索装置の一例である。

（自律飛行ロボット２００）
自律飛行ロボット２００は、例えば、クアッドロータ型の小型無人ヘリコプタである。なお、クアッドロータ型の小型無人ヘリコプタに限定されるものではなく、シングルロータ型の小型無人ヘリコプタについても同様に適用することができる。

自律飛行ロボット２００は、管理装置１００から、移動経路の一例である飛行経路に係る情報（後述する経路情報）を受信し、当該飛行経路に沿って飛行するよう制御する。以下、自律飛行ロボット２００を構成する位置・姿勢センサ２１０、通信部２２０、記憶部２３０、制御部２４０、モータ２５０、及びロータ２６０の各部について、詳細に説明する（図２参照）。

（位置・姿勢センサ２１０）
位置・姿勢センサ２１０は、自律飛行ロボット２００の現在位置および姿勢を取得するためのセンサである。位置・姿勢センサ２１０は、例えば、ＧＮＳＳ（Global Navigation Satellite System）等の航法衛星（人工衛星）から送信される電波（航法信号）を受信する受信機、加速度を計測する加速度センサ、方位を計測する電子コンパス、および角速度を計測するジャイロセンサを備えている。具体的には、位置・姿勢センサ２１０において、受信機が複数の航法衛星から送信される航法信号を受信して制御部２４０に出力するとともに電子コンパスおよびジャイロセンサによる計測信号を制御部２４０に出力する。

現在位置を取得するために受信機に代えてレーザスキャナおよび気圧センサを用いるなど、他の既知のセンサを用いて既知の従来技術により現在位置および姿勢を得るための情報を取得するものとしてもよい。

（通信部２２０）
通信部２２０は、管理装置１００との間で通信するための通信モジュールである。

（記憶部２３０）
記憶部２３０は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）等の情報記憶装置である。

記憶部２３０は、各種プログラムや各種データを記憶し、制御部２４０との間でこれらの情報を入出力する。

各種データには、位置・姿勢情報２３２、経路情報２３４等の制御部２４０の各処理に用いられる情報が含まれる。

位置・姿勢情報２３２は、位置・姿勢センサ２１０によって取得された、自律飛行ロボット２００の現在位置および姿勢を予め定められた回数だけ循環記憶した位置と姿勢の履歴である。ここで、履歴内の最新の位置及び姿勢を特に現在位置及び現在姿勢と称し、それ以外の位置及び姿勢を過去位置及び過去姿勢と称する。

経路情報２３４は、自律飛行ロボット２００が移動する予定である飛行経路に関する情報である。具体的には、飛行経路上における座標列（ｘ,ｙ,ｚ）である。経路情報２３４は、管理装置１００から受信する。

制御部２４０は、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータであり、記憶部２３０からプログラムやデータを読み出し、プログラムに従って、位置・姿勢算出手段２４２および飛行制御手段２４４等として機能する。

位置・姿勢算出手段２４２は、位置・姿勢センサ２１０の出力から飛行空間における自律飛行ロボット２００の現在位置および姿勢を算出し、位置・姿勢情報２３２として記憶部２３０に記憶させる。

位置・姿勢算出手段２４２は、例えば、位置・姿勢センサ２１０によって出力された航法信号から緯度・経度・高度を求め、予め記憶した変換規則を用いて飛行空間の座標系における位置に変換する。

また、位置・姿勢算出手段２４２は、位置・姿勢センサ２１０によって出力された加速度センサおよびジャイロセンサの計測信号から、飛行空間の座標系における現在姿勢を求める。

なお、位置・姿勢センサ２１０によって出力された電子コンパスの計測信号から、飛行空間の座標系における方位を求め、他のセンサからの計測信号を更に用いて現在姿勢を算出してもよい。

以上のように、位置・姿勢センサ２１０と通信部２２０と位置・姿勢算出手段２４２（制御部２４０）とが共働して自律飛行ロボット２００の現在位置及び現在姿勢を検出する。

なお、位置・姿勢算出手段２４２は、現在位置を算出する都度、通信部２２０を介して当該現在位置を管理装置１００に送信する。

飛行制御手段２４４は、経路情報２３４、位置・姿勢情報２３２を参照し、自律飛行ロボット２００を経路情報２３４に記された飛行経路に追従して移動するようモータ２５０の回転速度を制御する。

具体的には、飛行制御手段２４４は、経路情報２３４に記された現在時刻の位置（座標）と、位置・姿勢情報２３２に記された現在位置との誤差が小さくなるようにモータ２５０の回転速度を制御する。

（モータ２５０、ロータ２６０）
４つのロータ２６０と、その回転軸がそれぞれ対応するロータ２６０に連結された４つのモータ２５０からなる４つのユニットが自律飛行ロボット２００に搭載されている。

各モータ２５０は、制御部２４０に接続されて制御部２４０から回転速度を指示される。４つのロータ２６０が独立して回転することにより自律飛行ロボット２００に任意方向の加速度を発生させる。

（管理装置１００）
管理装置１００は、飛行空間内の所定位置に設置され、自律飛行ロボット２００の飛行先である目標位置を決定し、自律飛行ロボット２００から受信した現在位置に基づいて、現在位置から目標位置に至る飛行経路を求め、自律飛行ロボット２００に送信する。

なお、管理装置１００は、遠隔の監視センタに設置してある図示しないセンタ装置と広域網（インターネット、携帯電話網等）を介して通信し、センタ装置からの指示を受け、当該指示に基づいて目標位置を設定したり、センタ装置に自律飛行ロボット２００に係る各種情報を送信するようにしてもよい。

以下、管理装置１００を構成する通信部１１０、記憶部１２０、及び制御部１３０の各部について、詳細に説明する（図３参照）。

（通信部１１０）
通信部１１０は、自律飛行ロボット２００との間で通信するための通信モジュールである。

（記憶部１２０）
記憶部１２０は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）等の情報記憶装置である。

記憶部１２０は、各種プログラムや各種データを記憶し、制御部１３０との間でこれらの情報を入出力する。

各種データには、飛行可能空間情報１２１、位置情報１２２、ノード位置情報１２３、可視グラフ情報１２４、経路テーブル１２５、経路グラフ情報１２６、経路情報１２８等の制御部１３０の各処理に用いられる情報が含まれる。

（飛行可能空間情報１２１）
飛行可能空間情報１２１は、自律飛行ロボット２００が飛行可能な三次元空間の空間範囲（立体）を表す座標データからなる情報である。飛行可能空間情報１２１は、自律飛行ロボット２００が飛行する空間内に存在する障害物の位置や形状に基づいて生成される。障害物以外にも、自律飛行ロボット２００の所有者の敷地や、行政機関によって規定される飛行可能高度帯などといった、自律飛行ロボット２００が飛行範囲を制限する情報に基づいて生成してもよい。

本実施形態では、飛行可能空間を表す立体（立体図形、立体形状）は、複数の平面の組み合わせによって規定される。したがって、曲面を構成する障害物は平面によって近似された障害物に変換され、当該平面からなる障害物以外の空間が飛行可能空間として設定される。

例えば、図７（ａ）に示すように、飛行可能空間を上から見た図において、敷地の範囲を示す境界線Ａと、障害物（建物）Ｂとから少なくとも所定距離だけ離れた水平範囲ａｂｃｄｅｆｇｈを規定する。図７（ａ）では、水平範囲ａｂｃｄｅｆｇｈを破線で示している。次に、図７（ｂ）に示すように、自律飛行ロボット２００の飛行が許可された高度を示す高度限界値と、水平範囲ａｂｃｄｅｆｇｈとを用いて、飛行可能空間Ｆを求める。

なお、飛行可能空間は、管理者等により予め設定されていてもよいし、図示しないレーザセンサや監視カメラからの出力に基づいて車両などの移動障害物を検知したとき、センサ出力やカメラ画像から当該移動障害物の位置及び形状を認識し、認識結果に応じて、移動障害物の存在領域を含む所定範囲を飛行不可能とするよう飛行可能空間を更新してもよい。また、飛行可能空間情報１２１は、飛行可能空間を所定サイズのボクセルにて分割したデータ構造であってもよい。また、飛行可能空間は、移動可能空間の一例である。

（位置情報１２２）
位置情報１２２は、自律飛行ロボット２００の現在位置及び目標位置を示した情報である。

自律飛行ロボット２００から現在位置を受信すると、当該自律飛行ロボット２００の機体ＩＤと対応付けて位置情報１２２に記憶する。

また、目標位置設定手段１３２によって、ある自律飛行ロボット２００の目標位置が設定されると、当該自律飛行ロボット２００の機体ＩＤに対応付けて目標位置が位置情報１２２に記憶される。

（ノード位置情報１２３）
ノード位置情報１２３は、飛行可能空間情報１２１に基づいて可視グラフ算出手段１３４によって生成される情報であり、飛行可能空間情報１２１によって規定される立体の各頂点の位置がノード位置として求められる。ノード位置情報１２３は、ノードの識別子とノード位置とを対応付けたテーブルとして記憶される。ノード位置情報１２３は、例えば、図４に示すようなテーブル構造であり、ノードの識別子「Ｎ１」とノード位置（ｘ１、ｙ１、ｚ１）とが対応付けられ、ノードの識別子「Ｎ２」とノード位置（ｘ２、ｙ２、ｚ２）とが対応付けられ、ノードの識別子「Ｎ３」とノード位置（ｘ３、ｙ３、ｚ３）とが対応付けられている。

（可視グラフ情報１２４）
可視グラフ情報１２４は、ノード位置情報１２３と飛行可能空間情報１２１とに基づいて可視グラフ算出手段１３４にて生成される情報であり、ノード位置情報１２３にて規定された各ノードから、可視となる他のノードとの間をエッジで接続したグラフである。ここで「可視」とは、ノード間を結ぶ直線（線分）が飛行可能空間外に干渉しない状態のことをいう。

可視グラフ情報１２４は、例えば、図５に示すようなテーブル構造であり、対象ノードＩＤ「Ｎ１」と、可視ノードＩＤ「Ｎ２、Ｎ３、Ｎ４、Ｎ５、Ｎ６、Ｎ７、Ｎ８、Ｎ９、Ｎ１０、Ｎ１３、Ｎ１４」とが対応付けられ、対象ノードＩＤ「Ｎ２」と、可視ノードＩＤ「Ｎ１、Ｎ３、Ｎ４、Ｎ５、Ｎ６、Ｎ７、Ｎ８」とが対応付けられている。これらの対応付けの情報がノード同士の接続関係の情報、つまりエッジの情報である。

（経路テーブル１２５）
経路テーブル１２５は、ノード位置情報１２３と可視グラフ情報１２４とに基づいて経路グラフ算出手段１３８にて算出される情報であり、各ノードから他のノードまでに最短で到達するノードの組み合わせと、当該組み合わせで移動した場合の総距離とを対応付けた情報である。総距離は、ノード位置情報１２３を用いて幾何学的に算出される。

経路テーブル１２５は、例えば、図６に示すようなテーブル構造であり、始点ノードＩＤ「Ｎ１」と、終点ノードＩＤ「Ｎ２」と、最短経路「Ｎ１、Ｎ２」と、距離「１０」とが対応付けられ、始点ノードＩＤ「Ｎ１」と、終点ノードＩＤ「Ｎ１６」と、最短経路「Ｎ１、Ｎ１０、Ｎ１６」と、距離「２５」とが対応付けられている。

（経路グラフ情報１２６）
経路グラフ情報１２６は、可視グラフ情報１２４と経路テーブル１２５とに基づいて経路グラフ算出手段１３８にて算出される情報であり、現在位置（開始ノードＳ）から目標位置（目標ノードＧ）に至る距離が最小となる経路上のノードの組み合わせを示した情報である。後述する図９（ｂ）の例では、経路グラフ情報１２６は(Ｓ、Ｎ１０、Ｎ６、Ｇ)となる。

なお、経路グラフ情報１２６において開始ノードＳと目標ノードＧとを除いたノードを中間ノードという。

（経路情報１２８）
経路情報１２８は、経路グラフ情報１２６、ノード位置情報１２３、及び飛行可能空間情報１２１に基づいて経路算出手段１４０にて算出される情報であり、現在位置から始まり、各中間ノードの位置を経由し、目標位置に至る経路上の各位置の三次元座標のリストである。

（制御部１３０）
制御部１３０は、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータであり、記憶部１２０からプログラムやデータを読み出し、プログラムに従って、目標位置設定手段１３２、可視グラフ算出手段１３４、経路テーブル算出手段１３６、経路グラフ算出手段１３８、及び経路算出手段１４０等として機能する。

（目標位置設定手段１３２）
目標位置設定手段１３２は、各自律飛行ロボット２００の目標位置を設定し、機体ＩＤに対応付けて位置情報１２２に記憶する。

目標位置は、例えば、巡回警備をタスクとする自律飛行ロボット２００であれば、予め定めた時刻になったときに所定の巡回位置が目標位置となるよう設定する。また、侵入者への対応をタスクとする自律飛行ロボット２００であれば、図示しないセンサからの侵入検知出力に基づいて、当該センサの検知位置の近傍を目標位置に設定してもよい。

他にも、図示しないセンタ装置からの指示に基づいて、目標位置を設定してもよい。

（可視グラフ算出手段１３４）
可視グラフ算出手段１３４は、記憶部１２０から飛行可能空間情報１２１を読み出し、飛行可能空間情報１２１にて規定される空間を表す立体の角（頂点）にノードを設定し、各ノードの識別子と座標とを対応付けてノード位置情報１２３に記憶する。

例えば、図７（ｃ）に示すように、飛行可能空間Ｆを表す立体の全ての頂点にノードＮ１～Ｎ１６を設定し、各ノードの識別子と座標とを対応付けてノード位置情報１２３に記憶する。

また、各ノードから可視となる他のノードとの間をエッジで連結したグラフを求め、可視グラフ情報１２４に記憶する。

例えば、図８（ａ）に示すように、ノードＮ１の可視グラフを求める場合には、ノードＮ１は、ノードＮ２、Ｎ３、Ｎ４、Ｎ５、Ｎ６、Ｎ７、Ｎ８、Ｎ９、Ｎ１０、Ｎ１３、Ｎ１４が可視である（飛行可能空間内で線分にて接続できる）ため、ノードＮ１との間をエッジで接続したグラフを設定し、可視グラフ情報１２４に記憶させる。このとき、ノードＮ１に対して、ノードＮ１１、Ｎ１２、Ｎ１５、Ｎ１６は非可視である（飛行可能空間内で線分にて接続できない）ためノードＮ１とＮ１１、Ｎ１２、Ｎ１５、Ｎ１６とのエッジは可視グラフには含まれない。

また、可視グラフ算出手段１３４は、経路探索処理において、位置情報と飛行可能空間情報とを読み出し、現在位置に相当するノード（開始ノードＳ）から可視となる他のノードを求めて、求めたノードと開始ノードＳとのエッジを可視グラフ情報１２４に追記する。同様に、目標位置に相当するノード（目標ノードＧ）から可視となる他のノードを求めて、求めたノードと目標ノードＧとのエッジを可視グラフ情報１２４に追記する。

（経路テーブル算出手段１３６）
経路テーブル算出手段１３６は、ノード位置情報１２３と可視グラフ情報１２４とに基づいて、経路テーブル１２５を算出する。

例えば、図８（ｂ）に示すように、ノードＮ１からノードＮ１６に至る最短経路上のノードとして、ノードＮ１、Ｎ１０、Ｎ１６を算出し、経路テーブル１２５に記憶させる。また、図８（ｃ）に示すように、ノードＮ４からノードＮ１３に至る最短経路上のノードとして、ノードＮ４、Ｎ６、Ｎ１３を算出し、経路テーブル１２５に記憶させる。経路テーブル１２５の具体的な算出方法については後述する。

（経路グラフ算出手段１３８）
経路グラフ算出手段１３８は、位置情報１２２とノード位置情報１２３と可視グラフ情報１２４と経路テーブル１２５とに基づいて、開始ノードＳから目標ノードＧまでに最短で到達する経路上のノードの組み合わせを計算し、経路グラフ情報１２６として記憶する。このとき、経路テーブル１２５を用いることで、開始ノードＳから目標ノードＧに至る最短距離となる経路上のノードの組み合わせを高速に求めることができる。

具体的には、図９（ａ）の例に示す（Ｓ、Ｎ１０、Ｎ１、Ｇ）、（Ｓ、Ｎ１０、Ｎ２、Ｇ）、・・・、（Ｓ、Ｎ１６、Ｎ８、Ｇ）のように開始ノードＳから目標ノードＧに至る全通りの組み合わせについて総距離を計算する。すなわち、開始ノードＳから可視となるノードの数（Ｎ９～Ｎ１６の８通り）×目標ノードＧから可視となるノードの数（Ｎ１～Ｎ８の８通り）＝６４通りについて開始ノードＳから目標ノードＧに至る総距離を算出する。なお、開始ノードＳ及び目標ノードＧから可視となるノードまでの距離は、ノード位置情報１２３を用いて幾何学的に算出し、途中のパスの距離については経路テーブル１２５に記された距離値を用いる。

そして、開始ノードＳから目標ノードＧに至る全通りの組み合わせの中から総距離が最小となるノードの組み合わせを計算することによって、経路グラフ（Ｓ、Ｎ１０、Ｎ６、Ｇ）を求めることができる。求めた経路グラフを経路グラフ情報１２６として記憶する。

（経路算出手段１４０）
経路算出手段１４０は、経路グラフ情報１２６、ノード位置情報１２３、及び飛行可能空間情報１２１に基づいて現在位置から目標位置に至る最短経路を算出する。

具体的には、経路グラフ情報１２６に示される経路グラフの中間ノードの位置を、飛行可能空間を表す立体の辺に沿って変位させることによって経路を最短化させ、最短経路を求める。

ここで、最短経路を算出する処理は、中間ノードの変位方向を決定するステップと、変位させる位置を決定するステップとからなる。

まず、中間ノードの変位方向を決定するステップでは、変位対象の中間ノード（以下、「対象中間ノード」という）としてある中間ノードに着目したとき、以下の条件１を満たす方向を対象中間ノードの変位方向として決定する。

条件１：対象中間ノードの位置から飛行可能空間によって示される立体の辺（面と面が交わった端部）に沿った方向であって、移動先において当該対象中間ノードと接続された他の２つのノードの線分が飛行可能空間外とらない方向であること。

上記条件１では、複数方向が存在する場合があるため、計算量を減らすために、より好適には、以下の条件２を満たす方向を対象中間ノードの変位方向として決定する。

条件２：対象中間ノードと接続された他の２つのノードの線分のなす角の内積が増加（ノード間の角度が減少）する方向であること。

上図１０（ａ）の例が示すように、ノードＮ１０を対象中間ノードとした場合、「飛行可能空間によって示される立体の辺に沿った方向」は以下のａ）～ｃ）の３通り考えられる。

ａ）「ノードＮ１０の位置からノードＮ１２に向かう方向」
ｂ）「ノードＮ１０の位置からノードＮ９に向かう方向」
ｃ）「ノードＮ１０の位置からノードＮ６に向かう方向」

しかし、ａ）とｃ）の方向に移動した場合、「対象中間ノードと接続された他の２つのノードの線分が飛行可能空間外」となるため、ｂ）の方向が変位方向として決定される。

なお、上記の条件１を満たす方向が複数ある場合は、条件２を適用して変位方向を決定するのが好適である。しかし、複数方向に対して変位させたうえで、最短経路となるパスから飛行経路を決定してもよい。

また、上記のように対象中間ノードの変位方向が決定されると、対象中間ノードを変位させる位置を決定するステップが行われる。

このとき、図１０（ａ）に示すように、対象中間ノードをＮ１０とすると、直前のステップにより、ノードＮ１０の変位方向はＮ１０→Ｎ９の方向であることが判っている。本ステップでは、変位方向のどの位置に変位させるかを決定する。

変位させる位置を決定するステップでは、以下のように、対象中間ノードと接続された２つのノードに対して当該対象中間ノードを変位させる位置を決定する。

まず、対象中間ノード（Ｎ１０）と変位方向にあるノード（Ｎ９）及び対象中間ノードの直前ノード（Ｓ）の３つのノードの位置を結ぶ三角形平面の拡大平面Ｓを求める。

そして、経路グラフにおける対象中間ノードの次のノード（Ｎ６）の位置を、対象中間ノード（Ｎ１０）と変位方向にあるノード（Ｎ９）とを結ぶ線分を軸として回転させた際に出来る交点（Ｎ６’）を求める。

そして、当該交点Ｎ６’と直前ノード（Ｓ）とを線分で結んだ際に、当該線分と対象中間ノード（Ｎ１０）から変位方向にあるノード（Ｎ９）を結ぶ線分との交点を、変位先の位置（Ｎ１０’）として求める。

なお、上記の方法とは別の方法として、解析的に変位する位置を求めることができる。すなわち、対象中間ノードを変位方向に変位させた場合における、対象中間ノードに接続された２つのエッジの距離の和を表す式を幾何学的に求め、その式を微分演算して求めた極小値に相当する位置を、変位先の位置として求めてもよい。

次に、変位する位置（Ｎ１０’）を決定した後、ノードＮ１０をＮ１０’の位置に移動させる（ノード位置情報１２３を更新する）。この際のノードの移動量を対象中間ノードの識別子に対応付けて一時的に記憶する。

また、対象中間ノードをＮ６とした場合も、同様に、中間ノードの変位方向を決定するステップと、変位先の位置を決定するステップとを行い、対象中間ノードＮ６と接続された２つのノードＧ、Ｎ１０’に対して当該対象中間ノードＮ６を変位先の位置（Ｎ６’）を求める。

＜移動制御システムの動作＞
以下、図１１、図１３に示したフローチャートを参照しつつ、本発明を適用した移動制御システム１０００による経路テーブル生成処理、経路探索処理の動作を説明する。

管理装置１００の記憶部１２０に、飛行可能空間情報１２１が新規登録されたとき、または飛行可能空間情報１２１が更新されたときに、図１１に示す経路テーブル生成処理が実行される。

図１１に示す管理装置１００の経路テーブル生成処理では、最初に、可視グラフ算出手段１３４が、記憶部１２０から飛行可能空間情報１２１を読み出し、飛行可能空間情報１２１によって規定される立体の各頂点にノードを設定し、ノード位置情報１２３を更新する（ステップＳ１００）。

そして、可視グラフ算出手段１３４が、飛行可能空間情報１２１とノード位置情報１２３とに基づいて、各ノードについての可視グラフを生成し、可視グラフ情報１２４を更新する（ステップＳ１０２）。

次に、経路テーブル算出手段１３６が、ノード位置情報１２３と可視グラフ情報１２４とに基づいて、各ノードから他のノードに至る最短経路をなすノードの組み合わせを求め、記憶部１２０の経路テーブル１２５を更新し（ステップＳ１０４）、経路テーブル生成処理を終了する。

ここで、経路テーブル１２５の具体的な算出方法について説明する。経路テーブル１２５を算出する処理では、以下の処理１～処理３が行われる。なお、上記図８の例における始点ノードをＮ１２、終点ノードをＮ１とした場合の経路テーブル１２５の算出を例に説明する。

まず、処理１では、図１２に示すツリー構造Ｔにおいて、始点ノードＮ１２をＬ１とし、Ｌ１から可視となる（飛行可能空間外に干渉せずに直線にて到達可能である）ノードのリストをＬ２とし、Ｌ１の各ノードからＬ２の各ノードまでの距離をそれぞれ求める。ここで、当該距離は、ノード位置情報１２３を用いて幾何学的に算出される。

もし、Ｌ２の中に終点ノードＮ１が含まれていれば、Ｎ１までの最短距離のパス（経由するノード）を最短経路とし、当該最短経路とその距離（総距離）を経路テーブル１２５に記憶し、処理を終了する。

処理２では、Ｌ２の中に終点ノードＮ１が含まれていない場合には、Ｌ２の各ノードから可視であり、かつ、Ｌ２に含まれていないノードを求め、当該ノードのリストをＬ３とする。Ｌ２の各ノードからＬ３の各ノードまでの距離をそれぞれ計算する（処理２）。もし、Ｌ３の中に終点ノードＮ１が含まれていれば、Ｎ１までの最短距離のパス（経由するノード）を最短経路とし、当該最短経路とその距離（総距離）を経路テーブル１２５に記憶し、処理を終了する。Ｌ３の中に重複したノードがある場合、重複したノードのうち総距離（Ｌ１からＬ３までの距離の和）が最小なもの以外のノードについては、ツリー構造Ｔから除外する。

処理３では、リストＬｎに終点ノードが含まれるまで上記処理２を繰り返す。

また、複数の自律飛行ロボット２００の中で、少なくとも１台の自律飛行ロボット２００の目標位置が変更されたときに、目標位置が変更された１又は複数の自律飛行ロボット２００の各々を処理対象として選択し、図１３に示す経路探索処理が実行される。以下、選択された経路探索対象を「対象ロボット」と称する。

図１３に示す管理装置１００の経路探索処理では、最初に、可視グラフ算出手段１３４が、記憶部１２０から、対象ロボットの位置情報１２２の現在位置及び目標位置を読み出し、現在位置に対応する開始ノードＳと、目標位置に対応する目標ノードＧとをノード位置情報１２３に登録する（ステップＳ１２０）。

そして、可視グラフ算出手段１３４が、ノード位置情報１２３の開始ノードＳ及び目標ノードＧの位置と、飛行可能空間情報１２１を用いて、開始ノードＳ及び目標ノードＧの可視グラフを求め、可視グラフ情報１２４に登録（更新）する（ステップＳ１２２）。

そして、経路グラフ算出手段１３８が、開始ノードＳと、目標ノードＧと、開始ノードＳ及び目標ノードＧの間を接続する線分（エッジ）とからなるグラフが可視グラフとして形成されているか否かを判定する（ステップＳ１２４）。具体的には、開始ノードＳと目標ノードＧとが飛行可能空間内で線分にて接続されていない場合は可視グラフとして形成されていないとの否定判定をして、ステップＳ１２６へ移行するが、そうでない場合には、経路算出手段１４０が、開始ノードＳと、目標ノードＧと、開始ノードＳ及び目標ノードＧの間を接続するエッジとから、開始ノードＳから目標ノードＧまでの最短経路を求め、対象ロボットの飛行経路とし（ステップＳ１３６）、経路探索処理ルーチンを終了する。

そして、ステップＳ１２６では、経路グラフ算出手段１３８が、ノード位置情報１２３と可視グラフ情報１２４と経路テーブル１２５とに基づいて、開始ノードＳから目標ノードＧまでに最短で到達するノードの組み合わせを計算し、経路グラフ情報１２６として記憶する。

そして、ループ１のステップＳ１２８～Ｓ１３０の処理が、上記ステップＳ１２６で得られた経路グラフの各中間ノードに対して実行される。なお、ループ１において処理対象となっている中間ノードは、前述の「対象中間ノード」に相当する。

経路算出手段１４０が、経路グラフ情報１２６、ノード位置情報１２３、及び飛行可能空間情報１２１に基づいて、対象中間ノードの変位方向を決定する（ステップＳ１２８）。そして、経路算出手段１４０が、経路グラフ情報１２６、ノード位置情報１２３、飛行可能空間情報１２１、及び決定された変位方向に基づいて、対象中間ノードと接続された２つのノードに対する、当該対象中間ノードの変位先の位置及び変位量を算出する（ステップＳ１３０）。

そして、経路算出手段１４０が、全ての中間ノードの変位量が閾値Ｔｈ１以下であったか否かを判定する（ステップＳ１３２）。全ての中間ノードの変位量が閾値Ｔｈ１以下であった場合には、最短経路をなすパスが求まったとみなし、経路グラフと修正された中間ノードの位置から、開始ノードＳから目標ノードＧまでの最短経路を求め、対象ロボットの飛行経路とし（ステップＳ１３４）、経路探索処理ルーチンを終了する。

一方、少なくとも１つの中間ノードの変位量が閾値Ｔｈ１より大きい場合には、上記ループ１を再度繰り返す。

以上説明してきたように、本発明の実施の形態に係る移動制御システム１０００では、可視グラフ情報を用いて、開始位置から目標位置に至る最短距離の経路上の各ノードをエッジにて連結した経路グラフを求め、経路グラフを用いて飛行経路を求めることにより、飛行可能空間が広くなっても膨大な計算コストとなることなく飛行経路を生成することができる。

また、飛行可能空間の頂点をノードとした可視グラフを形成し、各ノード間の距離が最短距離となる経路を記した経路テーブルを予め記憶しておき、開始位置から飛行可能空間に干渉せずに直線で到達可能な各ノードまでの距離と、目標位置から前記飛行可能空間に干渉せずに直線で到達可能な各ノードまでの距離とを求め、求めた距離と、経路テーブルとを用いて、開始位置から目標位置に至る距離が最小となる経路グラフを求め、経路グラフの中間ノードの位置を修正して、三次元の飛行空間における開始位置から目標位置に至る飛行経路を求める。このように、二次元空間で利用されている可視グラフを用いた経路探索手法を三次元空間に拡張した手法を用いることにより、空間が広くなっても膨大な計算コストとなることなく飛行経路を生成することができる。また、飛行可能空間の全ての頂点に対応するノードの組み合わせについて最短距離をなすノードの組み合わせを算出し、経路テーブルとして予め記憶しておくことにより、空間の大きさによらずに少ない計算コストで飛行経路を生成できる。

＜変形例＞
以上、本発明の好適な実施形態について説明してきたが、本発明はこれらの実施形態に限定されるものではない。例えば、管理装置が備える全ての機能を、特定の自律飛行ロボットが備えるように構成してもよい。この場合、図１４に示すように、自律飛行ロボット３００は、位置・姿勢センサ２１０、通信部２２０、記憶部３３０、制御部３４０、モータ２５０、及びロータ２６０を備え、記憶部３３０に記憶される各種データには、位置・姿勢情報２３２、経路情報２３４だけでなく、飛行可能空間情報１２１、ノード位置情報１２３、可視グラフ情報１２４、経路テーブル１２５、経路グラフ情報１２６等が含まれる。

また、制御部３４０は、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータであり、記憶部３３０からプログラムやデータを読み出し、プログラムに従って、位置・姿勢算出手段２４２および飛行制御手段２４４等として機能すると共に、目標位置設定手段１３２、可視グラフ算出手段１３４、経路テーブル算出手段１３６、経路グラフ算出手段１３８、及び経路算出手段１４０等として機能する。

また、経路グラフ算出手段１３８は、位置・姿勢情報２３２とノード位置情報１２３と可視グラフ情報１２４と経路テーブル１２５とに基づいて、現在位置である開始ノードＳから目標ノードＧまでに最短で到達する経路上のノードの組み合わせを計算し、経路グラフ情報１２６として記憶する。経路算出手段１４０は、経路グラフ情報１２６、ノード位置情報１２３、及び飛行可能空間情報１２１に基づいて現在位置から目標位置に至る最短経路を算出して、飛行経路として経路情報２３４に記憶する。

また、特定の自律飛行ロボット３００が他の自律飛行ロボットと通信し、全ての自律飛行ロボットの飛行経路を生成してもよい。

また、個々の自律飛行ロボットを、上記自律飛行ロボット３００のように構成し、自らの飛行経路（経路情報）を生成するようしてもよい。

また、目標位置は、外部装置（例えば、管理装置１００）から受信してもよいし、自らが目標位置を設定してもよい。例えば、自律飛行ロボット３００が備える図示しないカメラからの撮影画像に基づいて、侵入者や不審車両等の追跡対象を検出し、当該追跡対象の付近に目標位置を設定してもよい。

また、上記実施形態では、移動障害物が検出された際などの飛行可能空間情報が更新された場合、経路テーブル生成処理を開始して、ノードを再設定する処理から開始する。しかし、これに限らず、飛行可能空間情報が更新された場合であっても、当該更新された部分（箇所）が、記憶部１２０に記憶している経路テーブル１２５によって求まるグラフのエッジに相当する線分に接触しない場合については、経路テーブル生成処理を実行しなくてもよい。あるいは、更新された部分が可視グラフのエッジに相当する線分に接触する場合であっても、経路探索処理において経路グラフを求める際、当該エッジを有するグラフを経路グラフとして採用しないようにし、当該更新された部分を飛行しないような経路を採用するようにしてもよい。

また、経路算出手段１４０が、経路グラフの中間ノードを変位させて最短経路を求める場合を例に説明したが、これに限定されるものではなく、中間ノードを変位させずに、経路グラフが表す経路を、飛行経路として採用してもよい。

また、飛行可能空間を表す立体の各頂点にノードを設定する場合を例に説明したが、これに限定されるものではなく、飛行可能空間を表す立体の辺上の、頂点ではない位置にノードを設定してもよい。

また、移動体の一例として自律飛行ロボットを用いる場合を例に説明したが、これに限定されるものではなく、自律飛行ロボット以外の移動体を対象としてもよい。例えば、水中ロボットの移動経路を算出する場合に本発明を適用してもよい。

以上のように、当業者は本発明の範囲内で、実施される形態に合わせて様々な変更を行うことができる。

１００ 管理装置
１１０ 通信部
１２０ 記憶部
１２１ 飛行可能空間情報
１２２ 位置情報
１２３ ノード位置情報
１２４ 可視グラフ情報
１２５ 経路テーブル
１２６ 経路グラフ情報
１２８ 経路情報
１３０ 制御部
１３２ 目標位置設定手段
１３４ 可視グラフ算出手段
１３６ 経路テーブル算出手段
１３８ 経路グラフ算出手段
１４０ 経路算出手段
２００、３００ 自律飛行ロボット
２１０ 位置・姿勢センサ
２２０ 通信部
２３０、３３０ 記憶部
２３２ 位置・姿勢情報
２３４ 経路情報
２４０、３４０ 制御部
２４２ 位置・姿勢算出手段
２４４ 飛行制御手段
２５０ モータ
２６０ ロータ
１０００ 移動制御システム

Claims (6)

1. 三次元の空間内における、開始位置から目標位置に至る飛行ロボットの移動経路を求める経路探索装置であって、
   前記空間内の障害物の位置に基づいて生成された移動可能空間を記憶し、かつ、前記移動可能空間を表す立体の各頂点をノードとし、各ノードについて、当該ノードから前記移動可能空間内の線分にて接続可能な他のノードをエッジにて連結した可視グラフを記憶した記憶部と、
   前記可視グラフに前記開始位置および前記目標位置から前記移動可能空間内の線分にて接続可能な前記ノードへのエッジを加えたグラフにおいて、前記開始位置から前記目標位置に至る最短経路上の前記ノードを前記エッジで連結した経路グラフを求める経路グラフ算出手段と、
   前記経路グラフを用いて前記移動経路を求める経路算出手段と、
   を備え、
   前記移動可能空間は、複数の直線で囲まれ規定された前記障害物の位置を除く敷地面内の水平範囲を第１面とし、前記第１面上の空間における前記飛行ロボットの飛行が許可された高度限界値の水平範囲を第２面とし、前記第１面及び前記第２面と複数の平面からなる側面とによって囲まれた多面体の空間であることを特徴とする経路探索装置。
2. 前記経路算出手段は、前記経路グラフを構成するノードの位置を、前記移動可能空間を表す立体の辺上に沿って変位させることによって前記エッジで表される経路を最短化させ前記移動経路を求め、
   変位対象のノードの位置を変位させる際に、当該変位対象のノードに接続された他の２つのノードに至る線分が前記移動可能空間内から外れない方向に当該変位対象のノードの位置を変位させる請求項１に記載の経路探索装置。
3. 前記経路算出手段は、変位対象のノードに接続された他の２つのノードまでの距離の和が最小となる位置まで当該変位対象のノードを変位させる請求項２に記載の経路探索装置。
4. 前記経路算出手段は、変位対象のノードに接続された他の２つのノードに至る線分のなす角度が減少する方向に当該変位対象のノードの位置を変位させる請求項２又は３に記載の経路探索装置。
5. 前記経路算出手段は、変位対象のノードの各々の変位量が閾値以下となるまで前記変位を繰り返すことにより前記移動経路を求める請求項２から請求項４の何れか一項に記載の経路探索装置。
6. 三次元の空間内における開始位置から目標位置に至る飛行ロボットの移動経路を求めるためのプログラムであって、
   前記空間内の障害物の位置に基づいて生成された移動可能空間を記憶し、かつ、前記移動可能空間を表す立体の各頂点をノードとし、各ノードについて、当該ノードから前記移動可能空間内の線分にて接続可能な他のノードをエッジにて連結した可視グラフについての可視グラフ情報を記憶した記憶部を含むコンピュータを、
   前記可視グラフに前記開始位置および前記目標位置から前記移動可能空間内の線分にて接続可能な前記ノードへのエッジを加えたグラフにおいて、前記開始位置から前記目標位置に至る最短経路上の前記ノードを前記エッジで連結した経路グラフを求める経路グラフ算出手段、及び
   前記経路グラフを用いて前記移動経路を求める経路算出手段
   として機能させるためのプログラムであって、
   前記移動可能空間は、複数の直線で囲まれ規定された前記障害物の位置を除く敷地面内の水平範囲を第１面とし、前記第１面上の空間における前記飛行ロボットの飛行が許可された高度限界値の水平範囲を第２面とし、前記第１面及び前記第２面と複数の平面からなる側面とによって囲まれた多面体の空間であるプログラム。