(57)【特許請求の範囲】
【請求項１】
自己位置を推定する自己位置推定装置であって、
航法衛星から受信した信号に含まれる情報を用いて所定の測位開始時刻から所定時間が経過するまでの期間である測位期間において信号を受信可能な複数の航法衛星の組み合わせからなる衛星セットを選択し、該衛星セットからの信号を用いて前記自己位置の候補である候補位置及び前記候補位置の精度を推定する測位処理を実行する複数の測位手段と、
前記精度が所定値以上となる測位手段を選択する選択操作と、該選択した測位手段が推定した前記候補位置を前記自己位置として出力する出力操作とを実行する位置出力手段と、を有し、
前記複数の測位手段のそれぞれは、他の前記測位手段における前記精度が所定値以上となったとき、前記測位処理を実行し直すリセット操作を実行することを特徴とする自己位置推定装置。
【請求項２】
待機状態及び前記自己位置を用いて自律移動する自律移動状態の二つの状態を有する前記自己位置推定装置であって、
前記複数の測位手段のそれぞれは、前記自律移動状態においては前記リセット操作を実行せず、
前記位置出力手段は、前記自律移動状態においては前記選択操作を実行せずに、前記待機状態において選択した測位手段が推定した前記候補位置を前記自己位置として出力する請求項１に記載の自己位置推定装置。
【請求項３】
移動空間内を移動する前記自己位置推定装置であって、
前記移動空間における障害物の位置を示した三次元地図情報を記憶する記憶手段を更に有し、
前記測位手段は、前記航法衛星から受信した信号に含まれる情報と、前記測位期間と、前記三次元地図情報と、を用いて前記衛星セットを選択する請求項１又は請求項２に記載の自己位置推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自らの現在位置である自己位置を出力する自己位置推定装置に関する。
【背景技術】
【０００２】
従前より、様々な分野において汎地球測位航法衛星システム（ＧＮＳＳ：Global Navigation Satellite System）が利用されている。特に近年では、自律的に飛行する自律飛行ロボットの自己位置推定にＧＮＳＳが利用されつつある。例えば、特許文献１には、ＧＮＳＳを用いて自己位置を推定し、飛行ルートを決定して的確に自動的に農薬散布を実施することを可能としたラジコンヘリコプタが提案されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００１−１２０１５１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ＧＮＳＳでは、様々な測位方法が存在するが、高精度な測位が必要とされる場合には、衛星信号の搬送波位相を利用したＲＴＫ（Real Time Kinematic）測位法が利用される。搬送波位相は、単独ＧＰＳ測位などで用いる擬似距離よりも分解度が千倍程度も高いことから、ＲＴＫ測位法を用いた場合の測位精度は数ｍｍ〜数十ｃｍ程度と極めて高い。一方で、ＲＴＫ測位法は、起動時（初期化）や捕捉衛星セットの変更時においては測位の精度が悪化し、時間の経過と共に精度が向上していくといった性質がある。このような精度が悪化している期間を、本明細書では「低精度期間」とよぶ。
【０００５】
このような低精度期間に自律移動すると、人や建物等の障害物への衝突の危険性が高まる。そのため、起動してから測位精度が所定の閾値以上となったときにのみ自律移動を許可することにより、安全性を保つことも考えられる。しかしながら、どのようなタイミングで出動命令がなされるかについて予想できない自律移動ロボットなどでは、出動しなければならないタイミングにおいて、低精度期間中であった場合、低精度期間が終えるまで出動することができないといった問題があった。
【０００６】
そこで、本発明は、複数の測位手段を備え、それぞれの測位手段における低精度期間が重ならないように測位処理を実行することにより、どのタイミングであっても安定した精度の自己位置を得ることができることを目的とする。
【課題を解決するための手段】
【０００７】
本発明の１つの態様は、自己位置を推定する自己位置推定装置であって、航法衛星から受信した信号に含まれる情報を用いて所定の測位開始時刻から所定時間が経過するまでの期間である測位期間において信号を受信可能な複数の航法衛星の組み合わせからなる衛星セットを選択し、該衛星セットからの信号を用いて前記自己位置の候補である候補位置及び前記候補位置の精度を推定する測位処理を実行する複数の測位手段と、前記精度が所定値以上となる測位手段を選択する選択操作と、該選択した測位手段が推定した前記候補位置を前記自己位置として出力する出力操作とを実行する位置出力手段と、を有し、前記複数の測位手段のそれぞれは、他の前記測位手段における前記精度が所定値以上となったとき、前記測位処理を実行し直すリセット操作を実行することを特徴とする自己位置推定装置である。
【０００８】
ここで、待機状態及び前記自己位置を用いて自律移動する自律移動状態の二つの状態を有する前記自己位置推定装置であって、前記複数の測位手段のそれぞれは、前記自律移動状態においては前記リセット操作を実行せず、前記位置出力手段は、前記自律移動状態においては前記選択操作を実行せずに、前記待機状態において選択した測位手段が推定した前記候補位置を前記自己位置として出力することが好適である。
【０００９】
また、移動空間内を移動する前記自己位置推定装置であって、前記移動空間における障害物の位置を示した三次元地図情報を記憶する記憶手段を更に有し、前記測位手段は、前記航法衛星から受信した信号に含まれる情報と、前記測位期間と、前記三次元地図情報と、を用いて前記衛星セットを選択することが好適である。
【発明の効果】
【００１０】
本発明によれば、どのタイミングであっても安定した精度の自己位置を得ることができる。
【図面の簡単な説明】
【００１１】
【図１】本発明の実施形態における自律飛行ロボットの構成を示す図である。
【図２】本発明の実施形態における自律飛行ロボットシステムの構成を示す図である。
【図３】本発明の実施形態における自律飛行ロボットの構成を示す機能ブロック図である。
【図４】本発明の実施形態における位置推定手段の構成を示す機能ブロック図である。
【図５】本発明の実施形態における測位処理の実行タイミングを説明する図である。
【図６】本発明の実施形態におけるローカル目標の算出処理を説明する図である。
【発明を実施するための形態】
【００１２】
本発明の実施の形態における自己位置推定装置は、図１の概観図に示すように、クアッドロータ型の小型無人ヘリコプタであって、自律的に飛行する自律飛行ロボット１である。なお、本発明の適用範囲は、クアッドロータ型の小型無人ヘリコプタに限定されるものではなく、シングルロータ型の小型無人ヘリコプタや、自律移動する走行型のロボットについても同様に適用することができる。
【００１３】
自律飛行ロボット１は、図２のシステム構成図に示すように、外部の警備センタ１００や管理装置１０２と通信し、移動空間内に存在する所定の移動物体を目標対象物Ｍとして追跡し、当該目標対象物Ｍに対して所定の機能を発揮するように構成されている。目標対象物Ｍとなる移動物体は、例えば、監視領域内に侵入した人物（賊等）である。本実施の形態では、所定の機能として、目標対象物Ｍを撮像する機能を例に説明するが、特に限定されるものではなく、目標対象物Ｍに対して音を発したり、発光による警告を行ったりする等の機能であってもよい。
【００１４】
警備センタ１００と管理装置１０２とはインターネット等の情報通信網１１０を介して情報伝達可能に接続される。また、自律飛行ロボット１と管理装置１０２は、無線通信等によって情報伝達可能に接続される。警備センタ１００は、管理装置１０２を介して自律飛行ロボット１と通信を行い、自律飛行ロボット１に対して出動命令を送信する。自律飛行ロボット１は、待機状態においては着陸場所１０６（帰還位置）にて静止状態となっており、警備センタ１００からの出動命令を受信することによって自律移動を開始し、後述するように目標対象物Ｍの周囲の位置に設定された移動目標位置まで移動する。また、警備センタ１００は、自律飛行ロボット１によって撮像された目標対象物Ｍの撮像画像を受信する。なお、着陸場所１０６は、後述するように飛行可能期間を更新可能な位置であって、バッテリ１０にエネルギーを補給する充電器の設置位置である。
【００１５】
管理装置１０２は、地面や壁面等に設置された固定型の移動物体検出センサ１０４（１０４ａ，１０４ｂ・・・）を備え、目標対象物Ｍの位置を検知する。移動物体検出センサ１０４は、例えば、レーザセンサとすることができる。レーザセンサは、一定の角度サンプル間隔の角度毎にレーザを二次元的にスキャンすることによって、地面（又は床面）から一定の高さの水平面における検知範囲内に存在する物体（障害物）との距離情報を極座標値として取得する。レーザセンサは、放射状にレーザ光である探査信号を走査し、物体に反射して戻ってきた探査信号を受信して、送信と受信の時間差から物体までの距離を算出し、レーザセンサの設置位置の座標及び探査信号を送信した方向と算出した距離から当該物体の位置の極座標値を求め、当該極座標値から３次元の直交座標値（Ｘt，Ｙt，Ｚt）を求める。管理装置１０２は、移動物体検出センサ１０４によって求められた目標対象物Ｍの位置を自律飛行ロボット１へ送信する。自律飛行ロボット１は、目標対象物Ｍの位置を受信すると、その位置に基づいて自らの移動経路を算出し、当該移動経路に沿って移動する。なお、管理装置１０２は、レーザセンサの検知範囲が重複する領域に存在する目標対象物Ｍの同一性を検証することで複数のレーザセンサの検知範囲に渡る目標対象物Ｍの追跡を行う。すなわち、レーザセンサ１０４ａの検知範囲に存在する目標対象物Ｍがレーザセンサ１０４ｂの検知範囲に移動したとしても、管理装置１０２は、当該目標対象物Ｍが同一の物体であると判定することができる。
【００１６】
また、管理装置１０２は、ＧＮＳＳによって自律飛行ロボット１の自己位置を推定するために、航法衛星からの信号を受信する衛星信号受信機１０５を備えている。本実施の形態では、既知の技術であるＲＴＫ（Real Time Kinematic）測位法によって自律飛行ロボット１の自己位置を推定する。ＲＴＫ測位法は、位置が既知の参照地点である基準局（基準点）で観測した航法衛星からの情報（例えば、航法衛星の軌道情報）を、測位する地点である移動局に無線等によって送信し、基準局から送信された情報と移動局にて観測した航法衛星からの情報とを解析することにより高精度な測量結果を得る測位方法である。すなわち、管理装置１０２は、ＲＴＫ測位における基準局として機能し、衛星信号受信機１０５によって受信した航法衛星からの情報を、移動局である自律飛行ロボット１に対して送信する。そして、自律飛行ロボット１は、管理装置１０２から受信した情報と、自らが観測した航法衛星からの情報とを解析することにより、自律飛行ロボット１の自己位置を推定する。自律飛行ロボット１における自己位置推定の詳細については後述する。
【００１７】
以下、図１の概観図及び図３の機能ブロック図を参照して、自律飛行ロボット１の構成及び機能について説明する。
【００１８】
自律飛行ロボット１は、図１に示すように、４枚のロータ（プロペラ）２（２ａ〜２ｄ）を一平面上に有する。各ロータ２は、バッテリ１０によって供給されるエネルギーにより駆動されるモータ４（４ａ〜４ｄ）を用いて回転させられる。一般的に、シングルロータ型のヘリコプタでは、メインロータによって発生する反トルクをテールロータが生み出すモーメントで相殺することによって方位角を保っている。一方、自律飛行ロボット１のようなクアッドロータ型のヘリコプタでは、前後・左右で異なる方向に回転するロータ２を用いることで反トルクの相殺を行っている。そして、各ロータ２の回転数（ｆａ〜ｆｄ）を制御することにより、様々な機体の移動や姿勢の調節を行うことができる。例えば、機体をヨー方向に回転させたいときは、前後のロータ２ａ、２ｃと左右ロータ２ｄ、２ｂの回転数に差を与えればよい。
【００１９】
撮像部３は、例えばレンズなどの光学系および所定画素数（例えば６４０×４８０画素からなる解像度）のＣＣＤやＣＭＯＳなどの２次元アレイ素子を有する二次元イメージセンサで構成され、移動空間の撮像画像を所定の時間間隔で取得するいわゆる可視カメラである。本実施の形態では、撮像部３は、その光軸が自律飛行ロボット１の正面方向を撮像するよう筐体部分に設置され、かつ、水平面（ＸＹ平面）から予め定めた俯角θにより斜め下方の空間を視野角φ（画角）において撮像するよう設置されている。取得した撮像画像は後述する制御部７に出力され、制御部７により記憶部８に記憶されたり、後述する通信部９を介して管理装置１０２に送信されたりする。
【００２０】
距離検出センサ５は、自律飛行ロボット１の周囲に存在する障害物と自律飛行ロボット１との間の距離を検出し、センサ検出範囲内に存在する障害物の相対的な位置を取得するセンサである。本実施の形態では、距離検出センサ５としてマイクロ波センサを備える。マイクロ波センサは、空間にマイクロ波を放出し、その反射波を検知することによって、自律飛行ロボット１の周囲にある物体を探知し、その物体までの距離を求める。距離検出センサ５は、例えば、自律飛行ロボット１の前方に向けて設け、目標対象物Ｍまでの距離を測定するために用いることができる。また、距離検出センサ５は、例えば、自律飛行ロボット１の下部に下向きに設け、地面との距離（高度）を測定するために用いることもできる。また、距離検出センサ５は、例えば、撮像部３の光軸方向に向けて設け、撮像部３の撮像対象物である目標対象物Ｍまでの距離を測定するために用いることができる。
【００２１】
位置検出センサ６は、自律飛行ロボット１の現在位置を取得するために用いるセンサである。本実施の形態における位置検出センサ６は、人口衛星である航法衛星から送信される信号を受信する。位置検出センサ６は、複数の航法衛星から送信される信号を受信して、当該信号に含まれる情報を制御部７へ入力する。本実施の形態において受信対象とする航法衛星は、ＧＰＳやＧＬＯＮＡＳＳ、Ｇａｌｉｌｅｏ、ＢｅｉＤｏｕ、準天頂衛星等である。なお、位置検出センサ６は、ジャイロセンサ、電子コンパス等の他のセンサを用いて、既知の技術により、自律飛行ロボット１の姿勢ＹＡＷを得るための情報を出力してもよい。
【００２２】
通信部９は管理装置１０２との間で、例えば無線ＬＡＮや携帯電話回線等により無線通信するための通信モジュールである。本実施の形態では、撮像部３によって取得した撮像画像を通信部９により管理装置１０２に送信し、当該撮像画像を管理装置１０２から警備センタ１００に送信することにより、警備員等が遠隔から侵入者を監視することを可能にする。また、通信部９は、管理装置１０２から目標対象物Ｍの位置（座標：Ｘt，Ｙt，Ｚt）を受信することにより、後述するような移動経路の設定を可能にする。
【００２３】
記憶部８は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）等の情報記憶装置である。記憶部８は、各種プログラムや各種データを記憶し、制御部７との間でこれらの情報を入出力する。各種データには、目標対象物位置８１、飛行可能期間８２、ボクセル情報８３、各種パラメータ８４等の制御部７の各処理に用いられる情報、各センサ等の出力値及び撮像画像等が含まれる。
【００２４】
目標対象物位置８１は、管理装置１０２から受信した目標対象物Ｍの位置情報（座標：Ｘt，Ｙt，Ｚt）である。本実施の形態では、目標対象物位置８１を目標対象物Ｍの足元位置、すなわち目標対象物Ｍが地面（床面）に接している位置とする。制御部７は、通信部９を介して目標対象物Ｍの位置を受信すると目標対象物位置８１として記憶部８に記憶させる。なお、管理装置１０２は、目標対象物Ｍの位置（座標：Ｘt，Ｙt，Ｚt）の時間変化に基づいて目標対象物Ｍの姿勢（例えば、目標対象物Ｍの正面方向を示した情報）を推定し、目標対象物位置８１として記憶してもよい。
【００２５】
飛行可能期間８２は、自律飛行ロボット１が自律的に飛行可能とされる期間を表す情報である。本実施の形態では、後述する飛行可能期間推定手段７５にて飛行可能期間８２を推定し、記憶部８に記憶されるものとする。
【００２６】
ボクセル情報８３は、移動空間をボクセル空間として複数のボクセルに分割して移動空間の障害物の構造等を表した情報であり、予め管理者等によって設定され記憶部８に記憶される情報である。本実施の形態では、移動空間を所定の大きさ（例えば１５ｃｍ×１５ｃｍ×１５ｃｍ）のボクセルに等分割し、各ボクセルの識別子であるボクセルＩＤと、移動空間におけるボクセルの位置（三次元座標）と、ボクセル属性と、ボクセルコスト値とを対応付けてボクセル情報８３として記憶する。ボクセル属性には、障害物に相当するボクセルを「占有ボクセル」と定義して、自律飛行ロボット１が移動できない空間とする。そして、占有ボクセルの近くに存在する空間に位置するボクセルを「近接ボクセル」、それ以外の自由に飛行可能なエリアに位置するボクセルを「自由ボクセル」と定義する。
【００２７】
各ボクセルには、後述する経路探索手段７３にて移動経路を生成する際に利用できるよう、占有度を示すボクセルコスト値が関連付けて登録される。ボクセルコスト値は、占有ボクセルにおいて最大値Ｃmaxをとり、占有ボクセルからの距離が大きくなるほど小さな値となるように設定される。例えば、あるボクセル（評価ボクセル）のボクセルコスト値は、ボクセルコスト値＝Ｃmax × ｅｘｐ｛−λ×Ｒ｝の計算式により算出することが好適である。ここでλは実験によって求めたパラメータとし、Ｒは評価ボクセルに最も近い占有ボクセルからの距離とする。そして、予め定めた閾値以上のボクセルコスト値を有するボクセルを「近接ボクセル」とする。また、ボクセルコスト値が当該閾値よりも小さいボクセルを「自由ボクセル」とし、自由ボクセルとみなされたボクセルのボクセルコスト値を０と設定する。
【００２８】
また、記憶部８には、各種パラメータ８４として離間距離等も記憶される。離間距離は、目標対象物Ｍに追従飛行するにあたって、自律飛行ロボット１と目標対象物Ｍとの水平面における維持すべき相対距離である。離間距離は、自律飛行ロボット１の管理者等によって予め設定される。自律飛行ロボット１を用いて所定の目標対象物Ｍを監視する場合、目標対象物Ｍに近づき、より詳細な撮像画像を取得できる必要がある。しかし、侵入者などの敵対する目標対象物Ｍから攻撃を受けないようにするためには一定距離以上離間する必要がある。そのため、本実施の形態の自律飛行ロボット１は、目標対象物Ｍの詳細な撮像画像を取得でき、かつ、当該目標対象物Ｍから攻撃を受け難い距離に離間距離を予め、当該離間距離を保ちつつ追従飛行するように制御される。離間距離は、例えば３ｍと設定される。この他にも、後述する角度閾値や精度閾値などの条件や式なども、各種パラメータ８４として記憶部８に記憶される。
【００２９】
制御部７は、ＣＰＵ等を備えたコンピュータで構成され、測位処理、速度推定処理、経路探索処理、経路追従制御等の一連の処理として、位置推定手段７１、速度推定手段７２、経路探索手段７３、移動制御手段７４、飛行可能期間推定手段７５を含んでいる。
【００３０】
飛行可能期間推定手段７５は、自律飛行ロボット１の自律飛行が可能な期間である飛行可能期間８２を推定する処理を行う。本実施の形態では、飛行可能期間推定手段７５は、現在のバッテリ１０の残存容量に基づいて飛行可能期間８２を推定する。なお、バッテリ１０の残存容量を測定する方法は従来から多数提案されており、適宜公知の方法を採用すればよい。例えば、バッテリ１０の残存容量はバッテリ１０の端子電圧と深い相関性があるため、バッテリ１０の端子電圧を検出することによって残存容量を推定する方法を採用してもよい。また、バッテリ１０の充放電特性及び充電容量等をバッテリプロファイルとして記憶しておき、その記憶されたバッテリプロファイルと自律飛行ロボット１の稼働状態とに基づいてバッテリ１０の残存容量を算出してもよい。飛行可能期間推定手段７５は、バッテリ１０の残存容量を求めると、求めた残存容量と自律飛行時の平均バッテリ消費量とに基づいて飛行可能期間８２を推定し（例えば、２０分間など）、記憶部８に記憶する。なお、平均バッテリ消費量ではなく、最大バッテリ消費量により飛行可能期間８２を推定してもよい。
【００３１】
位置推定手段７１は、飛行可能期間８２と、航法衛星から受信した信号に基づく情報とに基づいて、移動空間における自律飛行ロボット１の自己位置及び精度を推定する処理を行う。ここで、航法衛星から受信した信号に基づく情報とは、基準局として機能する管理装置１０２から受信した情報と、位置検出センサ６が航法衛星から受信した信号の情報を両方を意味する。位置推定手段７１は、図４に示すように、機能的に第一測位手段７１１、第二測位手段７１２及び位置出力手段７１３を含んで構成される。
【００３２】
第一測位手段７１１及び第二測位手段７１２は、飛行可能期間８２と航法衛星からの信号に基づく情報を用いてＲＴＫ測位法によって自律飛行ロボット１の位置及び精度を推定する測位処理を実行する。
【００３３】
測位処理では、まず、所定の測位開始時刻（例えば、現在時刻や５分後の時刻など）から飛行可能期間８２が経過するまでの測位終了時刻に至る期間を測位期間として設定する。
【００３４】
次に、測位処理では、設定した測位期間において受信可能な複数の航法衛星の組み合わせ（以下、「衛星セット」という）を選択する。この際、航法衛星からの信号に基づく軌道計算により、測位開始時刻における各航法衛星の位置を求め、当該航法衛星の位置と単独測位によって求めた大まかな現在位置とに基づいて各航法衛星の仰角を求め、求めた仰角が予め設定した角度閾値以上となる複数の航法衛星を求める。同様に、航法衛星からの信号に基づく軌道計算により、測位終了時刻における航法衛星の位置を求め、当該航法衛星の位置に基づいて航法衛星の仰角が角度閾値以上となる複数の航法衛星を求める。そして、測位開始時刻及び測位終了時刻の両方で角度閾値以上となる複数の航法衛星を、測位期間において受信可能な複数の航法衛星の組み合わせである衛星セットとして選択する。なお、上述した航法衛星からの信号に基づく軌道計算の方法（軌道計算アルゴリズム）は、従来から多数提案されており、適宜公知の方法を採用すればよい。軌道計算の方法ついては、例えば、「［図説］ＧＰＳ−測位の理論−」（西修二郎著、日本測量協会発行、２００７年刊）に詳細に記述されている。
【００３５】
次に、測位処理では、選択した衛星セットからの信号を用いてＲＴＫ測位法によって、所定の時間間隔（例えば、１秒間に５回）にて自律飛行ロボット１の現在の位置、及び当該位置の精度を算出する。本実施の形態では、ＲＴＫ測位法によって、ＥＮＵ座標系における基準点（衛星信号受信機１０５の既知の位置）からの相対位置を計算し、当該相対位置と基準点の位置に基づいて求めた自律飛行ロボット１の現在の位置（座標：Ｘs，Ｙs，Ｚs）を出力するものとする。また、本実施の形態では、測位処理にて出力される精度は、ＲＴＫ測位法よって算出されたカルマンフィルタの共分散行列の分散値が出力されるものとする。そのため、分散値が小さいほど推定された位置が高精度（精度が大きい）であることを意味する。
【００３６】
第一測位手段７１１及び第二測位手段７１２は、互いに異なるタイミングにより測位処理を開始する。以下に、第一測位手段７１１及び第二測位手段７１２にて実行される、待機状態（すなわち、警備センタ１００から出動命令を受信する前）における測位処理の実行タイミングについて、図５を用いて説明する。図５は、待機状態において第一測位手段７１１及び第二測位手段７１２にて実行される測位処理を説明する図であり、横軸を時間軸とし、第一測位手段７１１又は第二測位手段７１２にて実行される測位処理の測位期間を、黒色又はハッチングにより塗りつぶした棒グラフとして示している。すなわち、第一測位手段７１１及び第二測位手段７１２にて実行される測位処理は、測位期間の長さは飛行可能期間８２に相当するため同じであるが、測位期間及び当該測位期間における測位処理を開始するタイミングが互いに異なる。
【００３７】
まず、自律飛行ロボット１の初回起動時（時刻ｔ０）に第一測位手段７１１により測位処理が開始される。第一測位手段７１１は、測位処理として、まず現在時刻（時刻ｔ０）から飛行可能期間８２が経過するまでの時刻（ｔ５）に至る期間を測位期間として設定する。そして、第一測位手段７１１は、測位処理として、当該測位期間における衛星セットを選択し、当該衛星セットからの信号を用いて所定の時間間隔（例えば、１秒間に５回）により位置と精度をＲＴＫ測位法によって推定する。ＲＴＫ測位法では、測位処理を開始した直後から所定期間においては測位の精度が悪い低精度期間（いわゆる初期化期間）となる。図５では、第一測位手段７１１の測位処理は、時刻ｔ０から時刻ｔ１までの期間が低精度期間であることを示している。なお、本実施の形態では、低精度期間を、測位処理によって出力された精度が予め設定された精度閾値以下の期間とする。すなわち、前述のように精度として分散値を用いているため、当該分散値が所定の閾値以上となっているときに、精度が精度閾値以下であることを意味している。
【００３８】
そして、第一測位手段７１１による測位処理が高精度期間となったとき（時刻ｔ１）、第二測位手段７１２は、測位処理を開始する。すなわち、現在時刻（時刻ｔ１）から飛行可能期間８２が経過するまでの期間（時刻ｔ６）を測位期間として設定し、当該測位期間における衛星セットを選択し、ＲＴＫ測位法により、現在の位置と精度を算出する。図５では、第二測位手段７１２の測位処理は、時刻ｔ１から時刻ｔ２までの期間が低精度期間であることを示している。
【００３９】
そして、第二測位手段７１２による測位処理が高精度期間となったとき（時刻ｔ２）、第一測位手段７１１は、測位処理を最初から開始する（測位処理を実行し直す）リセット操作を実行する。すなわち、これまでｔ０〜ｔ５までを測位期間として計算してきた測位処理を中止し、新たな測位期間として現在時刻（時刻ｔ２）から飛行可能期間８２が経過するまでの期間（時刻ｔ７）を測位期間として設定し、当該測位期間における衛星セットを選択し、ＲＴＫ測位法により、現在の位置と精度を算出する。
【００４０】
そして、第一測位手段７１１による測位処理が高精度期間となったとき（時刻ｔ３）、第二測位手段７１２は、測位処理を最初から開始する（測位処理を実行し直す）リセット操作を実行する
以上のように、待機状態においては、第一測位手段７１１及び第二測位手段７１２は、他方の測位手段が高精度期間となったときに、自らの測位処理を最初から実行し直すリセット操作を繰り返すものとする。
【００４１】
ただし、待機状態から自律移動状態に移行した後（すなわち、警備センタ１００から出動命令を受信した後）においては、第一測位手段７１１及び第二測位手段７１２は、リセット操作を実行せずに、それぞれ現在設定している測位期間のまま測位処理を継続する。例えば、時刻ｔ１〜ｔ２の間の何れかの時刻において、警備センタ１００から出動命令を受信して自律移動状態となった場合、第一測位手段７１１は、ｔ０〜ｔ５の測位期間において選択した衛星セットのまま、当該衛星セットからの信号に基づいて測位処理を継続し、位置及び精度を出力する。そして、第二測位手段７１２は、ｔ１〜ｔ６の測位期間において選択した衛星セットのまま、当該衛星セットからの信号に基づいて測位処理を継続し、位置及び精度を出力する。
【００４２】
位置出力手段７１３は、第一測位手段７１１及び第二測位手段７１２から出力された位置（候補位置）と、当該位置の精度に基づいて、自律飛行ロボット１の自己位置を出力する位置出力処理を行う。位置出力処理では、待機状態（すなわち、警備センタ１００から出動命令を受信する前）においては、第一測位手段７１１及び第二測位手段７１２から出力された精度が精度閾値以上となっている（高精度期間となっている）何れかの測位手段を選択する選択操作と、当該選択操作によって選択した測位手段が求めた位置（候補位置）を自己位置（現在位置を示す座標：Ｘs，Ｙs，Ｚs）として出力する出力操作とを実行する。なお、本実施の形態では、前述のように精度として分散値を用いているため、当該分散値が所定の閾値以下となっているときに、精度が精度閾値以上であることを意味している。また、自律移動状態（すなわち、警備センタ１００から出動命令を受信した後）においては、選択操作を実行せずに、待機状態において選択操作によって選択した測位手段の測位処理によって求めた位置を自己位置として出力する。例えば、時刻ｔ１〜ｔ２の間の何れかの時刻において、警備センタ１００から出動命令を受信した場合、位置出力手段７１３は、待機状態において選択した（高精度期間となっている）第一測位手段の測位処理によって求めた位置を、自律飛行中における自己位置として出力する。
【００４３】
位置推定手段７１は、推定した自己位置（座標：Ｘs，Ｙs，Ｚs）と、管理装置１０２から受信した目標対象物Ｍの位置（座標：Ｘt，Ｙt，Ｚt）を経路探索手段７３へ出力する。この際、電子コンパスやジャイロセンサなどの位置検出センサ６からの出力を受けて自己位置として姿勢ＹＡＷも求めて、自己位置として出力してもよい。また、位置推定手段７１は、推定された自己位置を速度推定手段７２及び移動制御手段７４へ出力する。
【００４４】
速度推定手段７２は、後述する移動制御手段７４における移動制御で利用するため、自律飛行ロボット１の現在の飛行速度（ｖx，ｖy，ｖz，ｖyaw）を推定する速度推定処理を行う。本実施の形態では、位置推定手段７１にて推定した自己位置（座標：Ｘs，Ｙs，Ｚs及び姿勢ＹＡＷ）の時間変化から飛行速度を求める。この際、測定誤差等の影響を考慮して拡張カルマンフィルタを利用して飛行速度を推定することが好適である。この他にも、ＧＮＳＳにおけるドップラー効果を利用した速度推定方法を用いてもよい。速度推定手段７２は、推定した飛行速度を移動制御手段７４へ出力する。
【００４５】
経路探索手段７３は、位置推定手段７１で推定された自己位置と、記憶部８に記憶された移動物体位置８１及び各種情報とを用いて、自己位置から所定の移動目標位置に至る移動経路を算出する経路探索処理を行う。
【００４６】
経路探索処理では、まず、目標対象物位置８１から離間距離だけ離れた位置に移動候補位置を設定する処理を行う。すなわち、目標対象物位置８１から離間距離だけ離れた周囲の領域に複数の移動候補位置を設定する。本実施の形態では、移動候補位置を、目標対象物位置８１を中心として、予め設定された離間距離を半径とした円状の位置で、かつ、管理者等により予め定められた飛行高度（例えば３ｍ）の領域に、それぞれ等間隔になるように設定する。
【００４７】
次に、経路探索処理では、設定した移動候補位置の其々を評価し、最も高い評価値となる移動候補位置を移動目標位置として設定する処理を行う。この際、まず、自律飛行ロボット１が障害物に接触することを防止するため、ボクセル情報８３を参照して占有ボクセル又は近接ボクセルに含まれる位置には移動目標位置を設定しないよう、最低の評価値となるようにする。また、自律飛行ロボット１の自己位置から各移動候補位置までの距離が小さいほど評価値が高くなるようにする。更に、前回設定した移動目標位置から移動候補位置までの距離が小さいほど評価値が高くなるようにする。
【００４８】
次に、経路探索処理では、位置推定手段７１で推定された自己位置と、設定した移動目標位置とを用いて自律飛行ロボット１の移動経路を算出する移動経路生成処理を行う。具体的には、移動経路生成処理では、まず、記憶部８からボクセル情報８２を読出し、分割した移動可能な空間である自由ボクセル及び近接ボクセルの各ボクセルの中心をノードとし、当該ノードに隣接するノード間を連結した線分をエッジとしてグラフ構造を生成する。この際、エッジの重みとして、隣接するノード間の距離に基づいて求められる距離コストが設定されているものとする。このようにボクセル情報８２に基づいて全ての移動可能なボクセルについてグラフ構造を生成した後、自律飛行ロボット１が存在しているボクセルのノードから移動目標位置が存在しているボクセルのノードまでの移動経路を生成する。移動経路の生成方法については、さまざまな経路生成方法が適用可能であるが、本実施の形態ではＡ＊（エースター）経路探索法を用いて移動経路を探索する。この際、ボクセル情報８２として記憶されたボクセルコストと距離コストとを、Ａ＊経路探索法における移動コストとして利用する。経路探索手段７３で生成された生成された移動経路のデータは、経由点（ｘ，ｙ，ｚ）の集合データであり、この情報は記憶部８に一時的に記憶される。
【００４９】
移動制御手段７４は、経路探索手段７３にて算出された移動経路と位置推定手段７１にて推定された自己位置と速度推定手段７２で推定された飛行速度とを用いて、自律飛行ロボット１が経路探索手段７３で算出された移動経路に沿って飛行するように経路追従制御を行う。具体的には、移動経路、自己位置及び飛行速度を用いて各時刻での飛行制御値である制御指令値を求め、当該制御指令値に基づいてモータ４を制御し、ロータ２の回転数を制御する。
【００５０】
経路追従制御では、まず、各時刻での自律飛行ロボット１が目標とすべき直近の位置（以下、「ローカル目標」と呼ぶ）を算出する処理を行う。図６はローカル目標の算出を説明する図である。ローカル目標の算出にあたり、移動制御手段７４は、経路探索手段７３で生成された移動経路を記憶部８から読出し、自律飛行ロボット１が現在時刻で目指している経由点Ｗｐ１と前回通過済みである経由点Ｗｐ０との２点間を繋げた直線Ｗを求める。そして、移動制御手段７４は、求めた直線Ｗと自律飛行ロボット１の自己位置を中心とした球Ｓとの交点Ｌｐ’、Ｌｐを算出し、目指している経由点Ｗｐ１に近い交点Ｌｐをローカル目標として求める。このように、各時刻においてローカル目標を目指して自律飛行ロボット１が移動するよう飛行制御することで、常にローカル目標も移動経路上を移動目標位置Ｐｏに向かって移動していき、自律飛行ロボット１は移動経路に沿って飛行していくことになる。
【００５１】
次に、経路追従制御では、算出したローカル目標に向かって飛行するようＸ、Ｙ、Ｚ、ヨー角の各方向毎に制御指令値ｕ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の式のようなＰＤ制御により求める。なお、本実施の形態では、目標角度ψ'を目標対象物Ｍの方向、すなわち、目標対象物Ｍの位置の方向を向く角度とした。
【００５２】
このように、本実施の形態における自律飛行ロボット１は、第一測位手段７１１及び第二測位手段７１２において、航法衛星から受信した信号に含まれる情報を用いて所定の測位開始時刻から所定期間（飛行可能期間８２）が経過するまでの期間である測位期間において信号を受信可能な複数の航法衛星の組み合わせからなる衛星セットを選択し、当該衛星セットからの信号を用いて位置及び精度を推定する測位処理を実行する。そして、位置出力手段７１３において、精度が所定値以上となる測位手段を選択し、当該選択した測位手段が推定した位置を自己位置として出力する。また、第一測位手段７１１及び第二測位手段７１２は、他方の測位手段における精度が所定値以上となったとき、測位処理を最初から実行するリセット操作を実行する。したがって、常に高精度期間となっている測位手段からの位置を得られるため、どのタイミングであっても良好な精度の自己位置を得ることができる。また、リセット操作によって常に測位期間を最新な状態に更新しているため、測位期間を渡過したことによる古い衛星セットによって悪化した精度の自己位置を得る危険性をできるだけ抑制することができる。なお、上記実施の形態における自律飛行ロボット１は、第一測位手段７１１及び第二測位手段７１２にて実行される測位処理において、ＲＴＫ測位法によって自律飛行ロボット１の位置及び精度を推定しているが、これに限らず、単独ＧＰＳ測位法やＤ−ＧＰＳ測位法などの、航法衛星の信号を用いた様々な測位方法によって位置及び精度を推定してもよい。しかし、様々な測位方法の中でも、特にＲＴＫ測位法やＰＰＰ（Precise Point Positioning）測位法などのような、比較的長い初期化期間を要する測位法においては、自己位置を得たいタイミングで初期化期間（すなわち低精度期間）となっている可能性が高いため、より効果的に自己位置を得ることができる。
【００５３】
また、本実施の形態の第一測位手段７１１及び第二測位手段７１２におけるリセット操作は、待機状態においてのみ実行され、自律移動状態においては実行されない。そして、位置出力手段７１３は、自律移動状態においては選択操作を実行せずに、待機状態において選択された測位手段が推定した位置を自己位置として出力する。したがって、自律移動状態（自律飛行中）においては、高精度期間となっている一方の測位手段からの自己位置を得ることができ、当該測位手段の測位期間の間は、より安定的かつ高精度の自己位置を得ることができる。なお、他の実施の形態として、自律移動状態においてもリセット操作及び選択操作を行う場合であっても、どのタイミングでも良好な精度の自己位置を得ることができるといった効果を得ることができるものの、このような他の実施の形態の場合は、選択操作の際の測位手段の切り替え時に位置や精度の変動（ギャップ）が生じたり、高精度期間内における更なる精度向上の恩恵を得難い形態であるともいえる。
【００５４】
ところで、本発明は、上記実施の形態に限定されるものではなく、特許請求の範囲に記載した技術的思想の範囲内で、更に種々の異なる実施の形態で実施されてもよいものである。また、実施の形態に記載した効果は、これに限定されるものではない。
【００５５】
上記実施の形態では、経路探索手段７３は、経路探索処理において、目標対象物位置８１の周囲の位置に移動候補位置を設定し、各移動候補位置を評価することによってこれらの中から移動目標位置を設定している。しかし、これに限らず、経路探索手段７３は、位置出力手段７１３にて現在選択している測位手段における測位期間の終了間際については、移動目標位置を予め記憶している着陸場所１０６（帰還位置）に設定し、自己位置から当該着陸場所１０６に至る移動経路を生成してもよい。すなわち、着陸場所１０６に至るまでの期間である強制帰還期間を予め設定しておき、測位期間の終了時刻から強制帰還期間だけ遡った期間においては、目標対象物Ｍの位置に関わらず着陸場所１０６を移動目標位置に設定する。これにより、位置出力手段７１３にて現在選択している測位手段において設定した測位期間の期間外の測位精度が安定していない期間における自律飛行を予め制限しておくことができ、安全性の高い自律飛行を実現することが可能となる。また、このような強制帰還期間を、着陸場所１０６と現在の自己位置との間の距離に応じて設定してもよい。すなわち、着陸場所１０６と現在の自己位置との間の距離が大きくなるほど、着陸場所１０６に到着するまでの時間を要するため、強制帰還期間を長い期間として設定する。これにより、さらに安全性の高い自律飛行を実現することが可能となる。更に、位置出力手段７１３にて現在選択している測位手段における測位期間の期間外においては、自律飛行を制限する自律飛行判定手段を備えてもよい。すなわち、位置出力手段７１３にて現在選択している測位手段における測位期間の期間外においては、測位の精度が安定していないため、自律飛行を中断して、マニュアル操作モードに切り替えたり、移動制御手段７４に対して強制的に着陸制御を実行させることによって、より安全性の高い飛行を実現することが可能となる。
【００５６】
また、上記実施の形態における自律飛行ロボット１は、第一測位手段７１１及び第二測位手段７１２にて実行される測位処理において、それぞれの測位期間において受信可能な衛星セットを、航法衛星からの信号に基づく軌道計算によって求めた各航法衛星の仰角に基づいて選択している。しかし、これに限らず、移動空間の三次元地図情報であるボクセル情報８３を用いて、移動空間に存在する障害物をも考慮して衛星セットを選択してもよい。以下に当該他の実施の形態について、具体的に説明する。まず、ボクセル情報８３に基づいて、所定数の自由ボクセルで構成される部分空間をエリアとして設定し、移動空間を複数のエリアＡ1〜Ａn（ｎは所定の正数値）に分割する。次に、測位期間を所定時間間隔で分割した測位期間内の複数の時刻であるサンプル時刻（ｔ’1〜ｔ’l、ここでｌは所定の正数値）を設定し、それぞれのサンプル時刻における各航法衛星の位置を、航法衛星からの信号に基づく軌道計算により求める。そして、あるサンプル時刻ｔ’1及びあるエリアＡ1について着目したとき、当該サンプル時刻ｔ’1におけるある航法衛星ｓ1の位置と、当該エリアＡ1の重心位置とを結ぶ直線を求め、当該直線が占有ボクセルに交わらないとき、当該エリアＡ1では当該航法衛星ｓ1からの信号を受信可能であるとする。このような計算を全ての航法衛星ｓ1〜ｓm（ｍは所定の正数値）について行うことにより、サンプル時刻ｔ’及びエリアＡ1において、信号を受信可能な航法衛星を求めることができる。そして、このような計算を全てのエリアＡ1〜Ａnにおいて実行することにより、サンプル時刻ｔ’1において移動空間で受信可能な衛星の組み合わせを求めることができる。さらに、同様の計算を全てのサンプル時刻（ｔ’1〜ｔ’l）において行うことにより、測位期間において受信可能な衛星セットを求めることができる。このように、移動空間の三次元地図情報であるボクセル情報８３を用いて、移動空間に存在する障害物をも考慮して衛星セットを選択することにより、より正確な衛星セットを求めることができ、より安定した精度で自己位置を推定することができる。
【００５７】
また、上記実施の形態における自律飛行ロボット１は、飛行可能期間推定手段７５によってバッテリ１０の残存容量に基づいて飛行可能期間８２を推定している。しかし、これに限らず、飛行可能期間８２は予め設定された期間であってもよい。例えば、移動空間における条例などの社会的なルールによって自律的に連続飛行できる期間（飛行可能期間）が制限されているとき、当該制限された飛行可能期間を予め記憶しておいてもよい。また、バッテリではなく、燃料（ガソリン等）を利用した内燃機関によって飛行する自律飛行ロボットなどにおいては、燃料の残存容量に基づいて飛行可能期間を設定してもよい。この場合、着陸場所１０６（帰還位置）には、当該燃料を補給する補給器が設置されているものとする。
【００５８】
また、上記実施の形態における自律飛行ロボット１は、位置推定手段７１を構成する機能要素の一部として第一測位手段７１１と第二測位手段７１２の二つの測位手段を備えているが、これに限らず、測位手段は三つ以上備えてもよい。
【００５９】
また、上記実施の形態では、制御部７において位置推定処理、速度推定処理、移動経路生成処理、経路追従制御等の一連の処理を行っている。しかし、これに限らず、図示しない制御用のＰＣを用意し、当該ＰＣにこれらの一連の処理を実施させてもよい。すなわち、自律飛行ロボット１は、ＰＣによって行われた各処理又は制御によって得られた制御指令値を無線通信又は有線通信によりＰＣから受信し、当該制御指令値に基づいてモータ４の回転数を制御することにより、目的の位置に飛行するようにしてもよい。このように、外部ＰＣを用いて上記の一連の処理を分担することにより、自律飛行ロボット１のＣＰＵ処理負荷を低減することができ、ひいてはバッテリの消耗も抑えることができる。
【符号の説明】
【００６０】
１・・・自律飛行ロボット
２・・・ロータ
３・・・撮像部
４・・・モータ
５・・・距離検出センサ
６・・・位置検出センサ
７・・・制御部
８・・・記憶部
９・・・通信部
１０・・・バッテリ
７１・・・位置推定手段
７２・・・速度推定手段
７３・・・経路探索手段
７４・・・移動制御手段
７５・・・飛行可能期間推定手段
７１１・・・第一測位手段
７１２・・・第二測位手段
７１３・・・位置出力手段
８１・・・目標対象物位置
８２・・・飛行可能期間
８３・・・ボクセル情報
８４・・・各種パラメータ
１００・・・警備センタ
１０２・・・管理装置
１０４・・・移動物体検出センサ
１０５・・・衛星信号受信機
１０６・・・着陸場所
１１０・・・情報通信網
Ｍ・・・目標対象物 
【図１】
0006664258.tif 000002
【図２】
0006664258.tif 000003
【図３】
0006664258.tif 000004
【図４】
0006664258.tif 000005
【図５】
0006664258.tif 000006
【図６】
0006664258.tif 000007