JP7155686B2 - 自律移動体の位置推定装置 - Google Patents

Description

本発明は、自律移動体の位置推定装置に関する。

自律移動体の位置推定装置としては、例えば特許文献１に記載されているように、地図の構築及び使用を行う同時位置推定地図構築（ＳＬＡＭ）を用いて、自己位置の推定を行う技術が知られている。

特開２０１４－２２２５５０号公報

しかしながら、上記従来技術においては、自律移動体の周囲環境によっては自己位置推定結果にずれが生じることがあるため、自律移動体の自己位置を高精度に推定することが困難である。

本発明の目的は、自律移動体の自己位置の推定精度を向上させることができる自律移動体の位置推定装置を提供することである。

本発明の一態様は、自律移動体の位置を推定する自律移動体の位置推定装置であって、自己位置推定技術を用いて自律移動体の自己位置を推定する自己位置推定器と、自律移動体に具備されたセンサを有し、自律移動体の自己位置を検出する自己位置検出部と、自己位置推定器により得られた自律移動体の位置推定値と自己位置検出部により得られた自律移動体の位置検出値とを比較して、自律移動体の位置推定値の外れ値を抽出して除去する外れ値除去部と、外れ値除去部により外れ値が除去された自律移動体の位置推定値に基づいて、自律移動体の自己位置を最終的に推定する最終自己位置推定部とを備える。

このような位置推定装置においては、自己位置推定器により得られた自律移動体の位置推定値と自己位置検出部により得られた自律移動体の位置検出値との比較によって、自律移動体の位置推定値の外れ値が抽出されて除去される。そして、その外れ値が除去された自律移動体の位置推定値に基づいて、自律移動体の自己位置が最終的に推定される。このように自律移動体の自己位置を最終的に推定するときには、自律移動体の位置推定値の外れ値が使用されないため、自律移動体の自己位置が大きくずれることが防止される。これにより、自律移動体の自己位置の推定精度が向上する。

外れ値除去部は、自己位置検出部により今回得られた自律移動体の位置検出値と自己位置検出部により前回得られた自律移動体の位置検出値との相対位置と、自己位置推定器により今回得られた自律移動体の位置推定値と自己位置推定器により前回得られた自律移動体の位置推定値との相対位置との差分に関する計算式を計算し、計算式の計算値が閾値よりも大きいときに、今回得られた自律移動体の位置推定値を外れ値として除去してもよい。このような構成では、自律移動体の位置推定値の外れ値の抽出精度が高くなるため、自律移動体の自己位置の推定精度が更に向上する。

閾値は、自律移動体の周囲環境に応じて変更されてもよい。このような構成では、自律移動体の周囲環境に応じた最適な閾値が設定されるため、自律移動体の位置推定値の外れ値の抽出精度が更に高くなる。これにより、自律移動体の自己位置の推定精度が一層向上する。

自己位置推定器は、自律移動体の自己位置として自律移動体の二次元位置及び向きを推定し、自己位置検出部は、自律移動体の自己位置として自律移動体の二次元位置及び向きを検出してもよい。このような構成では、自律移動体の二次元位置だけでなく自律移動体の向きも考慮されるため、自律移動体の位置推定値の外れ値の抽出精度が一層高くなる。これにより、自律移動体の自己位置の推定精度がより一層向上する。

計算式は、自律移動体の二次元位置及び向きに関する変数を加算する式であってもよい。このような構成では、自律移動体の二次元位置及び向きの要素を含んだ１つの計算式を用いるので、自律移動体の二次元位置及び向きを総合的に判断して、自律移動体の位置推定値の外れ値を抽出することが可能となる。また、閾値の数が１つだけで済む。

本発明によれば、自律移動体の自己位置の推定精度を向上させることができる。

本発明の一実施形態に係る自律移動体の位置推定装置を示す概略構成図である。 図１に示された自己位置算出部により実行される自己位置算出処理手順の詳細を示すフローチャートである。 図１に示された外れ値除去部により実行される外れ値除去処理手順の詳細を示すフローチャートである。 図１に示された最終自己位置推定部により実行される自己位置推定処理手順の詳細を示すフローチャートである。 図３に示された外れ値除去処理手順の変形例を示すフローチャートである。

以下、本発明の実施形態について図面を参照して詳細に説明する。なお、図面において、同一または同等の要素には同じ符号を付し、重複する説明を省略する。

図１は、本発明の一実施形態に係る自律移動体の位置推定装置を示す概略構成図である。図１において、本実施形態の位置推定装置１は、自律移動体であるフォークリフト２の自動運転を実施する際に、フォークリフト２の位置を推定する装置である。位置推定装置１は、フォークリフト２に搭載されている。

位置推定装置１は、第１自己位置推定器３と、第２自己位置推定器４と、ホイールエンコーダ５と、ジャイロセンサ６と、コントローラ７と、表示器８とを備えている。

第１自己位置推定器３は、ＳＬＡＭ（Simultaneous Localization andMapping）を用いて、フォークリフト２の自己位置を推定する。ＳＬＡＭは、センサデータ及び地図データを使って自己位置推定を行う自己位置推定技術である。ＳＬＡＭは、センサとしてレーザレンジスキャナー等を利用して、自己位置推定と環境地図の作成とを同時に行う。ＳＬＡＭでは、建物の有無によって自己位置推定結果にずれが生じる。従って、ＳＬＡＭは、屋内での推定精度が屋外での推定精度よりも高い自己位置推定技術である。第１自己位置推定器３は、フォークリフト２の自己位置を絶対位置として推定する。

第２自己位置推定器４は、ＧＮＳＳ（Global NavigationSatellite System）を用いて、フォークリフト２の自己位置を推定する。ＧＮＳＳは、衛星を使って自己位置推定を行う自己位置推定技術である。ＧＮＳＳでは、衛星の状態によって自己位置推定結果にずれが生じる。従って、ＧＮＳＳは、屋外での推定精度が屋内での推定精度よりも高い自己位置推定技術である。第２自己位置推定器４は、フォークリフト２の自己位置を絶対位置として推定する。

第１自己位置推定器３及び第２自己位置推定器４は、フォークリフト２の自己位置としてフォークリフト２の二次元位置及び向きを推定する。ここでは、第１自己位置推定器３及び第２自己位置推定器４により推定された自己位置を位置推定値と称する。第１自己位置推定器３及び第２自己位置推定器４による自己位置推定は、単位時間（例えば数ｍＳ）毎に行われる。

ホイールエンコーダ５は、フォークリフト２のホイールに取り付けられ、フォークリフト２の速度を計測するセンサである。ジャイロセンサ６は、フォークリフト２の車体等に取り付けられ、フォークリフト２の角速度を計測するセンサである。

コントローラ７は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ７は、第１自己位置推定器３及び第２自己位置推定器４の自己位置推定結果とホイールエンコーダ５及びジャイロセンサ６の計測値とを入力し、所定の処理を行い、フォークリフト２の自己位置を推定する。コントローラ７は、自己位置算出部１０と、外れ値除去部１１と、最終自己位置推定部１２とを有している。

自己位置算出部１０は、ホイールエンコーダ５及びジャイロセンサ６の計測値に基づいて、フォークリフト２の自己位置を算出する。ホイールエンコーダ５、ジャイロセンサ６及び自己位置算出部１０は、フォークリフト２に具備されたセンサを有し、フォークリフト２の自己位置を検出する自己位置検出部１３を構成する。自己位置検出部１３は、フォークリフト２の自己位置としてフォークリフト２の二次元位置及び向きを検出する。ここでは、自己位置検出部１３により検出された自己位置を位置検出値と称する。

外れ値除去部１１は、第１自己位置推定器３及び第２自己位置推定器４により得られたフォークリフト２の位置推定値と自己位置検出部１３により得られたフォークリフト２の位置検出値とを比較して、フォークリフト２の位置推定値の外れ値を抽出して除去する。

最終自己位置推定部１２は、第１自己位置推定器３及び第２自己位置推定器４によりそれぞれ得られた２つのフォークリフト２の位置推定値に基づいて、フォークリフト２の自己位置を最終的に推定する。このとき、最終自己位置推定部１２は、外れ値除去部１１により外れ値が除去されたフォークリフト２の位置推定値に基づいて、フォークリフト２の自己位置を最終的に推定する。また、最終自己位置推定部１２は、フォークリフト２の周囲環境に基づいて第１自己位置推定器３及び第２自己位置推定器４の重み係数を設定し、その重み係数を用いてフォークリフト２の自己位置を最終的に推定する。

表示器８は、最終自己位置推定部１２によって最終的に推定されたフォークリフト２の自己位置を表示する。表示器８は、例えばフォークリフト２の二次元位置及び向きを画面表示する。

図２は、自己位置算出部１０により実行される自己位置算出処理手順の詳細を示すフローチャートである。図２において、自己位置算出部１０は、まずホイールエンコーダ５及びジャイロセンサ６の計測値を取得する（手順Ｓ１０１）。

続いて、自己位置算出部１０は、ホイールエンコーダ５及びジャイロセンサ６の計測値に基づいて、フォークリフト２の位置検出値を算出する（手順Ｓ１０２）。具体的には、自己位置算出部１０は、ホイールエンコーダ５により計測されたフォークリフト２の速度を積分することにより、フォークリフト２の二次元位置（ＸＹ座標の位置）を求める。また、自己位置算出部１０は、ジャイロセンサ６により計測されたフォークリフト２の角速度を積分することにより、フォークリフト２の向きを求める。続いて、自己位置算出部１０は、フォークリフト２の位置検出値を外れ値除去部１１に出力する（手順Ｓ１０３）。

図３は、外れ値除去部１１により実行される外れ値除去処理手順の詳細を示すフローチャートである。図３において、外れ値除去部１１は、まず第１自己位置推定器３及び第２自己位置推定器４により得られたフォークリフト２の位置推定値を取得する（手順Ｓ１１１）。

続いて、外れ値除去部１１は、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値と第１自己位置推定器３及び第２自己位置推定器４により前回得られたフォークリフト２の位置推定値との相対位置（ｘ，ｙ，θ）を算出する（手順Ｓ１１２）。前回得られたフォークリフト２の位置推定値は、単位時間前に推定されたフォークリフト２の自己位置であり、メモリに記憶されている。

相対位置（ｘ，ｙ，θ）は、例えば第１自己位置推定器３により得られた位置推定値と第２自己位置推定器４により得られた位置推定値との平均値として算出してもよいし、或いは第１自己位置推定器３により得られた位置推定値及び第２自己位置推定器４により得られた位置推定値のうち前回算出された値に近いほうを採用してもよい。

続いて、外れ値除去部１１は、自己位置算出部１０により得られたフォークリフト２の位置検出値を取得する（手順Ｓ１１３）。続いて、外れ値除去部１１は、自己位置算出部１０により今回得られたフォークリフト２の位置検出値と自己位置算出部１０により前回得られたフォークリフト２の位置検出値との相対位置（ｘ_ｄ，ｙ_ｄ，θ_ｄ）を算出する（手順Ｓ１１４）。前回得られたフォークリフト２の位置検出値は、単位時間前に検出されたフォークリフト２の自己位置であり、メモリに記憶されている。

続いて、外れ値除去部１１は、フォークリフト２の周囲環境に基づいて、閾値δｘ，δｙ，δθを設定する（手順Ｓ１１５）。閾値δｘ，δｙは、フォークリフト２の二次元位置（ｘ軸位置，ｙ軸位置）に係る閾値である。閾値δθは、フォークリフト２の向きに係る閾値である。フォークリフト２の周囲環境は、最終自己位置推定部１２により前回推定されたフォークリフト２の自己位置の環境である。なお、フォークリフト２の周囲環境は、環境地図を作成するＳＬＡＭを用いる第１自己位置推定器３によって把握することができる。

例えば、フォークリフト２が雨等で滑りやすい屋外を走行する場合、ホイールエンコーダ５は滑りの影響が大きいため、ホイールエンコーダ５の計測値に誤差が出やすいが、ジャイロセンサ６は滑りの影響が少ないため、ジャイロセンサ６の計測値に誤差が出にくい。従って、屋外では、屋内に比べて閾値δｘ，δｙを大きく（緩く）設定する。例えば、屋外では、閾値δｘ，δｙを１ｍに設定し、屋内では、閾値δｘ，δｙを０．５ｍに設定する。閾値δθについては、屋外と屋内とで等しくする。例えば、屋外及び屋内において、閾値δθを０．３ｒａｄに設定する。このように閾値δｘ，δｙ，δθは、フォークリフト２の周囲環境に応じて変更される。

また、フォークリフト２が例えば冷蔵庫の内外のような温度差が大きい屋内及び屋外を走行する場合には、ジャイロセンサ６の計測値に誤差が出やすい。このため、屋内では、屋外に比べて閾値δθを大きく（緩く）設定する。

続いて、外れ値除去部１１は、手順Ｓ１１４で算出された相対位置（ｘ_ｄ，ｙ_ｄ，θ_ｄ）と手順Ｓ１１２で算出された（ｘ，ｙ，θ）との差分に関する計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）を計算する（手順Ｓ１１６）。計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）は、下記式で表される。

続いて、外れ値除去部１１は、上記式にも示されるように、計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）の計算値がそれぞれ閾値δｘ，δｙ，δθよりも大きいかどうかを判断する（手順Ｓ１１７）。

外れ値除去部１１は、計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）の計算値が閾値δｘ，δｙ，δθよりも大きいと判断したときは、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値を外れ値として除去する（手順Ｓ１１８）。このとき、外れ値除去部１１は、計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）の計算値の少なくとも１つが閾値δｘ，δｙ，δθよりも大きいとき、つまり上記式の条件を１つでも満たすときは、今回得られたフォークリフト２の位置推定値を外れ値とみなす。

外れ値除去部１１は、計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）の計算値の全てが閾値δｘ，δｙ，δθよりも大きくない、つまり上記式の条件を全て満たさないと判断したときは、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値を最終自己位置推定部１２に出力する（手順Ｓ１１９）。

図４は、最終自己位置推定部１２により実行される自己位置推定処理手順の詳細を示すフローチャートである。ここでは、最終自己位置推定部１２は、第１自己位置推定器３及び第２自己位置推定器４によりそれぞれ得られた２つのフォークリフト２の位置推定値を融合し、線形最小分散推定法を利用してフォークリフト２の自己位置を最終的に推定する。

図４において、最終自己位置推定部１２は、まず第１自己位置推定器３により得られた最新のフォークリフト２の位置推定値（ｘｓ，ｙｓ，θｓ）と、第２自己位置推定器４により得られた最新のフォークリフト２の位置推定値（ｘｇ，ｙｇ，θｇ）とを取得する（手順Ｓ１２１）。

続いて、最終自己位置推定部１２は、フォークリフト２の周囲環境に基づいて、第１自己位置推定器３の誤差分散（σｓｘ^２，σｓｙ^２，σｓθ^２）と、第２自己位置推定器４の誤差分散（σｇｘ^２，σｇｙ^２，σｇθ^２）とを設定する（手順Ｓ１２２）。フォークリフト２の周囲環境は、最終自己位置推定部１２により前回推定されたフォークリフト２の自己位置の環境である。また、誤差分散とは、重み係数のことである。誤差分散σｓｘ^２，σｓｙ^２及び誤差分散σｇｘ^２，σｇｙ^２は、フォークリフト２の二次元位置（ｘ軸位置，ｙ軸位置）に係る重み係数である。誤差分散σｓθ^２及び誤差分散σｇθ^２は、フォークリフト２の向きに係る重み係数である。

このとき、屋内では、ＳＬＡＭの推定精度がＧＮＳＳの推定精度よりも高いため、第１自己位置推定器３の誤差分散を第２自己位置推定器４の誤差分散よりも低く設定する。例えば、第１自己位置推定器３の誤差分散を４とし、第２自己位置推定器４の誤差分散を３６とする。屋外では、ＧＮＳＳの推定精度がＳＬＡＭの推定精度よりも高いため、第２自己位置推定器４の誤差分散を第１自己位置推定器３の誤差分散よりも低く設定する。例えば、第１自己位置推定器３の誤差分散を３６とし、第２自己位置推定器４の誤差分散を４とする。また、屋内と屋内との中間では、第１自己位置推定器３の誤差分散と第２自己位置推定器４の誤差分散とを等しく設定する。例えば、第１自己位置推定器３及び第２自己位置推定器４の誤差分散を何れも２５とする。なお、屋内と屋内との中間とは、例えば屋根はないが建物の近くで柱が存在しているような場所である。

続いて、最終自己位置推定部１２は、第１自己位置推定器３により得られた最新のフォークリフト２の位置推定値（ｘｓ，ｙｓ，θｓ）、第２自己位置推定器４により得られた最新のフォークリフト２の位置推定値（ｘｇ，ｙｇ，θｇ）、第１自己位置推定器３の誤差分散（σｓｘ^２，σｓｙ^２，σｓθ^２）及び第２自己位置推定器４の誤差分散（σｇｘ^２，σｇｙ^２，σｇθ^２）を用いて、下記式により最終的なフォークリフト２の自己位置（ｘ_ｅｓｔ，ｙ_ｅｓｔ，θ_ｅｓｔ）を推定する。

以上のように本実施形態にあっては、第１自己位置推定器３及び第２自己位置推定器４により得られたフォークリフト２の位置推定値と自己位置検出部１３により得られたフォークリフト２の位置検出値との比較によって、フォークリフト２の位置推定値の外れ値が抽出されて除去される。そして、その外れ値が除去されたフォークリフト２の位置推定値に基づいて、フォークリフト２の自己位置が最終的に推定される。このようにフォークリフト２の自己位置を最終的に推定するときには、フォークリフト２の位置推定値の外れ値が使用されないため、フォークリフト２の自己位置が大きくずれることが防止される。これにより、フォークリフト２の自己位置の推定精度が向上する。その結果、フォークリフト２の自動運転時に、フォークリフト２の安定した走行が可能となる。

また、本実施形態では、自己位置検出部１３により今回得られたフォークリフト２の位置検出値と自己位置検出部１３により前回得られたフォークリフト２の位置検出値との相対位置（ｘ_ｄ，ｙ_ｄ，θ_ｄ）と、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値と第１自己位置推定器３及び第２自己位置推定器４により前回得られたフォークリフト２の位置推定値との相対位置（ｘ，ｙ，θ）との差分に関する計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）が計算され、計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）の計算値が閾値δｘ，δｙ，δθよりも大きいときに、今回得られたフォークリフト２の位置推定値が外れ値として除去される。従って、フォークリフト２の位置推定値の外れ値の抽出精度が高くなるため、フォークリフト２の自己位置の推定精度が更に向上する。

また、本実施形態では、閾値δｘ，δｙ，δθは、フォークリフト２の周囲環境に応じて変更される。従って、フォークリフト２の周囲環境に応じた最適な閾値δｘ，δｙ，δθが設定されるため、フォークリフト２の位置推定値の外れ値の抽出精度が更に高くなる。これにより、フォークリフト２の自己位置の推定精度が一層向上する。

また、本実施形態では、第１自己位置推定器３及び第２自己位置推定器４は、フォークリフト２の自己位置としてフォークリフト２の二次元位置及び向きを推定し、自己位置検出部１３は、フォークリフト２の自己位置としてフォークリフト２の二次元位置及び向きを検出する。このようにフォークリフト２の二次元位置だけでなくフォークリフト２の向きも考慮されるため、フォークリフト２の位置推定値の外れ値の抽出精度が一層高くなる。これにより、フォークリフト２の自己位置の推定精度がより一層向上する。

図５は、図３に示された外れ値除去処理手順の変形例を示すフローチャートである。図５において、外れ値除去部１１は、図３に示されるフローチャートと同様に、手順Ｓ１１１～Ｓ１１４を実行する。

外れ値除去部１１は、手順Ｓ１１４を実行した後、フォークリフト２の周囲環境に基づいて、閾値δ及び重み係数α，β，γを設定する（手順Ｓ１１５Ａ）。重み係数αは、フォークリフト２のｘ軸位置に係る重み係数である。重み係数βは、フォークリフト２のｙ軸位置に係る重み係数である。重み係数γは、フォークリフト２の向きに係る重み係数である。このとき、例えば雨等で滑りやすい屋外では、屋内に比べて重み係数α，βを小さく設定する。

続いて、外れ値除去部１１は、手順Ｓ１１４で算出された相対位置（ｘ_ｄ，ｙ_ｄ，θ_ｄ）と手順Ｓ１１２で算出された（ｘ，ｙ，θ）との差分に関する計算式ｆを計算する（手順Ｓ１１６Ａ）。計算式ｆは、下記式で表される。計算式ｆは、フォークリフト２の二次元位置及び向きに関する変数を加算する式である。

続いて、外れ値除去部１１は、上記式にも示されるように、計算式ｆの計算値が閾値δよりも大きいかどうかを判断する（手順Ｓ１１７Ａ）。外れ値除去部１１は、計算式ｆの計算値が閾値δよりも大きいと判断したときは、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値を外れ値として除去する（手順Ｓ１１８）。

外れ値除去部１１は、計算式ｆの計算値が閾値δよりも大きくないと判断したときは、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値を最終自己位置推定部１２に出力する（手順Ｓ１１９）。

本変形例においては、フォークリフト２の二次元位置及び向きの要素を含んだ１つの計算式ｆを用いるので、フォークリフト２の二次元位置及び向きを総合的に判断して、フォークリフト２の位置推定値の外れ値を抽出することが可能となる。また、閾値の数が１つだけで済む。

なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、ＳＬＡＭを用いてフォークリフト２の自己位置を推定する第１自己位置推定器３と、ＧＮＳＳを用いてフォークリフト２の自己位置を推定する第２自己位置推定器４とを使用しているが、特にその形態には限られず、第１自己位置推定器３及び第２自己位置推定器４の何れか一方のみを使用してもよい。この場合には、最終自己位置推定部１２は、第１自己位置推定器３及び第２自己位置推定器４によりそれぞれ得られた２つのフォークリフト２の位置推定値を融合しなくて済む。また、使用する自己位置推定技術としては、特にＳＬＡＭまたはＧＮＳＳには限られない。

また、上記実施形態では、ホイールエンコーダ５によりフォークリフト２の速度を計測し、ジャイロセンサ６によりフォークリフト２の角速度を計測しているが、使用するセンサとしては、特にそれには限られない。例えば、フォークリフト２の速度を計測するセンサとしては、ロータリーエンコーダ等を使用してもよく、フォークリフト２の角速度を計測するセンサとしては、操舵角センサまたは操舵ポテンショメータ等を使用してもよい。また、フォークリフト２の二次元位置及び向きをセンサにより直接検出してもよい。

また、上記実施形態では、最終自己位置推定部１２は、第１自己位置推定器３及び第２自己位置推定器４によりそれぞれ得られた２つのフォークリフト２の位置推定値を融合し、線形最小分散推定法を利用してフォークリフト２の自己位置を最終的に推定しているが、２つのフォークリフト２の位置推定値を融合する場合には、例えばカルマンフィルタを利用してフォークリフト２の自己位置を最終的に推定してもよい。

さらに、上記実施形態では、フォークリフト２の位置を推定しているが、本発明は、フォークリフト以外の産業車両あるいは移動ロボット等といった自律移動体にも適用可能である。

１…位置推定装置、２…フォークリフト（自律移動体）、３…第１自己位置推定器（自己位置推定器）、４…第２自己位置推定器（自己位置推定器）、５…ホイールエンコーダ（センサ）、６…ジャイロセンサ（センサ）、１１…外れ値除去部、１２…最終自己位置推定部、１３…自己位置検出部。

Claims (4)

1. 自律移動体の位置を推定する自律移動体の位置推定装置であって、
   自己位置推定技術を用いて前記自律移動体の自己位置を推定する第１自己位置推定器と、
   前記第１自己位置推定器とは異なる自己位置推定技術を用いて前記自律移動体の自己位置を推定する第２自己位置推定器と、
   前記自律移動体に具備されたセンサにより計測された前記自律移動体の速度及び角速度に基づいて、前記自律移動体の自己位置を検出する自己位置検出部と、
   前記第１自己位置推定器及び前記第２自己位置推定器により得られた前記自律移動体の位置推定値と前記自己位置検出部により得られた前記自律移動体の位置検出値とを比較して、前記第１自己位置推定器及び前記第２自己位置推定器により得られた前記自律移動体の位置推定値の外れ値を抽出して除去する外れ値除去部と、
   前記外れ値除去部により前記外れ値が除去された前記自律移動体の位置推定値に基づいて、線形最小分散推定法またはカルマンフィルタを利用して、前記自律移動体の自己位置を最終的に推定する最終自己位置推定部とを備え、
   前記外れ値除去部は、前記自己位置検出部により今回得られた前記自律移動体の位置検出値と前記自己位置検出部により前回得られた前記自律移動体の位置検出値との相対位置と、前記第１自己位置推定器及び前記第２自己位置推定器により今回得られた前記自律移動体の位置推定値と前記第１自己位置推定器及び前記第２自己位置推定器により前回得られた前記自律移動体の位置推定値との相対位置との差分に関する計算式を計算し、前記計算式の計算値が閾値よりも大きいときに、前記今回得られた自律移動体の位置推定値を前記外れ値として除去し、
   前記今回得られた前記自律移動体の位置推定値と前記前回得られた前記自律移動体の位置推定値との相対位置は、前記第１自己位置推定器により得られた前記自律移動体の位置推定値と前記第２自己位置推定器により得られた前記自律移動体の位置推定値との平均値、或いは前記第１自己位置推定器により得られた前記自律移動体の位置推定値及び前記第２自己位置推定器により得られた前記自律移動体の位置推定値のうち前回推定された前記自律移動体の自己位置に近いほうの値を用いて算出され、
   前記最終自己位置推定部は、前記自律移動体の周囲環境に基づいて前記第１自己位置推定器及び前記第２自己位置推定器の誤差分散を設定し、前記第１自己位置推定器により得られた前記自律移動体の位置推定値と前記第２自己位置推定器により得られた前記自律移動体の位置推定値と前記第１自己位置推定器及び前記第２自己位置推定器の誤差分散とに基づいて、前記自律移動体の自己位置を最終的に推定する自律移動体の位置推定装置。
2. 前記閾値は、前記自律移動体の周囲環境に応じて変更される請求項１記載の自律移動体の位置推定装置。
3. 前記第１自己位置推定器及び前記第２自己位置推定器は、前記自律移動体の自己位置として前記自律移動体の二次元位置及び向きを推定し、
   前記自己位置検出部は、前記自律移動体の自己位置として前記自律移動体の二次元位置及び向きを検出する請求項１または２記載の自律移動体の位置推定装置。
4. 前記計算式は、前記自律移動体の二次元位置及び向きに関する変数を加算する式である請求項３記載の自律移動体の位置推定装置。

Images