(57)【要約】【課題】自律移動体の自己位置の推定精度を向上させることができる自律移動体の位置推定装置を提供する。【解決手段】位置推定装置１は、異なる自己位置推定技術を用いてフォークリフト２の自己位置を推定する第１自己位置推定器３及び第２自己位置推定器４と、第１自己位置推定器３及び第２自己位置推定器４によりそれぞれ得られた２つのフォークリフト２の位置推定値に基づいて、フォークリフト２の自己位置を最終的に推定する最終自己位置推定部１２とを備え、最終自己位置推定部１２は、フォークリフト２の周囲環境に基づいて第１自己位置推定器３及び第２自己位置推定器４の誤差分散（重み係数）を設定し、第１自己位置推定器３及び第２自己位置推定器４の誤差分散を用いてフォークリフト２の自己位置を最終的に推定する。【選択図】図１
2020008461.tif 000002
【特許請求の範囲】
【請求項１】
自律移動体の位置を推定する自律移動体の位置推定装置であって、
異なる自己位置推定技術を用いて前記自律移動体の自己位置を推定する複数の自己位置推定器と、
前記複数の自己位置推定器によりそれぞれ得られた複数の前記自律移動体の位置推定値に基づいて、前記自律移動体の自己位置を最終的に推定する最終自己位置推定部とを備え、
前記最終自己位置推定部は、前記自律移動体の周囲環境に基づいて前記複数の自己位置推定器の重み係数を設定し、前記複数の自己位置推定器の重み係数を用いて前記自律移動体の自己位置を最終的に推定する自律移動体の位置推定装置。
【請求項２】
前記最終自己位置推定部は、前回最終的に推定された前記自律移動体の自己位置の環境に基づいて前記重み係数を設定する請求項１記載の自律移動体の位置推定装置。
【請求項３】
前記自己位置推定器は、前記自律移動体の自己位置として前記自律移動体の二次元位置及び向きを推定し、
前記最終自己位置推定部は、前記自己位置推定器の重み係数として前記自律移動体の二次元位置及び向きに係る重み係数を設定する請求項１または２記載の自律移動体の位置推定装置。
【請求項４】
前記自己位置推定器の数は２つであり、
前記２つの自己位置推定器の一方は、屋内での推定精度が屋外での推定精度よりも高い自己位置推定技術を用いて前記自律移動体の自己位置を推定し、
前記２つの自己位置推定器の他方は、屋外での推定精度が屋内での推定精度よりも高い自己位置推定技術を用いて前記自律移動体の自己位置を推定する請求項１〜３の何れか一項記載の自律移動体の位置推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自律移動体の位置推定装置に関する。
【背景技術】
【０００２】
自律移動体の位置推定装置としては、例えば特許文献１に記載されているように、地図の構築及び使用を行う同時位置推定地図構築（ＳＬＡＭ）を用いて、自己位置の推定を行う技術が知られている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１４−２２２５５０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、上記従来技術においては、自律移動体の周囲環境によっては自己位置推定結果にずれが生じることがあるため、自律移動体の自己位置を高精度に推定することが困難である。
【０００５】
本発明の目的は、自律移動体の自己位置の推定精度を向上させることができる自律移動体の位置推定装置を提供することである。
【課題を解決するための手段】
【０００６】
本発明の一態様は、自律移動体の位置を推定する自律移動体の位置推定装置であって、異なる自己位置推定技術を用いて自律移動体の自己位置を推定する複数の自己位置推定器と、複数の自己位置推定器によりそれぞれ得られた複数の自律移動体の位置推定値に基づいて、自律移動体の自己位置を最終的に推定する最終自己位置推定部とを備え、最終自己位置推定部は、自律移動体の周囲環境に基づいて複数の自己位置推定器の重み係数を設定し、複数の自己位置推定器の重み係数を用いて自律移動体の自己位置を最終的に推定する。
【０００７】
このような位置推定装置においては、自己位置推定技術が異なる複数の自己位置推定器により自律移動体の自己位置が推定され、複数の自己位置推定器により得られた各自律移動体の位置推定値に基づいて、自律移動体の自己位置が最終的に推定される。このとき、自律移動体の周囲環境に基づいて各自己位置推定器の重み係数が設定され、各自己位置推定器の重み係数を用いて自律移動体の自己位置が最終的に推定される。これにより、自律移動体の自己位置の推定精度が向上する。
【０００８】
最終自己位置推定部は、前回最終的に推定された自律移動体の自己位置の環境に基づいて重み係数を設定してもよい。このような構成では、現在の自律移動体の周囲環境が精度良く得られるため、各自己位置推定器の重み係数が適切に設定される。これにより、自律移動体の自己位置の推定精度が更に向上する。
【０００９】
自己位置推定器は、自律移動体の自己位置として自律移動体の二次元位置及び向きを推定し、最終自己位置推定部は、自己位置推定器の重み係数として自律移動体の二次元位置及び向きに係る重み係数を設定してもよい。このような構成では、自律移動体の二次元位置だけでなく自律移動体の向きも考慮されるため、自律移動体の自己位置の推定精度が一層向上する。
【００１０】
自己位置推定器の数は２つであり、２つの自己位置推定器の一方は、屋内での推定精度が屋外での推定精度よりも高い自己位置推定技術を用いて自律移動体の自己位置を推定し、２つの自己位置推定器の他方は、屋外での推定精度が屋内での推定精度よりも高い自己位置推定技術を用いて自律移動体の自己位置を推定してもよい。このような構成では、自律移動体が屋外及び屋内を移動する際に、屋外及び屋内に応じた適切な各自己位置推定器の重み係数が設定されることになるため、自律移動体の自己位置の推定精度がより一層向上する。
【発明の効果】
【００１１】
本発明によれば、自律移動体の自己位置の推定精度を向上させることができる。
【図面の簡単な説明】
【００１２】
【図１】本発明の一実施形態に係る自律移動体の位置推定装置を示す概略構成図である。
【図２】図１に示された自己位置算出部により実行される自己位置算出処理手順の詳細を示すフローチャートである。
【図３】図１に示された外れ値除去部により実行される外れ値除去処理手順の詳細を示すフローチャートである。
【図４】図１に示された最終自己位置推定部により実行される自己位置推定処理手順の詳細を示すフローチャートである。
【図５】図３に示された外れ値除去処理手順の変形例を示すフローチャートである。
【発明を実施するための形態】
【００１３】
以下、本発明の実施形態について図面を参照して詳細に説明する。なお、図面において、同一または同等の要素には同じ符号を付し、重複する説明を省略する。
【００１４】
図１は、本発明の一実施形態に係る自律移動体の位置推定装置を示す概略構成図である。図１において、本実施形態の位置推定装置１は、自律移動体であるフォークリフト２の自動運転を実施する際に、フォークリフト２の位置を推定する装置である。位置推定装置１は、フォークリフト２に搭載されている。
【００１５】
位置推定装置１は、第１自己位置推定器３と、第２自己位置推定器４と、ホイールエンコーダ５と、ジャイロセンサ６と、コントローラ７と、表示器８とを備えている。つまり、位置推定装置１は、複数（ここでは２つ）の自己位置推定器を備えている。
【００１６】
第１自己位置推定器３は、ＳＬＡＭ（Simultaneous Localization andMapping）を用いて、フォークリフト２の自己位置を推定する。ＳＬＡＭは、センサデータ及び地図データを使って自己位置推定を行う自己位置推定技術である。ＳＬＡＭは、センサとしてレーザレンジスキャナー等を利用して、自己位置推定と環境地図の作成とを同時に行う。ＳＬＡＭでは、建物の有無によって自己位置推定結果にずれが生じる。従って、ＳＬＡＭは、屋内での推定精度が屋外での推定精度よりも高い自己位置推定技術である。第１自己位置推定器３は、フォークリフト２の自己位置を絶対位置として推定する。
【００１７】
第２自己位置推定器４は、ＧＮＳＳ（Global NavigationSatellite System）を用いて、フォークリフト２の自己位置を推定する。ＧＮＳＳは、衛星を使って自己位置推定を行う自己位置推定技術である。ＧＮＳＳでは、衛星の状態によって自己位置推定結果にずれが生じる。従って、ＧＮＳＳは、屋外での推定精度が屋内での推定精度よりも高い自己位置推定技術である。第２自己位置推定器４は、フォークリフト２の自己位置を絶対位置として推定する。
【００１８】
第１自己位置推定器３及び第２自己位置推定器４は、フォークリフト２の自己位置としてフォークリフト２の二次元位置及び向きを推定する。ここでは、第１自己位置推定器３及び第２自己位置推定器４により推定された自己位置を位置推定値と称する。第１自己位置推定器３及び第２自己位置推定器４による自己位置推定は、単位時間（例えば数ｍＳ）毎に行われる。
【００１９】
ホイールエンコーダ５は、フォークリフト２のホイールに取り付けられ、フォークリフト２の速度を計測するセンサである。ジャイロセンサ６は、フォークリフト２の車体等に取り付けられ、フォークリフト２の角速度を計測するセンサである。
【００２０】
コントローラ７は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ７は、第１自己位置推定器３及び第２自己位置推定器４の自己位置推定結果とホイールエンコーダ５及びジャイロセンサ６の計測値とを入力し、所定の処理を行い、フォークリフト２の自己位置を推定する。コントローラ７は、自己位置算出部１０と、外れ値除去部１１と、最終自己位置推定部１２とを有している。
【００２１】
自己位置算出部１０は、ホイールエンコーダ５及びジャイロセンサ６の計測値に基づいて、フォークリフト２の自己位置を算出する。ホイールエンコーダ５、ジャイロセンサ６及び自己位置算出部１０は、フォークリフト２に具備されたセンサを有し、フォークリフト２の自己位置を検出する自己位置検出部１３を構成する。自己位置検出部１３は、フォークリフト２の自己位置としてフォークリフト２の二次元位置及び向きを検出する。ここでは、自己位置検出部１３により検出された自己位置を位置検出値と称する。
【００２２】
外れ値除去部１１は、第１自己位置推定器３及び第２自己位置推定器４により得られたフォークリフト２の位置推定値と自己位置検出部１３により得られたフォークリフト２の位置検出値とを比較して、フォークリフト２の位置推定値の外れ値を抽出して除去する。
【００２３】
最終自己位置推定部１２は、第１自己位置推定器３及び第２自己位置推定器４によりそれぞれ得られた２つのフォークリフト２の位置推定値に基づいて、フォークリフト２の自己位置を最終的に推定する。このとき、最終自己位置推定部１２は、外れ値除去部１１により外れ値が除去されたフォークリフト２の位置推定値に基づいて、フォークリフト２の自己位置を最終的に推定する。また、最終自己位置推定部１２は、フォークリフト２の周囲環境に基づいて第１自己位置推定器３及び第２自己位置推定器４の重み係数を設定し、その重み係数を用いてフォークリフト２の自己位置を最終的に推定する。
【００２４】
表示器８は、最終自己位置推定部１２によって最終的に推定されたフォークリフト２の自己位置を表示する。表示器８は、例えばフォークリフト２の二次元位置及び向きを画面表示する。
【００２５】
図２は、自己位置算出部１０により実行される自己位置算出処理手順の詳細を示すフローチャートである。図２において、自己位置算出部１０は、まずホイールエンコーダ５及びジャイロセンサ６の計測値を取得する（手順Ｓ１０１）。
【００２６】
続いて、自己位置算出部１０は、ホイールエンコーダ５及びジャイロセンサ６の計測値に基づいて、フォークリフト２の位置検出値を算出する（手順Ｓ１０２）。具体的には、自己位置算出部１０は、ホイールエンコーダ５により計測されたフォークリフト２の速度を積分することにより、フォークリフト２の二次元位置（ＸＹ座標の位置）を求める。また、自己位置算出部１０は、ジャイロセンサ６により計測されたフォークリフト２の角速度を積分することにより、フォークリフト２の向きを求める。続いて、自己位置算出部１０は、フォークリフト２の位置検出値を外れ値除去部１１に出力する（手順Ｓ１０３）。
【００２７】
図３は、外れ値除去部１１により実行される外れ値除去処理手順の詳細を示すフローチャートである。図３において、外れ値除去部１１は、まず第１自己位置推定器３及び第２自己位置推定器４により得られたフォークリフト２の位置推定値を取得する（手順Ｓ１１１）。
【００２８】
続いて、外れ値除去部１１は、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値と第１自己位置推定器３及び第２自己位置推定器４により前回得られたフォークリフト２の位置推定値との相対位置（ｘ，ｙ，θ）を算出する（手順Ｓ１１２）。前回得られたフォークリフト２の位置推定値は、単位時間前に推定されたフォークリフト２の自己位置であり、メモリに記憶されている。
【００２９】
相対位置（ｘ，ｙ，θ）は、例えば第１自己位置推定器３により得られた位置推定値と第２自己位置推定器４により得られた位置推定値との平均値として算出してもよいし、或いは第１自己位置推定器３により得られた位置推定値及び第２自己位置推定器４により得られた位置推定値のうち前回算出された値に近いほうを採用してもよい。
【００３０】
続いて、外れ値除去部１１は、自己位置算出部１０により得られたフォークリフト２の位置検出値を取得する（手順Ｓ１１３）。続いて、外れ値除去部１１は、自己位置算出部１０により今回得られたフォークリフト２の位置検出値と自己位置算出部１０により前回得られたフォークリフト２の位置検出値との相対位置（ｘｄ，ｙｄ，θｄ）を算出する（手順Ｓ１１４）。前回得られたフォークリフト２の位置検出値は、単位時間前に検出されたフォークリフト２の自己位置であり、メモリに記憶されている。
【００３１】
続いて、外れ値除去部１１は、フォークリフト２の周囲環境に基づいて、閾値δｘ，δｙ，δθを設定する（手順Ｓ１１５）。閾値δｘ，δｙは、フォークリフト２の二次元位置（ｘ軸位置，ｙ軸位置）に係る閾値である。閾値δθは、フォークリフト２の向きに係る閾値である。フォークリフト２の周囲環境は、最終自己位置推定部１２により前回推定されたフォークリフト２の自己位置の環境である。なお、フォークリフト２の周囲環境は、環境地図を作成するＳＬＡＭを用いる第１自己位置推定器３によって把握することができる。
【００３２】
例えば、フォークリフト２が雨等で滑りやすい屋外を走行する場合、ホイールエンコーダ５は滑りの影響が大きいため、ホイールエンコーダ５の計測値に誤差が出やすいが、ジャイロセンサ６は滑りの影響が少ないため、ジャイロセンサ６の計測値に誤差が出にくい。従って、屋外では、屋内に比べて閾値δｘ，δｙを大きく（緩く）設定する。例えば、屋外では、閾値δｘ，δｙを１ｍに設定し、屋内では、閾値δｘ，δｙを０．５ｍに設定する。閾値δθについては、屋外と屋内とで等しくする。例えば、屋外及び屋内において、閾値δθを０．３ｒａｄに設定する。このように閾値δｘ，δｙ，δθは、フォークリフト２の周囲環境に応じて変更される。
【００３３】
また、フォークリフト２が例えば冷蔵庫の内外のような温度差が大きい屋内及び屋外を走行する場合には、ジャイロセンサ６の計測値に誤差が出やすい。このため、屋内では、屋外に比べて閾値δθを大きく（緩く）設定する。
【００３４】
続いて、外れ値除去部１１は、手順Ｓ１１４で算出された相対位置（ｘｄ，ｙｄ，θｄ）と手順Ｓ１１２で算出された（ｘ，ｙ，θ）との差分に関する計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）を計算する（手順Ｓ１１６）。計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）は、下記式で表される。
【数１】
2020008461.tif 000003
【００３５】
続いて、外れ値除去部１１は、上記式にも示されるように、計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）の計算値がそれぞれ閾値δｘ，δｙ，δθよりも大きいかどうかを判断する（手順Ｓ１１７）。
【００３６】
外れ値除去部１１は、計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）の計算値が閾値δｘ，δｙ，δθよりも大きいと判断したときは、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値を外れ値として除去する（手順Ｓ１１８）。このとき、外れ値除去部１１は、計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）の計算値の少なくとも１つが閾値δｘ，δｙ，δθよりも大きいとき、つまり上記式の条件を１つでも満たすときは、今回得られたフォークリフト２の位置推定値を外れ値とみなす。
【００３７】
外れ値除去部１１は、計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）の計算値の全てが閾値δｘ，δｙ，δθよりも大きくない、つまり上記式の条件を全て満たさないと判断したときは、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値を最終自己位置推定部１２に出力する（手順Ｓ１１９）。
【００３８】
図４は、最終自己位置推定部１２により実行される自己位置推定処理手順の詳細を示すフローチャートである。ここでは、最終自己位置推定部１２は、第１自己位置推定器３及び第２自己位置推定器４によりそれぞれ得られた２つのフォークリフト２の位置推定値を融合し、線形最小分散推定法を利用してフォークリフト２の自己位置を最終的に推定する。
【００３９】
図４において、最終自己位置推定部１２は、まず第１自己位置推定器３により得られた最新のフォークリフト２の位置推定値（ｘｓ，ｙｓ，θｓ）と、第２自己位置推定器４により得られた最新のフォークリフト２の位置推定値（ｘｇ，ｙｇ，θｇ）とを取得する（手順Ｓ１２１）。
【００４０】
続いて、最終自己位置推定部１２は、フォークリフト２の周囲環境に基づいて、第１自己位置推定器３の誤差分散（σｓｘ２，σｓｙ２，σｓθ２）と、第２自己位置推定器４の誤差分散（σｇｘ２，σｇｙ２，σｇθ２）とを設定する（手順Ｓ１２２）。フォークリフト２の周囲環境は、最終自己位置推定部１２により前回推定されたフォークリフト２の自己位置の環境である。また、誤差分散とは、重み係数のことである。誤差分散σｓｘ２，σｓｙ２及び誤差分散σｇｘ２，σｇｙ２は、フォークリフト２の二次元位置（ｘ軸位置，ｙ軸位置）に係る重み係数である。誤差分散σｓθ２及び誤差分散σｇθ２は、フォークリフト２の向きに係る重み係数である。
【００４１】
このとき、屋内では、ＳＬＡＭの推定精度がＧＮＳＳの推定精度よりも高いため、第１自己位置推定器３の誤差分散を第２自己位置推定器４の誤差分散よりも低く設定する。例えば、第１自己位置推定器３の誤差分散を４とし、第２自己位置推定器４の誤差分散を３６とする。屋外では、ＧＮＳＳの推定精度がＳＬＡＭの推定精度よりも高いため、第２自己位置推定器４の誤差分散を第１自己位置推定器３の誤差分散よりも低く設定する。例えば、第１自己位置推定器３の誤差分散を３６とし、第２自己位置推定器４の誤差分散を４とする。また、屋内と屋内との中間では、第１自己位置推定器３の誤差分散と第２自己位置推定器４の誤差分散とを等しく設定する。例えば、第１自己位置推定器３及び第２自己位置推定器４の誤差分散を何れも２５とする。なお、屋内と屋内との中間とは、例えば屋根はないが建物の近くで柱が存在しているような場所である。
【００４２】
続いて、最終自己位置推定部１２は、第１自己位置推定器３により得られた最新のフォークリフト２の位置推定値（ｘｓ，ｙｓ，θｓ）、第２自己位置推定器４により得られた最新のフォークリフト２の位置推定値（ｘｇ，ｙｇ，θｇ）、第１自己位置推定器３の誤差分散（σｓｘ２，σｓｙ２，σｓθ２）及び第２自己位置推定器４の誤差分散（σｇｘ２，σｇｙ２，σｇθ２）を用いて、下記式により最終的なフォークリフト２の自己位置（ｘｅｓｔ，ｙｅｓｔ，θｅｓｔ）を推定する。
【数２】
2020008461.tif 000004
【００４３】
以上のように本実施形態にあっては、自己位置推定技術が異なる第１自己位置推定器３及び第２自己位置推定器４によりフォークリフト２の自己位置が推定され、第１自己位置推定器３及び第２自己位置推定器４により得られた各フォークリフト２の位置推定値に基づいて、フォークリフト２の自己位置が最終的に推定される。このとき、フォークリフト２の周囲環境に基づいて第１自己位置推定器３及び第２自己位置推定器４の誤差分散（重み係数）が設定され、第１自己位置推定器３及び第２自己位置推定器４の誤差分散を用いてフォークリフト２の自己位置が最終的に推定される。これにより、フォークリフト２の自己位置の推定精度が向上する。その結果、フォークリフト２の自動運転時に、フォークリフト２の安定した走行が可能となる。
【００４４】
また、本実施形態では、最終自己位置推定部１２により前回最終的に推定されたフォークリフト２の自己位置の環境に基づいて、第１自己位置推定器３及び第２自己位置推定器４の誤差分散が設定される。このような構成により、現在のフォークリフト２の周囲環境が精度良く得られるため、第１自己位置推定器３及び第２自己位置推定器４の誤差分散が適切に設定される。これにより、フォークリフト２の自己位置の推定精度が更に向上する。
【００４５】
また、本実施形態では、第１自己位置推定器３及び第２自己位置推定器４は、フォークリフト２の自己位置としてフォークリフト２の二次元位置及び向きを推定し、最終自己位置推定部１２は、第１自己位置推定器３及び第２自己位置推定器４の誤差分散としてフォークリフト２の二次元位置及び向きに係る誤差分散を設定する。このようにフォークリフト２の二次元位置だけでなくフォークリフト２の向きも考慮されるため、フォークリフト２の自己位置の推定精度が一層向上する。
【００４６】
また、本実施形態では、第１自己位置推定器３は、屋内での推定精度が屋外での推定精度よりも高い自己位置推定技術を用いてフォークリフト２の自己位置を推定し、第２自己位置推定器４は、屋外での推定精度が屋内での推定精度よりも高い自己位置推定技術を用いてフォークリフト２の自己位置を推定する。このような構成により、フォークリフト２が屋外及び屋内を移動する際に、屋外及び屋内に応じた適切な第１自己位置推定器３及び第２自己位置推定器４の誤差分散が設定されることになるため、フォークリフト２の自己位置の推定精度がより一層向上する。
【００４７】
図５は、図３に示された外れ値除去処理手順の変形例を示すフローチャートである。図５において、外れ値除去部１１は、図３に示されるフローチャートと同様に、手順Ｓ１１１〜Ｓ１１４を実行する。
【００４８】
外れ値除去部１１は、手順Ｓ１１４を実行した後、フォークリフト２の周囲環境に基づいて、閾値δ及び重み係数α，β，γを設定する（手順Ｓ１１５Ａ）。重み係数αは、フォークリフト２のｘ軸位置に係る重み係数である。重み係数βは、フォークリフト２のｙ軸位置に係る重み係数である。重み係数γは、フォークリフト２の向きに係る重み係数である。このとき、例えば雨等で滑りやすい屋外では、屋内に比べて重み係数α，βを小さく設定する。
【００４９】
続いて、外れ値除去部１１は、手順Ｓ１１４で算出された相対位置（ｘｄ，ｙｄ，θｄ）と手順Ｓ１１２で算出された（ｘ，ｙ，θ）との差分に関する計算式ｆを計算する（手順Ｓ１１６Ａ）。計算式ｆは、下記式で表される。計算式ｆは、フォークリフト２の二次元位置及び向きに関する変数を加算する式である。
【数３】
2020008461.tif 000005
【００５０】
続いて、外れ値除去部１１は、上記式にも示されるように、計算式ｆの計算値が閾値δよりも大きいかどうかを判断する（手順Ｓ１１７Ａ）。外れ値除去部１１は、計算式ｆの計算値が閾値δよりも大きいと判断したときは、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値を外れ値として除去する（手順Ｓ１１８）。
【００５１】
外れ値除去部１１は、計算式ｆの計算値が閾値δよりも大きくないと判断したときは、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値を最終自己位置推定部１２に出力する（手順Ｓ１１９）。
【００５２】
なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、最終自己位置推定部１２は、第１自己位置推定器３及び第２自己位置推定器４によりそれぞれ得られた２つのフォークリフト２の位置推定値を融合し、線形最小分散推定法を利用してフォークリフト２の自己位置を最終的に推定しているが、２つのフォークリフト２の位置推定値を融合する場合には、例えばカルマンフィルタを利用してフォークリフト２の自己位置を最終的に推定してもよい。
【００５３】
また、上記実施形態では、ＳＬＡＭを用いてフォークリフト２の自己位置を推定する第１自己位置推定器３と、ＧＮＳＳを用いてフォークリフト２の自己位置を推定する第２自己位置推定器４とを使用しているが、特にその形態には限られず、自己位置推定技術が異なる３つ以上の自己位置推定器を使用してもよい。この場合には、最終自己位置推定部１２は、各自己位置推定器によりそれぞれ得られた３つ以上のフォークリフト２の位置推定値を融合することになる。また、使用する自己位置推定技術としては、特にＳＬＡＭ及びＧＮＳＳには限られない。
【００５４】
また、上記実施形態では、ホイールエンコーダ５によりフォークリフト２の速度を計測し、ジャイロセンサ６によりフォークリフト２の角速度を計測しているが、使用するセンサとしては、特にそれには限られない。例えば、フォークリフト２の速度を計測するセンサとしては、ロータリーエンコーダ等を使用してもよく、フォークリフト２の角速度を計測するセンサとしては、操舵角センサまたは操舵ポテンショメータ等を使用してもよい。また、フォークリフト２の二次元位置及び向きをセンサにより直接検出してもよい。
【００５５】
さらに、上記実施形態では、フォークリフト２の位置を推定しているが、本発明は、フォークリフト以外の産業車両あるいは移動ロボット等といった自律移動体にも適用可能である。
【符号の説明】
【００５６】
１…位置推定装置、２…フォークリフト（自律移動体）、３…第１自己位置推定器（自己位置推定器）、４…第２自己位置推定器（自己位置推定器）、１２…最終自己位置推定部。
【図１】
2020008461.tif 000006
【図２】
2020008461.tif 000007
【図３】
2020008461.tif 000008
【図４】
2020008461.tif 000009
【図５】
2020008461.tif 000010