JP2019158640A - 位置推定システム、位置検出方法、およびプログラム - Google Patents

Abstract

【課題】自己位置情報の精度の低下を抑制する位置推定システム等を提供すること。【解決手段】本発明に係る位置推定システム１００は、移動装置１０が移動する際に駆動する車輪の角速度データを含む第１データを出力する第１センサを有する。また、位置推定システムは、車輪の角速度によらないで検出した移動装置１０の位置データ又は移動装置１０の姿勢変化に係るデータを含む第２データを出力する第２センサを有する。位置推定システムは、車輪の角速度の値に応じて第１データの優先度を設定し、設定した優先度に基づいて、第１データと第２データとの統合をすることにより、移動装置１０の位置を推定する自己位置推定部を有する。さらに、位置推定システムは、推定した移動装置１０の位置に関するデータを出力するデータ出力部を備える。【選択図】図２

Description

本発明は位置推定システム、位置検出方法、およびプログラムに関する。

種々の移動装置において、移動装置の位置を推定する技術が知られている。例えば、複数のセンサから取得したデータを統合的に処理する手法であるセンサフュージョンは、単体のセンサからは得ることができない高い精度の情報を生成することが可能である。

特許文献１には、センサフュージョンを利用した技術が提案されている。特許文献１に記載の自己位置推定装置は、第１自己位置と第２自己位置とを用いて算出される重み付き平均値を、現在のステップにおける最終自己位置とする。移動装置の最終自己位置を推定する際に用いる重み係数は、異常発生確率の関数となっている。異常発生確率は、異常発生確率計算式から算出される。異常発生確率計算式は、移動装置が学習移動することで取得される学習データを機械学習することにより得られる。

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

ところで、移動する手段として車輪を有する移動装置は、車輪と設置面との間に滑りを生じることが考えられる。車輪と設置面との間に滑りを生じると、車輪の駆動を検出するセンサが検出した車輪の回転量と、移動装置の実際の移動量とが一致しない場合がある。この場合、センサフュージョンに車輪の回転量を利用すると、自己位置推定装置の精度が低下するおそれがある。そこで、各駆動部の動きを検出したデータを利用しながらも、自己位置情報の精度の低下を抑制する技術が望まれる。

本発明は、このような課題を解決するためになされたものであり、自己位置情報の精度の低下を抑制する位置推定システム等を提供するものである。

本発明に係る位置推定システムは、移動装置が移動する際に駆動する車輪の角速度データを含む第１データを出力する第１センサを有する。また、位置推定システムは、前記車輪の角速度によらないで検出した前記移動装置の位置データ又は前記移動装置の姿勢変化に係るデータを含む第２データを出力する第２センサを有する。位置推定システムは、前記車輪の角速度の値に応じて前記第１データの優先度を設定し、設定した優先度に基づいて、前記第１データと前記第２データとの統合をすることにより、前記移動装置の位置を推定する自己位置推定部を有する。さらに、位置推定システムは、推定した前記移動装置の位置に関するデータを出力するデータ出力部を備える。

このような構成により、位置推定システムは、例えば、駆動輪と床面との間に比較的に滑りが大きく生じている場合には、かかる駆動輪から取得するデータの優先度を比較的低く設定することができる。

本発明に係る位置推定方法は、移動装置が移動する際に駆動する車輪の角速度データを含む第１データを出力するステップを有する。また、位置推定方法は、前記車輪の角速度によらないで検出した前記移動装置の位置データ又は前記移動装置の姿勢変化に係るデータを含む第２データを出力するステップを有する。位置推定方法は、前記車輪の角速度の値に応じて前記第１データの優先度を設定するステップと、設定した優先度に基づいて、前記第１データと前記第２データとの統合をすることにより、前記移動装置の位置を推定するステップとを有する。さらに、位置推定方法は、推定した前記移動装置の位置に関するデータを出力するステップを有する。

このような構成により、位置推定方法は、例えば、駆動輪と床面との間に比較的に滑りが大きく生じている場合には、かかる駆動輪から取得するデータの優先度を比較的低く設定することができる。

本発明に係るプログラムがコンピュータに実行させる方法は、移動装置が移動する際に駆動する車輪の角速度データを含む第１データを出力するステップを有する。また、係る方法は、前記車輪の角速度によらないで検出した前記移動装置の位置データ又は前記移動装置の姿勢変化に係るデータを含む第２データを出力するステップを有する。さらに、係る方法は、前記車輪の角速度の値に応じて前記第１データの優先度を設定するステップと、設定した優先度に基づいて、前記第１データと前記第２データとの統合をすることにより、前記移動装置の位置を推定するステップとを有する。さらに、係る方法は、推定した前記移動装置の位置に関するデータを出力するステップを有する。

このような構成により、係るプログラムがコンピュータに実行させる方法は、例えば、駆動輪と床面との間に比較的に滑りが大きく生じている場合には、かかる駆動輪から取得するデータの優先度を比較的低く設定することができる。

本発明により、自己位置情報の精度の低下を抑制する位置推定システム等を提供することができる。

実施の形態に係る移動システムの概略構成を示す構成図である。 実施の形態に係る移動システムの主要構成を示したハードウェア構成図である。 実施の形態に係る全方位台車の主要構成を示す図である。 実施の形態に係る位置推定システムの機能ブロック図である。 実施の形態に係る位置推定システムのフローチャートである。

説明の明確化のため、以下の記載および図面は、適宜、省略、および簡略化がなされている。また、様々な処理を行う機能ブロックとして図面に記載される各要素は、ハードウェア的には、ＣＰＵ（Central Processing Unit）、メモリ、その他の回路で構成することができ、ソフトウェア的には、メモリにロードされたプログラムなどによって実現される。したがって、これらの機能ブロックがハードウェアのみ、ソフトウェアのみ、又はそれらの組合せによっていろいろな形で実現できることは当業者には理解されるところであり、いずれかに限定されるものではない。よって、以下の説明に回路として例示した構成は、ハードウェア又はソフトウェアのいずれか又はその両方によって実現することが可能であり、ある機能を実現する回路として示された構成は、同様の機能を実現するソフトウェアの一部としても示され得る。例えば、制御回路と記載された構成は、制御部として記載され得る。なお、各図面において、同一の要素には同一の符号が付されており、必要に応じて重複説明は省略されている。

また、上述したプログラムは、様々なタイプの非一時的なコンピュータ可読媒体を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（Random Access Memory））を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、および電磁波を含む。一時的なコンピュータ可読媒体は、電線および光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
＜実施の形態＞
以下、図面を参照しながら本発明の実施の形態について説明する。なお、図１は、左手系の直交座標系が付されている。図１において、ｘ軸およびｙ軸は床面Ｇと平行である。ｚ軸は、床面Ｇに対して垂直である。図１に示す直交座標系と、図２に示す直交座標系は、同じ方向を示している。

図１は、実施の形態に係る移動システムの概略構成を示す構成図である。図１に示す移動システム１は、移動装置１０および操作装置９０を構成として有している。

操作装置９０は、移動装置１０を操作するための機能を有する装置である。操作装置９０は、例えばタブレット端末、スマートフォン、パーソナルコンピュータ等である。操作装置９０は、移動装置１０と通信可能に接続しており、移動装置１０から種々の情報を受け取る。また、操作装置９０は、使用者に移動装置１０の状態を通知するための表示部、使用者が移動装置１０の動作を指示するための入力部、および移動装置１０との間で情報の送受信をするための通信制御部等を有している。

移動装置１０は操作装置９０と通信可能に接続しており、操作装置９０の指示を受けて床面Ｇの上を移動する。

移動装置１０は、主な構成として、台車１１および本体部１２を有している。台車１１は、床面Ｇに接し本体部１２を支持している。本体部１２は、イメージセンサ１３、ロボットアーム１４、ジャイロセンサ１５、レーザレンジセンサ１６、および制御装置１７を有している。以下に各構成の詳細について説明する。

台車１１は、床面Ｇの上を移動する機能、および移動装置１０の姿勢を変化させる機能を有する全方位台車である。全方位台車は、全方向台車、全方位移動台車、又は全方向移動台車と称されてもよい。台車１１は主な構成として、駆動輪１１１Ｌ、駆動輪１１１Ｒ、旋回軸１１２、およびフレーム１１３を有している。台車１１は、本体部１２を任意の方向へ移動可能に構成されている。すなわち、台車１１が有する駆動輪１１１Ｌ、駆動輪１１１Ｒ、および旋回軸１１２は、それぞれ独立して駆動するように構成されている。

駆動輪１１１Ｌおよび駆動輪１１１Ｒは、床面Ｇに接する車輪である。駆動輪１１１Ｌおよび駆動輪１１１Ｒは、同軸上に離間して配置され、それぞれ独立に駆動可能に設けられている。駆動輪１１１Ｌおよび駆動輪１１１Ｒは、車輪を回転させるためのアクチュエータをそれぞれ有している。したがって、駆動輪１１１Ｌおよび駆動輪１１１Ｒが同じ方向に同期して回転すると、台車１１は直進する。また、駆動輪１１１Ｌと駆動輪１１１Ｒとの回転数又は回転方向に差が生じると、かかる差に従って台車１１は旋回しながら進んだり、移動せずに旋回のみを行ったりする。駆動輪１１１Ｌと駆動輪１１１Ｒとは、フレーム１１３により支持されている。なお、台車１１は、図示しない非駆動輪を有している。非駆動輪は、台車１１を床面Ｇに支持するとともに、駆動輪１１１Ｌおよび駆動輪１１１Ｒに従動する。

旋回軸１１２は、移動装置１０を旋回させる機能を有している。旋回軸１１２は、フレーム１１３に設けられ、床面Ｇに対して垂直なｚ軸方向に沿った回転軸を有する。また旋回軸１１２は、旋回軸１１２を回転させるためのアクチュエータを有している。したがって、旋回軸１１２が回転すると、移動装置１０は旋回軸１１２に沿って姿勢を変化させる。

駆動輪１１１Ｌ、駆動輪１１１Ｒ、および旋回軸１１２は、軸の回転を検出するための回転センサをそれぞれ有している。回転センサが軸の回転を検出することにより、軸の角速度を知ることができる。回転センサは、検出したデータに基づいて角速度を算出する機能を有していてもよい。回転センサは、例えば、光学式や磁気式のロータリーエンコーダである。回転センサは、制御装置１７にそれぞれ接続し、検出した値を制御装置１７に供給する。

イメージセンサ１３は、本体部が有するロボットアーム１４を含む画像を撮像する。イメージセンサ１３が撮像した画像のデータは、移動装置１０の周囲の物体を認識するのに利用される。また、イメージセンサ１３が撮像した画像のデータは、ロボットアーム１４の制御に利用される。

ロボットアーム１４は、本体部１２に設けられた多関節アームである。ロボットアーム１４は本体部１２から延伸し、先端部にはロボットハンド１４０を有している。ロボットアーム１４およびロボットハンド１４０はそれぞれの関節部にアクチュエータを有しており、操作装置９０から受けた指示に従って物体を掴んだり離したりする動作を行い得る。

ジャイロセンサ１５は、移動装置１０の移動や姿勢の変化を検出する。ジャイロセンサ１５は、例えば直交する３軸座標の各軸方向の加速度と、それぞれの軸周りの角速度とを検出する。

レーザレンジセンサ１６は、レーザ光を照射し、照射したレーザ光の反射光を検出することにより、周囲の物体の距離を検出する。

制御装置１７は、各構成から情報を受け取り、受け取った情報を処理し、処理結果に基づいて適宜各構成に指示を送る。制御装置１７は、複数の半導体等が実装された基板により構成される。例えば、制御装置１７は、操作装置９０からの指示を受け取り、受け取った指示に基づいて、移動装置１０の各構成を動作させる。

制御装置１７は、イメージセンサ１３やレーザレンジセンサ１６から受け取った情報等に基づいて、周辺地図情報を生成し得る。また、制御装置１７は、イメージセンサ１３、ジャイロセンサ１５、レーザレンジセンサ１６、および台車１１が有する回転センサから移動装置１０の移動に関する情報や姿勢変化に関する情報を受け取り、受け取った情報を利用して、移動装置１０の位置を推定する。

移動装置１０は、操作装置９０から目的地へ移動することを指示され、受け取った指示に基づいて自律移動を行い得る。この場合、制御装置１７は、生成した周辺地図情報と、推定した移動装置１０の位置とを合成し、障害物等を避けて目的地へ移動する移動ルートを生成する。制御装置１７は、移動ルートを生成すると、生成した移動ルートに従って台車１１を移動させる。制御装置１７は、台車１１を移動させながら、各センサから受け取る情報に基づいて、移動装置１０の位置の推定を逐次更新する。

次に、図２を参照しながら、移動システム１の主要なハードウェア構成である、移動装置１０の移動に関する構成および移動装置１０の位置を検出するシステムの構成について説明する。図２は、実施の形態に係る移動システムの主要構成を示したハードウェア構成図である。

操作装置９０は、例えば、移動装置１０が現在位置から目標位置へ移動するための速度指令に係る速度（Ｖｘ、Ｖｙ、θｓ）を周期的に生成する。ここで、Ｖｘは、ｘ軸方向の速度を示し、Ｖｙは、ｙ軸方向の速度を示している。したがって、ＶｘとＶｙとを合成したものは、並進速度を示す。また、θｓは、本体部１２の旋回の角速度を示している。操作装置９０は、生成した速度指令を制御装置１７に送信する。

制御装置１７は、プロセッサ１７１、メモリ１７２、およびＩ／Ｆ１７３（I/F=Interface）を有する。プロセッサ１７１は、ＣＰＵを主な構成として有する制御回路である。プロセッサ１７１は、Ｉ／Ｆ１７３を介して制御装置１７の外部と信号の送受信を行う。また、プロセッサ１７１は、メモリ１７２に接続し、適宜メモリ１７２に信号を記憶させたり、メモリ１７２に記憶させた信号を読み出したりする。メモリ１７２は、例えばフラッシュメモリ、ＤＲＡＭ（Dynamic Random Access Memory）、ＳＳＤ（Solid State Drive）等の揮発性又は不揮発性の記憶装置である。

制御装置１７は、操作装置９０から受け取った速度指令に基づいて、駆動輪１１１Ｌ、駆動輪１１１Ｒ、および旋回軸１１２を駆動するための信号を生成する。すなわち、制御装置１７は、受け取った速度指令に基づいて、各アクチュエータの回転制御を行うための角速度（ωＬ、ωＲ、ωｓ）を生成する。

ここで、図３を参照しながら、制御装置１７が各アクチュエータの回転制御を行うために生成する信号について説明する。図３は、実施の形態に係る全方位台車の主要構成を示す図である。図３は、台車１１をｘｙ平面に投影して模式的に主要構成を示している。理解を容易にするため、図３に示したｘｙｚ左手系直交座標のｚ軸は旋回軸の中心に一致している。

車輪アクチュエータ１１０Ｌは、駆動輪１１１Ｌに接続し、駆動輪１１１Ｌを回転させる。回転センサ１１４Ｌは、駆動輪１１１Ｌの回転を検出する。同様に、車輪アクチュエータ１１０Ｒは、駆動輪１１１Ｒに接続し、駆動輪１１１Ｒを回転させる。回転センサ１１４Ｒは、駆動輪１１１Ｒの回転を検出する。また、旋回アクチュエータ１１０Ｓは、旋回軸１１２に接続し、旋回軸１１２を回転させる。回転センサ１１４Ｓは、旋回軸１１２の回転を検出する。

駆動輪１１１Ｌおよび駆動輪１１１Ｒが有する車輪の半径は、いずれも半径ｒである。また、駆動輪１１１Ｌおよび駆動輪１１１Ｒは車軸１１１Ａ上に配置されている。駆動輪１１１Ｌの中心と駆動輪１１１Ｒの中心との距離は、Ｗである。旋回軸１１２は、台車１１の略中心かつｚ軸に平行に設けられている。旋回軸１１２の中心から車軸１１１Ａまでの距離は、ｓである。

このような構成において、操作装置９０が生成する速度指令に含まれる速度（Ｖｘ、Ｖｙ、θｓ）と、各駆動輪および旋回軸１１２の回転動作に係る角速度（ωＬ、ωＲ、ωｓ）との関係は、以下に示す式（１）および式（２）のとおりである。

ここで、Ｊはヤコビアンを示し、Ｊ^−１はＪの逆行列を示している。

図２に戻り説明を続ける。制御装置１７は、上述の式（１）および式（２）に従い、操作装置９０から受け取った速度指令に係る速度（Ｖｘ、Ｖｙ、θｓ）から各駆動輪および旋回軸１１２の角速度（ωＬ、ωＲ、ωｓ）を生成する。制御装置１７は、生成した角速度ωＬを車輪アクチュエータ１１０Ｌに送信し、角速度ωＲを車輪アクチュエータ１１０Ｌに送信し、角速度ωｓを旋回アクチュエータ１１０Ｓに送信する。制御装置１７から角速度に関する信号を受け取った車輪アクチュエータ１１０Ｌは、接続する駆動輪１１１Ｌを角速度ωＬで回転させるように動作する。同様に、制御装置１７から角速度に関する信号を受け取った車輪アクチュエータ１１０Ｒは、駆動輪１１１Ｒを角速度ωＲで回転させるように動作する。同様に、旋回アクチュエータ１１０Ｓは、旋回軸１１２を角速度ωｓで回転させるように動作する。

次に、移動システム１が有する位置推定システムについて説明する。位置推定システム１００は、種々のセンサから取得したデータに基づいて、移動装置１０の位置を検出するシステムである。

位置推定システム１００は、移動装置１０が有する台車１１の車輪の角速度を検出するセンサと、台車１１の車輪の角速度によらないで移動装置１０の位置又は姿勢変化に係るデータを検出するセンサとを有している。また、位置推定システム１００は、かかるセンサから取得したデータに基づいて移動装置１０の位置を推定する演算部を有する。また、演算部は、台車１１が有する車輪の角速度の値に応じて、係るセンサから取得したデータの優先度を設定する機能を有している。

ここで、優先度とは、複数のセンサから取得した複数のデータを統合することにより移動装置１０の位置を推定する場合の重みをつける優先の度合いをいう。つまり、あるセンサから取得したデータの優先度が低い場合は、係るセンサから取得したデータによる寄与度が比較的小さくなるように重みづけを小さく設定する。あるいは、この場合、係るセンサから取得したデータを採用せずに移動装置１０の位置を推定する。一方、あるセンサから取得したデータの優先度が高い場合は、係るセンサから取得したデータによる寄与度が比較的大きくなるように重みづけを大きく設定する。すなわち、係るセンサが出力するデータの信頼性が比較的低い場合と比較的高い場合とにより優先度を変化させて設定し、設定した優先度に基づいて、移動装置１０の位置を推定する。

例えば、台車１１が有する駆動輪１１１Ｌおよび駆動輪１１１Ｒは、摩擦を利用して床面Ｇを移動する。この場合、摩擦係数が低い場合には駆動輪と床面Ｇとの間に滑りが生じる。駆動輪と床面Ｇとの間に滑りが生じる場合、車輪アクチュエータが駆動した駆動輪の回転量と、実際に駆動輪が移動した移動量とが一致しなくなる。したがって、駆動輪と床面Ｇとの間に比較的に滑りが大きく生じている場合には、回転センサから取得するデータの優先度を比較的低く設定する。このように、優先度を設定する構成を採用することにより、移動装置１０の位置を推定する演算の精度が低下するのを抑制することができる。

以下に、位置推定システム１００についてより詳細を説明する。位置推定システム１００は、イメージセンサ１３、ジャイロセンサ１５、レーザレンジセンサ１６、回転センサ１１４Ｌ、回転センサ１１４Ｒ、および回転センサ１１４Ｓを有している。また、位置推定システム１００は、制御装置１７を有している。各センサが検出したデータは、制御装置１７に供給される。制御装置１７は、各センサから受け取ったデータを統合し、移動装置１０の位置を推定する。また、制御装置１７は、推定した移動装置１０の位置に関するデータを操作装置９０に出力する。

次に、図４を参照しながら、位置推定システム１００の機能について更に説明する。実施の形態にかかる位置推定システム１００は、複数のセンサから受け取ったデータに基づいて移動装置１０の位置を推定し、推定した位置に関するデータを、経路演算部１８６に出力する機能を有している。

図４は、実施の形態に係る位置推定システムの機能ブロック図である。位置推定システム１００は、ホイルオドメトリ演算部１８１、レーザオドメトリ演算部１８２、ジャイロオドメトリ演算部１８３、およびビジュアルオドメトリ演算部１８４を有している。また、位置推定システム１００は、自己位置推定部１８５を有している。

ホイルオドメトリ演算部１８１は、回転センサ１１４Ｌ、回転センサ１１４Ｒ、および回転センサ１１４Ｓから角速度データＤｗｈを受け取る。角速度データＤｗｈは、回転センサ１１４Ｌが検出した角速度ωＬ、回転センサ１１４Ｒが検出した角速度ωＲ、および回転センサ１１４Ｓが検出した角速度ωｓが含まれている。ホイルオドメトリ演算部１８１は、受け取った角速度データＤｗｈに基づいて、式（１）および式（２）からｘ軸方向の速度Ｖｘ、ｙ軸方向の速度Ｖｙおよび旋回軸１１２周りの角速度θｓを演算する。ホイルオドメトリ演算部１８１は、演算結果である移動データＤｗｏを自己位置推定部１８５に供給する。

また、ホイルオドメトリ演算部１８１は、角速度ωＬ、角速度ωＲ、又は角速度ωｓに関する参照データＤｗｐも自己位置推定部１８５に供給する。参照データＤｗｐは、角速度ωＬおよび角速度ωＲに関するデータが含まれる。また、参照データＤｗｐは、角速度ωｓに関するデータが含まれていてもよい。位置推定システム１００は、参照データＤｗｐを自己位置推定部１８５が行う演算において、優先度を設定するために利用する。

レーザオドメトリ演算部１８２は、レーザレンジセンサ１６から測距データＤｄｓを受け取り、受け取った測距データＤｄｓに基づいて、移動装置１０と移動装置１０の周囲に存在する物体との相対的な位置変化を演算する。レーザオドメトリ演算部１８２は、演算結果を自己位置推定部１８５に供給する。

ジャイロオドメトリ演算部１８３は、ジャイロセンサ１５から加速度データＤａｃを受け取り、受け取った加速度データＤａｃに基づいて、移動装置１０の移動量および姿勢変化量を演算する。ジャイロオドメトリ演算部１８３は、演算結果を自己位置推定部１８５に供給する。

ビジュアルオドメトリ演算部１８４は、イメージセンサ１３から画像データＤｖｓを受け取り、受け取った画像データＤｖｓに基づいて、移動装置１０と移動装置１０の周囲に存在する物体との相対的な位置変化および姿勢変化を演算する。ビジュアルオドメトリ演算部１８４は、演算結果を自己位置推定部１８５に供給する。

自己位置推定部１８５は、上述した各演算部から演算結果を受け取り、受け取った演算結果を統合し、移動装置１０の位置を推定する。自己位置推定部１８５は、受け取った演算結果を統合するに際し、ホイルオドメトリ演算部１８１から受け取った角速度データＤｗｈを利用する。すなわち、自己位置推定部１８５は、角速度ωＬ、角速度ωＲ、の値に応じてホイルオドメトリ演算部１８１から受け取ったデータの優先度を設定する。そして、自己位置推定部１８５は、設定した優先度に基づいて、各演算部から受け取ったデータの統合をする。

自己位置推定部１８５が行う演算は、例えば、複数のセンサから取得した値に対して予め設定された割合の係数を乗じるものであってもよい。かかる演算の一例は、カルマンフィルタを利用するものである。複数のセンサから取得したデータに対してカルマンフィルタを利用することにより、移動装置１０の位置を高い精度で推定し得る。また、自己位置推定部１８５が行う演算は、複数のセンサから取得したデータの選択を行うものであってもよい。この場合、自己位置推定部１８５は、適宜信頼し得るセンサからのデータを取得して、移動装置１０の位置を推定する。また、自己位置推定部１８５が行う演算は、上述の演算を組み合わせたものであってもよい。

以下に、自己位置推定部１８５が設定する優先度についていくつかの例を挙げて説明する。まず第１の例について説明する。第１の例において、参照データＤｗｐは、角速度ωＬおよび角速度ωＲの値が含まれる。自己位置推定部１８５は、参照データＤｗｐと予め設定された閾値とを比較する。角速度ωＬ又は角速度ωＲの少なくとも一方の値が予め設定された閾値より大きい場合、自己位置推定部１８５は、次のように優先度を設定する。すなわち、自己位置推定部１８５は、角速度ωＬおよび角速度ωＲの値のいずれもが予め設定された閾値より小さい場合に比べて、角速度データＤｗｈに基づいて算出した移動データＤｗｏの優先度を低く設定する。

次に、第２の例について説明する。第２の例において、参照データＤｗｐは、角速度ωＬ又は角速度ωＲの値が含まれる。自己位置推定部１８５は、参照データＤｗｐを受け取り、角速度ωＬ又は角速度ωＲの少なくとも一方の値が予め設定された閾値より大きい場合に、角速度データＤｗｈに基づいて算出した移動データＤｗｏを含めないで、データの統合をする。

次に、第３の例について説明する。第３の例において、参照データＤｗｐは、角速度ωＬ、角速度ωＲ、および角速度ωｓの値が含まれる。自己位置推定部１８５は、参照データＤｗｐを受け取り、角速度ωＬの値および角速度ωＲの値と、角速度ωｓの値とを比較する。例えば、自己位置推定部１８５は、角速度ωＬの値と角速度ωｓの値との差を算出する。あるいは、自己位置推定部１８５は、角速度ωＬの値から角速度ωｓの値を除する。そして、このように比較した結果、角速度ωＬ又は角速度ωＲの少なくとも一方の値と角速度ωｓの値との割合が、予め設定された閾値より高くなる場合がある。その場合、自己位置推定部１８５は、角速度ωＬおよび角速度ωＲの値のいずれもと角速度ωｓの値との割合が予め設定された閾値より低い場合に比べて、角速度データＤｗｈに基づいて算出した移動データＤｗｏの優先度を低く設定する。

次に、第４の例について説明する。第４の例において、参照データＤｗｐは、角速度ωＬ、角速度ωＲ、および角速度ωｓの値が含まれる。自己位置推定部１８５は、参照データＤｗｐを受け取り、角速度ωＬの値および角速度ωＲの値と、角速度ωｓの値とを比較する。例えば、自己位置推定部１８５は、角速度ωＬの値と角速度ωｓの値との差を算出する。あるいは、自己位置推定部１８５は、角速度ωＬの値から角速度ωｓの値を除する。そして、このように比較した結果、角速度ωＬ又は角速度ωＲの少なくとも一方の値と角速度ωｓの値との割合が予め設定された閾値より高い場合に、自己位置推定部１８５は、角速度データＤｗｈに基づいて算出した移動データＤｗｏを含めないで、データの統合をする。

なお、図４において説明した位置推定システム１００の機能は、制御装置１７に含まれ得る。しかし、図４において説明した位置推定システム１００の機能は、その一部または全部が、操作装置９０に含まれていてもよい。すなわち、移動装置１０は、各センサから取得したデータを操作装置９０に送信し得る。そして、操作装置９０は、移動装置１０から受け取ったデータを演算し、移動装置１０の位置を推定しうる。

次に、図５を参照しながら、位置推定システム１００の処理について説明する。図５は、実施の形態に係る位置推定システムのフローチャートである。図５に示すフローチャートは、図４に示す自己位置推定部１８５の処理を示している。

まず自己位置推定部１８５は、各オドメトリ演算部からオドメトリデータを取得する（ステップＳ１０）。すなわち、図４の例において、自己位置推定部１８５は、ホイルオドメトリ演算部１８１から移動データＤｗｏおよび参照データＤｗｐを取得する。また、自己位置推定部１８５は、レーザオドメトリ演算部１８２、ジャイロオドメトリ演算部１８３、およびビジュアルオドメトリ演算部１８４からオドメトリデータを取得する。

次に、自己位置推定部１８５は、参照データＤｗｐから移動データＤｗｏの優先度を設定する（ステップＳ１１）。移動データＤｗｏの優先度を設定する処理は、図４を参照しながら説明したとおりである。

次に、自己位置推定部１８５は、オドメトリデータの統合をする演算を行う（ステップＳ１２）。かかる演算を行うに際し、自己位置推定部１８５は、ステップＳ１１において設定した移動データＤｗｏの優先度に基づいて処理を行う。

次に、自己位置推定部１８５は、オドメトリデータの統合演算によって算出された位置推定データを、経路演算部１８６に出力する（ステップＳ１３）。

このような構成により、位置推定システムは、例えば、駆動輪と床面との間に比較的に滑りが大きく生じている場合には、かかる駆動輪から取得するデータの優先度を比較的低く設定することができる。よって、本実施の形態により、自己位置情報の精度の低下を抑制する位置推定システム等を提供することができる。

尚、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、移動装置１０は、イメージセンサ１３、ジャイロセンサ１５、レーザレンジセンサ１６の全てを有している必要はない。すなわち、移動装置１０は、イメージセンサ１３、ジャイロセンサ１５、レーザレンジセンサ１６のうちいずれか１つを有しているものであってもよい。

また、移動装置１０は、イメージセンサ１３、ジャイロセンサ１５、レーザレンジセンサ１６に代えて、ミリ波レーダ等の、車輪の角速度によらないで移動装置１０の位置又は移動装置１０の姿勢変化を検出するセンサを有していてもよい。

また、台車１１は、床面Ｇに駆動輪を有するものであれば、上述した構成でなくともよい。台車１１は、全方位台車であってもよいし、全方位台車でなくてもよい。

１ 移動システム
１０ 移動装置
１１ 台車
１２ 本体部
１３ イメージセンサ
１４ ロボットアーム
１５ ジャイロセンサ
１６ レーザレンジセンサ
１７ 制御装置
９０ 操作装置
１００ 位置推定システム
１１０Ｌ 車輪アクチュエータ
１１０Ｒ 車輪アクチュエータ
１１０Ｓ 旋回アクチュエータ
１１１Ａ 車軸
１１１Ｌ 駆動輪
１１１Ｒ 駆動輪
１１２ 旋回軸
１１３ フレーム
１１４Ｌ 回転センサ
１１４Ｒ 回転センサ
１１４Ｓ 回転センサ
１４０ ロボットハンド
１７１ プロセッサ
１７２ メモリ
１７３ Ｉ／Ｆ
１８１ ホイルオドメトリ演算部
１８２ レーザオドメトリ演算部
１８３ ジャイロオドメトリ演算部
１８４ ビジュアルオドメトリ演算部
１８５ 自己位置推定部
１８６ 経路演算部

Claims (11)

1. 移動装置が移動する際に駆動する車輪の角速度データを含む第１データを出力する第１センサと、
   前記車輪の角速度によらないで検出した前記移動装置の位置データ又は前記移動装置の姿勢変化に係るデータを含む第２データを出力する第２センサと、
   前記車輪の角速度の値に応じて前記第１データの優先度を設定し、設定した優先度に基づいて、前記第１データと前記第２データとの統合をすることにより、前記移動装置の位置を推定する自己位置推定部と、
   推定した前記移動装置の位置に関するデータを出力するデータ出力部と、を備える
   位置推定システム。
2. 前記自己位置推定部は、
   前記第１データに含まれる前記車輪の角速度の値が予め設定された閾値より大きい場合に、前記車輪の角速度の値が予め設定された閾値より小さい場合に比べて、前記第１データの優先度を低く設定する、
   請求項１に記載の位置推定システム。
3. 前記自己位置推定部は、
   前記第１データに含まれる前記車輪の角速度の値が予め設定された閾値より大きい場合に、前記第１データを含めないで前記統合をする、
   請求項１に記載の位置推定システム。
4. 前記移動装置は、前記移動装置の姿勢を変化させる際に回転する旋回軸を有し、
   前記第１データは、前記旋回軸の角速度データを更に含み、
   前記自己位置推定部は、
   前記第１データに含まれる前記車輪の角速度の値と前記旋回軸の角速度の値とを比較し、前記車輪の角速度の値の割合が予め設定された閾値より高い場合に、前記車輪の角速度の値の割合が予め設定された閾値より低い場合に比べて、前記第１データの優先度を低く設定する、
   請求項１に記載の位置推定システム。
5. 前記移動装置は、前記移動装置の姿勢を変化させる際に回転する旋回軸を有し、
   前記第１データは、前記旋回軸の角速度データを更に含み、
   前記自己位置推定部は、
   前記第１データに含まれる前記車輪の角速度の値と前記旋回軸の角速度の値とを比較し、前記車輪の角速度の値の割合が予め設定された閾値より高い場合に、前記第１データを含めないで前記統合をする、
   請求項１に記載の位置推定システム。
6. 前記移動装置は、全方位台車を含み、
   前記全方位台車は、前記車輪および前記旋回軸を含む、
   請求項４又は５に記載の位置推定システム。
7. 前記第１データは、前記移動装置のホイルオドメトリに関するデータである、
   請求項１〜６のいずれか一項に記載の位置推定システム。
8. 前記自己位置推定部は、前記第１データと前記第２データとを、カルマンフィルタにより統合する、
   請求項１〜７のいずれか一項に記載の位置推定システム。
9. 前記車輪は、それぞれ独立して回転する複数の車輪であって、前記第１データは、複数の前記車輪の角速度をそれぞれ含んでいる、
   請求項１〜８のいずれか一項に記載の位置推定システム。
10. 移動装置が移動する際に駆動する車輪の角速度データを含む第１データを出力するステップと、
    前記車輪の角速度によらないで検出した前記移動装置の位置データ又は前記移動装置の姿勢変化に係るデータを含む第２データを出力するステップと、
    前記車輪の角速度の値に応じて前記第１データの優先度を設定するステップと、
    設定した優先度に基づいて、前記第１データと前記第２データとの統合をすることにより、前記移動装置の位置を推定するステップと、
    推定した前記移動装置の位置に関するデータを出力するステップと、を備える
    位置検出方法。
11. 移動装置が移動する際に駆動する車輪の角速度データを含む第１データを出力するステップと、
    前記車輪の角速度によらないで検出した前記移動装置の位置データ又は前記移動装置の姿勢変化に係るデータを含む第２データを出力するステップと、
    前記車輪の角速度の値に応じて前記第１データの優先度を設定するステップと、
    設定した優先度に基づいて、前記第１データと前記第２データとの統合をすることにより、前記移動装置の位置を推定するステップと、
    推定した前記移動装置の位置に関するデータを出力するステップと、を備える方法を、
    コンピュータに実行させるプログラム。

Images