JP7336223B2 - 自己位置推定方法 - Google Patents

Description

本発明は、移動体の自己位置を推定する方法に関する。

従来より、移動ロボット等の移動体の自己位置を、複数のセンサの検出情報を用いて推定する種々様々な技術が提案されている。例えば、特許文献１には、エンコーダもしくはジャイロスコープ等の内界センサの検出情報と、測距センサ、ＩＲセンサ、超音波センサ、カメラ等の外界センサの検出情報とを用いて移動体の自己位置を推定する技術が提案されている。

また、一つ以上のセンサの検出情報を用いて、移動体の自己位置を推定するアルゴリズムも、従来より、種々様々なアルゴリズムが提案されている。

特開２００７－１４９０８８号公報

ところで、複数のセンサの検出情報から複数のアルゴリズムにより、それぞれ自己位置を推定し、これらの複数の自己位置の推定値から移動体の自己位置を特定する場合、該複数のアルゴリズムのうちの１つのアルゴリズムとして、ＳＬＡＭ（Simultaneous Localization and Mapping）のアルゴリズムを採用し得る。

しかしながら、ＳＬＡＭのアルゴリズムは、移動体の初期位置を基準とするローカル座標系で、該移動体の移動量（位置変化量）を逐次算出し、この移動量を積算することにより初期位置に対する移動体の移動位置を推定するように構成されている。このため、移動体の移動環境に対して任意に設定されたグローバル座標系での移動体の自己位置を推定する場合、グローバル座標系で見た移動体の初期位置の設定値の誤差、あるいは、逐次算出する移動量の誤差等に起因して、ＳＬＡＭのアルゴリズムにより得られる移動体の自己位置の誤差が積算的に増大していく場合がある。ひいては、ＳＬＡＭのアルゴリズムにより得られた移動体の自己位置を反映させて移動体の自己位置を特定すると、当該特定した自己位置の信頼性が損なわれてしまう虞がある。

本発明はかかる背景に鑑みてなされたものであり、ＳＬＡＭのアルゴリズムを含む複数のアルゴリズムを用いて移動体の自己位置を推定する方法において、ＳＬＡＭのアルゴリズムによる自己位置の推定値を適切に補正しながら、移動体の自己位置の推定（特定）を適切に行うことを可能とする方法を提供することを目的とする。

本発明の自己位置推定方法は、上記の目的を達成するために、複数のセンサの検出情報から、該複数のセンサのうちの一つ以上のセンサの検出情報を各々用いる複数のアルゴリズムのそれぞれにより移動体の自己位置を推定し、該複数のアルゴリズムのそれぞれにより得られた前記自己位置の推定値から、前記移動体の自己位置を特定する自己位置推定方法であって、
前記複数のアルゴリズムは、ＳＬＡＭ（Simultaneous Localization and Mapping）のアルゴリズムである第Ａアルゴリズムと、該ＳＬＡＭと異なるアルゴリズムである第Ｂアルゴリズムとを含んでおり、
前記第Ｂアルゴリズムにより得られた前記自己位置の推定値である第Ｂ自己位置推定値と、前記特定した自己位置である自己位置特定値とのうちのいずれか一方の自己位置に応じて、前記第Ａアルゴリズムにより得られる前記自己位置の推定値である第Ａ自己位置推定値を補正することを間欠的に実行する補正ステップを備えることを基本構成とする。

なお、本発明において、移動体の「自己位置」は特にことわらない限り、移動体の移動環境に任意に設定されたグローバル座標系で見た自己位置を意味する。また、該「自己位置」は、通常の意味での移動体の位置（座標位置）に限らず、該移動体の姿勢（向き）を含み得る。

上記基本構成を有する本発明によれば、前記補正ステップを備えているため、前記一方の自己位置（前記第Ｂ自己位置推定値又は前記自己位置特定値）に応じて、ＳＬＡＭのアルゴリズムである第Ａアルゴリズムにより得られる第Ａ自己位置推定値を補正することが間欠的に（例えば、所定時間の経過毎に）実行される。これにより、第Ａ自己位置推定値の誤差が積算的に拡大していくのが防止される。ひいては、信頼性の高い第Ａ自己位置推定値を用いて、移動体の自己位置を特定する（自己位置特定値を得る）ことが可能となる。

よって、前記基本構成を有する本発明によれば、ＳＬＡＭのアルゴリズムによる自己位置の推定値を適切に補正しながら、移動体の自己位置の推定（特定）を適切に行うことを可能となる。

上記基本構成を有する本発明の前記補正ステップでは、所定期間における前記第Ｂ自己位置推定値及び前記自己位置特定値のうちの前記一方の自己位置の時系列と、該所定期間における前記第Ａ自己位置推定値の時系列とに基づいて、前記第Ａ自己位置推定値の時系列により示される軌道を、前記一方の自己位置の時系列により示される軌道に近づけるように前記第Ａ自己位置推定値を補正するためのパラメータを決定し、当該決定したパラメータを用いて前記所定期間以後の前記第Ａ自己位置推定値を補正することが好ましい（第１発明）。

これによれば、前記第１自己位置推定値を補正するためのパラメータとして、信頼性の高いパラメータを決定できる。ひいては、当該決定したパラメータを用いて前記所定期間以後の前記第Ａ自己位置推定値を補正することで、該第Ａ自己位置推定値を好適に補正することが可能となる。
また、前記基本構成を有する本発明では、前記複数のアルゴリズムのそれぞれ毎に、各アルゴリズムの推定処理により得られる一つ以上の状態量であって、各アルゴリズムにより推定された前記自己位置の確からしさに関連する状態量を学習済のニューラルネットワークに入力し、当該入力された状態量から該ニューラルネットワークにより前記複数のアルゴリズムのそれぞれ毎の重み係数を決定するステップをさらに備えており、
前記複数のアルゴリズムのそれぞれにより推定された前記自己位置を、前記決定された重み係数により合成して得られる自己位置を、前記移動体の自己位置特定値として特定することが好ましい（第２発明）。
これによれば、前記複数のアルゴリズムのそれぞれ毎の重み係数を決定するニューラルネットワークは、各アルゴリズム毎に、各アルゴリズムの推定処理（移動体の自己位置を推定する処理）により得られる一つ以上の状態量が入力され、これらの状態量から、各アルゴリズム毎の重み係数を決定する。
この場合、各アルゴリズム毎の状態量は、各アルゴリズムにより推定された移動体の自己位置の確からしさに関連する状態量であるので、各アルゴリズム毎の当該状態量は、該アルゴリズムの推定処理が、移動体の自己位置の信頼性の高い推定値を求める上で、どの程度適切になされたかの度合いを反映したものとなる。
このため、前記ニューラルネットワークの事前学習によって、該ニューラルネットワークは、各アルゴリズムに対応する重み係数が、該アルゴリズムによる自己位置の推定処理を他のアルゴリズムよりも適切に行い得る状況では相対的に高くなり、且つ、該アルゴリズムによる自己位置の推定処理を適切に行うことが他のアルゴリズムよりも困難な状況では相対的に低くなるように、入力される各アルゴリズム毎の状態量の全体に応じて各アルゴリズム毎の重み係数を決定できるようになる。
また、前記ニューラルネットワークにより決定される各アルゴリズム毎の重み係数は、各アルゴリズムにより推定された移動体の自己位置の精度ではなく、各アルゴリズム毎の前記状態量に依存して決定されるものであるので、該重み係数は、移動体の移動環境の場所の影響を受けにくいものとなる。
そして、第２発明によれば、前記複数のアルゴリズムのそれぞれにより推定された前記自己位置を、上記の如く決定された重み係数により合成して得られる位置が、前記移動体の自己位置特定値として特定される。
これにより、第２発明によれば、移動体の移動環境の影響を受け難い態様で移動体の自己位置の推定を行うことが可能となる。

上記第１発明又は第２発明では、前記複数のセンサは、前記移動体の外界を撮像するカメラを含み得る。この場合、前記第Ａアルゴリズムは、前記移動体の自己位置の推定のために、前記カメラの撮像画像から特徴点を検出し、当該検出された特徴点と、該撮像画像よりも過去の撮像画像から検出された特徴点とのマッチングを行う処理を逐次実行するように構成されたVisual_SLAMのアルゴリズムであることが好ましい（第３発明）。

これによれば、第Ａアルゴリズムが、Visual_SLAMのアルゴリズムであるので、該第Ａアルゴリズムによる自己位置の推定を好適に行うことが可能となる。

また、上記第１～第３発明では、前記複数のセンサは、前記移動体の外界物までの距離を測定する測距センサを含み得る。この場合、前記第Ｂアルゴリズムは、前記測距センサによる測距データと、粒子フィルタとを用いて前記移動体の自己位置を推定するアルゴリズムであることが好ましい（第４発明）。

これによれば、移動体の周囲に他の移動物体が存在しない状況等では、前記第Ｂアルゴリズムによって、信頼性の高い（換言すれば、前記確からしさが高い）第Ｂ自己位置推定値を得ることが可能である。さらには、該第Ｂアルゴリズムによって得られた第Ｂ自己位置推定値をベースとして、信頼性の高い自己位置特定値を得ることが可能である。

このため、前記一方の自己位置（前記第Ｂ自己位置推定値又は前記自己位置特定値）に応じて前記第Ａ自己位置推定を補正することの信頼性を好適に高めることが可能となる。

補足すると、第２発明において、前記第Ａアルゴリズムが、前記第３発明におけるアルゴリズム（Visual_SLAMのアルゴリズム）である場合には、前記第Ａアルゴリズムに係る前記状態量は、例えば、前記撮像画像から検出された特徴点の総数のうち、前記マッチングによる対応付けがなされた特徴点の個数の割合を示す状態量を含むことが好ましい。これにより、該状態量は、第Ａアルゴリズムにより推定される移動体の自己位置の確からしさに関連する状態量として適正なものとなる。

また、第２発明において、前記第Ｂアルゴリズムが、前記第４発明におけるアルゴリズムである場合には、前記第Ｂアルゴリズムに係る前記状態量は、該第Ｂアルゴリズムが前記移動体の自己位置を推定する過程で生成する共分散を含むことが好ましい。これにより、該状態量は、第Ｂアルゴリズムにより推定される移動体の自己位置の確からしさに関連する状態量として適正なものとなる。

本発明の実施形態における移動体の概略構成を示す図。 図１に示す自己位置推定処理装置による処理を示すブロック図。 図３Ａ、図３Ｂ及び図３Ｃは図２に示す補正処理部の処理を説明するための図。

本発明の一実施形態を図１～図３Ｃを参照して以下に説明する。図１を参照して、本実施形態での移動体１は、例えば車輪型の移動体であり、その移動環境の床面上に、複数の車輪２を介して支持される機体３を有する。そして、移動体１は、図示を省略する電動モータ等のアクチュエータにより、一つ以上の車輪２を回転駆動することにより、あるいは、一つ以上の車輪２の回転駆動と操舵（方向転換）とを行うことにより、床面上を移動する。

この移動体１には、自己位置を推定するために使用する複数のセンサが搭載されている。例えば、一つ以上の車輪２の回転角を検出する角度センサ等のオドメトリ用のセンサ５と、移動体１の周囲に存在する外界物までの距離を測定する測距センサとしてのレーザ・レンジ・ファインダ６（以降、ＬＲＦ６という）と、移動体１の上方に存在する天井を撮像するカメラ７とが移動体１に搭載されている。なお、オドメトリ用のセンサ５は、車輪２の回転角を検出する角度センサの他、車輪２のうちの操舵輪の操舵角を検出するセンサを含み得る。また、カメラ７は、本実施形態では、ステレオカメラである。

移動体１には、これらのセンサの検出情報を用いて自己位置推定を行う自己位置推定処理装置１０が搭載されている。該自己位置推定処理装置１０は、例えば、マイコン、メモリ、インターフェース回路等を含む一つ以上の電子回路ユニットにより構成される。

そして、自己位置推定処理装置１０は、実装されたハードウェア構成及びプログラム（ソウフトウェア構成）の両方又は一方により実現される機能として、図２のブロック図で示す如く、第１自己位置推定処理部１１と、第２自己位置推定処理部１２と、重み係数学習処理部１４と、自己位置特定処理部１５と、補正処理部１６とを含む。なお、自己位置推定処理装置１０は、上記の機能に限らず、例えば移動体１の移動制御等を行う機能も含み得る、

第１自己位置推定処理部１１は、ＬＲＦ６の検出情報とオドメトリ用センサ５の検出情報とを用いて移動体１の自己位置を推定する処理を実行する機能部、第２自己位置推定処理部１１は、カメラ７の検出情報を用いて移動体１の自己位置を推定する処理を逐次実行する機能部である。

なお、本実施形態では、第１及び第２自己位置推定処理部１１，１２がそれぞれ推定する移動体１の自己位置は、より詳しくは、移動体１の移動環境に任意に設定されるグローバル座標系で見た、移動体１の位置と姿勢（向き）との組である。第１自己位置推定処理部１１が推定する移動体１の自己位置は本発明における第Ｂ自己位置推定値に相当し、第２自己位置推定処理部１２が推定する移動体１の自己位置は本発明における第Ａ自己位置推定値に相当する。以降の説明では、第１及び第２自己位置推定処理部１１，１２によりそれぞれ推定される自己位置を、推定位置姿勢ということがある。

また、重み係数学習処理部１４は、第１及び第２自己位置推定処理部１１，１２のそれぞれから入力される後述の状態量Ａ，Ｂに応じて、第１及び第２自己位置推定処理部１１，１２のそれぞれに対応する重み係数（詳しくは、第１及び第２自己位置推定処理部１１，１２のそれぞれから出力される移動体１の推定位置姿勢に対する重み係数）を出力する機能部、自己位置特定処理部１５は、第１及び第２自己位置推定処理部１１，１２のそれぞれから出力される移動体１の推定位置姿勢を、重み係数学習処理部１４から出力される重み係数に応じて合成することによって、移動体１の自己位置を特定する（移動体１の推定位置姿勢を確定する）機能部である。該自己位置特定処理部１５が特定する移動体１の自己位置は、本発明における自己位置特定値に相当する。

また、補正処理部１６は、第２自己位置推定処理部１２が推定する自己位置（推定位置姿勢）を補正するための補正用パラメータを適宜生成して、第２自己位置推定処理部１２に入力する機能部である。

以下に、上記各機能部の詳細と併せて、自己位置推定処理装置１０の全体の処理を説明する。
第１及び第２自己位置推定処理部１１，１２の処理が以下に説明する如く実行される。第１自己位置推定処理部１１には、ＬＲＦ６及びオドメトリ用センサ５のそれぞれの検出情報が入力されると共に、移動体１の移動環境のマップデータ（例えば、２次元の占有格子地図等）とが入力される。この場合、マップデータは、自己位置推定処理装置１０のメモリにあらかじめ記憶保持され、あるいは、外部のサーバ等から随時、ダウンロードされる。

そして、第１自己位置推定処理部１１は、ＬＲＦ６の検出情報（移動体１の周囲の外界物の測距データ）と、オドメトリ用センサ５の検出情報と、マップデータとから、自己位置推定用の所定のアルゴリズムにより、移動体１の自己位置を逐次推定する（推定位置姿勢を逐次決定する）。

本実施形態では、第１自己位置推定処理部１１は、自己位置推定用のアルゴリズムとして、粒子フィルタを用いて自己位置推定を行うアルゴリズム、例えば、AMCL（Adaptive Monte Carlo Localization）という公知の手法のアルゴリズムを使用する。該アルゴリズムは、本発明における第Ｂアルリズムに相当するものである。

このAMCLのアルゴリズムでは、次のような処理によって、移動体１の自己位置（グローバル座標系で見た自己位置）が逐次推定される。すなわち、まず、移動体１の初期位置及び初期姿勢（向き）に応じて、移動体１の近辺に、粒子フィルタにおける複数の粒子（パーティクル）が生成される。なお、移動体１の初期位置及び初期姿勢は、例えば、移動体１の移動開始時又はその直前に、ＬＲＦ６の検出データ（移動体１の周囲の外界物の測距データ）とマップとを用いて特定される。

そして、オドメトリ用センサ５の検出情報から推定される移動体１の移動速度（並進速度及び角速度）に応じて、各粒子の新たな移動位置が推定される。次いで、ＬＲＦ６による移動体１の周囲の外界物の測距データを取得し、この測距データと、マップデータとに基づいて、各粒子の新たな移動位置の尤度が算出される。そして、リサンプリング処理によって、尤度が低い粒子が消滅され、さらに、尤度の高い粒子の近辺に新たな粒子が生成される。

AMCLのアルゴリズムでは、以上の如き処理を逐次実行することで得られる尤度の高い粒子の位置の時系列に基づいて、移動体１の自己位置が逐次推定される（移動体１の推定位置姿勢が逐次決定される）。ただし、本実施形態では、AMCLのアルゴリズムより推定する移動体１の自己位置（推定位置姿勢）のうちの姿勢は、ヨー方向（上下方向の軸周り方向）での移動体１の姿勢であり、ロール方向（移動体１の前後方向の軸周り方向）及びピッチ方向（移動体１の左右方向の軸周り方向）の姿勢は含まれない。

また、AMCLのアルゴリズムでは、移動体１の自己位置の推定処理の実行過程で、推定位置姿勢の確からしさに関連する状態量としての共分散（詳しくは分散共分散）が生成される。そして、第１自己位置推定処理部１１は、上記の如くAMCLのアルゴリズムにより推定した移動体１の自己位置（推定位置姿勢）と該推定時の時刻である推定時刻とを逐次出力すると共に、該推定位置姿勢の確からしさに関連する状態量（以降、状態量１という）として、上記共分散（分散共分散）を出力する。

補足すると、第１自己位置推定処理部１１は、AMCLのアルゴリズムにより決定した移動体１の推定位置姿勢が所定の範囲から逸脱したり、該推定位置姿勢の変化量（推定処理周期毎の変化量）が所定の範囲から逸脱する等の異常が発生した場合には、推定位置姿勢を自己位置特定処理部１５により後述する如く特定された移動体１の自己位置に応じて補正する。例えば、移動体１の推定位置姿勢が、自己位置特定処理部１５が直前に特定した自己位置に一致し、もしくは近づけるように補正される。

第２自己位置推定処理部１２には、カメラ７の検出情報としての撮像画像（移動体１の上方の天井の撮像画像）が入力される。なお、本実施形態では、カメラ７はステレオカメラであるので、第２自己位置推定処理部１２に入力されるカメラ７の検出情報（撮像画像）はステレオ画像（一対の撮像画像）である。ただし、カメラ７は、ステレオカメラに限らず、単眼カメラであってもよい。

そして、第２自己位置推定処理部１２は、入力された撮像画像から、自己位置推定用の所定のアルゴリズムにより、移動体１の自己位置を逐次推定する（推定位置姿勢を逐次決定する）。

本実施形態では、第２自己位置推定処理部１２は、自己位置推定用のアルゴリズムとして、入力された撮像画像からの特徴点の検出と、該特徴点と、過去に入力された撮像画像から検出された特徴点とのマッチングとを行うアルゴリズムを使用する。より具体的には、第２自己位置推定処理部１２は、当該アルゴリズムとして、例えば、Visual_SLAMの一手法としてのORB_SLAMという公知の手法のアルゴリズムを使用する。該アルゴリズムは、本発明における第Ａアルリズムに相当するものである。

このORB_SLAMのアルゴリズムでは、次のような処理によって、移動体１の自己位置が逐次推定される。すなわち、移動体１の初期位置姿勢（グローバル座標系で見た移動体１の初期位置及び初期姿勢の組）を設定した上で、カメラ７による撮像画像（ステレオ画像）が第２自己位置推定処理部１２に逐次入力されると共に、入力された撮像画像毎に、該撮像画像からＯＲＢ特徴点を検出する処理が逐次実行される。なお、第２自己位置推定処理部１２の処理での移動体１の初期位置姿勢は、例えば、移動体１の移動開始時には、該移動開始時又はその直前に前記第１自己位置推定処理部１１が求めた移動体１の推定位置姿勢が設定される。ただし、該初期位置姿勢は、補正処理部１６の後述する処理により、適宜、リセット（再設定）される。

そして、新たに入力された撮像画像（時刻ｔの撮像画像）から検出されたＯＲＢ特徴点と、一つ前に入力された過去の撮像画像（時刻ｔ－Δｔの撮像画像）から検出されたＯＲＢ特徴点とが比較され、ＯＲＢ特徴点同士のマッチングが行われる。このマッチングでは、特徴量の差の大きさ（絶対値）が所定の閾値以下となるＯＲＢ特徴点同士が互いに同じＯＲＢ特徴点（互いに対応するＯＲＢ特徴点）であると判定される。なお、Δｔは、撮像画像のサンプリング周期である。

そして、当該マッチングに基づいて、上記二つの時刻ｔ，ｔ－Δｔの間の期間での移動体１の移動量及び移動方向が推定され、さらに、この移動量及び移動方向の推定値と、時刻ｔ－Δｔでの移動体１の推定位置姿勢とから、時刻ｔでの移動体１の推定位置姿勢が決定される。

より詳しくは、二つの時刻ｔ，ｔ－Δｔの間の期間での移動体１の移動量及び移動方向は、ORB_SLAMのアルゴリズムによる自己位置の推定処理用のローカル座標系（初期位置を原点とするローカル座標系）で見た移動量及び移動方向として推定される。そして、時刻ｔ，ｔ－Δｔの間の期間での移動体１の移動量及び移動方向の推定値を、グローバル座標系で見た移動量及び移動方向に座標変換し、この座標変換後の移動量及び移動方向を、時刻ｔ－Δｔでの移動体１の推定位置姿勢（グローバル座標系での推定位置姿勢）に加えることよって、時刻ｔでの移動体１の推定位置姿勢（グローバル座標系での推定位置姿勢）が求められる。この場合、上記座標変換は、グローバル座標系で見た移動体１の初期位置姿勢により規定される上記ローカル座標系とグローバル座標系との相対的な位置及び姿勢関係に応じて決定される。

ORB_SLAMのアルゴリズムでは、基本的には、以上の如き処理を逐次実行することで、移動体１の自己位置が逐次推定される（移動体１の推定位置姿勢が逐次決定される）。この場合、ORB_SLAMのアルゴリズムにより求められる推定位置姿勢のうちの姿勢には、ヨー方向の姿勢だけでなくロール方向及びピッチ方向の姿勢も含まれる。

なお、本実施形態では、第２自己位置推定処理部１２は、補正処理部１６により後述する如く決定される補正パラメータが該補正処理部１６から与えられた場合には、上記の如く推定した自己位置（推定位置姿勢）を該補正用パラメータに応じて補正する。この補正処理については後述する。

また、ORB_SLAMのアルゴリズムでは、移動体１の自己位置の推定処理の実行過程で、推定位置姿勢の確からしさに関連する状態量として、移動体１の推定速度・角速度（並進速度及び角速度の推定値）と、検出されたＯＲＢ特徴点の個数（総数）及びマッチング個数（前記マッチングにより互いに同じあると判定されたＯＲＢ特徴点の個数）と、誤差関数の誤差値（前記マッチングにおける特徴量の差）と、推定状態（移動体１の推定速度・角速度の推定精度の高低度合い）とが生成される。

そして、第２自己位置推定処理部１２は、推定した移動体１の自己位置（推定位置姿勢）と該推定時の時刻である推定時刻とを逐次出力すると共に、該推定位置姿勢の確からしさに関連する状態量（以降、状態量２という）として、移動体１の推定速度・角速度（並進速度及び角速度の推定値）と、検出されたＯＲＢ特徴点の個数及びマッチング個数と、誤差関数の誤差値（前記マッチングにおける特徴量の差）と、推定状態（移動体１の推定速度・角速度の推定精度の高低度合い）とを出力する。なお、ＯＲＢ特徴点の個数及びマッチング個数の代わりに、ＯＲＢ特徴点の個数（総数）に対するマッチング個数の割合を出力するようにしてもよい。

第１及び第２自己位置推定処理部１１，１２からそれぞれ出力される前記状態量１，２は、重み係数学習処理部１４に入力される。この重み係数学習処理部１４は、本実施形態では、ニューラルネットワークにより構成される。そして、このニューラルネットワークは、前記状態量１，２から構成される特徴ベクトルをマルチモーダル情報として用いるマルチモーダル学習があらかじめ実施されている。

このマルチモーダル学習は、より詳しくは、種々様々な状態量１，２から構成される特徴ベクトルから、第１及び第２自己位置推定処理部１１，１２のそれぞれのアルゴリズムによる移動体１の自己位置の推定処理が、他のアルゴリズムに比してどの程度適切になされているかの度合いに相当する重み係数（第１及び第２自己位置推定処理部１１，１２から出力される推定位置姿勢のそれぞれに対応する重み係数）を生成する学習である。

重み係数学習処理部１４のニューラルネットワークは、このようなマルチモーダル学習があらかじめ実施されている。このため、第１及び第２自己位置推定処理部１１，１２のそれぞれから状態量１，２が入力された重み係数学習処理部１４は、該状態量Ｂ，Ａにより構成される特徴ベクトルから、第１及び第２自己位置推定処理部１１，１２のそれぞれに対応する重み係数をニューラルネットワークにより生成して出力する。

この場合、当該ニューラルネットワークは、基本的には、第１及び第２自己位置推定処理部１１，１２のそれぞれが出力する移動体１の推定位置姿勢のそれぞれの確からしさがより高いものほど、重み係数が大きな値となるように第１及び第２自己位置推定処理部１１，１２のそれぞれに対応する重み係数を生成して出力する。

例えば第１自己位置推定処理部１１による移動体１の自己位置の推定処理が、第２自己位置推定処理部１２に比して、より適切になされる状況では、第１自己位置推定処理部１３に対応する重み係数が、第２自己位置推定処理部１４に対応する重み係数よりも大きな値になるようにそれぞれの重み係数が生成される。

また、例えば、第２自己位置推定処理部１２による移動体１の自己位置の推定処理が、第１自己位置推定処理部１１に比して、より適切になされる状況では、第２自己位置推定処理部１２に対応する重み係数が、第１自己位置推定処理部１１に対応する重み係数よりも大きな値になるようにそれぞれの重み係数が生成される。

重み係数学習処理部１４から出力される第１及び第２自己位置推定処理部１１，１２のそれぞれに対応する重み係数は、自己位置特定処理部１５に入力される。また、該自己位置特定処理部１５には、第１及び第２自己位置推定処理部１１，１２のそれぞれから、移動体１の推定位置姿勢及び推定時刻が入力される。

そして、自己位置特定処理部１５は、第１及び第２自己位置推定処理部１１，１２のそれぞれの互いに同一時刻での推定位置姿勢を、入力された重み係数に応じて合成してなる位置姿勢を、移動体１の自己位置として特定する。すなわち、第１及び第２自己位置推定処理部１１，１２のそれぞれの推定位置姿勢に，それぞれに対応する重み係数を乗じたもの総和が移動体１の自己位置として特定される。なお、このとき各重み係数の和は１である。また、本実施形態では、自己位置特定処理部１５が特定する移動体１の自己位置のうちの姿勢は、ヨー方向での移動体１の姿勢であり、ロール方向及びピッチ方向の姿勢は含まれない。

本実施形態では、自己位置特定処理部１５は、以上説明した如く移動体１の自己位置を逐次特定する。そして、自己位置特定処理部１５は、特定した自己位置と、それに対応する推定時刻との組を出力する。

本実施形態の自己位置推定処理装置１０は、上記の如く第１及び第２自己位置推定処理部１１，１２，重み係数学習処理部１４及び自己位置特定処理部１５の処理を実行しながら、さらに補正処理部１６の処理を実行する。なお、以降の説明では、第１自己位置推定処理部１１が推定した自己位置（推定位置姿勢）を第１推定位置姿勢、第２自己位置推定処理部１２が推定した自己位置（推定位置姿勢）を第２推定位置姿勢、自己位置特定処理部１５が特定した自己位置を特定位置姿勢ということがある。

補正処理部１６には、第１自己位置推定処理部１１から出力される第１推定位置姿勢及び推定時刻の組と、第２自己位置推定処理部１２から出力される第２推定位置姿勢及び推定時刻の組と、自己位置特定処理部１５から出力される特定位置姿勢及び推定時刻の組とが入力される。そして、補正処理部１６は、入力された第１推定位置姿勢、第２推定位置姿勢及び特定位置姿勢のデータをそれぞれに対応する推定時刻と共に、図示しないメモリに時系列的に記憶保持する。この場合、メモリに記憶保持されるデータは、最新のものから所定時間前までの期間で補正処理部１５に入力されたデータである。

そして、補正処理部１６は、まず、第２推定位置姿勢が入力される毎に、第２自己位置推定処理部１２による推定処理が正常になされているか否かを判断する。ここで、本実施形態では、移動体１は、天井を有する建物内の床面上を移動するので、該移動体１の姿勢は、ロール方向（移動体１の前後方向の軸周り方向）、あるいは、ピッチ方向（移動体１の左右方向の軸周り方向）にはさほど変化しない。そこで、補正処理部１６は、例えば、第２自己位置推定処理部１２が最新の処理周期の時刻で新たに決定した第２推定位置姿勢のうちの姿勢が、その１つ前の処理周期の時刻で決定した第２推定位置姿勢のうちの姿勢から、ロール方向又はピッチ方向に所定量よりも大きく変化した場合には、第２自己位置推定処理部１２による移動体１の自己位置の推定処理が正常になされていないと判断する。

そして、この場合には、補正処理部１６は、最新の第２推定位置姿勢に対応する推定時刻又はこれにほぼ一致する時刻で第１自己位置推定処理部１１から出力された第１推定位置姿勢を、第２自己位置推定処理部１２の処理での移動体１の新たな初期位置姿勢として再設定するように第２自己位置推定処理部１２に指令すると共に、該第１推定位置姿勢を第１自己位置推定処理部１２に入力する。

このとき、第２自己位置推定処理部１２は、入力された第１推定位置姿勢を、移動体１の新たな初期位置姿勢として再設定し、ひいては、第２自己位置推定処理部１２の推定処理でのローカル座標系を再設定する。そして、第２自己位置推定処理部１２は、以後は、再設定したローカル座標系を用いて移動体１の自己位置の推定処理を実行する。

また、第２推定位置姿勢のうちの姿勢の、ロール方向及びピッチ方向への変化が所定量以下に留まっている場合には、補正処理部１６は、第２自己位置推定処理部１２による移動体１の自己位置の推定処理が正常になされていると判断する。

この場合には、補正処理部１６は、例えば、所定期間が経過する毎に（ひいては、間欠的に）、以下に説明する補正パラメータの決定処理を実行する。なお、該所定期間は、例えば、その期間の開始時から、第２自己位置推定処理部１２が取得するカメラ７の撮像画像のサンプリング数（フレーム数）が所定数に達するまでの期間である。

上記補正パラメータの決定処理では、補正処理部１６は、移動体１の移動開始後、１回目の所定期間が経過したときには、該１回目の所定期間内で、第２自己位置推定処理部１２から出力された第２推定位置姿勢の時系列データと、第１自己位置推定処理部１１から出力された第１推定位置姿勢の時系列データとを図示しないメモリから抽出する。また、２回目以降の所定期間が経過したときには、各回の所定期間内で、第１自己位置推定処理部１１から出力された第１推定位置姿勢の時系列データと、自己位置特定処理部１５から出力された特定位置姿勢の時系列データとを図示しないメモリから抽出する。なお、２回目以降の所定期間の起点のタイミングは、第２自己位置指定処理部１２での移動体１の初期位置姿勢を前記した如く再設定したときには、その再設定時が、新たな所定期間の起点のタイミングとされる。

ここで、第２自己位置推定処理部１２の推定処理は、移動体１の初期位置姿勢から、逐次求める移動体１の移動量及び移動方向を積算していく処理である。このため、移動体１の初期位置姿勢（グローバル座標系で見た初期位置姿勢）の誤差、あるいは、第２自己位置推定処理部１２が、逐次求める移動体１の移動量及び移動方向の推定誤差等に起因して、該第２自己位置推定処理部１２が逐次求める移動体１の第２推定位置姿勢の誤差が積算的に増大していく場合がある。

ひいては、第２推定位置姿勢の時系列データにより示される移動体１の移動軌道（以降、移動軌道Ｌｂという）が、第１推定位置姿勢の時系列データ又は特定位置姿勢の時系列データにより示される移動体１の移動軌道（以降、移動軌道Ｌａという）に対して、図３Ａに例示する如く乖離していく場合がある。また、本実施形態では、自己位置特定処理部１５は、前記した如く移動体１の特定位置姿勢を求めるので、移動体１の移動開始直後では、該自己位置特定処理部１５が求める特定位置姿勢の安定性が乏しい可能性がある。

そこで、補正処理部１６は、移動体１の移動開始直後の１回目の所定期間の経過時には、該所定期間内で、第２自己位置推定処理部１２から出力された第２推定位置姿勢の時系列データと、第１自己位置推定処理部１１から出力された第１推定位置姿勢の時系列データとを用いて、第２推定位置姿勢の時系列データにより示される移動体１の移動軌道Ｌｂを、第１推定位置姿勢の時系列データにより示される移動体１の移動軌道Ｌａに近づけように回転変換させる回転変換行列Ｒを決定する。該回転変換行列Ｒは、例えば、公知の点群処理ライブラリであるＰＣＬ（Point Cloud Library）を用いて決定される。図３Ｂに示す移動軌道Ｌｂ’は、移動軌道Ｌｂを回転変換行列Ｒにより回転変換した後の移動軌道を表している。

さらに、補正処理部１６は、第２推定位置姿勢の時系列データにより示される移動体１の移動軌道Ｌｂを回転変換行列Ｒにより回転変換してなる移動軌道Ｌｂ’の終端を、図３Ｃに例示する如く、第１推定位置姿勢の時系列データにより示される移動体１の移動軌道Ｌａの終端に一致させるよう並進移動させる並進変換操作量Ｔ（例えば並進移動ベクトル）を決定する、該並進変換操作量Ｔは、回転変換後の移動軌道Ｌｂ’の終端の位置（グローバル座標系での位置）と、移動軌道Ｌ２の移動体１の移動軌道の終端の位置（グローバル座標系での位置）とから求められる。図３Ｃに示す移動軌道Ｌｂ’’は、移動軌道Ｌｂ’を並進変換操作量Ｔにより並進変換した後の移動軌道を表している。

２回目以降の各回の所定域間の経過時には、補正処理部１６は、第１推定位置姿勢の時系列データの代わりに、自己位置特定処理部１５から出力された特定位置姿勢の時系列データを用いて、上記と同様に、回転変換行列Ｒ及び並進変換操作量Ｔを決定する。

本実施形態では、補正処理部１６は、基本的には、所定期間の経過毎に、上記の如く決定する回転変換行列Ｒ及び並進変換操作量Ｔを、第２推定位置姿勢を補正するための補正パラメータとして第２自己位置推定処理部１２に出力する。ただし、本実施形態では、第１自己位置推定処理部１１が求める推定位置姿勢の姿勢と、自己位置特定処理部１５が特定する特定位置姿勢の姿勢とは、ヨー方向の姿勢であるので、回転変換行列Ｒは、第２推定位置姿勢をヨー方向又は、ほぼヨー方向に回転させるものであることが望ましい。

そこで、本実施形態では、補正処理部１６は、上記の如く決定した回転変換行列Ｒのうちのヨー方向の回転変換に係る成分（具体的には、３行３列の成分）が「１」に十分近い値である場合（詳しくは、該成分と「１」との偏差の絶対値がゼロ近辺の所定の閾値よりも小さい場合）にだけ、回転変換行列Ｒ及び並進変換操作量Ｔを、補正パラメータとして第２自己位置推定処理部１２に出力すると共に、該補正パラメータによる第２推定位置姿勢の補正を第２自己位置推定処理部１２に指令する。

このとき、第２自己位置推定処理部１２は、以後の推定処理において、前記した如くORB_SLAMのアルゴリズムにより求めた第２推定位置姿勢を、補正処理部１６から入力された補正パラメータ（回転変換行列Ｒ及び並進変換操作量Ｔ）により補正する。すなわち、第２自己位置推定処理部１２は、ORB_SLAMのアルゴリズムにより求めた第２推定位置姿勢を、回転変換行列Ｒにより回転変換する共に、並進変換操作量Ｔにより並進変換することで、該２推定位置姿勢を補正する。そして、第２自己位置推定処理部１２は、この補正後の第２推定位置姿勢を出力する。

また、補正処理部１６は、回転変換行列Ｒのうちのヨー方向の回転変換に係る成分が「１」に十分近い値でない場合（該成分と「１」との偏差の絶対値がゼロ近辺の所定の閾値よりも大きい場合）には、第２自己位置推定処理部１２に、第２推定位置姿勢の補正を指令しないと共に、回転変換行列Ｒ及び並進変換操作量Ｔを出力しない。このとき、第２自己位置推定処理部１２は、ORB_SLAMのアルゴリズムにより求めた第２推定位置姿勢を補正せずに出力する。

本実施形態では、以上説明した如く、自己位置推定処理装置１０の処理が実行される。この場合、VISUAL_SLAMの一例としてのORB_SLAMのアルゴリズムにより移動体１の自己位置を推定する第２自己位置推定処理部１２が求める第２推定位置姿勢は、補正処理部１７が所定期間の経過毎に（間欠的に）、第１自己位置推定処理部１１が出力する第１推定位置姿勢又は自己位置特定処理部１２が出力する特定位置姿勢を用いて決定する補正パラメータ（回転変換行列Ｒ及び並進変換操作量Ｔ）に応じて補正される。これにより、第２自己位置推定処理部１２の推定処理での移動体１の初期位置姿勢の誤差や、該推定処理で逐次求める移動体１の移動量及び移動方向の誤差に起因して、第２推定位置姿勢の誤差が積算的に増大していくのが防止される。ひいては、自己位置特定処理部１５が出力する特定位置姿勢の信頼性を維持することを高いロバスト性で実現できる。

また、補正パラメータとしての回転変換行列Ｒ及び並進変換操作量Ｔは、第２自己位置推定処理部１２が出力する第２推定位置姿勢の時系列データにより示される移動軌道Ｌｂを、第１自己位置推定処理部１１が出力する第１推定位置姿勢又は自己位置特定処理部１２が出力する特定位置姿勢の時系列データより示される移動軌道Ｌａに近づけるように決定される。このため、補正パラメータとして適切な回転変換行列Ｒ及び並進変換操作量Ｔを決定することができる。

また、実施形態では、第１及び第２自己位置推定処理部１１，１２のそれぞれにより推定された自己位置（第１推定位置姿勢及び第２推定位置姿勢）を、重み係数学習処理部１４により第１及び第２自己位置推定処理部１１～１３のそれぞれ毎の状態量１，２に応じて決定される重み係数により合成することによって移動体１の自己位置が特定される。

そして、この場合、重み係数学習処理部１４は、第１及び第２自己位置推定処理部１１，１２のそれぞれ毎の状態量１，２を入力情報とするものであるので、移動体１の種々様々な移動環境で、状態量１，２に応じて出力すべき重み係数を事前に学習しておくことができる。

そのため、自己位置推定処理装置１０は、移動体１の種々様々な移動環境で、移動体１の自己位置を高い信頼性で推定（特定）することができる。

なお、本発明は以上説明した実施形態に限定されるものではなく、他の実施形態を採用することもできる。以下に、他の実施形態をいくつか例示する。

前記実施形態では、推定処理部１６は、２回目以降の各所定期間の経過時には、自己位置特定処理部１５から出力された特定位置姿勢の時系列データを用いて補正パラメータ（回転変換行列Ｒ及び並進変換操作量Ｔ）を決定した。ただし、２回目以降の各所定期間の経過時においても、第１自己位置推定処理部１１から出力された第１推定位置姿勢の時系列データを用いて補正パラメータ（回転変換行列Ｒ及び並進変換操作量Ｔ）を決定してもよい。

あるいは、例えば、第１自己位置推定処理部１１から出力される状態量１、もしくは、重み係数学習処理部１４が決定する重み係数に基づいて、第１自己位置推定処理部１１から出力された第１推定位置姿勢の信頼性又は確からしさの高低を判断し、その判断結果に応じて、第１推定位置姿勢の時系列データと特定位置姿勢の時系列データとのいずれを補正パラメータを決定するために使用するかを選定するようにしてもよい。より具体的には、第１推定位置姿勢の信頼性又は確からしさが高いと判断される場合には、第１推定位置姿勢の時系列データを補正パラメータを決定するために使用し、そうでない場合には、特定位置姿勢の時系列データを補正パラメータを決定するために使用するようにしてもよい。

また、前記実施計形態では、第１自己位置推定処理部１１により求める第１推定位置姿勢のうちの姿勢と、自己位置特定処理１５により求める特定位置姿勢のうちの姿勢とは、ヨー方向の姿勢だけであるが、ロール方向の姿勢及びピッチ方向の姿勢を含んでいてもよい。

また、第２自己位置推定処理部１２の推定処理用のアルゴリズムは、ORG_SLAMに限らず、他のSLAMのアルゴリズムであってもよい。

また、前記実施形態では、自己位置推定処理部として、二つの自己位置推定処理部（第１及び第２自己位置推定処理部１１，１２）を備えたものを例示したが、例えば第１自己位置推定処理部１１の代わりに、あるいは、第１及び第２自己位置推定処理部１１，１２に加えて、他のアルゴリズムにより移動体１の自己位置を推定し得る他の自己位置推定処理部を備えていてもよい。当該他の自己位置推定処理部としては、例えば、移動体１に搭載され得る磁気センサと、前記オドメトリ用センサ５と、移動体１の移動環境の磁気状態のマップとを用いて移動体１の自己位置を推定する自己位置推定処理部を採用し得る。この場合の自己位置の推定処理のアルゴリズとしては、第１自己位置推定処理部１１と同様のアルゴリズム（AMCLのアルゴリズム）を使用し得る。

なお、自己位置推定処理装置に、３つ以上の自己位置推定処理部を備えた場合には、前記実施形態と同様の手法で、各自己位置推定処理部に対応する重み係数を重み係数学習処理部１４により決定し、それらの重み係数を用いて、各自己位置推定処理部により推定した自己位置を合成することで、移動体１の自己位置を特定すればよい。

また、前記実施形態では、重み係数学習処理部１４として、マルチモーダル学習を行うニューラルネットワークを例示した。ただし、重み係数学習処理部１４として、例えば、LSTM（Long short-term memory）等の再帰型ニューラルネットワーク（RNN）等を使用してもよい。

また、前記実施形態では第２自己位置推定処理部１２に係る状態量２として、移動体１の推定速度・角速度（並進速度及び角速度の推定値）と、検出されたＯＲＢ特徴点の個数及びマッチング個数と、誤差関数の誤差値（前記マッチングにおける特徴量の差）と、推定状態とを用いた。ただし、これらの状態量のうちの一部の状態量、例えば移動体１の推定速度・角速度、誤差関数の誤差値、推定状態等を省略してもよい。

また、複数の自己位置推定処理部によりそれぞれ推定した自己位置から、移動体１の実際の自己位置を特定する手法は、前記実施形態で説明した手法に限られない。例えば、移動体１の動作状態、あるいは周囲環境の状態に基づいて、複数の自己位置推定処理部から選定した１つの自己位置推定処理部で推定された自己位置を、そのまま移動体１の実際の自己位置として特定するようにしてもよい。

また、前記実施形態では、測距センサとしてＬＲＦ６を使用した。ただし、ＬＲＦ６の代わりに、例えば超音波式の測距センサ等を使用してもよい。

また、前記実施形態では、移動体として、車輪型の移動体１を例示した。ただし、本発明の適用対象の移動体は、車輪型の移動体に限らず、例えば、脚式の移動体、倒立振子型の移動体等であってよい。

１…移動体、５…オドメトリ用センサ、６…レーザ・レンジ・ファインダ（測距センサ）、７…カメラ（センサ）。

Claims (4)

1. 複数のセンサの検出情報から、該複数のセンサのうちの一つ以上のセンサの検出情報を各々用いる複数のアルゴリズムのそれぞれにより移動体の自己位置を推定し、該複数のアルゴリズムのそれぞれにより得られた前記自己位置の推定値から、前記移動体の自己位置を特定する自己位置推定方法であって、
   前記複数のアルゴリズムは、ＳＬＡＭ（Simultaneous Localization and Mapping）のアルゴリズムである第Ａアルゴリズムと、該ＳＬＡＭと異なるアルゴリズムである第Ｂアルゴリズムとを含んでおり、
   前記第Ｂアルゴリズムにより得られた前記自己位置の推定値である第Ｂ自己位置推定値と、前記特定した自己位置である自己位置特定値とのうちのいずれか一方の自己位置に応じて、前記第Ａアルゴリズムにより得られる前記自己位置の推定値である第Ａ自己位置推定値を補正することを間欠的に実行する補正ステップを備えており、
   前記補正ステップでは、所定期間における前記第Ｂ自己位置推定値及び前記自己位置特定値のうちの前記一方の自己位置の時系列と、該所定期間における前記第Ａ自己位置推定値の時系列とに基づいて、前記第Ａ自己位置推定値の時系列により示される軌道を、前記一方の自己位置の時系列により示される軌道に近づけるように前記第Ａ自己位置推定値を補正するためのパラメータを決定し、当該決定したパラメータを用いて前記所定期間以後の前記第Ａ自己位置推定値を補正することを特徴とする自己位置推定方法。
2. 複数のセンサの検出情報から、該複数のセンサのうちの一つ以上のセンサの検出情報を各々用いる複数のアルゴリズムのそれぞれにより移動体の自己位置を推定し、該複数のアルゴリズムのそれぞれにより得られた前記自己位置の推定値から、前記移動体の自己位置を特定する自己位置推定方法であって、
   前記複数のアルゴリズムは、ＳＬＡＭ（Simultaneous Localization and Mapping）のアルゴリズムである第Ａアルゴリズムと、該ＳＬＡＭと異なるアルゴリズムである第Ｂアルゴリズムとを含んでおり、
   前記第Ｂアルゴリズムにより得られた前記自己位置の推定値である第Ｂ自己位置推定値と、前記特定した自己位置である自己位置特定値とのうちのいずれか一方の自己位置に応じて、前記第Ａアルゴリズムにより得られる前記自己位置の推定値である第Ａ自己位置推定値を補正することを間欠的に実行する補正ステップと、
   前記複数のアルゴリズムのそれぞれ毎に、各アルゴリズムの推定処理により得られる一つ以上の状態量であって、各アルゴリズムにより推定された前記自己位置の確からしさに関連する状態量を学習済のニューラルネットワークに入力し、当該入力された状態量から該ニューラルネットワークにより前記複数のアルゴリズムのそれぞれ毎の重み係数を決定するステップとを備えており、
   前記複数のアルゴリズムのそれぞれにより推定された前記自己位置を、前記決定された重み係数により合成して得られる自己位置を、前記移動体の自己位置特定値として特定することを特徴とする自己位置推定方法。
3. 請求項１又は２記載の自己位置推定方法において、
   前記複数のセンサは、前記移動体の外界を撮像するカメラを含み、
   前記第Ａアルゴリズムは、前記移動体の自己位置の推定のために、前記カメラの撮像画像から特徴点を検出し、当該検出された特徴点と、該撮像画像よりも過去の撮像画像から検出された特徴点とのマッチングを行う処理を逐次実行するように構成されたVisual_SLAMのアルゴリズムであることを特徴とする自己位置推定方法。
4. 請求項１～３のいずれか１項に記載の自己位置推定方法において、
   前記複数のセンサは、前記移動体の外界物までの距離を測定する測距センサを含み、
   前記第Ｂアルゴリズムは、前記測距センサによる測距データと、粒子フィルタとを用いて前記移動体の自己位置を推定するアルゴリズムであることを特徴とする自己位置推定方法。