JP6469492B2

JP6469492B2 - 自律移動ロボット

Info

Publication number: JP6469492B2
Application number: JP2015061642A
Authority: JP
Inventors: 晴之天本
Original assignee: Secom Co Ltd
Current assignee: Secom Co Ltd
Priority date: 2015-03-24
Filing date: 2015-03-24
Publication date: 2019-02-13
Anticipated expiration: 2035-03-24
Also published as: JP2016181177A

Description

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

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

例えば、特許文献１には、レーザスキャナによって検出した距離情報と地図情報とに基づいて現時点の自己位置を推定し、当該自己位置から所定の移動目標位置（例えば、進入者から所定距離離れた位置）に至る最短の移動経路を算出し、当該移動経路に沿うよう飛行制御することにより自律飛行する自律飛行ロボットが開示されている。

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

しかしながら、現時点の自己位置から移動目標位置までの距離が離れている場合、一般的に移動経路を生成するための計算量が増大するため、移動経路が生成されるまでの時間的な遅れ（タイムラグ）が生じる。このタイムラグにより、移動経路の生成開始時点における自己位置と、移動経路が生成された直後の時点の自己位置との間に位置のずれが生じ、この位置のずれがスムーズな飛行を妨げるという問題があった。すなわち、移動経路が生成された直後の時点の自己位置から、生成された移動経路におけるスタート位置（移動経路の生成開始時点における自己位置に等しい）に移動しようとするため、移動経路の生成後に僅かな「戻り」移動が生じ、これがスムーズな移動を妨げる原因となっていた。この問題は、特に、自律移動ロボットを高速に移動させているときに顕著な問題となって現れていた。

本発明は、移動経路の生成処理によるタイムラグの影響を考慮した経路生成を行うことにより、自律移動ロボットのスムーズな移動を可能とすることを目的とする。

本発明の１つの態様は、移動空間内を自律的に移動する自律移動ロボットであって、自律移動ロボットの現在の自己位置を推定する位置推定手段と、自律移動ロボットの現在の移動速度を推定する速度推定手段と、前記移動空間内における移動経路を生成する経路探索手段と、前記移動経路に基づいて移動制御する移動制御手段と、を備え、前記経路探索手段は、前記自己位置と前記移動速度との関係から所定の未来時刻における予想到達位置を求め、当該予想到達位置から移動目標位置に至るように前記移動経路を生成することを特徴とする自律移動ロボットである。

ここで、前記経路探索手段は、前記自己位置と前記移動目標位置との間の距離が大きいほど、より未来の時刻となるように前記未来時刻を設定することが好適である。

また、前記移動空間を所定単位の大きさの単位空間によって分割したときに、前記単位空間のそれぞれを１つのノードとして移動可能な隣接するノード間をエッジで連結したグラフ構造と、ノードのそれぞれの間の距離に応じて設定された距離コストと、を移動空間グラフ情報として記憶した記憶部をさらに備え、前記経路探索手段は、前記自己位置のノードと前記移動目標位置のノードとの間に存在するノードの数が多いほどより未来の時刻となるように前記未来時刻を設定し、前記予想到達位置のノードから前記移動目標位置のノードに至る経路上にあるノードの前記距離コストの総コスト値から前記移動経路を生成することが好適である。

また、前記経路探索手段は、前記移動経路を生成した直後の時点における前記自己位置と前記予想到達位置との差が所定閾値以上であるとき、前記自己位置から最も近い前記移動経路上の位置である局所目標位置までの局所経路を求め、前記局所経路と前記局所目標位置から移動目標位置までの前記移動経路とを用いて新たな移動経路を生成することが好適である。

本発明によれば、経路生成されるまでのタイムラグによって生ずる移動の起点のずれを抑制し、スムーズに移動することができる自律移動ロボットを提供することができる。

本発明の実施の形態における自律移動ロボットの構成を示す図である。本発明の実施の形態における自律移動ロボットシステムの構成を示す図である。本発明の実施の形態における自律移動ロボットの構成を示す機能ブロック図である。本発明の実施の形態における移動経路の生成に用いられるボクセルのグラフ構造を示す図である。本発明の実施の形態におけるローカル目標の算出処理を説明する図である。本発明の実施の形態における予測到達位置を考慮した自律移動ロボットの移動制御を説明する図である。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

想定タイムラグ８３は、後述する経路探索手段７３における経路探索処理の開始から終了までにかかる想定タイムであって、経路探索処理において生成される移動経路の開始位置となる予想到達位置を算出するために用いられるパラメータである。想定タイムラグ８３は、経路探索手段７３における経路探索処理の開始から終了までの時間とほぼ同等の時間に設定することが好適である。本実施の形態では、想定タイムラグ８３を、予め実験により定めた固定値（例えば、１秒間）に簡易的に設定することとする。

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

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

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

具体的には、位置検出センサ６から得られた複数の航法衛星からの航法信号に基づいて既知の周知技術に基づいて推定した緯度・経度と、距離検出センサ５から得られた高度とから自己位置の座標（Ｘ_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をローカル目標として求める。このように、各時刻においてローカル目標を目指して自律移動ロボット１が移動するよう飛行制御することで、常にローカル目標も移動経路上を移動目標位置Ｐ₀に向かって移動していき、自律移動ロボット１は移動経路に沿って飛行していくことになる。

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

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

従来の自律移動ロボットでは、図６（ｂ）に示すように、経路探索処理を開始する時点の自己位置を起点Ｓ₀として当該起点Ｓ₀から移動目標位置に至る移動経路を生成していたので、経路生成されるまでのタイムラグによって実際に移動を開始する位置Ｓ₁と経路生成の起点Ｓ₀とに誤差が生じていた。したがって、経路生成後に位置Ｓ₁から経路生成開始時の起点Ｓ₀まで自律移動ロボットを復帰させる等の無駄な移動が発生したため、スムーズな移動が妨げられていた。これに対して、本実施形態の自律移動ロボット１では、図６（ｃ）に示すように、経路探索処理を開始する起点Ｓ₀ではなく、自律移動ロボット１の速度を考慮して経路探索処理に費やす時間後に到達しているであろう位置、すなわち現時点から想定タイムラグ８３後の未来時刻における予想到達位置Ｑを用いて移動経路を生成する。これによって、経路生成されるまでのタイムラグによって生ずる実際の移動の起点となる位置Ｓ₁と経路の起点Ｓ₀とのずれによる無駄な移動を抑制でき、自律移動ロボット１をスムーズに移動させることが可能となる。

上記のように、想定タイムラグ８３は、経路探索手段７３における経路探索処理の開始から終了までの時間に自律移動ロボット１が移動する予想到達位置を算出するために用いられる。したがって、経路探索手段７３における経路探索処理に費やされる時間が長くなるほど想定タイムラグ８３をより大きな値となるよう、すなわち予想到達位置を算出するための未来時刻をより未来の時刻となるように設定することが好適である。経路探索処理に費やされる時間は、自律移動ロボット１の自己位置と移動目標位置との間の距離が大きいほど長くなる。そこで、自己位置と移動目標位置との間の距離が大きいほど想定タイムラグ８３を大きい値に設定し、予想到達位置を算出するための未来時刻をより未来の時刻となるように設定することが好適である。ここで、自己位置と移動目標位置との間の距離は直線距離又は水平距離とすればよい。また、経路探索に費やされる時間は、自己位置のノードと移動目標位置のノードとの間に存在するノードの数が大きいほど長くなる。そこで、自己位置のノードと移動目標位置のノードとの間に存在するノードの数が多いほど想定タイムラグ８３を大きい値に設定し、予想到達位置を算出するための未来時刻をより未来の時刻となるように設定することが好適である。ここで、自己位置のノードと移動目標位置のノードとの間に存在するノードの数は、自己位置のノードと移動目標位置のノードとの間を前述のコスト値とは無関係に最短（略直線）で繋ぐ経路に沿ったノード数とすればよい。あるいは、自己位置と移動目標位置との間の距離（直線距離又は水平距離）をＬとし、隣接ノード間の平均距離ｗとしたとき、Ｌ÷ｗから求めた値から、簡易的に自己位置のノードと移動目標位置のノードとの間に存在するノードの数として求めてもよい。

また、経路探索手段７３は、移動経路を生成した直後の時点における自己位置と移動経路を生成する前に設定した予想到達位置との差（各位置間の相違を表す距離値）が所定の基準値（例えば、１ｍ）以上である場合、移動経路を生成した直後の時点における自己位置から最も近い移動経路上の位置である局所的な目標の位置（局所目標位置）までの新たな局所的な移動経路（局所経路）を求めてもよい。

この際、まず、移動制御手段７４での経路追従制御処理におけるローカル目標と同様に、移動経路を生成した直後の時点における自己位置と生成した移動経路との間の局所目標位置を求める。そして、経路探索手段７３において、その局所目標位置を新たな移動目標位置として自己位置との間の局所経路を探索する。そして、移動経路を生成した直後の時点における自己位置から（当該移動経路上に設定された）局所目標位置に至る局所経路と、当該移動経路上の経路であって局所目標位置から移動目標位置に至る経路と、を合わせて新たな移動経路として設定する。その後は、移動制御手段７４によって当該新たな移動経路に沿って自律移動ロボット１を移動制御する。

これにより、実際の移動経路が算出された時点における自己位置と予想到達位置との差異が大きい場合であっても、新たに現在の自己位置から局所目標位置に至る局所経路を生成し、局所目標位置に到達するまでは当該局所経路に沿って移動制御することによってよりスムーズな移動を実現することが可能となる。なお、局所経路を算出するのに要する時間は、自己位置から移動目標位置までの移動経路を算出するのに要する時間と比較して、ごく僅かな時間に過ぎない。そのため、局所経路の算出ではほとんどタイムラグを生じないため前述したようなタイムラグによるスムーズな移動が妨げられるといった問題は生じえない。

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

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

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

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

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

Claims

移動空間内を自律的に移動する自律移動ロボットであって、
自律移動ロボットの現在の自己位置を推定する位置推定手段と、
自律移動ロボットの現在の移動速度を推定する速度推定手段と、
前記移動空間内における移動経路を生成する経路探索手段と、
前記移動経路に基づいて移動制御する移動制御手段と、を備え、
前記経路探索手段は、前記自己位置と前記移動速度との関係から所定の未来時刻における予想到達位置を求め、当該予想到達位置から移動目標位置に至るように前記移動経路を生成するものであって、前記自己位置と前記移動目標位置との間の距離が大きいほど、より未来の時刻となるように前記未来時刻を設定することを特徴とする自律移動ロボット。
移動空間内を自律的に移動する自律移動ロボットであって、
自律移動ロボットの現在の自己位置を推定する位置推定手段と、
自律移動ロボットの現在の移動速度を推定する速度推定手段と、
前記移動空間内における移動経路を生成する経路探索手段と、
前記移動経路に基づいて移動制御する移動制御手段と、
前記移動空間を所定単位の大きさの単位空間によって分割したときに、前記単位空間のそれぞれを１つのノードとして移動可能な隣接するノード間をエッジで連結したグラフ構造と、ノードのそれぞれの間の距離に応じて設定された距離コストと、を移動空間グラフ情報として記憶した記憶部と、を備え、
前記経路探索手段は、前記自己位置と前記移動速度との関係から所定の未来時刻における予想到達位置を求め、当該予想到達位置から移動目標位置に至るように前記移動経路を生成するものであって、前記自己位置のノードと前記移動目標位置のノードとの間に存在するノードの数が多いほどより未来の時刻となるように前記未来時刻を設定し、前記予想到達位置のノードから前記移動目標位置のノードに至る経路上にあるノードの前記距離コストの総コスト値から前記移動経路を生成することを特徴とする自律移動ロボット。
移動空間内を自律的に移動する自律移動ロボットであって、
自律移動ロボットの現在の自己位置を推定する位置推定手段と、
自律移動ロボットの現在の移動速度を推定する速度推定手段と、
前記移動空間内における移動経路を生成する経路探索手段と、
前記移動経路に基づいて移動制御する移動制御手段と、を備え、
前記経路探索手段は、前記自己位置と前記移動速度との関係から所定の未来時刻における予想到達位置を求め、当該予想到達位置から移動目標位置に至るように前記移動経路を生成するものであって、前記移動経路を生成した直後の時点における前記自己位置と前記予想到達位置との差が所定閾値以上であるとき、前記自己位置から最も近い前記移動経路上の位置である局所目標位置までの局所経路を求め、前記局所経路と前記局所目標位置から移動目標位置までの前記移動経路とを用いて新たな移動経路を生成することを特徴とする自律移動ロボット。