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

Description

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

従来、現在位置である自己位置から所定の移動目標位置に至る移動経路を算出し、当該移動経路に沿うよう自律的に飛行可能な自律飛行ロボットが提案されている。例えば、特許文献１には、賊等の移動物体から所定の離間距離だけ離れた位置に移動目標位置を設定し、自己位置から当該移動目標位置に至る移動経路を生成し、当該移動経路に沿うよう飛行制御される自律飛行ロボットが開示されている。

上記従来技術では、移動物体をできるだけ色々な角度から撮影できるよう、移動物体から所定距離だけ離れた周囲の位置に複数の移動目標位置の候補位置（移動候補位置）を設定し、これまで移動物体を撮影できていない方向を撮影できる位置が、移動目標位置として設定されやすいように工夫されている。

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

ところで、複数の移動物体が監視エリアに存在しているときに、撮影された画像から監視員が監視エリアの状況把握したい場合や、これらの移動物体を短期間で画像認識したい場合など、いずれもできる限り多くの移動物体を認識可能な解像度で同時撮影することが要求される。しかしながら、上記従来技術では、単体の移動物体のみを考慮するだけで複数の移動物体が存在している場面について考慮していないため、このような要求を満たす位置に適切に移動することができなかった。

そこで本発明は、複数の移動物体が存在する監視空間において、移動物体を認識可能な解像度でできる限り多くの移動物体を同時撮影する位置に移動できるようにすることを目的とする。

本発明の１つの態様は、移動空間内を自律移動し、撮像部により複数の移動物体の撮像画像を取得する自律移動ロボットであって、前記撮像部の視野角を記憶した記憶部と、前記各移動物体の移動物***置を取得する移動物***置取得手段と、前記自律移動ロボットの自己位置を推定する位置推定手段と、前記移動物体の集合であるクラスタの前記移動物***置を包含する最小の円のサイズが前記視野角に応じた上限サイズ以下となるようクラスタリングし、所定基準に従って該クラスタの中から撮像対象とする撮像クラスタを選択し、該撮像クラスタのクラスタサイズと前記視野角とに基づいて前記撮像部の撮像範囲内に該撮像クラスタが含まれる移動目標位置を設定し、前記自己位置から当該移動目標位置に至る移動経路を求める経路探索手段と、前記移動経路に沿って移動するよう制御する移動制御手段と、を有することを特徴とする自律移動ロボットである。

ここで、前記記憶部は、前記撮像部の解像度を更に記憶し、前記経路探索手段は、前記視野角及び前記解像度に応じた前記上限サイズを設定することが好適である。

また、前記経路探索手段は、前記撮像クラスタのクラスタサイズ、前記視野角及び前記解像度に応じた前記撮像クラスタからの離間距離範囲を求め、該撮像クラスタから当該離間距離範囲内に前記移動目標位置を設定することが好適である。

また、前記経路探索手段は、予め定めた離間距離及び前記視野角に応じた撮像可能範囲を前記上限サイズに設定して前記複数の移動物体をクラスタリングすることが好適である。

また、前記経路探索手段は、前記複数の移動物体の中から優先撮影対象を選択し、該優先撮影対象に選択された移動物体が含まれる前記クラスタを撮像クラスタとして選択することが好適である。

本発明によれば、複数の移動物体が存在する監視空間において、できる限り多くの移動物体を同時撮影する位置に移動することができる。

本発明の実施の形態における自律飛行ロボットの構成を示す図である。 本発明の実施の形態における自律飛行ロボットシステムの構成を示す図である。 本発明の実施の形態における自律飛行ロボットの構成を示す機能ブロック図である。 本発明の実施の形態における経路探索処理のフロー図である。 本発明の実施の形態における上限サイズの算出を説明する図である。 本発明の実施の形態におけるクラスタリングを説明する図である。 本発明の実施の形態における離間距離範囲を説明する図である。 本発明の実施の形態における移動経路の生成を説明する図である。 本発明の実施の形態におけるローカル目標の算出処理を説明する図である。

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

自律飛行ロボット１は、図２のシステム構成図に示すように、外部の警備センタ１００や管理装置１０２と通信し、移動空間内に存在する複数の移動物体Ｍを自律移動して撮像するように構成されている。移動物体Ｍは、例えば、監視領域内に侵入した人物（賊等）や車両等である。

警備センタ１００と管理装置１０２とはインターネット等の情報通信網１１０を介して情報伝達可能に接続される。また、自律飛行ロボット１と管理装置１０２は、無線通信等によって情報伝達可能に接続される。警備センタ１００は、管理装置１０２を介して自律飛行ロボット１と通信を行い、自律飛行ロボット１によって撮像された移動物体Ｍの撮像画像を受信する。警備センタ１００は、撮像画像に対して画像処理を行い、警備センタ１００にて異常監視している管理者等（図示しない）に警告を発するような機能を備えていてもよい。

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

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

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

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

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

位置検出センサ６は、自律飛行ロボット１の現在位置を取得するためのセンサである。位置検出センサ６は、例えば、ＧＮＳＳ（Global Navigation Satellite System）等の航法衛星（人工衛星）から送信される電波（航法信号）を受信する。位置検出センサ６は、複数の航法衛星から送信される航法信号を受信して制御部７へ入力する。なお、位置検出センサ６は、レーザスキャナ、ジャイロセンサ、電子コンパス、気圧センサ等の他のセンサを用いて既知の技術により自己位置を得るための情報を取得するものとしてもよい。

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

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

移動物***置８１は、管理装置１０２から受信した各移動物体Ｍの識別子である移動物体ＩＤと、当該移動物体Ｍの位置情報（座標：Ｘ_t，Ｙ_t，Ｚ_t）とを対応付けた情報である。本実施の形態では、移動物***置８１を各移動物体Ｍの足元位置、すなわち移動物体Ｍが地面（床面）に接している位置とする。制御部７は、通信部９を介して各移動物体Ｍの移動物体ＩＤ及び位置を受信すると移動物***置８１として記憶部８に記憶させる。

クラスタ情報８２は、移動物体Ｍの集合であるクラスタに関する情報であり、クラスタの識別子であるクラスタＩＤと、クラスタを構成する移動物体Ｍの移動物体ＩＤと、クラスタの大きさを表す値であるクラスタサイズと、クラスタの移動空間における位置を表すクラスタ位置と、を対応付けた情報である。本実施の形態では、クラスタを構成する各移動物体Ｍの移動物***置８１を包含する最小の円を求め、当該円の半径をクラスタサイズとし、当該円の地表面における中心位置の座標（Ｘ，Ｙ）をクラスタ位置として、後述するように経路探索手段７３によって算出されるものとする。

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

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

撮像条件情報８４は撮像部３の視野を表す情報であり、本実施の形態では撮像部３の視野角φ及び解像度が撮像条件情報８４として記憶部８に記憶されている。すなわち、撮像条件情報８４は、自律飛行ロボット１に設置した撮像部３の性能にしたがって管理者等によって適宜設定される値である。なお、視野角φを変更可能なズーム機能や、撮像する解像度を変更可能な解像度変更機能を備えた撮像部３を搭載した他の実施の形態においては、適宜現在の視野角φ及び解像度についての情報を取得し、撮像条件情報８４として記憶部８に記憶しもよい。

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

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

具体的には、位置検出センサ６から得られた複数の航法衛星からの航法信号に基づいて周知技術によって推定した緯度・経度と、距離検出センサ５から得られた高度とから自己位置の座標（Ｘ_s，Ｙ_s，Ｚ_s）を計算する。さらに、電子コンパスやジャイロセンサなどの位置検出センサ６からの出力を受けて自己位置として姿勢ＹＡＷを求める。なお、自己位置の推定方法はこれに限定されるものではなく、他の方法を用いて自律飛行ロボット１の現在位置を推定してもよい。

位置推定手段７１は、推定された自己位置（座標：Ｘ_s，Ｙ_s，Ｚ_s及び姿勢ＹＡＷ）と管理装置１０２から受信した移動物体Ｍの位置（座標：Ｘ_t，Ｙ_t，Ｚ_t）を経路探索手段７３へ出力する。また、位置推定手段７１は、推定された自己位置を速度推定手段７２及び移動制御手段７４へ出力する。

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

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

経路探索手段７３は、位置推定手段７１で推定された自己位置と、記憶部８に記憶された移動物***置８１及び各種情報とを用いて、自己位置から所定の移動目標位置に至る移動経路を算出する経路探索処理を行う。以下、本実施の形態の自律飛行ロボット１に係る経路探索手段７３が実行する経路探索処理の流れの一例について、図４を参照しながら詳細に説明する。

経路探索処理では、まず、後述するクラスタリングにおけるクラスタの上限サイズを決定する処理を行う（ＳＴ１）。経路探索手段７３は、撮像条件情報８４の視野角φと解像度とに基づいて、移動物体Ｍを認識可能となるよう、クラスタの上限サイズを算出する。本実施の形態では、まず、解像度に基づいて移動物体Ｍからの最大離間距離Ｌ_maxを求める。すなわち、解像度が高くなるほど移動物体Ｍから遠くに離れても当該移動物体Ｍを認識でき、反対に解像度が低くなるほど移動物体Ｍに近づかなければ当該移動物体Ｍを認識できないため、解像度が高くなるほど、大きくなるよう最大離間距離Ｌ_maxを求める。最大離間距離Ｌ_maxは、予め実験により距離と解像度との関係を求めておき、適用する解像度に応じて求めてもよい。そして、求めた最大離間距離Ｌ_maxと視野角φとを用いて上限サイズｒ_maxを、ｒ_max＝｛Ｌ_max×ｓｉｎ（φ／２）｝／｛ｓｉｎ（φ／２）＋１｝から求める。すなわち、図５に表すように、自律飛行ロボット１から最大離間距離Ｌ_maxの位置に存在していると仮定した移動物体Ｍ₀を、クラスタの最遠端の位置に存在する移動物体とし、当該移動物体Ｍ₀の位置を最遠端位置とする円であって、視野角φで定まる撮像範囲に接する円の半径として、上限サイズｒ_maxを求める。なお、本実施の形態では、上限サイズを撮像部３の視野角φと解像度に基づいて算出しているが、これに限らず、予め実験により定めた固定値を設定し、当該固定値を用いてもよい。

次に、経路探索処理では、ＳＴ１にて求めた上限サイズｒ_maxと、移動物***置８１とを用いて、複数の移動物体Ｍの組み合わせからなるクラスタを生成するクラスタリングを実行する（ＳＴ２）。本実施の形態では、既知の技術であるユークリッドクラスタリング手法を用いてクラスタリングする。この際、上限サイズｒ_maxが、生成されるクラスタの最大半径となるよう、ユークリッドクラスタリング距離を設定する。なお、クラスタリングの方法は従来から多数提案されており、適宜公知の方法を採用すればよい。これによって、上限サイズｒ_max以下の半径からなる円に含まれる位置に存在する複数の移動物体Ｍからなるクラスタを生成することが可能となる。経路探索手段７３は、クラスタリングによってクラスタを生成すると、各クラスタの情報をクラスタ情報８２として記憶部８に記憶する。図６は、移動空間の一部を真上から見下ろしたときの図であり、移動空間内に存在する移動物体Ｍ１〜Ｍ６を上限サイズｒ_maxによりクラスタリングした結果を表す図である。同図では、クラスタＣ１として移動物体Ｍ１〜Ｍ４がクラスタリングされ、クラスタＣ２として移動物体Ｍ５、Ｍ６がクラスタリングされている。なお、クラスタＣ１、Ｃ２の半径であるクラスタサイズは、上限サイズｒ_max以下となるようクラスタリングされているものとする。

次に、経路探索処理では、ＳＴ２にて生成したクラスタの中から、自律飛行ロボット１の撮像対象とするクラスタである撮像クラスタを選択する処理を行う（ＳＴ３）。本実施の形態では、位置推定手段７１にて求めた自己位置に最も近いクラスタを撮像クラスタとして選択する。ここでは、図６のクラスタＣ１を選択クラスタに設定したとして、以下では説明する。

次に、経路探索処理では、ＳＴ３にて選択した撮像クラスタのクラスタサイズをクラスタ情報８２から読み出し、当該クラスタサイズと撮像条件情報８４とに基づいて撮像クラスタからの離間距離の範囲を示す離間距離範囲を求める（ＳＴ４）。ここで、離間距離とは、選択クラスタに追従飛行するにあたって、自律飛行ロボット１と撮像クラスタとの水平面における維持すべき相対距離である。本実施の形態では、図７に表すように、撮像クラスタＣ₁のクラスタサイズをｒ₁としたとき、当該撮像クラスタＣ₁からの最も小さい離間距離である最小離間距離Ｌ_minを、Ｌ_min＝ｒ₁／｛ｓｉｎ（φ／２）｝により求める。すなわち、撮像部３の撮像範囲内に撮像クラスタが含まれる最も近い距離として最小離間距離Ｌ_minを求める。そして、当該最小離間距離Ｌ_minと、ＳＴ１にて求めた最大離間距離Ｌ_maxとを用いて、撮像クラスタからの離間距離範囲を求める。

次に、経路探索処理では、ＳＴ４にて求めた離間距離範囲内の離間距離を設定する（ＳＴ５）。本実施の形態では、予め設定された方針に従って離間距離を選択する。例えば、移動物体Ｍの詳細を撮影するために、移動物体Ｍにできるだけ近づく方針が設定されていれば、離間距離範囲における最も小さい値である最小離間距離Ｌ_minを離間距離として設定する。一方、移動物体Ｍからの攻撃を回避しやすいよう、移動物体Ｍからできるだけ距離をとる方針が設定されていれば、離間距離範囲における最も大きい値である最大離間距離Ｌ_maxを離間距離として設定する。この他にも、現状に応じて方針を変更してもよい。例えば、撮像クラスタを構成する移動物体Ｍの平均移動速度が大きいほど、離間距離範囲内における大きい離間距離として設定してもよい。いずれの場合も、ＳＴ４にて求めた離間距離範囲内の離間距離であれば、撮像部３の撮像範囲内に撮像クラスタが含まれる位置を求めることができる。

次に、経路探索処理では、ＳＴ５にて設定した離間距離と、クラスタ情報８２から読み出した撮像クラスタの中心位置とを用いて、撮像クラスタから離間距離だけ離れた周囲の位置に移動候補位置を設定する処理を行う（ＳＴ６）。すなわち、撮像クラスタの中心位置から離間距離だけ離れた周囲の領域に複数の移動候補位置を設定する。本実施の形態では、移動候補位置を、撮像クラスタの中心位置から、ＳＴ５にて設定した離間距離を半径とした円状の位置で、かつ、管理者等により予め定められた飛行高度（例えば３ｍ）の領域に、それぞれ等間隔になるように設定する。

次に、経路探索処理では、ＳＴ６にて設定した移動候補位置の其々を評価し、最も高い評価値となる移動候補位置を移動目標位置として設定する処理を行う（ＳＴ７）。この際、まず、自律飛行ロボット１が障害物に接触することを防止するため、ボクセル情報８３を参照して占有ボクセル又は近接ボクセルに含まれる位置には移動目標位置を設定しないよう、最低の評価値となるようにする。また、自律飛行ロボット１の自己位置から各移動候補位置までの距離が小さいほど評価値が高くなるようにする。更に、前回設定した移動目標位置から移動候補位置までの距離が小さいほど評価値が高くなるようにする。

次に、経路探索処理では、位置推定手段７１で推定された自己位置と、ＳＴ７にて設定した移動目標位置とを用いて自律飛行ロボット１の移動経路を算出する移動経路生成処理を行う（ＳＴ８）。具体的には、移動経路生成処理では、まず、記憶部８からボクセル情報８２を読出し、分割した移動可能な空間である自由ボクセル及び近接ボクセルの各ボクセルの中心をノードとし、当該ノードに隣接するノード間を連結した線分をエッジとしてグラフ構造を生成する。この際、エッジの重みとして、隣接するノード間の距離に基づいて求められる距離コストが設定されているものとする。このようにボクセル情報８２に基づいて全ての移動可能なボクセルについてグラフ構造を生成した後、自律飛行ロボット１が存在しているボクセルのノードから移動目標位置が存在しているボクセルのノードまでの移動経路を生成する。移動経路の生成方法については、さまざまな経路生成方法が適用可能であるが、本実施の形態ではＡ＊（エースター）経路探索法を用いて移動経路を探索する。この際、ボクセル情報８２として記憶されたボクセルコストと距離コストとを、Ａ＊経路探索法における移動コストとして利用する。経路探索手段７３で生成された生成された移動経路のデータは、経由点（ｘ，ｙ，ｚ）の集合データであり、この情報は記憶部８に一時的に記憶される。図８は、移動候補位置を設定してから移動経路を生成するまでの概略を模式的に表した図である。同図では、撮像クラスタＣ１の中心位置Ｏ１から、ＳＴ５にて設定された離間距離Ｒ１だけ離れた位置に、等間隔に複数の移動候補位置Ｐｃが設定されている。そして、これらの移動候補位置Ｐｃの中から、移動目標位置としてＰｏが設定され、自己位置から当該移動目標位置Ｐｏに至る移動経路Ｗ０が生成されている。

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

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

次に、経路追従制御では、算出したローカル目標に向かって飛行するようＸ、Ｙ、Ｚ、ヨー角の各方向毎に制御指令値ｕ_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・ｖ_yawの式のようなＰＤ制御により求める。なお、本実施の形態では、目標角度ψ'を撮像クラスタＣ１の中心位置Ｏ１の方向を向く角度とした。

このように、制御部７は、上述した位置推定手段７１、速度推定手段７２、経路探索手段７３、移動制御手段７４における各処理を逐次繰り返す。これにより、本実施の形態の自律飛行ロボット１は、撮像クラスタＣ１から離間距離だけ離れた位置に対して移動目標位置Ｐｏを逐次設定・更新し、その都度移動経路についても逐次設定・更新していくことによって、撮像クラスタに追従移動することができる。特に本実施の形態における自律飛行ロボット１は、撮像クラスタのクラスタサイズと視野角φとに基づいて撮像部３の撮像範囲内に当該撮像クラスタが含まれる位置を移動目標位置Ｐｏに設定している。これにより、複数の移動物体が存在する移動空間において、多くの移動物体Ｍを同時撮影する位置にて追従移動することができる。

また、本実施の形態における自律飛行ロボット１は、経路探索処理（ＳＴ１）にて、撮像条件情報８４の視野角φと解像度とに基づいて、移動物体Ｍを認識可能となるよう、クラスタの上限サイズを算出している。これにより、たとえ移動空間内に複数の移動物体Ｍが広く分散して位置していたとしても、クラスタリングによって生成されるクラスタのサイズを、移動物体Ｍを認識可能なサイズに設定することができ、撮像クラスタを構成する複数の移動物体Ｍを認識可能な解像度により、できる限り多くの移動物体を同時撮影しながら追従移動することができる。

また、本実施の形態における自律飛行ロボット１は、経路探索処理（ＳＴ４）にて、撮像クラスタのクラスタサイズと視野角φと解像度とに応じて撮像クラスタからの離間距離の範囲を示す離間距離範囲を求め、撮像クラスタから当該離間距離範囲以内の離間距離の位置に移動目標位置を設定している。これにより、クラスタサイズが様々であるクラスタの中から選択した撮像クラスタを、撮像部３の撮像範囲内に含めることができる位置であって、撮像クラスタを構成する複数の移動物体Ｍを認識可能な解像度により、できる限り多くの移動物体を同時撮影できる位置を正確に求めることができる。

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

上記実施の形態における経路探索処理では、ＳＴ１にてクラスタの上限サイズを決定し、ＳＴ４にて当該クラスタ（撮像クラスタ）が撮像範囲内に含まれる離間距離範囲を求めて、ＳＴ５にて当該離間距離範囲内の距離値として離間距離を設定している。しかし、これに限らず、離間距離を予め固定値として管理者等により記憶部８に設定しておき、選択クラスタの位置（例えば中心位置）から当該設定された離間距離だけ離れた位置に移動候補位置を設定してもよい。この場合、経路探索手段７３は、経路探索処理（ＳＴ１）にて、自己位置から予め設定した離間距離だけ離れた位置にクラスタの中心位置があり、視野角φに基づいて求めた撮像部３の撮像可能範囲内に収まる最大のクラスタサイズを、上限サイズとして求めるものとする。そして、当該上限サイズを用いてクラスタリング（ＳＴ２）して求めたクラスタの中から撮像クラスタを選択し（ＳＴ３）、当該撮像クラスタから設定された離間距離だけ離れた位置に移動候補位置を設定する（ＳＴ６）。このように、離間距離が固定値であったとしても、複数の移動物体が存在する移動空間において、多くの移動物体Ｍを同時撮影する位置にて追従移動することができる。

また、上記実施の形態における経路探索処理では、ＳＴ３にて撮像クラスタを選択する際、自己位置に最も近いクラスタを撮像クラスタとして選択している。しかし、これに限らず、複数の移動物体Ｍの中から優先撮像対象となる移動物体Ｍを選択し、当該優先撮像対象として選択された移動物体Ｍを含んだクラスタを撮像クラスタとして選択してもよい。例えば、移動物体検出センサ１０４によって最初に検出した移動物体Ｍを優先撮像対象として設定し、当該優先撮像対象を含むクラスタを撮像クラスタとして選択する。または、検出した移動物体Ｍの中から所定の画像特徴を有する移動物体Ｍ（例えば、武器等を所持している移動物体Ｍや、マスクをしている移動物体Ｍ）を検出し、当該移動物体Ｍを優先撮像対象として設定して、当該優先撮像対象を含むクラスタを撮像クラスタとして選択してもよい。

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

また、自律飛行ロボット１に撮像部３としてカラーカメラの代わりに距離画像センサを搭載して、当該距離画像センサから取得した距離画像を用いて、既知の移動物体抽出技術により移動物体Ｍを抽出して、抽出した移動物体Ｍと自律飛行ロボット１との距離値と自己位置とから移動物体Ｍの位置を推定する画像解析手段（移動物***置取得手段）を備えてもよい。また、自律飛行ロボット１にレーザスキャナを搭載し、位置推定手段７１（移動物***置取得手段）が、当該レーザスキャナの出力値と自己位置とを用いて移動物体Ｍの位置を推定してもよい。

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

１・・・自律飛行ロボット
２・・・ロータ
３・・・撮像部
４・・・モータ
５・・・距離検出センサ
６・・・位置検出センサ
７・・・制御部
８・・・記憶部
９・・・通信部
７１・・・位置推定手段
７２・・・速度推定手段
７３・・・経路探索手段
７４・・・移動制御手段
８１・・・移動物***置
８２・・・クラスタ情報
８３・・・ボクセル情報
８４・・・撮像条件情報
１００・・・警備センタ
１０２・・・管理装置
１０４・・・移動物体検出センサ
１１０・・・情報通信網
Ｍ・・・移動物体

Claims (5)

1. 移動空間内を自律移動し、撮像部により複数の移動物体の撮像画像を取得する自律移動ロボットであって、
   前記撮像部の視野角を記憶した記憶部と、
   前記各移動物体の移動物***置を取得する移動物***置取得手段と、
   前記自律移動ロボットの自己位置を推定する位置推定手段と、
   前記移動物体の集合であるクラスタの前記移動物***置を包含する最小の円のサイズが前記視野角に応じた上限サイズ以下となるようクラスタリングし、所定基準に従って該クラスタの中から撮像対象とする撮像クラスタを選択し、該撮像クラスタのクラスタサイズと前記視野角とに基づいて前記撮像部の撮像範囲内に該撮像クラスタが含まれる移動目標位置を設定し、前記自己位置から当該移動目標位置に至る移動経路を求める経路探索手段と、
   前記移動経路に沿って移動するよう制御する移動制御手段と、
   を有することを特徴とする自律移動ロボット。
2. 前記記憶部は、前記撮像部の解像度を更に記憶し、
   前記経路探索手段は、前記視野角及び前記解像度に応じた前記上限サイズを設定する請求項１に記載の自律移動ロボット。
3. 前記経路探索手段は、前記撮像クラスタのクラスタサイズ、前記視野角及び前記解像度に応じた前記撮像クラスタからの離間距離範囲を求め、該撮像クラスタから当該離間距離範囲内に前記移動目標位置を設定する請求項２に記載の自律移動ロボット。
4. 前記経路探索手段は、予め定めた離間距離及び前記視野角に応じた撮像可能範囲を前記上限サイズに設定して前記複数の移動物体をクラスタリングする請求項１に記載の自律移動ロボット。
5. 前記経路探索手段は、前記複数の移動物体の中から優先撮影対象を選択し、該優先撮影対象に選択された移動物体が含まれる前記クラスタを撮像クラスタとして選択する請求項１〜請求項４のいずれか一項に記載の自律移動ロボット。
   
   

Images