JP6527726B2 - 自律移動ロボット - Google Patents

Description

本発明は、移動空間内を自律的に移動する自律移動ロボットに関する。

従来、予め記憶した障害物マップ及び自身に備わる各種センサからのセンサ情報に基づいて自律的に移動可能な自律移動ロボットが提案されている。

例えば、特許文献１には、距離検出センサを用いて測定した周囲の障害物等からの距離と、予め記憶した建物等の障害物を含む障害物マップとに基づいて自己位置を推定する自律飛行ロボットであって、予め記憶していない自動車などの移動障害物により自己位置を推定できないとき、当該移動障害物の少ない位置に上昇することにより自己位置を推定できるようにする自律飛行ロボットが開示されている。

特開２０１４−１４９６２２号公報

しかしながら、上記の従来技術のような距離検出センサを用いて自己位置を推定する方法ではなく、ＧＮＳＳ(Global Navigation Satellite System)等の人工衛星からの電波に基づいて自己位置を推定する方法が適用された場合、自律移動ロボットを上昇させることが必ずしも自己位置の推定精度の向上に寄与するとは限らない。例えば、人工衛星からの電波を遮るような障害物が自律移動ロボットの上部に存在する場合、上昇することが自己位置の推定精度の改善に寄与しないばかりか、上昇することにより当該障害物への衝突の危険性を増すことに繋がることにもなりうる。

本発明は、自己位置の推定精度が悪い場合であっても、過去に自己位置の推定精度が高かった位置まで自律移動することにより、自律移動ロボットが安全に自律移動できるようにすることを目的とする。

本発明の１つの態様は、移動空間内に設定された移動経路に基づいて移動制御する移動制御手段を有する自律飛行型の自律移動ロボットであって、前記移動空間を複数のボクセルに分割し、前記ボクセルの各々について自律移動ロボットの移動の障害となる度合いを示すボクセルコスト値を関連付けて記憶すると共に、自律移動ロボットが現在までに移動した前記移動空間内の座標群からなる移動履歴情報を記憶した記憶部と、自律移動ロボットの自己位置を推定すると共に、該自己位置の推定精度を求め、該自己位置と該推定精度とを関連付けて前記移動履歴情報として記憶する位置推定手段と、前記ボクセルコスト値に基づいて、移動目標位置に到達するまでに通過する前記ボクセルの前記ボクセルコスト値を加算した総コスト値が最も小さくなる前記移動経路を探索する経路探索手段と、を備え、前記移動制御手段は、自律移動ロボットの現在の自己位置における推定精度が第１閾値以下となる推定困難位置であるとき、該推定困難位置における該推定精度が低いほど該推定困難位置に該当する前記ボクセルの前記ボクセルコスト値をより高い値に変更する共に、前記移動履歴情報として記憶されている座標群の中で推定困難位置ではない座標に前記移動目標位置を設定し、前記経路探索手段にて探索された前記移動経路に基づいて自律移動ロボットを復帰制御することを特徴とする自律移動ロボットである。

ここで、前記記憶部は、前記移動履歴情報として前記位置推定手段にて求められた推定精度を各座標に対応付けて記憶し、前記復帰制御は、前記移動履歴情報に記憶されている座標群の中で前記第１閾値より大きい第２閾値以上の推定精度をもつ座標であって前記現在の自己位置から最も近い座標を前記移動目標位置として前記復帰制御することが好適である。

本発明によれば、自己位置の推定精度の悪い位置にあっても過去の移動履歴に応じて推定精度が高い安全な位置まで自律的に移動できる自律移動ロボットを提供することができる。

本発明の実施の形態における自律移動ロボットの構成を示す図である。 本発明の実施の形態における自律移動ロボットシステムの構成を示す図である。 本発明の実施の形態における自律移動ロボットの構成を示す機能ブロック図である。 本発明の実施の形態における移動履歴情報の登録例を示す図である。 本発明の実施の形態における移動経路の生成に用いられるボクセルのグラフ構造を示す図である。 本発明の実施の形態におけるローカル目標の算出処理を説明する図である。 本発明の実施の形態における自己位置の推定精度による移動制御を説明する図である。

本発明の実施の形態における自律移動ロボット１は、図１の概観図に示すように、クアッドロータ型の小型無人ヘリコプタである。なお、本発明の適用範囲は、クアッドロータ型の小型無人ヘリコプタに限定されるものではなく、シングルロータ型の小型無人ヘリコプタや、自律移動する走行型のロボットについても同様に適用することができる。

自律移動ロボット１は、図２のシステム構成図に示すように、外部の警備センタ１００や管理装置１０２と通信し、監視空間（移動空間）内に存在する所定の移動物体を目標対象物Ｍとして追跡し、当該目標対象物Ｍに対して所定の機能を発揮するように構成されている。目標対象物Ｍとなる移動物体は、例えば、監視領域内に侵入した人物（賊等）である。本実施の形態では、所定の機能として、目標対象物Ｍを撮像する機能を例に説明するが、特に限定されるものではなく、目標対象物Ｍに対して音を発したり、発光による警告を行ったりする等の機能であってもよい。

警備センタ１００と管理装置１０２とはインターネット等の情報通信網１１０を介して情報伝達可能に接続される。また、自律移動ロボット１と管理装置１０２は、無線通信等によって情報伝達可能に接続される。

警備センタ１００は、管理装置１０２を介して自律移動ロボット１と通信を行い、自律移動ロボット１によって撮像された目標対象物Ｍの撮像画像を受信する。警備センタ１００は、撮像画像に対して画像処理を行い、警備センタ１００にて異常監視している管理者等（図示しない）に警告を発するような機能を備えていてもよい。また、管理装置１０２から目標対象物Ｍの位置（座標）に関する情報を受信し、当該目標対象物Ｍと自律移動ロボット１によって撮像された撮像画像とを関連付けて管理するというような機能を備えてもよい。

管理装置１０２は、地面や壁面等に設置された固定型の目標対象物検出センサ１０４（１０４ａ，１０４ｂ・・・）を備え、目標対象物Ｍの位置を検知する。目標対象物検出センサ１０４は、例えば、レーザセンサとすることができる。レーザセンサは、一定の角度サンプル間隔の角度毎にレーザを二次元的にスキャンすることによって、地面（又は床面）から一定の高さの水平面における検知範囲内に存在する物体（障害物）との距離情報を極座標値として取得する。レーザセンサは、放射状にレーザ光である探査信号を走査し、物体に反射して戻ってきた探査信号を受信して、送信と受信の時間差から物体までの距離を算出し、レーザセンサの設置位置の座標及び探査信号を送信した方向と算出した距離から当該物体の位置の極座標値を求め、当該極座標値から３次元の直交座標値（Ｘ_t，Ｙ_t，Ｚ_t）を求める。管理装置１０２は、目標対象物検出センサ１０４によって求められた物体の位置を目標対象物Ｍの位置として自律移動ロボット１へ送信する。自律移動ロボット１は、目標対象物Ｍの位置を受信すると、その位置に基づいて移動経路を推定し、当該移動経路に沿って移動する。なお、管理装置１０２は、レーザセンサの検知範囲が重複する領域に存在する目標対象物Ｍの同一性を検証することで複数のレーザセンサの検知範囲に渡る目標対象物Ｍの追跡を行う。すなわち、レーザセンサ１０４ａの検知範囲に存在する目標対象物Ｍがレーザセンサ１０４ｂの検知範囲に移動したとしても、管理装置１０２は、当該目標対象物Ｍが同一の物体であると判定することができる。

以下、図１の概観図及び図３の機能ブロック図を参照して、自律移動ロボット１の構成及び機能について説明する。

自律移動ロボット１は、図１に示すように、４枚のロータ（プロペラ）２（２ａ〜２ｄ）を一平面上に有する。各ロータ２は、バッテリ（二次電池：図示しない）により駆動されるモータ４（４ａ〜４ｄ）を用いて回転させられる。一般的に、シングルロータ型のヘリコプタでは、メインロータによって発生する反トルクをテールロータが生み出すモーメントで相殺することによって方位角を保っている。一方、自律移動ロボット１のようなクアッドロータ型のヘリコプタでは、前後・左右で異なる方向に回転するロータ２を用いることで反トルクの相殺を行っている。そして、各ロータ２の回転数（ｆａ〜ｆｄ）を制御することにより、様々な機体の移動や姿勢の調節を行うことができる。例えば、機体をヨー方向に回転させたいときは、前後のロータ２ａ、２ｃと左右ロータ２ｄ、２ｂの回転数に差を与えればよい。

撮像部３は、例えばレンズなどの光学系および所定画素（例えば６４０×４８０画素）のＣＣＤやＣＭＯＳなどの２次元アレイ素子を有する二次元イメージセンサで構成され、飛行空間の撮像画像を所定の時間間隔で取得するいわゆるカラーカメラである。本実施の形態では、撮像部３は、その光軸が自律移動ロボット１の正面方向を撮像するよう筐体部分に設置され、かつ、水平面（ＸＹ平面）から予め定めた俯角θにより斜め下方の空間を視野角φにおいて撮像するよう設置されている。取得した撮像画像は後述する制御部７に出力され、制御部７により記憶部８に記憶されたり、後述する通信部９を介して管理装置１０２に送信されたりする。

距離検出センサ５は、自律移動ロボット１の周囲に存在する障害物と自律移動ロボット１との間の距離を検出し、センサ検出範囲内に存在する障害物の相対的な位置を取得するセンサである。本実施の形態では、距離検出センサ５としてマイクロ波センサを備える。マイクロ波センサは、空間にマイクロ波を放出し、その反射波を検知することによって、自律移動ロボット１の周囲にある物体を探知し、その物体までの距離を求める。距離検出センサ５は、例えば、自律移動ロボット１の前方に向けて設け、目標対象物Ｍまでの距離を測定するために用いることができる。また、距離検出センサ５は、例えば、自律移動ロボット１の下部に下向きに設け、地面との距離（高度）を測定するために用いることもできる。また、距離検出センサ５は、例えば、撮像部３の画角方向に向けて設け、撮像部３の撮像対象物である目標対象物Ｍまでの距離を測定するために用いることができる。

位置検出センサ６は、自律移動ロボット１の現在位置を取得するためのセンサである。位置検出センサ６は、例えば、ＧＮＳＳ(Global Navigation Satellite System)等の航法衛星（人工衛星）から送信される電波（航法信号）を受信する。位置検出センサ６は、複数の航法衛星（人工衛星）から送信される航法信号を受信して制御部７へ入力する。

なお、位置検出センサ６は、レーザスキャナ、ジャイロセンサ、電子コンパス、気圧センサ等の他のセンサを用いて既知の従来技術により自己位置を得るための情報を取得するものとしてもよい。

通信部９は管理装置１０２との間で、例えば無線ＬＡＮや携帯電話回線等により無線通信するための通信モジュールである。本実施の形態では、撮像部３によって取得した撮像画像を通信部９により管理装置１０２に送信し、当該撮像画像を管理装置１０２から警備センタ１００に送信することにより、警備員等が遠隔から侵入者を監視することを可能にする。また、通信部９は、管理装置１０２から目標対象物Ｍの位置（座標：Ｘ_t，Ｙ_t，Ｚ_t）を受信することにより、後述するような移動経路の設定を可能にする。

記憶部８は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）等の情報記憶装置である。記憶部８は、各種プログラムや各種データを記憶し、制御部７との間でこれらの情報を入出力する。各種データには、移動履歴情報８１、第１閾値８２、第２閾値８３、ボクセル情報８４、移動空間グラフ情報８５等の制御部７の各処理に用いられる各種パラメータ８６、各センサ等の出力値及び撮像画像等が含まれる。

移動履歴情報８１は、過去に自律移動ロボット１が移動した位置の履歴に関する情報である。移動履歴情報８１には、図４に示すように、自律移動ロボット１が過去に移動した位置（座標：Ｘ，Ｙ，Ｚ）とその時刻及びその位置における推定精度が関連付けて登録される。自律移動ロボット１の位置は、後述の位置推定手段７１にて位置検出センサ６において受信した緯度・経度の情報及び距離検出センサ５において測定された高度の情報に基づいて算出される。なお、位置検出センサ６にて受信された高度の情報を用いてもよい。推定精度は、当該位置における自己位置を推定する処理における確からしさを表わす指標である。推定精度は、例えば、位置検出センサ６において補足された衛星数、ＤＯＰ値及び２ｄｒｍｓ値の少なくとも１つに基づいて算出される値を用いることが好適である。推定精度は、自己位置の推定の確度が高いほど高い値となるように設定される。また、位置検出センサ６をレーザスキャナ、ジャイロセンサ、電子コンパス、気圧センサ等の他のセンサとした場合、自己位置の推定精度は、位置検出センサ６の測定の精度と後述する位置推定手段７１における推定処理の精度とを反映した値とすればよい。

第１閾値８２は、自律移動ロボット１の移動制御を行う際に、位置推定手段７１にて推定した自己位置が良好な推定精度とならない位置（以下、「推定困難位置」という）であるか否かを判定するための基準となる閾値である。また、第２閾値８３は、自己位置が良好な推定精度とならない推定困難位置であった場合、自律移動ロボット１を推定精度が高い位置に戻す際の判定に用いるための閾値である。第１閾値８２及び第２閾値８３を用いた処理については後述する。

ボクセル情報８４は、飛行空間をボクセル空間として複数のボクセルに分割して飛行空間の障害物の構造等を表した情報であり、予め管理者等によって設定され記憶部８に記憶される情報である。本実施の形態では、飛行空間を所定の大きさ（例えば１５ｃｍ×１５ｃｍ×１５ｃｍ）のボクセルに等分割し、各ボクセルの識別子であるボクセルＩＤと、飛行空間におけるボクセルの位置（座標）と、ボクセル属性と、ボクセルコスト値とを対応付けてボクセル情報８４として記憶する。ボクセル属性には、建造物等の障害物に位置するボクセルを「占有ボクセル」と定義して、自律移動ロボット１が移動できない空間とする。そして、占有ボクセルの近くに存在する空間に位置するボクセルを「近接ボクセル」、それ以外の自由に飛行可能なエリアに位置するボクセルを「自由ボクセル」として定義する。

各ボクセルには、後述する経路探索手段７３にて移動経路を生成する際に利用できるよう、占有度を示すボクセルコスト値が関連付けて登録される。ボクセルコスト値は、占有ボクセルにおいて最大値をとり、占有ボクセルからの距離が大きくなるほど小さな値となるように設定される。例えば、ボクセルコスト値＝ｅｘｐ｛−λ・（占有ボクセルからの距離）｝の計算式からボクセルコスト値を算出することが好適である。ここでλは実験によって求めたパラメータである。そして、予め定めた閾値以上のボクセルコスト値を有するボクセルを「近接ボクセル」とする。また、ボクセルコスト値が当該閾値よりも小さいボクセルを「自由ボクセル」とし、自由ボクセルとみなされたボクセルのボクセルコスト値を０と設定する。なお、自律移動ロボット１が移動空間における予め定めた移動可能空間の外に出ないようにするため、移動可能空間とその外部との境界となるボクセルを占有ボクセルと設定することが好適である。

移動空間グラフ情報８５は、ボクセル情報８４に基づいて作成された３次元グラフ情報である。具体的には、ボクセル情報８４に基づいて、各ボクセルの中心位置をノードとし、当該ノードに隣接するノード間を連結した線分をエッジとしたグラフ構造からなる情報である。図５は移動空間グラフ情報８５のグラフ構造を説明する図であり、飛行空間におけるボクセルの一部（２７個）を切り欠いたものである。図５において符号Ｂで表す個々の立方体はボクセルを表すものである。また、これらのボクセルＢの中心位置にある黒又はハッチングにて塗りつぶした球はノードであり、ノード間を連結する点線で表示する線分はエッジである。

また、記憶部８には、各種パラメータ８６として離間距離や撮像条件情報等も記憶される。離間距離は、目標対象物Ｍに追従飛行するにあたって、自律移動ロボット１と目標対象物Ｍとの水平面における維持すべき相対距離である。離間距離は、自律移動ロボット１の管理者等によって予め設定される。自律移動ロボット１を用いて所定の目標対象物Ｍを監視する場合、目標対象物Ｍに近づき、より詳細な撮像画像を取得できる必要がある。しかし、侵入者などの敵対する目標対象物Ｍから攻撃を受けないようにするためには一定距離以上離間する必要がある。そのため、本実施の形態の自律移動ロボット１は、目標対象物Ｍの詳細な撮像画像を取得でき、かつ、当該目標対象物Ｍから攻撃を受け難い距離に離間距離を予め、当該離間距離を保ちつつ追従飛行するように制御される。離間距離は、例えば３ｍとして設定される。また、撮像条件情報は、撮像部３の視野を表す情報である。本実施の形態では、撮像部３の俯角θ及び視野角φが撮像条件情報として記憶される。撮像条件情報は、撮像部３の特性及び設置角度等に基づいて管理者等によって適宜設定される。なお、自律移動ロボット１に対して撮像部３の撮像方向を変更可能なカメラ制御装置を搭載した場合の他の実施形態においては、当該カメラ制御装置から撮像方向についての情報を取得するようにしてもよい。

制御部７は、ＣＰＵ等を備えたコンピュータで構成され、位置推定処理、速度推定処理、経路探索処理、移動制御処理（経路追従制御及び復帰制御）を行う一連の処理として、位置推定手段７１、速度推定手段７２、経路探索手段７３、移動制御手段７４を含んでいる。

位置推定手段７１は、位置検出センサ６の出力に基づいて、飛行空間における自律移動ロボット１の現在位置（自己位置）を推定する位置推定処理を行う。

具体的には、位置検出センサ６から得られた複数の航法衛星からの航法信号に基づいて既知の周知技術に基づいて推定した緯度・経度と、距離検出センサ５から得られた高度とから自己位置の座標（Ｘ_s，Ｙ_s，Ｚ_s）を計算する。なお、自己位置の推定方法はこれに限定されるものではなく、他の方法を用いて自律移動ロボット１の現在位置を推定してもよい。

位置推定手段７１は、推定された自己位置（座標：Ｘ_s，Ｙ_s，Ｚ_s）と管理装置１０２から受信した目標対象物Ｍの位置（座標：Ｘ_t，Ｙ_t，Ｚ_t）を経路探索手段７３へ出力する。

また、位置推定手段７１は、推定された自己位置に関連付けて、自己位置を推定したときの時刻及び当該自己位置の推定精度を移動履歴情報８１に追記する。

なお、位置推定手段７１は、目標対象物Ｍの位置に基づいてボクセル情報８４を更新する処理を行う。具体的には、記憶部８のボクセル情報８４に基づいたボクセル空間に目標対象物Ｍの位置を中心として予め定めた目標対象物Ｍの大きさと略同じ大きさの円柱モデル（例えば、監視対象の目標対象物Ｍを侵入者であるとしたとき、底面の半径０．３ｍ、高さ１．７ｍの円柱モデル）を配置し、当該円柱モデルと干渉するボクセルを占有ボクセルとして設定することによりボクセル情報８４を更新する。後述するように、自律移動ロボット１は、占有ボクセルには移動しないように飛行制御されるが、上記のように目標対象物Ｍの位置に基づいてボクセル情報８４を更新することにより、自律移動ロボット１と目標対象物Ｍとの接触を回避することができる。

速度推定手段７２は、後述する移動制御手段７４における移動制御で利用するため、自律移動ロボット１の現在の飛行速度（ｖ_x，ｖ_y，ｖ_z，ｖ_yaw）を推定する処理を行う。本実施の形態では、位置推定手段７１にて推定した自己位置（Ｘ_s，Ｙ_s，Ｚ_s）の時間変化から飛行速度を求める。この際、測定誤差等の影響を考慮して拡張カルマンフィルタを利用して飛行速度を推定することが好適である。

経路探索手段７３は、位置推定手段７１で推定された自己位置及び目標対象物Ｍの位置と、記憶部８に記憶された各種情報とを用いて自律移動ロボット１の移動経路を算出する処理を行う。本実施の形態では、経路探索手段７３は、自己位置から目標対象物Ｍの近傍に設定した移動目標位置に至る移動経路を生成する。

経路探索手段７３は、目標対象物Ｍの位置に基づいて移動目標位置を設定する。本実施の形態では、（目標対象物Ｍの位置の時間変化により求めた）目標対象物Ｍの移動方向を目標対象物Ｍの正面方向として定め、当該正面方向に予め設定した離間距離（３ｍ）だけ水平方向に離れた位置であって高度３ｍの位置を移動目標位置として設定する。そして設定された移動目標位置と、記憶部８に記憶されたボクセル情報８４及び移動空間グラフ情報８５と位置推定手段７１にて算出された自己位置とを用いて自律移動ロボット１の移動経路を算出する経路生成処理を行う。

経路生成処理では、ボクセル情報８４及び移動空間グラフ情報８５を参照し、自己位置に対応するボクセルのノード（以下「スタートノード」という）から移動目標位置に対応するボクセルのノード（以下「ゴールノード」という）に至る総コスト値Ｃが最も小さくなる経路をＡ＊経路探索法により探索する。

Ａ＊経路探索法では、ある評価ノードｎ（ノードＩＤがｎのノード）における総コスト値Ｃ（ｎ）を数式（１）で表わす。

ここで、ｇ（ｎ）は、評価ノードｎについてのＡ＊経路探索法におけるｇコスト値であり、本実施の形態では、スタートノードから評価ノードｎに至るまでの移動距離や障害物への接触危険性を考慮したコスト値として求める。すなわち、評価ノードｎのｇコスト値であるｇ（ｎ）は、隣接ノードにおけるｇコスト値であるｇ（ｎ−１）と、当該隣接ノードから評価ノードに至る距離に基づいて設定された距離コストＣ_M、評価ノードにおけるボクセルコストＣ_v（ｎ）との和とし、ｇ（ｎ）＝ｇ（ｎ−１）＋Ｃ_M＋Ｃ_v（ｎ）により求める。また、ｈ（ｎ）は、評価ノードｎについてのＡ＊経路探索法におけるｈコスト値であり、評価ノードｎからゴールノードに至る距離の推定コスト値である。本実施形態では、評価ノードｎからゴールノードに至る直線距離によりｈ（ｎ）を求める。Ａ＊経路探索法では、スタートノードから順に隣接ノードの総コスト値Ｃ（ｎ）を算出していくことを繰り返し、最終的にゴールノードに至る総コスト値Ｃ（ｎ）が最も小さくなる経路を探索していく。

以下に、Ａ＊経路探索法による移動経路の生成について簡単に説明する。Ａ＊経路探索法では、まず、スタートノードに隣接する一又は複数のノード（隣接ノード）を評価ノードとして設定するところから開始する。そして、それぞれの評価ノードについて式（１）により総コスト値を求める。次に、評価ノードの中で最小の総コスト値となるノードを注目ノードと称したとき、当該注目ノードの（これまで評価ノードと設定されていない新たな）隣接ノードを新たに評価ノードとして追加し、新たに追加された評価ノードに対しても同様に総コスト値を求める。続いて、同様に全ての評価ノードの中で最小の総コスト値となる評価ノードを注目ノードに再設定する。このように、評価ノードの総コスト値に基づいた注目ノードの再設定と、当該再設定に伴う新たな評価ノードの追加及び総コスト値の計算を繰り返し、最終的にゴールノードが注目ノードとして設定されたとき、経路の探索は終了する。

このようにして、起点となるスタートノードから終点となるゴールノードに至る最良の移動経路が生成される。経路探索手段７３で生成された移動経路のデータは、経由点となるノードの位置（ｘ，ｙ，ｚ）の集合データであり、この情報は記憶部８に一時的に記憶される。

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

移動制御手段７４は、経路探索手段７３にて算出された移動経路と位置推定手段７１にて推定された自己位置と速度推定手段７２で推定された飛行速度とを用いて、自律移動ロボット１が経路探索手段７３で算出された移動経路に沿って飛行するように経路追従制御を行う。具体的には、移動経路、自己位置及び飛行速度を用いて各時刻での飛行制御値である速度指令値を求め、当該速度指令値に基づいてモータ４を制御し、ロータ２の回転数を制御する。

経路追従制御では、まず、各時刻での自律移動ロボット１が目標とすべき直近の位置（以下、「ローカル目標」と呼ぶ）を算出する処理を行う。図６はローカル目標の算出を説明する図である。ローカル目標の算出にあたり、移動制御手段７４は、経路探索手段７３で生成された移動経路を記憶部８から読出し、自律移動ロボット１が現在時刻で目指している経由点Ｗ_p1と前回通過済みである経由点Ｗ_p0との２点間を繋げた直線Ｗを求める。そして、移動制御手段７４は、求めた直線Ｗと自律移動ロボット１の自己位置を中心とした球Ｓとの交点Ｌ_p’、Ｌ_pを算出し、目指している経由点Ｗ_p1に近い交点Ｌ_pをローカル目標として求める。このように、各時刻においてローカル目標を目指して自律移動ロボット１が移動するよう飛行制御することで、常にローカル目標も移動経路上を移動目標位置Ｐ_oに向かって移動していき、自律移動ロボット１は移動経路に沿って飛行していくことになる。

次に、経路追従制御では、算出したローカル目標に向かって飛行するようＸ、Ｙ、Ｚ、ヨー角の各方向毎に速度指令値ｕ_x，ｕ_y，ｕ_z，ｕψを算出する処理を行う。この際、現在の自己位置とローカル目標の位置との差異が小さくなるような速度指令値を求める。具体的には、ＸＹＺ軸方向の速度指令値ｕ＝（ｕ_x，ｕ_y，ｕ_z）は、位置推定手段７１で求められた自己位置ｒ＝（Ｘ_s，Ｙ_s，Ｚ_s）と速度推定手段７２で推定した速度ｖ＝（ｖ_x，ｖ_y，ｖ_z）とを利用し、ＰＩＤ制御により求める。ＸＹＺ軸方向の各速度指令値をｕ＝（ｕ_x，ｕ_y，ｕ_z）、ローカル目標をｒ’＝（ｘ，ｙ，ｚ）としたとき、速度指令値は、ｕ＝Ｋ_p（ｒ’−ｒ）＋Ｋ_d・ｖ＋Ｋ_i・ｅの式で算出される。ここで、Ｋ_p、Ｋ_d、Ｋ_iはそれぞれＰＩＤ制御のゲインのことであり、ｅ＝（ｅ_x，ｅ_y，ｅ_z）は誤差の積分値である。一方、ヨー角方向の速度指令値ｕψは、ψ'を目標角度、ψを位置推定手段７１にて推定した自律移動ロボット１の姿勢（角度）、ｖ_yawを速度推定手段７２で推定した角速度とすると、ｕψ＝Ｋ_p（ψ’−ψ）＋Ｋ_d・ｖψの式のようなＰＤ制御により求める。なお、本実施の形態では、目標角度ψ'を目標対象物Ｍの方向、すなわち、目標対象物Ｍの位置の方向を向く角度とした。

このように、制御部７は、上述した位置推定手段７１、速度推定手段７２、経路探索手段７３、移動制御手段７４における各処理を逐次繰り返す。これにより、本実施の形態の自律移動ロボット１は、目標対象物Ｍから離間距離の位置に対して移動目標位置を逐次更新し、その都度移動経路についても逐次更新していくことによって、目標対象物Ｍを適切に追従することができる。

以上のように、自律移動ロボット１は、自己位置から移動目標位置までの経路を探索し、探索された経路に沿って自律的に移動する。しかしながら、位置推定手段７１による移動経路の探索の起点となる自己位置の推定精度が低い場合、その起点を基準として探索された移動経路に沿って自律移動ロボット１が移動した場合に障害物等に衝突するおそれがある。また、経路追従制御においても自己位置を用いて飛行制御しているため、自己位置の推定精度が低い場合、移動経路に適切に追従できないばかりか、障害物等に衝突するおそれもある。

そこで、自律移動ロボット１では、自己位置の推定精度が低い場合には過去の移動履歴に基づいて推定精度が高い位置まで自律移動ロボット１を移動させる復帰制御を行う。以下、図７を参照しつつ、復帰制御について説明する。図７は、ボクセルで表示した飛行空間の一部を真上から見下ろしたときの図である。同図において、黒塗りされたボクセルは占有ボクセルであり、平行斜線（ハッチング）で塗られたボクセルは近接ボクセルであり、白色のボクセルは自由ボクセルである。図７に表すように、目標対象物Ｍから正面方向に離間距離（３ｍ）だけ離れた位置Ｐを移動目標位置に設定されていることとする。

移動制御手段７４は、位置推定手段７１から推定された自己位置の入力を受けると共に、記憶部８に記憶されている移動履歴情報８１から当該自己位置に関連付けて登録されている推定精度を読み出す。移動制御手段７４は、読み出された推定精度が記憶部８に予め記憶されている第１閾値８２と比較し、第１閾値８２以下であれば当該自己位置は推定困難位置であるとして復帰制御を開始する。図７（ａ）では、自律移動ロボット１は、時刻ｔ０における自己位置Ｐ０において生成した移動経路Ｒ１に沿って移動し、その移動中における時刻ｔ１の自己位置にて当該位置が推定困難位置Ｐ１であると判定したことを表している。

復帰制御は、移動履歴情報８１として記憶されている位置（座標）中で推定困難位置ではない所定の位置に自律移動ロボット１を移動させる制御とする。移動制御手段７４は、自律移動ロボット１を移動制御する際に、位置推定手段７１において現在の自己位置の推定に対する推定精度が第１閾値８２以下であれば、移動履歴情報８１から第１閾値８２より大きい第２閾値８３以上の推定精度を有する座標で表される位置（以下、臨時目標位置という）を読み出す。そして、移動制御手段７４は、現在の自己位置から臨時目標位置に至る移動経路に沿って自律移動ロボット１を移動させるように復帰制御を行う。図７（ｂ）では、復帰制御により、推定困難位置Ｐ１から臨時目標位置Ｐ２として設定された位置までの移動経路Ｒ２を求め、当該移動経路Ｒ２に沿うよう移動し、時刻ｔ２において臨時目標位置Ｐ２まで移動したことを表している。

なお、現在の自己位置から臨時目標位置までの移動経路は、上記経路探索手段７３による現在の自己位置から移動目標位置までの移動経路の経路探索処理と同様に行えばよい。すなわち、移動目標位置を臨時目標位置に置き換えて、上記経路探索処理を行うことにより現在の自己位置から臨時目標位置までの移動経路を定めることができる。

また、具体的な移動制御は、移動目標位置までの経路に沿った移動制御と同様にローカル目標を適宜定めながら経路追従制御を行えばよい。

このような構成とすることによって、自律移動ロボット１は、たとえ自己位置の推定精度が悪い位置（推定困難位置）に移動したとしても、過去に移動した位置であって推定精度が第１閾値８２より大きい安全な位置（臨時目標位置）まで自動的に復帰することができる。

また、移動制御手段７４は、移動履歴情報８１として記憶されている位置のうちその推定精度が第１閾値８２を超える第２閾値８３以上となる位置のうち現在の自己位置の座標に最も近い座標の位置を臨時目標位置とすることが好適である。

これにより、自律移動ロボット１は、現在の推定困難位置から、過去に移動した位置の中で推定精度がより高い位置に向かって素早く復帰することができる。

また、移動制御手段７４は、移動履歴情報８１として記憶されている移動履歴を遡るように自律移動ロボット１を推定精度の高い位置に復帰させるように移動制御してもよい。この場合、移動制御手段７４は、移動履歴情報８１を参照して現在の自己位置から過去に遡る移動履歴を読み出す。このとき、移動制御手段７４は、移動履歴を遡りつつ推定精度が初めて第１閾値８２を超える第２閾値８３以上となる臨時目標位置までの移動経路の履歴を読み出す。すなわち、現在の自己位置から過去に移動してきた経路を遡りつつ、移動履歴情報８１として記憶されている座標群の中で第１閾値８２より大きい第２閾値８３以上の推定精度である最も近い座標を臨時目標位置として、現在の自己位置から臨時目標位置までの移動経路履歴を読み出す。そして、移動制御手段７４は、読み出された現在の自己位置から臨時目標位置まで遡る移動経路履歴に沿って自律移動ロボット１を復帰させるように移動制御を行う。

これにより、自律移動ロボット１は、推定困難位置である現在の自己位置からこれまで移動してきた推定精度が第２閾値８３以上の位置であって、安全な移動が確認できている経路に沿って臨時目標位置まで自動的に移動することができる。

また、経路探索手段７３において移動履歴情報８１として記憶されている過去に移動した位置の推定精度を考慮せずに経路探索を行ったが、推定精度を考慮して経路探索を行う構成としてもよい。この場合、移動制御手段７４は、現在の自己位置の推定精度が第１閾値８２以下であれば当該位置に対応するボクセルのボクセルコストを高くする処理を行う。

例えば、推定困難位置を含むボクセルにおける現在のボクセルコスト値にｅｘｐ｛−δ・（推定精度）｝の計算式から算出した値を足すことにより、新たなボクセルコスト値を求めることが好適である。ここでδは実験によって求めたパラメータである。また、推定困難位置を含むボクセルにおける現在のボクセルコスト値を占有ボクセルと同じ最大値に変更してもよい。図７（ｃ）では、自律移動ロボット１は、時刻ｔ３において、推定困難位置Ｐ１を含むボクセルのボクセルコスト値を高め、自由ボクセルから近接ボクセルにボクセル属性を変更していることを表している。また、これにより、新たに生成された移動経路Ｒ３は、推定困難位置Ｐ１の近傍を含まない経路となっていることを表している。

このように、推定精度が低い位置（推定困難位置）におけるボクセルコスト値を高めることによって、経路探索手段７３において移動履歴情報８１に記憶されている座標群の中で推定困難位置である座標の近傍を含まない移動経路を生成することができる。

ところで、本発明は、上記実施形態に限定されるものではなく、特許請求の範囲に記載した技術的思想の範囲内で、更に種々の異なる実施形態で実施されてもよいものである。また、実施形態に記載した効果は、これに限定されるものではない。

上記実施形態では、管理装置１０２に接続された目標対象物検出センサ１０４を用いて目標対象物Ｍを検出している。しかしながら、これに限定されるものではなく、撮像部３で取得した撮像画像を画像解析することにより、目標対象物Ｍの位置を推定してもよい。例えば、撮像画像の各フレームを画像処理して目標対象物の画像領域を抽出する処理を行う。この際、既知の従来技術（特開２００６−１４６５５１号公報を参照）であるオプティカルフロー法、ブースティング学習（例えば、Ｈａａｒ−ｌｉｋｅ特徴を用いているＡｄａＢｏｏｓｔベース識別器による顔検出手法）による識別器等を用いて目標対象物の画像領域（人物領域）を抽出する。次に、当該抽出された画像領域の位置に基づいて目標対象物と自律移動ロボット１との距離を推定する。具体的には、抽出した目標対象物の画像領域の頭頂部の（撮像画像における）ｙ座標位置と距離との対応表を予め飛行高度毎に作成しておき、現在の飛行高度及び目標対象物の頭頂部のｙ座標位置を当該対応表に照らし合わせて自律移動ロボット１との距離を推定する。しかし、これに限らず、抽出した目標対象物の頭部の大きさから距離を算出してもよい。すなわち、頭部の大きさと距離との対応表を予め作成しておき、抽出された目標対象物の頭部の大きさを当該対応表に照らし合わせて自律移動ロボット１との距離を推定してもよい。

また、自律移動ロボット１に撮像部３としてカラーカメラの代わりに距離画像センサを搭載して、当該距離画像センサから取得した距離画像を用いて、既知の移動物体抽出技術により目標対象物を抽出して、抽出した目標対象物と自律移動ロボット１との距離値と自己位置とから目標対象物の位置を推定してもよい。また、自律移動ロボット１にレーザスキャナを搭載し、当該レーザスキャナの出力値と自己位置とを用いて目標対象物の位置を推定してもよい。

また、上記実施形態では、制御部７において位置推定処理、速度推定処理、経路探索処理、移動制御処理（経路追従制御及び復帰制御）の一連の処理を行っている。しかし、これに限らず、図示しない制御用のＰＣを用意し、当該ＰＣにこれらの一連の処理を実施させてもよい。すなわち、自律移動ロボット１は、ＰＣによって行われた位置推定処理、速度推定処理、経路探索処理、移動制御処理によって得られた速度指令値を無線通信又は有線通信によりＰＣから受信し、当該速度指令値に基づいてモータ４の回転数を制御することにより、目的の位置に飛行するようにしてもよい。このように、外部ＰＣを用いて上記の一連の処理を分担することにより、自律移動ロボット１のＣＰＵ処理負荷を低減することができ、ひいてはバッテリの消耗も抑えることができる。

１ 自律移動ロボット、２（２ａ〜２ｄ） ロータ、３ 撮像部、４（４ａ〜４ｄ） モータ、５ 距離検出センサ、６ 位置検出センサ、７ 制御部、８ 記憶部、９ 通信部、７１ 位置推定手段、７２ 経路探索手段、７２ 速度推定手段、７３ 経路探索手段、７４ 移動制御手段、８１ 移動履歴情報、８２ 第１閾値、８３ 第２閾値、８４ ボクセル情報、８５ 移動空間グラフ情報、８６ 各種パラメータ、１００ 警備センタ、１０２ 管理装置、１０４ 目標対象物検出センサ、１１０ 情報通信網。

Claims (2)

1. 移動空間内に設定された移動経路に基づいて移動制御する移動制御手段を有する自律飛行型の自律移動ロボットであって、
   前記移動空間を複数のボクセルに分割し、前記ボクセルの各々について自律移動ロボットの移動の障害となる度合いを示すボクセルコスト値を関連付けて記憶すると共に、自律移動ロボットが現在までに移動した前記移動空間内の座標群からなる移動履歴情報を記憶した記憶部と、
   自律移動ロボットの自己位置を推定すると共に、該自己位置の推定精度を求め、該自己位置と該推定精度とを関連付けて前記移動履歴情報として記憶する位置推定手段と、
   前記ボクセルコスト値に基づいて、移動目標位置に到達するまでに通過する前記ボクセルの前記ボクセルコスト値を加算した総コスト値が最も小さくなる前記移動経路を探索する経路探索手段と、
   を備え、
   前記移動制御手段は、自律移動ロボットの現在の自己位置における推定精度が第１閾値以下となる推定困難位置であるとき、該推定困難位置における該推定精度が低いほど該推定困難位置に該当する前記ボクセルの前記ボクセルコスト値をより高い値に変更する共に、前記移動履歴情報として記憶されている座標群の中で推定困難位置ではない座標に前記移動目標位置を設定し、前記経路探索手段にて探索された前記移動経路に基づいて自律移動ロボットを復帰制御することを特徴とする自律移動ロボット。
2. 請求項１に記載の自律移動ロボットであって、
   前記記憶部は、前記移動履歴情報として前記位置推定手段にて求められた推定精度を各座標に対応付けて記憶し、
   前記復帰制御は、前記移動履歴情報に記憶されている座標群の中で前記第１閾値より大きい第２閾値以上の推定精度をもつ座標であって前記現在の自己位置から最も近い座標を前記移動目標位置として前記復帰制御することを特徴とする自律移動ロボット。