JP7126366B2

JP7126366B2 - 移動制御システム

Info

Publication number: JP7126366B2
Application number: JP2018062574A
Authority: JP
Inventors: 晴之天本
Original assignee: Secom Co Ltd
Current assignee: Secom Co Ltd
Priority date: 2018-03-28
Filing date: 2018-03-28
Publication date: 2022-08-26
Anticipated expiration: 2038-03-28
Also published as: JP2019175128A

Description

本発明は、自律移動ロボット等の制御体を目標位置まで移動させる移動制御システムに関する。

防犯、防災、運輸、案内サービスなどの様々な目的で、ロボットを自律移動させるシステムが研究・開発されている。これらのシステムにおいては、ロボットが、センサによって検出した障害物を回避しながら、ロボットの現在位置から目標位置に至る経路を追従移動する。

例えば、自律移動ロボットにおいて、自律移動ロボットの経路探索手段が、位置推定手段で推定された自己位置、管理装置から受信した目標対象物位置、飛行空間の障害物の構造等を表した空間情報を参照して、既知の障害物を回避した移動経路を設定して飛行する技術が開示されている（特許文献１）。

また、例えば、飛行ロボット制御システムにおいて、飛行ロボットが、測距センサにて検知した周囲情報に基づいて周辺における障害物の有無を判定する障害物検知手段と、障害物を回避しながら移動するようにロータ駆動部を制御して飛行ロボットの高度や速度を制御するロータ制御手段を備え、周囲に現れた障害物を回避して経路を追従飛行する技術が開示されている（特許文献２）。

特開２０１７－０１６３５９号公報特開２０１６－１７３７３９号公報

しかしながら、機体における障害物センサの設置スペース、障害物センサの特性、障害物センサの設置コスト等の制約により、自律移動ロボットの周囲の一部には障害物センサの死角が生じ得る。そのため、空間情報の作成後に設置されたポール、空間情報の記憶対象外である鳥など、未知の障害物が死角に現れると回避が間に合わずに衝突するおそれがあった。このような問題は、空中を移動するロボットのみならず、地上、水上、水中を移動するロボットにも生じ得る。

本発明は上記問題を鑑みてなされたものであり、障害物センサの死角に現れ得る障害物に衝突する危険を低減して、制御体を目標位置まで安全に移動させることができる移動制御システムを提供することを目的とする。

本発明の１つの態様は、障害物が存在する空間において制御体を当該制御体の現在位置から目標位置まで移動させる移動制御システムであって、前記制御体に設置されて前記制御体の周囲に現れた障害物を検出する障害物検出手段の死角領域と、前記制御体と、の位置関係を記憶している死角情報記憶手段と、前記空間において既知の障害物が存在しない移動可能領域を記憶している空間情報記憶手段と、前記現在位置から前記目標位置に至る前記移動可能領域内を通る最短経路を算出し、当該最短経路に基づいて主経路を算出する移動計画算出手段と、を備え、前記移動計画算出手段は、前記最短経路上の点であって当該点に移動した前記制御体の前記死角領域に前記最短経路上の他点を含む不良点を検出して、前記最短経路における前記不良点から前記目標位置に至る区間の一部又は全部を修正し、前記最短経路上の任意点に移動した前記制御体の前記死角領域に経路上の他点を含まないように前記最短経路を修正することで主経路を算出することを特徴とする移動制御システムである。

ここで、前記移動計画算出手段は、前記主経路上の点の候補である候補点と当該候補点における前記制御体の姿勢の候補である候補姿勢を組み合わせた仮説を複数設定し、前記仮説ごとに前記候補点における前記候補姿勢となっている前記制御体の前記死角領域を通過せずに移動可能な他の前記仮説の前記候補点との間の局所経路を検出し、前記局所経路を連結して前記主経路を算出することが好適である。

また、前記移動計画算出手段は、前記最短経路から所定距離内の前記候補点を有する前記仮説を設定することが好適である。

また、前記障害物検出手段が前記主経路近傍の障害物を検出した場合に、前記空間内の当該障害物から所定距離以上離れた位置を終端とし、前記現在位置から前記終端に至る前記移動可能領域内及び前記障害物検出手段によって検出された障害物が存在しない領域を通る経路であって、当該経路上の任意点に移動した前記制御体の前記死角領域に当該経路上の他点を含まない回避経路を算出する回避計画算出手段をさらに備え、
前記障害物検出手段が前記主経路近傍の障害物を検出した場合に、前記主経路の追従移動を停止させて前記回避経路に沿って前記制御体を追従移動させることが好適である。

本発明の別の態様は、障害物が存在する空間において制御体を移動させる移動制御システムであって、前記制御体に設置されて前記制御体の周囲に現れた障害物を検出する障害物検出手段の死角領域と、前記制御体と、の位置関係を記憶している死角情報記憶手段と、前記空間において既知の障害物が存在しない移動可能領域を記憶している空間情報記憶手段と、前記障害物検出手段が障害物を検出した場合に、前記空間内の当該障害物から所定距離以上離れた位置を終端とし、前記制御体の現在位置から前記終端に至る前記移動可能領域内及び前記障害物検出手段によって検出された障害物が存在しない領域を通る最短経路を算出し、当該最短経路に基づいて回避経路を算出する回避計画算出手段と、を備え、前記回避計画算出手段は、前記最短経路上の点であって当該点に移動した前記制御体の前記死角領域に前記最短経路上の他点を含む不良点を検出して、前記最短経路における前記不良点から前記目標位置に至る区間の一部又は全部を修正し、前記最短経路上の任意点に移動した前記制御体の前記死角領域に経路上の他点を含まないように前記最短経路を修正することで前記回避経路を算出し、前記障害物検出手段が障害物を検出した場合に前記回避経路に沿って前記制御体を追従移動させることを特徴とする移動制御システムである。

本発明によれば、障害物センサの死角に現れ得る障害物に衝突する危険を低減して、制御体を目標位置まで安全に移動させることができる。

本発明の実施の形態における移動制御システムの構成を示す図である。本発明の実施の形態における自律移動ロボットの構成を示す機能ブロック図である。本発明の実施の形態における障害物センサの死角領域の例を示す図である。本発明の実施の形態における障害物センサの死角領域の別例を示す図である。本発明の実施の形態における主経路の設定方法を示すフローチャートである。本発明の実施の形態における主経路の設定方法を示すフローチャートである。本発明の実施の形態における主経路の設定方法を説明するための図である。本発明の実施の形態における移動制御方法を示すフローチャートである。

以下、図面に基づいて、本発明の好適な実施の形態について説明する。

図１は、本発明に係る移動制御システムの一実施の形態を示す概略構成図である。本実施の形態における移動制御システム１は、自律移動ロボット６の移動範囲となる空間において自律移動ロボット６を自律移動ロボット６の現在位置から目標位置まで移動させる制御を行うシステムである。

本実施の形態においては、監視空間に侵入物（人間や車両等）が侵入したことを検出した場合、カメラを搭載した自律移動ロボット６を侵入物の近傍（目標位置）まで移動させて侵入物を撮影させる監視システムに移動制御システム１を適用する場合を想定して説明する。

移動制御システム１は、侵入物センサ２、監視センタ装置４、管理装置５及び自律移動ロボット６を有している。本実施の形態の説明に用いない構成については省略する。侵入物センサ２と管理装置５の間は所定の閉域網を介して通信可能であり、侵入物センサ２及び管理装置５はそれぞれ当該各閉域網に有線接続又は無線接続される。管理装置５と自律移動ロボット６の間は所定の閉域網を介して通信可能であり、管理装置５及び自律移動ロボット６はそれぞれ当該各閉域網に無線接続される。上記各閉域網のプロトコルは、全て共通でも、一部共通でも、それぞれ異なっていてもよい。

監視センタ装置４と管理装置５の間は所定の広域網を介して通信可能であり、監視センタ装置４及び管理装置５はそれぞれ当該広域網に有線接続又は無線接続される。広域網はインターネット、携帯電話網等である。

侵入物センサ２は、自律移動ロボット６を移動させて監視する範囲として予め定めた監視空間内の１又は複数の所定位置に設置されて、監視空間に現れた侵入物を検出する。監視空間は、例えば、複数の建物と植林とを含んだ施設の敷地とすることができる。侵入物センサ２は、侵入物を検出すると、その侵入物の位置（侵入物位置）を侵入物センサ２の識別符号とともに管理装置５に送信する。侵入物センサ２は、侵入物を検出するための不図示のセンサ部と、管理装置５と通信するための不図示の通信部を備える。センサ部は、例えばレーザセンサ、マイクロ波センサ、赤外線センサ、超音波センサなどを用いることができる。

監視センタ装置４は監視空間から離れた遠隔地の監視センタに設置され、遠隔地から監視空間を監視する。監視センタ装置４は、サーバコンピュータ及び監視員端末を有している。サーバコンピュータと監視員端末はＬＡＮ等の閉域網で接続される。監視センタ装置４は、自律移動ロボット６等に対する監視員の指示を管理装置５に送信する。また、監視センタ装置４は、自律移動ロボット６が生成した情報等を管理装置５から受信して表示することにより監視員に伝達する。上記指示は、例えば、自律移動ロボット６が待機所から目標位置まで移動した後に待機所まで帰還することを指示するための帰還指示である。サーバコンピュータは管理装置５及び監視員端末と通信するための不図示の通信部を備える。監視員端末は、サーバコンピュータと通信するための不図示の通信部、監視員から指示を受け付けるためのキーボードやマウス等である不図示の入力部、情報を表示するための液晶ディスプレイ等である不図示の表示部を備える。

管理装置５は、監視空間内の所定位置に設置され、制御装置（コントローラ）として機能する。例えば、管理装置５は、侵入物センサ２から侵入物位置を受信し、受信した各情報を自律移動ロボット６に送信する。また、監視センタ装置４から指示を受信して当該指示を自律移動ロボット６に送信するとともに、自律移動ロボット６から受信した情報を監視センタ装置４に送信する。管理装置５は、侵入物センサ２、監視センタ装置４及び自律移動ロボット６と通信するための不図示の通信部を備える。

管理装置５は、受信する侵入物位置が侵入物センサ２のローカル座標系の値である場合は当該値を監視空間のグローバル座標系の値に変換する座標変換手段を備えてもよい。その場合の座標変換手段は、グローバル座標系における各侵入物センサ２の設置条件を予め記憶する不図示の記憶部と、変換を行う不図示の演算部によって実現される。

なお、ローカル座標系は、侵入物センサ２や自律移動ロボット６等の各構成が個々に侵入物等の物体の位置を特定するための座標系である。これに対して、グローバル座標系は、移動制御システム１において侵入物等の物体の位置を特定するための座標系である。例えば、自律移動ロボット６からしてみれば侵入物が正面にいると認識しても、自律移動ロボット６の向きによって正面が示す方向が特定できない。上記座標変換手段は、自律移動ロボット６のローカル座標系における座標値を、この座標値及び検出されている自律移動ロボット６の向きを考慮してグローバル座標系における座標値に変換する。

また、管理装置５は、侵入物センサ２から侵入物位置が受信されなくなった場合に自律移動ロボット６に対する帰還指示を生成する指示生成手段を備えてもよい。その場合の指示生成手段は受信履歴を記憶する記憶部と帰還指示の生成を行う演算部によって実現される。

自律移動ロボット６は、ロボットであり、本発明における制御体に相当する。例えば、自律移動ロボット６は、クワッドロータを有するドローンである。自律移動ロボット６は、侵入物位置や帰還指示を受信すると適宜目標位置を定めて、障害物を回避しながら目標位置まで移動する。平時は監視空間内に設置された待機所にて待機し、侵入物センサ２が侵入物を検出すると侵入物位置の近傍まで自律移動して撮影を行う。また、移動中に監視センタ装置４又は管理装置５からの帰還指示を受信すると、待機所まで自律移動して待機状態に戻る。

「自律移動」というのは、目標位置が定まると、その後は管理装置５等外部からの指示や制御を受けなくても目標位置まで自律的に移動制御を行って移動することをいう。また、「追従移動」というのは、計画された経路に沿って移動し、経路に沿って移動している途中で、例えば風の影響を受けて経路から外れた場合には経路まで戻って移動を継続するよう移動制御を行いながら移動することをいう。

図２は、本実施の形態における自律移動ロボット６の概略構成図である。本実施の形態における自律移動ロボット６は、通信部６０、ロボット位置・姿勢センサ６１、障害物センサ６２、カメラ６３、記憶部６５、モータ６６等が制御線にて制御部６４に接続されるとともに、モータ６６とロータ６７とが制御線にて接続されて構成される。

通信部６０は、管理装置５との間で通信するための通信モジュールである。通信部６０は、侵入物センサ２が検出した侵入物位置を受信して制御部６４に出力する。また、通信部６０は、監視センタ装置４又は管理装置５からの指示を受信して制御部６４に出力する。また、通信部６０は、ロボット位置・姿勢センサ６１によって取得された情報、カメラ６３によって撮影された画像等の情報を制御部６４から受信して管理装置５に送信する。

ロボット位置・姿勢センサ６１は、自律移動ロボット６の現在位置及び姿勢を取得するためのセンサである。例えば、ＧＮＳＳ（ＧｌｏｂａｌＮａｖｉｇａｔｉｏｎＳａｔｅｌｌｉｔｅＳｙｓｔｅｍ）等の航法衛星（人工衛星）から送信される電波（航法信号）を受信する受信機、方位を計測する電子コンパス及び角速度を計測するジャイロセンサを備える。受信機は、複数の航法衛星から送信される航法信号を受信して制御部６４に出力するとともに、電子コンパス及びジャイロセンサによる計測信号を制御部６４に出力する。なお、現在位置を取得するために受信機に代えてレーザスキャナ及び気圧センサを用いるなど、他の既知のセンサを用いて既知の技術を利用することにより現在位置及び姿勢を得るための情報を取得するものとしてもよい。

障害物センサ６２は、自律移動ロボット６の周囲の有効距離内に存在する障害物を検出するためのセンサである。障害物センサ６２は、例えば、マイクロ波センサ、レーザセンサ、超音波センサ等とすることができる。マイクロ波センサで実現する場合、障害物センサ６２は、自律移動ロボット６の周囲にマイクロ波を送信して障害物からの反射波を受信し、反射波信号を制御部６４に出力する。有効距離はセンサの性能に合わせて予め設定される。

カメラ６３は、自律移動ロボット６の周囲を撮影して画像を制御部６４に出力する。

制御部６４は、ＣＰＵ等を備えたコンピュータにより実現される。制御部６４は、記憶部６５からプログラムやデータを読み出し、プログラムに従って、目標位置設定手段６４０、ロボット位置・姿勢算出手段６４１、移動計画算出手段６４２、障害物領域算出手段６４３及び移動制御手段６４４等として機能する。移動計画算出手段６４２には、最短経路算出手段６４２ａ及び主経路算出手段６４２ｂが含まれる。

記憶部６５は、ＲＯＭ（ＲｅａｄＯｎｌｙＭｅｍｏｒｙ）、ＲＡＭ（ＲａｎｄｏｍＡｃｃｅｓｓＭｅｍｏｒｙ）等の情報記憶装置により実現される。記憶部６５は、各種プログラムや各種データを記憶し、制御部６４との間でこれらの情報を入出力する。記憶部６５は、空間情報記憶手段６５０、位置・姿勢情報記憶手段６５１、死角情報記憶手段６５２及び計画記憶手段６５３等として機能する。

更に、自律移動ロボット６には、４つのロータ６７と、その回転軸がそれぞれ対応するロータ６７に連結された４つのモータ６６からなる４つのユニットが搭載される。各モータ６６は、制御部６４に接続されて制御部６４から回転速度が指示される。４つのロータ６７が独立して回転することにより自律移動ロボット６に任意方向の加速度を発生させる。

空間情報記憶手段６５０は、監視空間の３次元構造等を表した空間情報を記憶する。空間情報は、移動制御システム１の管理者等によって予め設定され、少なくとも監視空間において自律移動ロボット６の移動の障害となり得る障害物の位置を含む。障害物は、例えば建物や樹木等である。具体的には、監視空間を例えば自律移動ロボット６よりも小さく予め定められた立方体である複数のボクセルに区分し、各ボクセルを、監視空間のグローバル座標系における当該ボクセルの位置（三次元座標）と当該ボクセルが表す領域に障害物が含まれるか否かを区別するための符号からなるデータ（ボクセル属性）とを含むボクセル情報により管理する。障害物が含まれることを示す符号が付与されたボクセルが障害物領域を表す。また、空間情報記憶手段６５０は、緯度・経度・高度を監視空間のグローバル座標系の位置に変換する変換規則、方位をグローバル座標系の方向に変換する変換規則を記憶する。例えば、グローバル座標系の原点の緯度・経度・高度、グローバル座標系のＸ軸・Ｙ軸に対応する方位、グローバル座標系の単位長さに対応する緯度・経度・高度の変位量を記憶する。

位置・姿勢情報記憶手段６５１は、侵入物位置、自律移動ロボット６の目標位置を記憶する。また、自律移動ロボット６の現在位置及び姿勢の検出結果を予め定められた回数だけロボット状態履歴として循環記憶する。なお、本実施の形態では、ロボット状態履歴内の最新の位置を特に「現在位置」と称し、それ以外の位置を「過去位置」と称する。同様に、ロボット状態履歴内の最新の姿勢を特に「現在姿勢」と称し、それ以外の姿勢を「過去姿勢」と称する。また、自律移動ロボット６の待機所内に定められた位置（待機位置）を予め記憶する。

死角情報記憶手段６５２は、自律移動ロボット６における障害物センサ６２の死角となる領域（死角領域）の位置を記憶する。すなわち自律移動ロボット６の内部、表面または外部近傍に予め基準位置を定め、当該基準位置に対する自律移動ロボット６に設けられた障害物センサ６２の死角領域の座標データを記憶する。具体的には、基準位置を原点とする座標系における、予め定めた自律移動ロボット６の近傍範囲を表す立体形状と、近傍範囲内における障害物センサ６２の検知範囲の立体形状とを記憶することにより、近傍範囲内におけるいずれの障害物センサ６２の検知範囲でもない領域を死角領域として記憶していることになる。なお、死角領域の立体形状自体を記憶するようにしてもよい。

例えば、死角領域の座標データは、自律移動ロボット６の胴体部重心を座標原点とし、４つのロータ６７がなす平面に並行であり第一ロータと第二ロータの中間方向を正方向とするｘ軸と、４つのロータ６７がなす平面に平行でありｘ軸と直交するｙ軸と、自律移動ロボット６の胴体上部に向かうｚ軸を有した右手直交座標系ｘｙｚで表す。図３は、障害物センサ６２を自律移動ロボット６の胴体部の前後左右上下に設置したときの検知範囲１０を例示する図である。図３において、近傍範囲は、ｘｙｚ座標系の原点を中心とする半径Ｒ_Ｎの球としている。また、障害物センサ６２の各々の検知範囲は、ｘｙｚ座標系の原点を頂点とし、それぞれｙ軸上の正方向、ｘ軸上の正方向、ｙ軸上の負方向、ｘ軸上の負方向、ｚ軸上の正方向、ｚ軸上の負方向に向かう回転中心軸を有する頂角が６０度の６つの円錐の範囲とする。したがって、上記半径Ｒ_Ｎの球から上記６つの円錐の領域を除いた立体的な領域が死角領域に相当する。

また、図４は、検知範囲１０を死角近傍領域１２とそれ以外の領域１１に分割した様子を示す。ここで、領域１１以外の領域を死角領域としてもよい。このように死角領域を多めに見積もることでより安全な経路を算出できる。

計画記憶手段６５３は、監視空間において自律移動ロボット６が移動する予定である経路を移動計画の情報として記憶する。また、計画記憶手段６５３は、上記経路を移動するときの加速度制限値を移動計画に含めて記憶してもよい。

目標位置設定手段６４０は、侵入物位置を受信してから帰還指示が受信されるまでの間、侵入物位置の近傍に目標位置を設定する。また、目標位置設定手段６４０は、帰還指示が受信されると、待機所の位置を読み出し、待機所を目標位置に設定する。そして、目標位置設定手段６４０は、侵入物位置及び目標位置を位置・姿勢情報記憶手段６５１に記憶させる。

以上のように、目標位置設定手段６４０は、制御体の働きかけの対象となる侵入物や待機所の位置である対象位置が与えられると、当該対象位置の近傍（侵入物位置の近傍）又は当該対象位置そのもの（待機位置）を目標位置に設定して、目標位置を記憶部６５に記憶させる。

ロボット位置・姿勢算出手段６４１は、ロボット位置・姿勢センサ６１の出力から監視空間における自律移動ロボット６の現在位置及び姿勢を算出し、位置・姿勢情報記憶手段６５１のロボット状態履歴に追記する。例えば、ロボット位置・姿勢センサ６１が航法信号を出力する場合、ロボット位置・姿勢算出手段６４１は、航法信号を解析して当該航法信号から緯度・経度・高度を抽出し、空間情報記憶手段６５０に記憶されている変換規則を用いて緯度・経度・高度を監視空間のグローバル座標系の位置に変換する。また、例えばロボット位置・姿勢センサ６１が電子コンパス及びジャイロセンサの計測信号を出力する場合、ロボット位置・姿勢算出手段６４１は、計測信号を解析して当該計測信号から方位及びロボット位置・姿勢センサ６１のローカル座標系における姿勢変化量を抽出するとともに、位置・姿勢情報記憶手段６５１からグローバル座標系での値である現在姿勢（この時点では一時点過去の姿勢）を読み出す。方位は、電子コンパスの計測信号をグローバル座標系における方位へ変換し、他のセンサから算出される方位と融合させて算出させてもよい。そして、ロボット位置・姿勢算出手段６４１は、予め設定された自律移動ロボット６の主軸方向に対するロボット位置・姿勢センサ６１の主軸方向の相対方向を用いて姿勢変化量を自律移動ロボット６のローカル座標系に変換し、変換した姿勢変化量を加えて現在姿勢を更新する。

以上のように、ロボット位置・姿勢センサ６１と通信部６０とロボット位置・姿勢算出手段６４１（制御部６４）とが協働して制御体位置検出手段及び制御体姿勢検出手段をなす。制御体位置検出手段は、制御体である自律移動ロボット６の現在位置を検出して現在位置を記憶部６５に記憶させる。制御体姿勢検出手段は、制御体である自律移動ロボット６の現在姿勢を検出して現在姿勢を記憶部６５に記憶させる。

移動計画算出手段６４２は、空間情報記憶手段６５０から空間情報を読み出すとともに、位置・姿勢情報記憶手段６５１から現在位置及び目標位置を読み出し、自律移動ロボット６の現在位置から目標位置までの、障害物領域を経由せず且つ死角方向への移動を要さない経路（主経路）を算出して、算出した主経路を含めた移動計画を計画記憶手段６５３に記憶させる。

最短経路算出手段６４２ａは、障害物を回避して、すなわち前述した障害物領域を経由せずに現在位置から目標位置に至る経路のうちの、移動距離が最短と推定される最短経路を算出する。具体的には、最短経路算出手段６４２ａは、空間情報において障害物を含まないことを示す符号が付与されたボクセルからなる空間に、例えばＡ＊経路探索法を適用して最短経路を算出する。

Ａ＊経路探索法では、各ボクセルの中心位置をノードと称すると、移動目標位置に対応するボクセルのノード（以下「スタートノード」という）から自己位置に対応するボクセルのノード（以下「ゴールノード」という）に至る総コスト値が最も小さくなる経路を最短経路として探索する。より具体的に説明すると、Ａ＊経路探索法では、まずスタートノードを注目ノードとして設定し、その注目ノードに隣接する１又は複数のノードであってこれまで評価ノードとして設定されていないノード（隣接ノード）を評価ノードとして設定する。評価ノードを設定する際、ボクセルの属性を参照し、障害物が含まれているボクセル（以下「占有ボクセル」という）に対応するノード（及び占有ボクセルに隣接するボクセル）を除外し、経路が生成されないようにする。そして、各評価ノードにおける総コスト値を求め、その中から最小の総コスト値となる評価ノードを注目ノードとして選出する。以上の処理をゴールノードが注目ノードとして選出されるまで繰り返し行う。

なお、経路の探索方法は、Ａ＊経路探索法に限定されるものでなく、ダイクストラ法等の他の経路探索方法を適用してもよい。

また、最短経路の算出は、自律移動ロボット６の姿勢を含めた死角領域の制約を無視して算出する分、後述する主経路算出手段６４２ｂによる主経路の算出と比較して処理負荷が少ない。

主経路算出手段６４２ｂは、空間情報記憶手段６５０に記憶されている空間情報において障害物を含まないことを示す符号が付与されたボクセルからなる領域（移動可能領域）内の現在位置から目標位置に至る経路であって、当該経路上の任意点に自律移動ロボット６を移動させた場合の死角領域に当該経路上の他点を含まない主経路を算出する手段である。

主経路は、監視空間内の各位置（例えば各ボクセルの重心）と当該位置における自律移動ロボット６の各姿勢（例えば全天方向について１度刻み）の組み合わせについて全探索して算出してもよい。しかしながら、自律移動ロボット６の応答性を高めるために、部分的な探索によって処理負荷を抑えることが望ましい。

そこで、例えば、仮説検証型の探索を行って主経路を算出することが好適である。それぞれが主経路上の点の候補である候補点と当該候補点における自律移動ロボット６の姿勢の候補である候補姿勢と組み合わせた複数の仮説を生成し、仮説ごとに候補点における候補姿勢の自律移動ロボット６を移動させた場合の死角領域を通過せずに移動可能な候補点を有する他の仮説との間の局所経路を検出し、局所経路を連結して主経路とする。ここで、仮説は、例えば、候補点を表すＸ座標値、Ｙ座標値およびＺ座標値と、候補姿勢を表すヨー角でありＸ軸に対して自律移動ロボット６の主軸方向がなす角とが列挙されたベクトルとなる。

例えば、主経路算出手段６４２ｂは、監視空間内の全域に密度Ｄ_Ｈで候補点を生成する。密度Ｄ_Ｈは、事前実験を通じて、計算精度と処理負荷のバランスが許容範囲内である値に定めておくことが好適である。

この場合、該手段６４２ｂは、現在位置と任意の候補点を結んだ局所経路、候補点同士を結んだ局所経路および目標位置と任意の候補点を結んだ局所経路を設定する。そして、該手段６４２ｂは、候補点に対応する候補姿勢に基づいて各候補点における死角領域を算出し、これらの局所経路の中から障害物領域および死角領域のいずれも通過しない局所経路を検出し、検出した局所経路を連結して主経路を算出する。

ここで、複数通りの主経路の候補が算出された場合、主経路算出手段６４２ｂは、主経路の候補の各々に対して評価値を算出し、評価値が最大の主経路候補を選択する。具体的には、該手段６４２ｂは、主経路の候補ごとに、局所経路の長さの算出、各局所経路の曲率の算出、自律移動ロボット６に当該主経路候補上を仮想移動させるシミュレーションに基づく速度履歴の算出、同シミュレーションに基づく加速度履歴の算出、同シミュレーションに基づく加速度変化履歴の算出のうちのいずれか１以上を行う。そして、該手段６４２ｂは、主経路候補ごとに、長さを算出する場合は総長が短いほど高い総長評価値、曲率を算出する場合は曲率の総和が小さいほど高い曲率評価値、速度履歴を算出する場合は所定の速度制限値以上の速度を含めば最低評価値となり含まなければ標準評価値となる速度評価値、加速度履歴を算出する場合は所定の加速度制限値以上の加速度を含めば最低評価値となり含まなければ標準評価値となる加速度評価値、加速度変化履歴を算出する場合は所定の加速度変化制限値以上の加速度変化を含めば最低評価値となり含まなければ標準評価値となる加速度変化評価値を算出して、算出した値を重み付け加算して評価値を算出する。

また、例えば、主経路算出手段６４２ｂは、監視空間内の全域ではなく、最短経路算出手段６４２ａで算出される最短経路から半径Ｒ_Ｈ内に限定して新たな候補点を生成するようにしてもよい。これによって、監視空間内の全域に密度Ｄ_Ｈで候補点を生成する場合に比べて処理負荷の低減と生成される経路長の短さを両立することができる。

この半径Ｒ_Ｈは自律移動ロボット６の長軸より長く設定することが好適である。半径Ｒ_Ｈは、例えば、自律移動ロボット６の長軸の３倍に設定すればよい。これにより、障害物センサ６２の死角を回避して移動するための自由度を確保することができる。なお、この場合も、主経路算出手段６４２ｂは、複数通りの主経路の候補が算出された場合には、主経路の候補の各々に対して上記評価値を算出して、評価値が最大の主経路候補を選択すればよい。

また、例えば、主経路算出手段６４２ｂは、監視空間内に逐次的に候補点を生成するようにしてもよい。

この場合、まず、該手段６４２ｂは、現在位置を中心とする半径Ｒ_Ｈの球内に予め定めた個数Ｎ_Ｈ１だけ候補点をランダムに設定する。さらに、該手段６４２ｂは、設定された候補点ごとに候補姿勢をランダムに設定する。そして、該手段６４２ｂは、候補点と候補姿勢とを組み合わせて新規仮説とする。ここで、上述した半径Ｒ_Ｈと密度Ｄ_Ｈを用いてＮ_Ｈ１＝４／３×π×Ｒ_Ｈ ^３×Ｄ_Ｈとすることができる。次に、該手段６４２ｂは、現在位置と各候補点とを結んで局所経路を設定し、障害物領域又は死角領域を通過する局所経路をなす候補点を有する新規仮説を削除する。続いて、該手段６４２ｂは、削除されずに残った残存仮説が示す候補点のそれぞれに対して、当該候補点を中心とする半径Ｒ_Ｈの球内に個数Ｎ_Ｈ１の新たな候補点をランダムに生成するとともに、新たな候補点ごとに候補姿勢をランダムに設定して新規仮説を生成する。そして、該手段６４２ｂは、各残存仮説が示す候補点と各新規仮説が示す候補点とを結んで局所経路を算出し、障害物領域又は死角領域を通過する局所経路をなす候補点を有する新規仮説を削除する。以降、該手段６４２ｂは、目標位置とほぼ一致する候補点を有する残存仮説が得られるまで、又は目標位置との距離が一定値ε以下の候補点を有する残存仮説が得られるまで仮説の生成と削除を繰り返す。

また、この場合も、主経路算出手段６４２ｂは、監視空間内の全域ではなく、最短経路算出手段６４２ａで算出される最短経路から半径Ｒ_Ｈ内に限定して新たな候補点を生成するようにしてもよい。具体的には、該手段６４２ｂは、残存仮説が示す候補点からの最短経路上の最近傍点（候補点から最短経路に下した垂線と最短経路の交点）を中心とする半径Ｒ_Ｈの球内に新たな候補点を生成する。これによって、監視空間内の全域に密度Ｄ_Ｈで候補点を生成する場合に比べて処理負荷の低減と生成される経路長の短さを両立することができる。

また、例えば、主経路算出手段６４２ｂは、最短経路上の区間であって当該区間に移動した自律移動ロボット６の死角領域に最短経路上の他区間を含む不良点を検出し、最短経路における当該不良点から目標位置までの区間の一部または全部を修正して主経路を算出することによって、さらに処理負荷を低減させることができる。

この場合、まず、主経路算出手段６４２ｂは、最短経路上に所定間隔で複数のサンプル点を設定して、サンプル点の各々に移動したときの自律移動ロボット６の姿勢を求めて、当該姿勢に基づいてサンプル点ごとに死角領域を決定する。そして、該手段６４２ｂは、複数のサンプル点のうち１つを着目点として選択し、着目点の死角領域に最短経路上の他のサンプル点を含むときに着目点を不良点とする。また、該手段６４２ｂは、最短経路上に、不良点よりも所定距離だけ現在位置に近い修正開始点を定める。次に、該手段６４２ｂは、現在位置を中心とする半径Ｒ_Ｈの球内に予め定めた個数Ｎ_Ｈ１だけ新たな候補点をランダムに生成するとともに、候補点ごとに候補姿勢をランダムに設定して、候補点と候補姿勢とを組み合わせた新規仮説を生成する。続いて、該手段６４２ｂは、現在位置と各候補点とを結んで局所経路を設定し、障害物領域または死角領域を通過する局所経路をなす候補点を有する新規仮説を削除する。続いて、該手段６４２ｂは、削除されずに残った残存仮説のそれぞれに対して、当該残存仮説が示す候補点を中心とする半径Ｒ_Ｈの球内にＮ_Ｈ１個の新たな候補点をランダムに生成するとともに、新たな候補点のそれぞれに対して候補姿勢をランダムに設定し、新たな候補点と候補姿勢とを組み合わせたＮ_Ｈ１個の新規仮説を生成する。そして、該手段６４２ｂは、各残存仮説が示す候補点と当該残存仮説に対して生成したＮ_Ｈ１個の新規仮説のそれぞれが示す候補点とを結んで局所経路を算出し、障害物領域または死角領域を通過する局所経路をなす候補点を有する新規仮説を削除する。以降、該手段６４２ｂは、目標位置とほぼ一致する候補点を有する残存仮説が得られるまで、又は目標位置との距離が一定値ε以下の候補点を有する残存仮説が得られるまで繰り返す。

なお、不良点から先を修正する場合も、主経路算出手段６４２ｂは、最短経路から半径Ｒ_Ｈ内に限定して新たな候補点を生成するようにしてもよい。すなわち該手段６４２ｂは、残存仮説が示す候補点からの最短経路上の最近傍点を中心とする半径Ｒ_Ｈの球内に新たな候補点を生成することによって新規仮説を生成する。或いは該手段６４２ｂは、修正開始点から目標位置までの最短経路から半径Ｒ_Ｈ内に密度Ｄ_Ｈで仮説を設定し、修正開始点と任意の候補点を結んだ局所経路、候補点同士を結んだ局所経路および目標位置と任意の候補点を結んだ局所経路を設定して、これらの局所経路の中から障害物領域および死角領域のいずれも通過しない局所経路を検出し、検出した局所経路を連結して修正開始点から先の主経路を算出する。これらによって、監視空間内の全域に密度Ｄ_Ｈで候補点を生成する場合に比べて処理負荷の低減と生成される経路長の短さを両立することができる。

また、不良点よりも先の新たな候補点を最短経路から半径Ｒ_Ｈ内に限定して生成する場合、主経路算出手段６４２ｂは、最短経路上の候補点を有する仮説が得られた場合に当該仮説が示す候補点から再び不良点の検出処理を行うようにしてもよい。再び不良点の検出処理を行う際に逐次的な仮説生成を行う場合、該手段６４２ｂは、新規仮説の一部（Ｎ_Ｈ２（＜Ｎ_Ｈ１）個）に最短経路上の候補点を含ませ、最短経路上の候補点を有する残存仮説が得られた場合に当該残存仮説が示す候補点（修正完了点）から再び不良点の検出処理を行うようにしてもよい。これによって、最短経路からの修正を最小限に留め易くなり、処理負荷の低減と生成される経路長の短さを両立する効果が顕著となる。

障害物領域算出手段６４３は、障害物センサ６２の出力から、自律移動ロボット６の周囲の障害物領域を算出し、移動制御手段６４４に出力する。例えば、障害物センサ６２が反射波信号を出力する場合、障害物領域算出手段６４３は、障害物領域算出手段６４３は、反射波信号を解析して障害物センサ６２から障害物までの距離（障害物距離）及び障害物センサ６２の主軸方向に対する障害物の存在方向（障害物方向）を算出する。さらに、障害物領域算出手段６４３は、位置・姿勢情報記憶手段６５１からグローバル座標系での値である現在位置及び現在姿勢を読み出す。そして、障害物領域算出手段６４３は、現在位置、現在姿勢及びローカル座標系の値として予め設定された自律移動ロボット６の主軸方向に対する障害物センサ６２の主軸方向の相対方向を用いて、障害物センサ６２のローカル座標系での値である障害物距離及び障害物方向をグローバル座標系における障害物領域の情報に変換する。

以上のように、障害物センサ６２と障害物領域算出手段６４３（制御部６４）とが協働して障害物領域検出手段をなし、障害物領域検出手段は制御体である自律移動ロボット６の周囲の障害物領域を検出する。

移動制御手段６４４は、計画記憶手段６５３に記憶されている移動計画の情報、ロボット位置・姿勢算出手段６４１に記憶されている現在位置であって逐次更新される現在位置、及び障害物領域算出手段６４３から入力される障害物領域の情報を参照し、自律移動ロボット６を移動計画に記された主経路及び回避経路に追従移動させる。

すなわち、移動制御手段６４４は、主経路が算出されると自律移動ロボット６を主経路に追従させる。また、主経路の追従中に障害物領域算出手段６４３から障害物領域の情報が入力された場合（すなわち障害物検出手段が主経路近傍の障害物を検出した場合）、移動制御手段６４４は、主経路の追従を一時停止させ、回避計画算出手段６４４ａによって回避経路を含めた回避計画を算出し、回避経路を追従して移動する制御に切り替える。移動制御手段６４４は、現在位置が回避経路の終端に達すると主経路を追従して移動する制御を再開する。また、移動制御手段６４４は、現在位置が主経路の終端に達すると主経路の追従は終了となる（例えば、終端が目標位置であれば侵入物の追尾動作を開始し、終端が待機所であれば待機状態に移行）。

追従移動中、移動制御手段６４４は、現在位置と追従対象の経路（対象経路）を比較して現在位置よりも目標位置に近く現在位置から所定距離以内である対象経路上の近傍点を複数設定して、近傍点のうち現在位置から障害物領域を経由せずに飛行可能な近傍点に加速度制限値以下の加速度で飛行するために要する４つのモータ６６の回転速度を導出し、各モータ６６に導出した回転速度を指示する、という制御を対象経路の終端に到達するまで逐次的に繰り返す。上記所定距離には障害物センサ６２の有効距離以下の距離が予め設定される。

回避計画算出手段６４４ａは、現在位置を始端として当該障害物から所定距離以上離れた位置を終端とする経路であって、当該経路上の任意点に自律移動ロボット６が移動した場合の死角領域に当該経路上の他点を含まない回避経路を算出し、回避経路を含めた回避計画を計画記憶手段６５３に記憶させる。

回避経路も主経路と同様にして算出することができる。すなわち回避計画算出手段６４４ａは、回避経路上の点の候補である候補点と当該候補点における自律移動ロボット６の姿勢の候補である候補姿勢を組み合わせた仮説を複数設定し、仮説ごとに候補点における候補姿勢となっている自律移動ロボット６の死角領域を通過せずに移動可能な他の仮説の候補点との間の局所経路を検出し、検出した局所経路を連結して回避経路を算出する。また、その際に、該手段６４４ａは上記始端から上記終端までの最短経路を算出し、最短経路の一部または全部を修正することにより回避経路を算出してもよい。

次に、移動計画算出手段６４２で実行される処理について図５及び図６に示すフローチャートを用いて説明する。図７は、最短経路算出手段６４２ａで求められた最短経路１６を主経路算出手段６４２ｂにおいて障害物センサ６２の死角を考慮した経路に修正する様子を示す。

目標位置設定手段６４０によって位置・姿勢情報記憶手段６５１に記憶されている目標位置Ｐ_Ｇが更新されると、図５及び図６のフローチャートに沿った処理が開始される。

まず、移動計画算出手段６４２は、ロボット位置・姿勢算出手段６４１が算出して位置・姿勢情報記憶手段６５１に記憶されている現在位置Ｐ_Ｓを取得する（Ｓ１）。最短経路算出手段６４２ａに現在位置Ｐ_Ｓと目標位置Ｐ_Ｇを入力して最短経路を算出させる（Ｓ２）。最短経路算出手段６４２ａは、入力値および空間情報記憶手段６５０に記憶されている空間情報を参照して、既知である地面１３や建物１５等が存在する障害物領域を回避し最短距離で到達可能な最短経路１６を生成する。

次に、ステップＳ３～Ｓ９においては、主経路算出手段６４２ｂが、死角情報記憶手段６５２で記憶されている死角情報をもとに、最短経路１６上を移動した場合に死角領域へ移動することになってしまう位置（不良点）ＰＲを算出する。なお、ステップＳ２の実行直後、主経路算出手段６４２ｂは、主経路をクリアした上で処理をステップＳ３に進める。そのために、主経路算出手段６４２ｂは、最短経路上に順次サンプル点を設定して（Ｓ３）、ステップＳ３～Ｓ９のループ処理を行う。ステップＳ２の直後に実行されるステップＳ３で設定されるサンプル点は現在位置である。後述するステップＳ２０の直後に実行されるステップＳ３で設定されるサンプル点は後述するステップＳ１８で検出される残存仮説が示す候補点である。また、その他の時点で実行されるステップＳ３で設定されるサンプル点は、１つ前に設定されたサンプル点から目標位置に向かって所定距離（サンプル点間隔）だけ進めた位置である。サンプル点の間隔は、障害物センサ６２の有効距離以下の値に定められる。例えば、当該有効距離と同値のサンプル点の間隔を予め定めることができる。

主経路算出手段６４２ｂは、サンプル点（現サンプル点）を設定した後、現サンプル点が目標位置に達しているか否かを確認する（Ｓ４）。目標位置に達していれば（ステップＳ４にてＹＥＳ）、主経路算出手段６４２ｂは、最短経路における１つ前のサンプル点から現サンプル点までの区間を主経路に追加して（Ｓ５）、処理をステップＳ２１に進める。ちなみに、後述するステップＳ９、Ｓ１７および／またはＳ２０の追加を経てステップＳ５の追加を終えた主経路は現在位置から目標位置までを結んだ経路となっている。

現サンプル点が目標位置に達していない場合（ステップＳ４にてＮＯ）、主経路算出手段６４２ｂは、現サンプル点における自律移動ロボット６の姿勢を設定し（Ｓ６）、当該姿勢での死角領域を算出する（Ｓ７）。ステップＳ２の直後に設定された現サンプル点に対しては現在姿勢が設定される。後述するステップＳ２０の直後に設定された現サンプル点に対しては後述するステップＳ１８で検出される残存仮説が示す候補姿勢が設定される。また、その他の時点で実行される現サンプル点に対しては、１つ前に設定されたサンプル点から現サンプル点への移動をシミュレートして設定する。

続いて、主経路算出手段６４２ｂは、最短経路と死角領域とを比較して、死角領域が最短経路に重複し且つ重複した区間が現サンプル点よりも目標位置に近い区間を含むか否かを確認する（Ｓ８）。当該条件を満たさない場合（ステップＳ８にてＮＯ）、現サンプル点は不良点でないとされ、主経路算出手段６４２ｂは、最短経路における１つ前のサンプル点から現サンプル点までの区間を主経路に追加して（Ｓ９）、処理をステップＳ３に戻す。他方、条件を満たした場合（ステップＳ８にてＹＥＳ）、現サンプル点は不良点であると判定され、処理はステップＳ１０に進められる。

ステップＳ１０～Ｓ２０においては、主経路算出手段６４２ｂが、最短経路の一部を修正して主経路を算出する。まず、主経路算出手段６４２ｂは、不良点の１つ前のサンプル点を候補点とするとともに、当該サンプル点に対してステップＳ６で設定した姿勢を候補姿勢とし、当該候補点と当該候補姿勢を組み合わせて残存仮説の初期値に設定する（Ｓ１０）。次に、主経路算出手段６４２ｂは、各残存仮説が示す候補点に当該残存仮説が示す候補姿勢の自律移動ロボット６が存在する場合の死角領域を算出する（Ｓ１１）。ちなみに、ステップＳ１０の直後に設定されている残存仮説は１つであるが、後述するステップＳ１８の直後には複数の残存仮説が設定され得る。

主経路算出手段６４２ｂは、残存仮説ごとに、当該残存仮説が示す候補点の最短経路上の最近傍点を中心とする半径Ｒ_Ｈの球内にＮ_Ｈ１個の新規仮説を生成する（Ｓ１２）。このとき、主経路算出手段６４２ｂは、残存仮説ごとのＮ_Ｈ１個の新規仮説のうちのＮ_Ｈ２個を最短経路上の上記球の中心よりも目標位置に近い区間に生成する。すなわち、主経路算出手段６４２ｂは、残存仮説ごとに、当該残存仮説が示す候補点から最短経路への垂線を導出して、最短経路と垂線の交点を球の中心に設定する。そして、主経路算出手段６４２ｂは、残存仮説ごとに、上記球内のＸ座標、Ｙ座標、Ｚ座標をランダムに定めるとともに、全方向の中からヨー角をランダムに定め、定めた各座標値とヨー角を組み合わせた新規仮説を生成する処理を（Ｎ_Ｈ１－Ｎ_Ｈ２）回ずつ繰り返す。さらに、主経路算出手段６４２ｂは、残存仮説ごとに、上記球内且つ最短経路上且つ上記中心よりも目標位置に近いＸ座標、Ｙ座標、Ｚ座標をランダムに定めるとともに、全方向の中からヨー角をランダムに定め、定めた各座標値とヨー角を組み合わせた新規仮説を生成する処理をＮ_Ｈ２回ずつ繰り返す。

主経路算出手段６４２ｂは、残存仮説ごとに、当該残存仮説が示す候補点と当該残存仮説に対して生成した新規仮説のそれぞれが示す候補点とを結ぶ局所経路を算出する（Ｓ１３）。局所経路は単純に線分とすることもできる。また、局所経路を移動する自律移動ロボット６の姿勢が、残存仮説が示す候補姿勢から新規仮説が示す候補姿勢に滑らかに移行できるように複数の中間姿勢を求め、さらに各中間姿勢に移行するための加速度等の制御値を求めて、求めた制御値で移動する場合の経路としてもよい。

主経路算出手段６４２ｂは、ステップＳ１３で算出した全ての局所経路をステップＳ１１で算出した死角領域と比較して、死角領域を通過する局所経路を特定し、ステップＳ１２で生成した新規仮説のうちの局所経路が死角領域を通過する新規仮説を削除する（Ｓ１４）。主経路算出手段６４２ｂは、ステップＳ１４で削除されずに残った全ての局所経路を空間情報として記憶されている障害物領域と比較して、障害物領域を通過する局所経路を特定し、ステップＳ１４で削除されずに残った新規仮説のうちの局所経路が障害物領域を通過する新規仮説を削除する（Ｓ１５）。

ステップＳ１３，Ｓ１４で削除されずに残った新規仮説は、これまでの残存仮説に代わって以降の処理での新たな残存仮説とされる。

続いて、主経路算出手段６４２ｂは、全ての残存仮説が示す候補点と目標位置との距離を算出して閾値εと比較し、距離がε以下の候補点があるか否かを確認する（Ｓ１６）。距離がε以下の候補点がある場合（ステップＳ１６にてＹＥＳ）、候補点が目標位置近傍の残存仮説があるとして、主経路算出手段６４２ｂは、不良点手前のサンプル点から当該残存仮説の候補点までの局所経路を主経路に追加して（Ｓ１７）、処理をステップＳ２１に進める。なお、ステップＳ５、Ｓ９および／または後述するステップＳ２０の追加を経てステップＳ１７の追加を終えた主経路は現在位置から目標位置近傍までを結んだ経路となっている。

他方、距離がε以下の候補点が無い場合（ステップＳ１６にてＮｏ）、主経路算出手段６４２ｂは、全ての残存仮説が示す候補点に対して最短経路上の候補点であるか否かの確認を行う（Ｓ１８）。最短経路上の候補点がある場合（ステップＳ１８にてＹＥＳ）、候補点が最短経路上の残存仮説があるとして、主経路算出手段６４２ｂは、当該残存仮説に対してステップＳ１２で算出した最近傍点から目標位置までの区間を死角領域が含むか否かを確認する（Ｓ１９）。条件を満たす場合（ステップＳ１９にてＹＥＳ）、当該残存仮説での死角領域に先の最短経路を含むとして、主経路算出手段６４２ｂは、不良点手前のサンプル点から当該残存仮説の候補点までの局所経路を主経路に追加して（Ｓ２０）、処理をステップＳ３に戻す。これによりさらなる不良点の検出処理が行われる。他方、候補点が最短経路上の残存仮説が無い（ステップＳ１８にてＮＯ）、または候補点が最短経路上の残存仮説はあるが当該残存仮説での死角領域に先の最短経路を含まない（ステップＳ１９にてＮＯ）場合、処理はステップＳ１１に戻され、最短経路からの修正が継続される。

上述したステップＳ５またはＳ１７を経てステップＳ２１に処理が進められた場合、主経路算出手段６４２ｂは、算出した主経路を計画記憶手段６５３に記憶させて主経路算出処理を終了する。

次に、図８のフローチャートを参照して、算出された主経路に沿って自律移動ロボット６を追従移動させる制御について説明する。移動制御手段６４４は、移動計画算出手段６４２において主経路が設定された後、ステップＳ５０からの処理を開始する。

まず、移動制御手段６４４は、自律移動ロボット６が主経路を追従移動するように制御する主経路追従モードに設定される（Ｓ５０）。なお、移動制御手段６４４は、主経路追従モードと、障害物センサ６２によって新たな障害物１４が発見された場合に当該障害物１４を回避する際の回避経路追従モードの２つがある。

移動制御手段６４４は、現在のモードが主経路追従モードであるか否かを確認する（Ｓ５１）。主経路追従モードであれば（ステップＳ５１にてＹＥＳ）、移動制御手段６４４は、自律移動ロボット６を主経路に沿って追従させる追従制御を行う（Ｓ５２）。そして、自律移動ロボット６が目標位置に到達したか否かを確認し（Ｓ５３）、目標位置に到達すれば（ステップＳ５３にてＹＥＳ）、移動制御を終了させ、目標位置に到達していなければ（ステップＳ５３にてＮＯ）、処理をステップＳ５６に進める。

一方、ステップＳ５１において回避経路追従モードであれば（ステップＳ５１にてＮＯ）、移動制御手段６４４は、自律移動ロボット６を回避経路に沿って追従させる回避追従制御を行う（Ｓ５４）。そして、自律移動ロボット６が回避完了点に到達したか否かを確認し（Ｓ５５）、回避完了点に到達すれば（ステップＳ５５にてＹＥＳ）、回避移動制御を終了させてステップＳ５０に処理を戻す。回避完了点に到達していなければ（ステップＳ５５にてＮＯ）、処理をステップＳ５６に進める。

ステップＳ５６では、障害物センサ６２によって障害物１４が検出されたか否かが確認される。障害物１４が検出されなかった場合、処理をステップＳ５１に戻す。障害物１４が検出された場合、ステップＳ５７～Ｓ５９に処理を移し、主経路から離れて、検出された障害物１４を避けるための回避経路の算出処理が行われる。

回避経路を算出するために、障害物１４が検出された主経路上の現在位置を回避開始点（始端）とし、検出された障害物１４の領域から所定距離以上離れた主経路上の点を回避完了点（終端）と設定する（Ｓ５７）。そして、回避開始点から回避完了点までの回避経路を算出する（Ｓ５８）。回避経路は、主経路の算出処理において目標位置を回避完了点に置き換えて、回避開始点から回避完了点に至る移動可能領域内及び障害物センサ６２によって検出された障害物１４が存在しない領域を通る経路であって、当該回避経路上の任意点に移動した自律移動ロボット６の死角領域に当該回避経路上の他点を含まない経路として算出することができる。回避経路を算出後、移動制御手段６４４は、自律移動ロボット６が回避経路を追従移動するように制御する回避経路追従モードに設定され（Ｓ５９）、ステップＳ５１に処理を戻す。

このようにして、予め設定された主経路を追従しつつ、障害物センサ６２によって新たな障害物１４が検出された場合には当該障害物１４を回避するための回避経路を設定し、主経路を離れて回避経路を追従するように自律移動ロボット６を移動させる制御を行うことができる。

なお、上記実施形態では、監視空間を空中とする例を示したが、監視空間は陸上、水上又は水中であってもよい。したがって、監視空間を移動する制御体は、移動するロボットに限らず、例えば、走行するロボット、歩行するロボット又は航行するロボットであってもよい。

また、上記実施形態およびその変形例においては、自律移動ロボット６が目標位置設定手段６４０を備える例を示したが、監視センタ装置４または管理装置５が目標位置設定手段６４０を備えてもよい。例えば、監視センタ装置４が備える目標位置設定手段６４０は監視員の操作による目標位置の入力を受け付け、当該目標位置が監視センタ装置４から管理装置５を経由して自律移動ロボット６に送信される。また、管理装置５が目標位置設定手段６４０を備える場合、侵入物の位置や帰還指示に代えて目標位置が、管理装置５から自律移動ロボット６に送信される。

また、上記実施形態およびその変形例においては、自律移動ロボット６が移動計画算出手段６４２を備える例を示したが、管理装置５が移動計画算出手段６４２を備えてもよい。その場合、管理装置５も空間情報記憶手段６５０および死角情報記憶手段６５２を備え、侵入物の位置や帰還指示に代えて移動計画が管理装置５から自律移動ロボット６に送信される。

また、上記実施形態およびその変形例においては、自律移動ロボット６が回避計画算出手段６４４ａを備える例を示したが、管理装置５が回避計画算出手段６４４ａを備えてもよい。その場合、管理装置５も空間情報記憶手段６５０および死角情報記憶手段６５２を備え、障害物領域検出手段が検出した障害物領域が自律移動ロボット６から管理装置５に送信され、回避計画が管理装置５から自律移動ロボット６に送信される。

１移動制御システム、２侵入物センサ、４監視センタ装置、５管理装置、６自律移動ロボット、１０検知範囲、１１領域、１２死角近傍領域、１３地面、１４障害物、１５建物、１６最短経路、１７エッジ、１８候補点、６０通信部、６１ロボット位置・姿勢センサ、６２障害物センサ、６３カメラ、６４制御部、６５記憶部、６６モータ、６７ロータ、６４０目標位置設定手段、６４１ロボット位置・姿勢算出手段、６４２移動計画算出手段、６４２ａ最短経路算出手段、６４２ｂ主経路算出手段、６４３障害物領域算出手段、６４４移動制御手段、６４４ａ回避計画算出手段、６５０空間情報記憶手段、６５１位置・姿勢情報記憶手段、６５２死角情報記憶手段、６５３計画記憶手段。

Claims

障害物が存在する空間において制御体を当該制御体の現在位置から目標位置まで移動させる移動制御システムであって、
前記制御体に設置されて前記制御体の周囲に現れた障害物を検出する障害物検出手段の死角領域と、前記制御体と、の位置関係を記憶している死角情報記憶手段と、
前記空間において既知の障害物が存在しない移動可能領域を記憶している空間情報記憶手段と、
前記現在位置から前記目標位置に至る前記移動可能領域内を通る最短経路を算出し、当該最短経路に基づいて主経路を算出する移動計画算出手段と、
を備え、
前記移動計画算出手段は、前記最短経路上の点であって当該点に移動した前記制御体の前記死角領域に前記最短経路上の他点を含む不良点を検出して、前記最短経路における前記不良点から前記目標位置に至る区間の一部又は全部を修正し、前記最短経路上の任意点に移動した前記制御体の前記死角領域に経路上の他点を含まないように前記最短経路を修正することで主経路を算出することを特徴とする移動制御システム。
請求項１に記載の移動制御システムであって、
前記移動計画算出手段は、前記主経路上の点の候補である候補点と当該候補点における前記制御体の姿勢の候補である候補姿勢を組み合わせた仮説を複数設定し、前記仮説ごとに前記候補点における前記候補姿勢となっている前記制御体の前記死角領域を通過せずに移動可能な他の前記仮説の前記候補点との間の局所経路を検出し、前記局所経路を連結して前記主経路を算出する、移動制御システム。
請求項２に記載の移動制御システムであって、
前記移動計画算出手段は、前記最短経路から所定距離内の前記候補点を有する前記仮説を設定する、移動制御システム。
請求項１～３のいずれか１項に記載の移動制御システムであって、
前記障害物検出手段が前記主経路近傍の障害物を検出した場合に、前記空間内の当該障害物から所定距離以上離れた位置を終端とし、前記現在位置から前記終端に至る前記移動可能領域内及び前記障害物検出手段によって検出された障害物が存在しない領域を通る経路であって、当該経路上の任意点に移動した前記制御体の前記死角領域に当該経路上の他点を含まない回避経路を算出する回避計画算出手段をさらに備え、
前記障害物検出手段が前記主経路近傍の障害物を検出した場合に、前記主経路の追従移動を停止させて前記回避経路に沿って前記制御体を追従移動させること、
を特徴とする移動制御システム。
障害物が存在する空間において制御体を移動させる移動制御システムであって、
前記制御体に設置されて前記制御体の周囲に現れた障害物を検出する障害物検出手段の死角領域と、前記制御体と、の位置関係を記憶している死角情報記憶手段と、
前記空間において既知の障害物が存在しない移動可能領域を記憶している空間情報記憶手段と、
前記障害物検出手段が障害物を検出した場合に、前記空間内の当該障害物から所定距離以上離れた位置を終端とし、前記制御体の現在位置から前記終端に至る前記移動可能領域内及び前記障害物検出手段によって検出された障害物が存在しない領域を通る最短経路を算出し、当該最短経路に基づいて回避経路を算出する回避計画算出手段と、
を備え、
前記回避計画算出手段は、前記最短経路上の点であって当該点に移動した前記制御体の前記死角領域に前記最短経路上の他点を含む不良点を検出して、前記最短経路における前記不良点から前記目標位置に至る区間の一部又は全部を修正し、前記最短経路上の任意点に移動した前記制御体の前記死角領域に経路上の他点を含まないように前記最短経路を修正することで前記回避経路を算出し、
前記障害物検出手段が障害物を検出した場合に前記回避経路に沿って前記制御体を追従移動させることを特徴とする移動制御システム。