JP7322799B2 - 自己位置推定装置 - Google Patents

Description

本発明は、自己位置推定装置に関する。

従来の自己位置推定装置としては、例えば特許文献１に記載されている技術が知られている。特許文献１に記載の自己位置推定装置は、自動搬送車の移動量を検出するエンコーダと、自動搬送車の周囲にレーザを照射し、壁や柱等で反射したレーザを受信することで、自動搬送車の周囲の環境情報を検出する外界センサと、自動搬送車の現在地及び目的地等の経路情報を入力する情報入力部と、エンコーダ及び外界センサの検出情報と情報入力部の入力情報とに基づいて、地図情報と環境情報とを参照するＳＬＡＭ手法を用いて、自動搬送車を自律走行させるＣＰＵとを備えている。ＣＰＵは、自動搬送車の移動量に基づき自動搬送車の第１仮想値を演算し、特定環境情報に基づき、パーティクルフィルタによって自動搬送車の複数の第２仮想値を演算し、第１仮想値及び各第２仮想値から確定値を決定し、その確定値に応じて自動搬送車を自律走行させる。

特開２０１８－１３８６０号公報

しかしながら、上記従来技術においては、以下の問題点が存在する。即ち、例えば移動体である自動搬送車の周囲の環境が変化することで、環境情報が地図情報と異なった状況になると、自動搬送車の自己位置の推定精度が低下する。その状態で、自動搬送車の走行を継続させると、自動搬送車の自己位置推定値のずれが累積されてしまう。

本発明の目的は、移動体の自己位置推定値のずれの累積を抑制することができる自己位置推定装置を提供することである。

本発明の一態様は、移動体の走行時に移動体の自己位置の推定を行う自己位置推定装置において、移動体から移動体の周囲に存在する物体までの距離を検出する距離検出部と、移動体の移動量を検出する移動量検出部と、距離検出部の検出データと地図データとをマッチングさせて、移動体の自己位置を推定する自己位置推定部と、移動量検出部により検出された移動体の移動量に基づいて、自己位置推定部により推定された移動体の自己位置を補間することで、移動体の自己位置推定値を取得する自己位置補間部と、自己位置推定部により推定された移動体の最新自己位置が自己位置補間部により補間されることで取得された移動体の第１自己位置推定値と、移動体の前回自己位置が自己位置補間部により補間されることで取得された移動体の第２自己位置推定値との差分を自己位置ずれ量として算出する自己位置ずれ算出部と、自己位置ずれ算出部により算出された自己位置ずれ量に基づいて、自己位置ずれ算出部において次回使用される前回自己位置を決定する前回自己位置決定部と、自己位置推定部により移動体の自己位置の推定を開始する際の移動体の基準位置を設定入力する設定入力部と、自己位置ずれ算出部により算出された自己位置ずれ量が予め決められた第１閾値以上であるときに、基準位置を再設定する基準位置再設定部とを備え、自己位置推定部は、設定入力部により設定入力された基準位置に基づいて、移動体の自己位置の推定を開始すると共に、基準位置再設定部により基準位置が再設定されたときは、再設定された基準位置に基づいて、移動体の自己位置の推定を継続して行う。

このような自己位置推定装置においては、距離検出部によって移動体から移動体の周囲に存在する物体までの距離が検出され、その検出データと地図データとのマッチングにより移動体の自己位置が推定される。また、移動量検出部によって移動体の移動量が検出され、その移動体の移動量に基づいて移動体の自己位置が補間されることで、移動体の自己位置推定値が取得される。そして、移動体の最新自己位置が補間されることで取得された移動体の第１自己位置推定値と、移動体の前回自己位置が補間されることで取得された移動体の第２自己位置推定値との差分が自己位置ずれ量として算出される。そして、自己位置ずれ量に基づいて、次回使用される前回自己位置が決定される。ここで、移動体の自己位置の推定は、設定入力部により設定入力された基準位置に基づいて開始される。移動体の自己位置の推定が開始された後は、上記の自己位置ずれ量が第１閾値以上になると、基準位置が再設定される。そして、再設定された基準位置に基づいて、移動体の自己位置の推定が継続して行われる。このため、検出データと地図データとのマッチングによる移動体の自己位置の推定精度が低下しやすい状況になっても、移動体の自己位置推定値が正しい位置に近づきやすくなる。これにより、移動体の自己位置推定値のずれの累積が抑制される。

前回自己位置決定部は、自己位置ずれ量が予め決められた第２閾値以下であるときは、第１自己位置推定値を次回使用される前回自己位置として決定し、自己位置ずれ量が第２閾値よりも大きいときは、第２自己位置推定値を次回使用される前回自己位置として決定してもよい。このような構成では、第１自己位置推定値と第２自己位置推定値との誤差に関わらず、適切な前回自己位置が得られる。従って、移動体の自己位置推定値のずれの累積が更に抑制される。

第１閾値は、第２閾値よりも小さくてもよい。このような構成では、第１自己位置推定値と第２自己位置推定値との誤差が増加すると、早い段階で基準位置が再設定されることになる。従って、移動体の自己位置の推定ずれの累積が一層抑制される。

基準位置再設定部は、自己位置ずれ量が第１閾値以上であるときに、基準位置を第２自己位置推定値または第２自己位置推定値に対応した位置に再設定してもよい。このような構成では、移動体の前回自己位置が補間されることで取得された第２自己位置推定値または第２自己位置推定値に対応した位置が基準位置として使用されるため、適切な基準位置が容易に得られる。

本発明によれば、移動体の自己位置推定値のずれの累積を抑制することができる。

本発明の一実施形態に係る自己位置推定装置を備えた走行制御装置の構成を概略的に示すブロック図である。 第１自己位置推定値及び第２自己位置推定値を示す概念図である。 図１に示された前回自己位置決定部により実行される決定処理の手順を示すフローチャートである。 図３に示された決定処理において使用される閾値Ａを第１自己位置推定値及び第２自己位置推定値と共に示す概念図である。 図１に示された基準位置再設定部により実行される再設定処理の手順を示すフローチャートである。 図５に示された再設定処理において使用される閾値Ｂを閾値Ａ、第１自己位置推定値及び第２自己位置推定値と共に示す概念図である。 図１に示された自己位置推定部により実行される自己位置推定処理の手順の詳細を示すフローチャートである。 自己位置の推定精度が低下していない状態と自己位置の推定精度が低下した状態とで、時間経過に従って前回自己位置が決定される様子を第１自己位置推定値、第２自己位置推定値及び真値と共に示す概念図である。 自己位置の推定精度が低下した状態において移動体の基準位置が再設定されるときに、時間経過に従って前回自己位置が決定される様子を第１自己位置推定値、第２自己位置推定値及び真値と共に示す概念図である。 図３に示された決定処理の手順の変形例を示すフローチャートである。

以下、本発明の実施形態について図面を参照して詳細に説明する。

図１は、本発明の一実施形態に係る自己位置推定装置を備えた走行制御装置の構成を概略的に示すブロック図である。図１において、走行制御装置１は、例えばフォークリフト等の移動体２（図２参照）を目的地まで自動的に走行させる装置である。走行制御装置１は、移動体２に搭載されている。

走行制御装置１は、レーザセンサ３と、オドメトリセンサ４と、入力器５と、駆動部６と、コントローラ７とを備えている。

レーザセンサ３は、レーザ光を用いて、移動体２から移動体２の周囲に存在する物体までの距離を検出する距離検出部である。レーザセンサ３は、移動体２の周囲にレーザ光を照射し、そのレーザ光の反射光を受光することにより、移動体２の周囲に存在する物体までの距離を検出する。物体は、壁や柱等であり、地図データ（後述）に登録されている。レーザセンサ３としては、例えばレーザレンジファインダが使用される。レーザセンサ３から照射されるレーザ光としては、２Ｄレーザでもよいし、３Ｄレーザでもよい。

オドメトリセンサ４は、移動体２の移動量を検出する移動量検出部である。オドメトリセンサ４としては、例えば移動体２の車輪の回転角度を計測することにより、移動体２の移動量を検出するエンコーダ等が使用される。

入力器５は、ユーザが移動体２の基準位置を含む各種情報を入力するための機器である。ここでの基準位置は、後述する自己位置推定部１０により移動体２の自己位置の推定を開始する際の移動体２の初期位置である。基準位置は、２次元座標（ＸＹ座標）及び向きで表される。ユーザは、入力器５によって基準位置の２次元座標及び向きを直接入力する。入力器５は、移動体２の基準位置を設定入力する設定入力部を構成している。

駆動部６は、特に図示はしないが、移動体２の車輪を回転させる走行モータと、移動体２の車輪を転舵させる操舵モータとを有している。

コントローラ７は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ７は、自己位置推定部１０と、自己位置補間部１１と、自己位置ずれ算出部１２と、前回自己位置決定部１３と、基準位置再設定部１４と、駆動制御部１５とを有している。

ここで、レーザセンサ３、オドメトリセンサ４、入力器５、自己位置推定部１０、自己位置補間部１１、自己位置ずれ算出部１２、前回自己位置決定部１３及び基準位置再設定部１４は、本実施形態の自己位置推定装置１６を構成している。自己位置推定装置１６は、移動体２の走行時に移動体２の自己位置の推定を行う装置である。

自己位置推定部１０は、レーザセンサ３の検出データと移動体２の周囲環境の地図データとを用いて、移動体２の自己位置を推定する。自己位置推定部１０は、レーザＳＬＡＭ（simultaneous localization andmapping）手法を用いて、移動体２の自己位置を推定する。ＳＬＡＭは、センサデータ及び地図データを使って自己位置推定を行う自己位置推定技術である。ＳＬＡＭは、センサデータを利用して、自己位置推定と環境地図の作成とを同時に行う。

具体的には、自己位置推定部１０は、レーザセンサ３の検出データと移動体２の周囲環境の地図データとをマッチングさせて、移動体２の自己位置の推定演算を行う。このとき、自己位置推定部１０は、入力器５により設定入力された初期位置（基準位置）に基づいて、移動体２の自己位置の推定を開始する。なお、移動体２の自己位置は、２次元座標及び向きで表される。

自己位置推定部１０による移動体２の自己位置の推定演算には時間がかかるため、移動体２の自己位置の推定演算が行われている間にも移動体２が移動してしまう。従って、自己位置推定部１０により移動体２の自己位置の推定演算が行われている間に移動体２が進んだ距離を補間する必要がある。

自己位置補間部１１は、オドメトリセンサ４により検出された移動体２の移動量に基づいて、自己位置推定部１０により推定された移動体２の自己位置を補間することで、移動体２の自己位置推定値を取得する。具体的には、自己位置補間部１１は、自己位置推定部１０により推定された移動体２の自己位置に、オドメトリセンサ４により検出された移動体２の移動量を加算することにより、移動体２の自己位置を補間する（下記の（Ａ）式及び（Ｂ）式参照）。

自己位置ずれ算出部１２は、移動体２の第１自己位置推定値と移動体２の第２自己位置推定値との差分を自己位置ずれ量として算出する。

第１自己位置推定値は、自己位置推定部１０により直近に推定された移動体２の最新自己位置が自己位置補間部１１により補間されることで取得された自己位置推定値である。具体的には、第１自己位置推定値は、移動体２の最新自己位置の推定演算が行われている間に移動体２が進んだ移動量だけ移動体２の最新自己位置が補間されることで取得される。

第２自己位置推定値は、移動体２の前回自己位置が自己位置補間部１１により補間されることで取得された自己位置推定値である。前回自己位置は、自己位置推定部１０により直近以前に推定された移動体２の自己位置が自己位置補間部１１により補間されることで取得される。具体的には、第２自己位置推定値は、移動体２の前回自己位置の推定演算が行われたときから移動体２の最新自己位置の推定演算が行われている間に移動体２が進んだ移動量だけ移動体２の前回自己位置が補間されることで取得される。

例えば図２に示されるように、時刻ｔ１において自己位置推定部１０により今周期（直近）に推定された移動体２の最新自己位置をｘ_１とし、オドメトリセンサ４により検出された移動体２の移動量をΔｘ_１２とすると、時刻ｔ２における第１自己位置推定値ｘ_２ａは下記式で表される。
ｘ_２ａ＝ｘ_１＋Δｘ_１２ …（Ａ）

一方、時刻ｔ０における移動体２の前回自己位置をｘ_０とし、オドメトリセンサに４より検出された移動体２の移動量をΔｘ_０２とすると、時刻ｔ２における第２自己位置推定値ｘ_２ｂは下記式で表される。なお、前回自己位置ｘ_０は、自己位置推定部１０により前周期（直近以前）に推定された移動体２の自己位置が自己位置補間部１１により補間されることで取得された自己位置である。
ｘ_２ｂ＝ｘ_０＋Δｘ_０２ …（Ｂ）

そして、自己位置ずれ量ｄは、下記式の絶対値で表される。
ｄ＝ｘ_２ａ－ｘ_２ｂ …（Ｃ）

前回自己位置決定部１３は、自己位置ずれ算出部１２により算出された自己位置ずれ量ｄに基づいて、自己位置ずれ算出部１２において次回使用される前回自己位置を決定する。

図３は、前回自己位置決定部１３により実行される決定処理の手順を示すフローチャートである。図３において、前回自己位置決定部１３は、まず自己位置ずれ算出部１２により算出された自己位置ずれ量ｄを取得する（手順Ｓ１０１）。

続いて、前回自己位置決定部１３は、自己位置ずれ量ｄが閾値Ａ以下であるかどうかを判断する（手順Ｓ１０２）。閾値Ａは、予め決められた第２閾値である。

前回自己位置決定部１３は、自己位置ずれ量ｄが閾値Ａ以下であると判断したときは、第１自己位置推定値を前回自己位置に決定する（手順Ｓ１０３）。従って、図４（ａ）に示されるように、第１自己位置推定値Ｐと第２自己位置推定値Ｑとの差分である自己位置ずれ量ｄが閾値Ａ以下であるときは、第１自己位置推定値Ｐが妥当であると判定され、前回自己位置が第１自己位置推定値Ｐに更新される。

前回自己位置決定部１３は、自己位置ずれ量ｄが閾値Ａよりも大きいと判断したときは、第２自己位置推定値を前回自己位置に決定する（手順Ｓ１０４）。従って、図４（ｂ）に示されるように、第１自己位置推定値Ｐと第２自己位置推定値Ｑとの差分である自己位置ずれ量ｄが閾値Ａよりも大きいときは、第１自己位置推定値Ｐが妥当でないと判定される。このため、前回自己位置は、更新されずに第２自己位置推定値Ｑのままとなる。

基準位置再設定部１４は、自己位置ずれ算出部１２により算出された自己位置ずれ量ｄが閾値Ｂ以上であるときに、移動体２の基準位置を再設定する。つまり、基準位置再設定部１４は、自己位置推定部１０による移動体２の自己位置の推定処理をリセットする。

図５は、基準位置再設定部１４により実行される再設定処理の手順を示すフローチャートである。図５において、基準位置再設定部１４は、まず自己位置ずれ算出部１２により算出された自己位置ずれ量ｄを取得する（手順Ｓ１１１）。

続いて、基準位置再設定部１４は、自己位置ずれ量ｄが閾値Ｂ以上であるかどうかを判断する（手順Ｓ１１２）。閾値Ｂは、予め決められた第１閾値である。閾値Ｂは、図６に示されるように、閾値Ａよりも小さい値である。

基準位置再設定部１４は、自己位置ずれ量ｄが閾値Ｂ以上であると判断したときは、移動体２の基準位置を第２自己位置推定値に再設定する（手順Ｓ１１３）。基準位置再設定部１４は、自己位置ずれ量ｄが閾値Ｂよりも小さいと判断したときは、手順Ｓ１１３を実行しない。

従って、図６（ａ）に示されるように、第１自己位置推定値Ｐと第２自己位置推定値Ｑとの差分である自己位置ずれ量ｄが閾値Ｂよりも小さいときは、基準位置の再設定処理は実施されない。一方、図６（ｂ）に示されるように、第１自己位置推定値Ｐと第２自己位置推定値Ｑとの差分である自己位置ずれ量ｄが閾値Ｂ以上であるときは、基準位置の再設定処理が実施される。

駆動制御部１５は、自己位置補間部１１により得られた補間後の移動体２の自己位置に基づいて、移動体２を目的地に向けて走行させるように駆動部６を制御する。

上記の自己位置推定部１０は、入力器５により設定入力された基準位置に基づいて、移動体２の自己位置の推定を開始すると共に、基準位置再設定部１４により基準位置が再設定されたときは、再設定された基準位置に基づいて、移動体２の自己位置の推定を継続して行う。

図７は、自己位置推定部１０により実行される自己位置推定処理の手順の詳細を示すフローチャートである。図７において、自己位置推定部１０は、まず入力器５により移動体２の初期位置（基準位置）が設定入力されたかどうかを判断する（手順Ｓ１２１）。

自己位置推定部１０は、入力器５により移動体２の初期位置が設定入力されたと判断したときは、初期位置に基づいて、上述した移動体２の自己位置の推定演算を開始する（手順Ｓ１２２）。

続いて、自己位置推定部１０は、基準位置再設定部１４により移動体２の基準位置が再設定されたかどうかを判断する（手順Ｓ１２３）。自己位置推定部１０は、基準位置再設定部１４により基準位置が再設定されたと判断したときは、再設定された新しい基準位置に基づいて、上述した移動体２の自己位置の推定を継続して行う（手順Ｓ１２４）。そして、自己位置推定部１０は、手順Ｓ１２３を再度実行する。

自己位置推定部１０は、基準位置再設定部１４により基準位置が再設定されていないと判断したときは、再設定されていない現在の基準位置に基づいて、上述した移動体２の自己位置の推定を継続して行う（手順Ｓ１２５）。そして、自己位置推定部１０は、手順Ｓ１２３を再度実行する。

以上のような自己位置推定装置１６において、図８（ａ）に示されるように、時刻ｔ０で得られた第１自己位置推定値Ｐと第２自己位置推定値Ｑとの差分である自己位置ずれ量ｄが閾値Ａ以下であるときは、第１自己位置推定値Ｐが次周期（ｔ１）で使用される基準位置として採用される。そして、時刻ｔ１で得られた第１自己位置推定値Ｐと第２自己位置推定値Ｑとの自己位置ずれ量ｄも閾値Ａ以下であるときは、第１自己位置推定値Ｐが次周期（ｔ２）で使用される基準位置として採用される。なお、図８中の菱形印は、実際の移動体２の自己位置である真値Ｒを表している。ただし、コントローラ７による処理では、真値Ｒは不明である。

しかし、例えば移動体２の周囲の環境が変化することで、移動体２の周囲の環境が地図データと異なった状況になると、移動体２の自己位置の推定精度が悪くなる。この場合には、第１自己位置推定値Ｐと第２自己位置推定値Ｑとの差分である自己位置ずれ量ｄが累積されるため、不適切な基準位置が採用されることがある。

例えば図８（ｂ）に示されるように、時刻ｔ１で得られた第１自己位置推定値Ｐと第２自己位置推定値Ｑとの差分である自己位置ずれ量ｄが閾値Ａ以下であるために、第１自己位置推定値Ｐが次周期（ｔ２）で使用される基準位置として採用されても、時刻ｔ２で得られた第１自己位置推定値Ｐと第２自己位置推定値Ｑとの差分である自己位置ずれ量ｄが閾値Ａよりも大きくなることがある。この場合には、真値Ｒに近い第１自己位置推定値Ｐが得られているにも関わらず、第１自己位置推定値Ｐが基準位置として採用されない。このように自己位置ずれ量ｄが累積すると、移動体２の自己位置推定値を正しい位置に戻すことができなくなる。

しかし、本実施形態では、図９に示されるように、時刻ｔ０で得られた第１自己位置推定値Ｐと第２自己位置推定値Ｑとの差分である自己位置ずれ量ｄが閾値Ｂ以上であるときは、移動体２の基準位置が第２自己位置推定値Ｑに再設定される。そして、再設定された基準位置に基づいて、移動体２の自己位置の推定が継続して実施される。

すると、時刻ｔ１において、真値Ｒに近い第１自己位置推定値Ｐが得られ、第１自己位置推定値Ｐと第２自己位置推定値Ｑとの差分である自己位置ずれ量ｄが閾値Ｂよりも小さくなる。そして、時刻ｔ２においても、真値Ｒに更に近い第１自己位置推定値Ｐが得られ、第１自己位置推定値Ｐと第２自己位置推定値Ｑとの自己位置ずれ量ｄが閾値Ｂよりも小さくなる。これにより、移動体２の周囲環境の変化等によって移動体２の自己位置の推定精度が悪くなっても、移動体２の自己位置推定値を正しい位置に戻すことができる。

以上のように本実施形態によれば、レーザセンサ３によって移動体２から移動体２の周囲に存在する物体までの距離が検出され、その検出データと地図データとのマッチングにより移動体２の自己位置が推定される。また、オドメトリセンサ４によって移動体２の移動量が検出され、その移動体の移動量に基づいて移動体２の自己位置が補間されることで、移動体２の自己位置推定値が取得される。そして、移動体２の最新自己位置が補間されることで取得された移動体２の第１自己位置推定値と、移動体２の前回自己位置が補間されることで取得された移動体２の第２自己位置推定値との差分が自己位置ずれ量として算出される。そして、自己位置ずれ量に基づいて、次回使用される前回自己位置が決定される。ここで、移動体２の自己位置の推定は、入力器５により設定入力された基準位置に基づいて開始される。移動体２の自己位置の推定が開始された後は、上記の自己位置ずれ量が閾値Ｂ以下になると、基準位置が再設定される。そして、再設定された基準位置に基づいて、移動体２の自己位置の推定が継続して行われる。このため、検出データと地図データとのマッチングによる移動体２の自己位置の推定精度が低下しやすい状況になっても、移動体２の自己位置推定値が正しい位置に近づきやすくなる。これにより、移動体２の自己位置推定値のずれの累積が抑制される。その結果、移動体２の自己位置の推定精度が良い状態で、移動体２の走行を継続させることができる。

また、本実施形態では、自己位置ずれ量が閾値Ａ以下であるときは、第１自己位置推定値が次回使用される前回自己位置として決定され、自己位置ずれ量が閾値Ａよりも大きいときは、第２自己位置推定値が次回使用される前回自己位置として決定される。このため、第１自己位置推定値と第２自己位置推定値との誤差に関わらず、適切な前回自己位置が得られる。従って、移動体２の自己位置推定値のずれの累積が更に抑制される。

また、本実施形態では、閾値Ｂは閾値Ａよりも小さいので、第１自己位置推定値と第２自己位置推定値との誤差が増加すると、早い段階で基準位置が再設定されることになる。従って、移動体２の自己位置の推定ずれの累積が一層抑制される。

また、本実施形態では、自己位置ずれ量が閾値Ｂ以上であるときに、基準位置が第２自己位置推定値に再設定される。このように移動体２の前回自己位置が補間されることで取得された第２自己位置推定値が基準位置として使用されるため、適切な基準位置が容易に得られる。

なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、閾値Ｂは閾値Ａよりも小さいが、特にその形態には限られず、閾値Ｂは閾値Ａと等しくてもよいし、或いは閾値Ｂは閾値Ａよりも大きくてもよい。

また、上記実施形態では、入力器５により移動体２の初期位置（基準位置）が設定入力されているが、特にそのような形態には限られない。例えば、コントローラ７は、移動体２の初期位置と関連付けられた番号等の特定情報を記憶する記憶部と、入力器５により入力された特定情報に応じた初期位置を記憶部から読み出して設定する設定部とを有していてもよい。この場合には、入力器５、記憶部及び設定部が、上記の設定入力部を構成する。

また、上記実施形態では、移動体２の第１自己位置推定値Ｐと移動体２の第２自己位置推定値Ｑとの差分である自己位置ずれ量ｄが閾値Ａ以下であるときは、第１自己位置推定値Ｐが前回自己位置に決定され、自己位置ずれ量ｄが閾値Ａよりも大きいときは、第２自己位置推定値Ｑが前回自己位置に決定されているが、その処理に加えて、移動体２の自己位置推定の信頼度に応じて前回自己位置を決定してもよい。

図１０は、図３に示された決定処理の手順の変形例を示すフローチャートである。図１０において、前回自己位置決定部１３は、上記の手順Ｓ１０１を実行した後、自己位置推定部１０による移動体２の自己位置推定の信頼度が高いかどうかを判断する（手順Ｓ１０７）。

このとき、前回自己位置決定部１３は、例えばレーザセンサ３の検出データに基づいて移動体２の複数の自己位置候補点の分散値を算出する。そして、前回自己位置決定部１３は、複数の自己位置候補点の分散値が予め決められた閾値以上であるときは、移動体２の自己位置精度が高いと判定し、複数の自己位置候補点の分散値が閾値以上よりも低いときは、移動体２の自己位置精度が低いと判定する。

前回自己位置決定部１３は、移動体２の自己位置推定の信頼度が高いと判断したときは、上記の手順Ｓ１０２を実行する。このため、移動体２の自己位置推定の信頼度が高い場合に、自己位置ずれ量ｄが閾値Ａ以下であるときは、第１自己位置推定値が前回自己位置に決定される。前回自己位置決定部１３は、移動体２の自己位置推定の信頼度が低いと判断したときは、上記の手順Ｓ１０４を実行する。このため、移動体２の自己位置推定の信頼度が低いときは、自己位置ずれ量ｄに関わらず、第２自己位置推定値が前回自己位置に決定される。

また、上記実施形態では、レーザセンサ３の検出データと地図データとをマッチングさせて、移動体２の自己位置の推定演算を行っているが、そのようなレーザセンサ３の検出データと地図データとのマッチングに加えて、オドメトリセンサ４により検出された移動体２の移動量に基づいて、例えばパーティクルフィルタと呼ばれる時系列データの予測手法を用いて、移動体２の自己位置を確率的に推定してもよい。パーティクルフィルタでは、現状態から起こりうる多数の次状態を多数のパーティクルで表現し、全パーティクルの尤度（追跡したい対象物らしさ）に従って算出された重みつき平均を次状態であると推測して追跡を行う。

また、上記実施形態では、基準位置再設定部１４において、基準位置が第２自己位置推定値に再設定されているが、特にその形態には限られず、基準位置を第２自己位置推定値に対応した位置に再設定してもよい。例えば、上記のパーティクルフィルタを用いて移動体２の自己位置を推定する場合には、第２自己位置推定値を基準としてパーティクルの数を変更して移動体２の自己位置を再計算し、その計算結果を基準位置として再設定してもよい。

また、上記実施形態では、オドメトリセンサ４により移動体２の移動量が検出されているが、特にオドメトリセンサ４には限られず、移動体２の角速度及び加速度を計測する慣性計測ユニットを使用し、慣性計測ユニットの計測値を用いて移動体２の移動量を検出してもよい。

また、上記実施形態では、レーザセンサ３の検出データを使用したレーザＳＬＡＭによって、移動体２の自己位置が推定されているが、特にレーザＳＬＡＭには限られず、例えば移動体２の周囲を撮像するカメラの画像データを使用した画像ＳＬＡＭによって、移動体２の自己位置を推定してもよい。

２…移動体、３…レーザセンサ（距離検出部）、４…オドメトリセンサ（移動量検出部）、５…入力器（設定入力部）、１０…自己位置推定部、１１…自己位置補間部、１２…自己位置ずれ算出部、１３…前回自己位置決定部、１４…基準位置再設定部、１６…自己位置推定装置、Ａ…閾値（第２閾値）、Ｂ…閾値（第１閾値）、Ｐ…第１自己位置推定値、Ｑ…第２自己位置推定値。

Claims (4)

1. 移動体の走行時に前記移動体の自己位置の推定を行う自己位置推定装置において、
   前記移動体から前記移動体の周囲に存在する物体までの距離を検出する距離検出部と、
   前記移動体の移動量を検出する移動量検出部と、
   前記距離検出部の検出データと地図データとをマッチングさせて、前記移動体の自己位置を推定する自己位置推定部と、
   前記移動量検出部により検出された前記移動体の移動量に基づいて、前記自己位置推定部により推定された前記移動体の自己位置を補間することで、前記移動体の自己位置推定値を取得する自己位置補間部と、
   前記自己位置推定部により推定された前記移動体の最新自己位置が前記自己位置補間部により補間されることで取得された前記移動体の第１自己位置推定値と、前記移動体の前回自己位置が前記自己位置補間部により補間されることで取得された前記移動体の第２自己位置推定値との差分を自己位置ずれ量として算出する自己位置ずれ算出部と、
   前記自己位置ずれ算出部により算出された自己位置ずれ量に基づいて、前記自己位置ずれ算出部において次回使用される前記前回自己位置を決定する前回自己位置決定部と、
   前記自己位置推定部により前記移動体の自己位置の推定を開始する際の前記移動体の基準位置を設定入力する設定入力部と、
   前記自己位置ずれ算出部により算出された自己位置ずれ量が予め決められた第１閾値以上であるときに、前記基準位置を再設定する基準位置再設定部とを備え、
   前記自己位置推定部は、前記設定入力部により設定入力された前記基準位置に基づいて、前記移動体の自己位置の推定を開始すると共に、前記基準位置再設定部により前記基準位置が再設定されたときは、前記再設定された基準位置に基づいて、前記移動体の自己位置の推定を継続して行う自己位置推定装置。
2. 前記前回自己位置決定部は、前記自己位置ずれ量が予め決められた第２閾値以下であるときは、前記第１自己位置推定値を前記次回使用される前回自己位置として決定し、前記自己位置ずれ量が前記第２閾値よりも大きいときは、前記第２自己位置推定値を前記次回使用される前回自己位置として決定する請求項１記載の自己位置推定装置。
3. 前記第１閾値は、前記第２閾値よりも小さい請求項２記載の自己位置推定装置。
4. 前記基準位置再設定部は、前記自己位置ずれ量が前記第１閾値以上であるときに、前記基準位置を前記第２自己位置推定値または前記第２自己位置推定値に対応した位置に再設定する請求項１～３の何れか一項記載の自己位置推定装置。

Images