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

Abstract

【課題】自己位置の計測精度が低下する場所であっても、安定した自己位置の推定を行う。【解決手段】移動体の自己位置を計測する計測部と、デッドレコニングにより推定された前記自己位置の推定値に、前記計測部で計測された前記自己位置の計測値を反映させることで当該推定値を修正する推定部と、を備え、前記推定部は、前記計測値の品質に応じて前記計測値を反映させる度合いを調整する。【選択図】図１

Description

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

移動体の状態を知るための内界センサの値を用いて当該移動体の移動量（前回の時刻から現在の時刻までに移動した量）を検出し、その移動量に基づいて当該移動体の相対位置を推定するデッドレコニング（例えば、オドメトリ）で推定された推定値を、ＧＮＳＳ（Global Navigation Satellite System）センサ等の外界センサで計測した自己位置の計測値を用いて修正する方法が提案されている。

特許第５５０３４１９号公報

ただし、例えば移動体がコンテナヤードを走行する場合では、コンテナやトランスファークレーンの影響により、観測可能な衛星数が減少したり、衛星信号のマルチパスが発生したりすると、ＧＮＳＳセンサの計測精度が低下する。したがって、ＧＮＳＳセンサに限らず外界センサによる自己位置の計測精度が低下するような場所を移動体が移動する場合には、自己位置の推定精度を低下させてしまうおそれがあり、安定した自己位置の推定を行うことができない。

本発明は、このような事情に鑑みてなされたもので、その目的は、自己位置の計測精度が低下する場所であっても、安定した自己位置の推定を行うことができる自己位置推定装置を提供することとである。

本発明の一態様は、移動体の自己位置を計測する計測部と、デッドレコニングにより推定された前記自己位置の推定値に、前記計測部で計測された前記自己位置の計測値を反映させることで当該推定値を修正する推定部と、を備え、前記推定部は、前記計測値の品質に応じて前記計測値を反映させる度合いを調整することを特徴とする、自己位置推定装置である。

本発明の一態様は、上述の自己位置推定装置であって、前記計測部は、前記計測値の品質として、前記計測部の計測状態を示す指標を算出し、前記推定部は、前記計測部が算出した前記指標に応じて前記計測値を反映させる度合いを調整する。

本発明の一態様は、上述の自己位置推定装置であって、前記計測部は、衛星からの信号に基づいて前記自己位置の第１の計測値を計測するとともに、前記第１の計測値の計測状態を示す第１の指標を算出するＧＮＳＳセンサを備え、前記推定部は、前記第１の計測値を前記推定値に反映させる度合いを前記第１の指標に応じて調整しながら前記推定値を修正する。

本発明の一態様は、上述の自己位置推定装置であって、前記計測部は、距離センサが前記移動体の周囲をスキャンすることで計測した計測点の点群を用いてＩＣＰアルゴリズムのスキャンマッチングを行うことにより前記自己位置の第２の計測値を計測するとともに、前記第２の計測値の計測状態を示す第２の指標を算出するスキャンマッチング部を更に備え、前記推定部は、前記第２の計測値を前記推定値に反映させる度合いを前記第２の指標に応じて調整しながら、前記第１の計測値と前記第２の計測値とを前記推定値に反映させて当該推定値を修正する。

本発明の一態様は、上述の自己位置推定装置であって、前記推定部は、カルマンフィルタを用いて前記第１の計測値と前記第２の計測値とを前記推定値に反映させて前記推定値を修正する推定部と、前記カルマンフィルタにおける前記第１の計測値の誤差分散値を前記第１の指標に基づいて算出し、前記カルマンフィルタにおける前記第２の計測値の誤差分散値を前記第２の指標に基づいて算出する誤差分散値算出部と、を備える。

本発明の一態様は、上述の自己位置推定装置であって、前記ＧＮＳＳセンサは、前記ＧＮＳＳセンサの計測結果がＦｌｏａｔ解かＦｉｘ解かのいずれかであるかを判定し、その判定結果を前記第１の指標として前記推定部に送信し、前記誤差分散値算出部は、前記判定結果がＦｉｘ解である場合には前記第１の計測値の誤差分散値を第１の値に設定し、前記判定結果が前記Ｆｌｏａｔ解である場合には前記第１の計測値の誤差分散値を前記第１の値よりも大きい第２の値に設定する。

本発明の一態様は、上述の自己位置推定装置であって、前記スキャンマッチング部は、前記スキャンマッチングに用いるマップデータ及び前記移動体の現在位置で前記距離センサが計測した計測点の点群の二乗平均値と、前記点群に含まれる点数とを算出し、前記誤差分散値算出部は、前記二乗平均値が第１の閾値未満であって、且つ、前記点数が第２の閾値以上である条件を満たす場合には、前記第２の計測値の誤差分散値を第３の値に設定し、前記条件を満たさない場合には、前記第２の計測値の誤差分散値を第３の値よりも大きい第４の値に設定する。

以上説明したように、本発明によれば、自己位置の計測精度が低下する場所であっても、安定した自己位置の推定を行うことができる。

本発明の一実施形態に係る自己位置推定装置Ａの概略構成の一例を示す図である。 本発明の一実施形態に係る推定部３の概略構成の一例を示す図である。 本発明の一実施形態に係る統合部５の概略構成図である。 本発明の一実施形態に係る誤差分散値算出部９の誤差分散値Ｑ_１の算出方法の一例を示す図である。 本発明の一実施形態に係る誤差分散値算出部９の誤差分散値Ｑ_２の算出方法の一例を示す図である。 本発明の一実施形態に係るスキャンマッチング部４の動作の流れを示す図であるである。 本発明の一実施形態に係る統合部５が第１の計測値と第２の計測値とを用いて時刻ｋにおける運搬車両Ｍの自己位置を推定する方法を示す図である。

以下、本発明の一実施形態に係る自己位置推定装置を、図面を用いて説明する。

本発明の一実施形態に係る自己位置推定装置は、コンテナＣを目的の場所に運搬する運搬車両Ｍに搭載され、運搬車両Ｍの自律走行や遠隔操縦等を実現するために必要な運搬車両Ｍの自己位置を推定する装置である。なお、運搬車両Ｍは、本発明の「移動体」の一例である。すなわち、本発明における移動体は、移動するものであれば特に限定されず、例えば、移動体Ｍは、ロボットであってもよいし、一般車両であってもよいし、ダム堤体の施工や盛土工事に用いられる材料等を運搬する工事車両であってもよい。なお、移動体は、有人であるか、無人であるかは問わない。

以下に、本発明の一実施形態に係る自己位置推定装置Ａの構成について、説明する。
図１は、本発明の一実施形態に係る自己位置推定装置Ａの概略構成の一例を示す図である。図１に示すように、自己位置推定装置Ａは、ＧＮＳＳセンサ１、距離センサ２、及び推定部３を備える。なお、ＧＮＳＳセンサ１は、本発明の「計測部」の一例である。

ＧＮＳＳセンサ１は、運搬車両Ｍに設置されている。例えば、ＧＮＳＳセンサ１は、運搬車両Ｍのトレーラヘッド（トラクタともいう。）に設置されている。

ＧＮＳＳセンサ１は、衛星からの信号に基づいて、運搬車両Ｍの自己位置を計測する。すなわち、ＧＮＳＳセンサ１は、ＧＰＳ(Global Positioning System)に代表される全地球航法衛星システム（ＧＮＳＳ：Global Navigation Satellite System）によって自己位置を測位するセンサである。本実施形態では、ＧＮＳＳセンサ１は、予め定められた基準点に設置された基準局からの補正情報により運搬車両Ｍ（移動局）の衛星測位情報を補正して、運搬車両Ｍの自己位置を求めるＲＴＫ測位（リアルタイムキネマティック測位）を用いている。例えば、ＧＮＳＳセンサ１は、ＲＴＫ−ＧＮＳＳコンパスである。

このＧＮＳＳセンサ１は、運搬車両Ｍの自己位置として、運搬車両Ｍの現在位置（例えば、緯度ｘｇ及び経度ｙｇの絶対位置）と方位θｇを計測するともに、運搬車両Ｍの車速Ｖを算出する。
さらに、ＧＮＳＳセンサ１は、上記自己位置の計測状態（例えば、良好さ）を示す指標（以下、「第１の指標」という。）を算出する。例えば、ＧＮＳＳセンサ１は、運搬車両Ｍの自己位置の計測結果が、ＲＴＫが成立しているＦｉｘ解か、計算が収束しないＦｌｏａｔ解かのいずれかであるかを判定し、その判定結果を第１の指標とする。

ＧＮＳＳセンサ１は、運搬車両Ｍの自己位置の計測値（ｘｇ，ｙｇ，θg）、車速Ｖ及び第１の指標を所定の時間ごとに推定部３に送信する。なお、以下において、ＧＮＳＳセンサ１が計測した運搬車両Ｍの自己位置の計測値（ｘｇ，ｙｇ，θg）を、「第１の計測値」と称する場合がある。

距離センサ２は、運搬車両Ｍに設置されている。例えば、距離センサ２は、運搬車両Ｍのトレーラヘッドに設置されている。

距離センサ２は、水平方向にレーザ光を走査（スキャン）して運搬車両Ｍと周囲物体との距離を計測する。そして、距離センサ２は、計測した計測点の点群を推定部３に送信する。本実施形態において、距離センサ２がレーザ光を走査する水平方向は、例えば、３６０°の全方位である。なお、距離センサ２は、水平方向に加えて、垂直方向に対してもレーザ光を走査してもよい。例えば、距離センサ２は、２次元又は３次元のＬＲＦ（Laser Range Finder）や２次元又は３次元のＬＩＤＡＲ（Light Detection and Ranging、Laser Imaging Detection and Ranging）である。

推定部３は、ＧＮＳＳセンサ１及び距離センサ２から得られた情報を統合、すなわちセンサフュージョンして運搬車両Ｍの自己位置を推定する。なお、推定部３は、ＣＰＵ又はＭＰＵなどのマイクロプロセッサ、ＭＣＵなどのマイクロコントローラなどにより構成されてよい。
以下に、本発明の一実施形態に係る推定部３の構成について、説明する。図２は、本発明の一実施形態に係る推定部３の概略構成の一例を示す図である。

図２に示すように、推定部３は、スキャンマッチング部４及び統合部５を備える。なお、スキャンマッチング部４は、本発明の「計測部」の一例である。

スキャンマッチング部４は、距離センサ２が運搬車両Ｍの周囲をスキャンすることで計測した計測点の点群を用いてＩＣＰアルゴリズム等のスキャンマッチングを行うことにより、運搬車両Ｍの自己位置（ｘｓ，ｙｓ，θｓ）を計測する。具体的には、スキャンマッチング部４は、予め設定されているマップデータと、現時で距離センサ２が計測した点群（以下。「入力点群」という。）とを位置合わせを行いながら運搬車両Ｍの絶対位置の自己位置を計測する。ここで、マップデータとは、地図や前の時刻で距離センサ２が計測した計測点の点群が用いられ、そのデータがあればより精度が高くなる。例えば、マップデータは、ボクセルマップである。

なお、スキャンマッチング部４は、スキャンマッチングを行う前に、半径フィルタやｋｄ木（kｄ-tree）による最近傍探索を行ってもよい。
例えば、距離センサ２の計測精度や運用環境により、距離センサ２から取得した計測点の点群のうち、ある範囲の点群だけをスキャンマッチングに使用したいというケースがある。例えば、ある範囲の点群だけをスキャンマッチングに使用したいというケースとは、距離センサ２の振動が大きい場合には近傍の点群のみを使用するというケースや、車両の方位角が定まりづらい環境では遠方のみを使用するというケースである。したがって、スキャンマッチング部４は、運搬車両Ｍの中心軸から所定の距離にある点をのみを抽出する半径フィルタを適用することにより、所望の点群のみを入力点群として抽出することができる。

また、スキャンマッチング部４は、半径フィルタを適用した後に、大まかな位置合わせを行うために、ｋｄ木による最近傍探索を行ってもよい。例えば、予めスキャンマッチング部４の不図示の記憶部（以下、「地図メモリ」という。）に格納されているマップデータにおいて、スキャンマッチングを行う点群の分布が短時間（短い走行距離）で大きく変化している場合には、誤った位置合わせ結果が得られるケースがある。例えば、建物や植木の隙間から遠方の壁が見えるようなケースにおいては、外れ値が発生して上記のような問題が発生する場合がある。このような外れ値を含むデータにおいてパラメータ推定を行う場合、ロバスト推定を利用するのは一般的である。ただし、ＶＴＫ（Visualization Toolkit）のような汎用ライブラリに実装されているスキャンマッチング手法を利用する場合には、このようなロバスト推定法は必ずしも含まれていない。そこで、本実施形態では、スキャンマッチング部４は、スキャンマッチングに用いる点群間でｋｄ木を用いた距離計算を行い、マップデータに対して明らかに距離が離れている計測点を予め削除することで、外れ値の影響を低減してもよい。

さらに、スキャンマッチング部４は、自身が計測した自己位置の計測状態（例えば、計測精度）を示す指標（以下、「第２の指標」という。）を算出する。本実施形態では、スキャンマッチング部４は、第２の指標としてマップデータ及び入力点群の二乗平均値（以下、単に「ＲＭＳ」とする。）と、入力点群に含まれる点数（以下、単に「ＮＵＭ」とする。）を算出する。このＲＭＳは、収束計算による誤差の量を示しており、小さいほど誤差の小さい計算精度で自己位置が計測されていることを示す。一方、ＮＵＭは、距離センサ２の視野内にコンテナなどの物体が少ないと減少する傾向にあって、減少するとｋｄ木による最近傍探索に誤差が生じ易くなる。そのため、入力点群の点数であるＮＵＭが多いほどスキャンマッチングによる自己位置の計測誤差が小さくなる。

スキャンマッチング部４は、スキャンマッチングにより計測した運搬車両Ｍの自己位置の計測値（ｘｓ，ｙｓ，θｓ）と、第２の指標として算出したＲＭＳ及びＮＵＭとを統合部５に出力する。なお、以下において、スキャンマッチング部４が計測した運搬車両Ｍの自己位置の計測値（ｘｓ，ｙｓ，θｓ）を、「第２の計測値」と称する場合がある。

統合部５は、第１の計測値と第２の計測値とを、第１の指標及び第２の指標に従って統合、すなわち、センサフュージョンして運搬車両Ｍの自己位置を推定する。本実施形態の統合部５は、このセンサフュージョンのアルゴリズムとして、拡張カルマンフィルタを適用する。以下に、本発明の一実施形態に係る統合部５の概略構成について、図３を用いて説明する。図３は、本発明の一実施形態に係る統合部５の概略構成図である。
なお、第１の計測値及び第２の計測値のそれぞれを区別しない場合には、単に「計測値」と称する。

図３に示すように、統合部５は、自己位置推定部６、第１評価部７、第２評価部８、及び誤差分散値算出部９を備える。

自己位置推定部６は、オドメトリ等のデッドレコニングにより時刻ｋでの運搬車両Ｍの自己位置を推定し、その推定した自己位置の推定値（以下、「事前推定値」という。）Ｐｋに、第１の計測値や第２の計測値を反映させることで事前推定値Ｐｋを修正する。この修正後の事前推定値Ｐｋが、最終的な時刻ｋでの運搬車両Ｍの自己位置であって、事後推定値Ｐ´ｋと称する。ここで、事後推定値Ｐ´ｋを求めるにあたって、第１の計測値や第２の計測値を事前推定値Ｐｋに反映させる度合いは、各計測値の品質に応じて調整される。

なお、本実施形態では、自己位置推定部６は、拡張カルマンフィルタを用いて、事前推定値Ｐｋに対して第１の計測値や第２の計測値を反映させて、時刻ｋにおける運搬車両Ｍの自己位置である事後推定値Ｐ´ｋを推定する。したがって、第１の計測値を事前推定値Ｐｋに反映させる度合いは、時刻ｋにおける第１の計測値の誤差分散値Ｑ_１に相当し、第２の計測値を事前推定値Ｐｋに反映させる度合いは、時刻ｋにおける第２の計測値の誤差分散値Ｑ_２に相当する。

第１評価部７は、第１の指標に基づいて、第１の計測値の品質を評価する。例えば、この第１の計測値の品質とは、第１の計測値の計測状態である。したがって、本実施形態では、第１評価部７は、ＧＮＳＳセンサ１から得らえた第１の指標に基づいて、第１の計測値の計測状態が良好か不良かを評価する。例えば、第１評価部７は、第１の指標として、運搬車両Ｍの自己位置の計測結果がＦｌｏａｔ解である情報をＧＮＳＳセンサ１から取得した場合には、ＧＮＳＳセンサ１で計測した第１の計測値の計測状態が不良であると評価する。一方、第１評価部７は、第１の指標として、運搬車両Ｍの自己位置の計測結果がＦｉｘ解である情報をＧＮＳＳセンサ１から取得した場合には、ＧＮＳＳセンサ１で計測した第１の計測値の計測状態が良好であると評価する。そして、第１評価部７は、第１の計測値の評価結果を誤差分散値算出部９に送信する。

第２評価部８は、第２の指標に基づいて、第２の計測値の品質を評価する。例えば、この第２の計測値の品質とは、第２の計測値の計測状態である。したがって、本実施形態では、第２評価部８は、スキャンマッチング部４から得られた第２の指標に基づいて、第２の計測値の計測状態が良好か不良かを評価する。なお、第２の計測値の品質とは、距離センサ２が計測した点群の品質（例えば、距離センサ２の計測状態）を含む。

例えば、第２評価部８は、ＲＭＳが第１の閾値未満であって、且つ、ＮＵＭが第２の閾値以上である条件を満たす場合には、第２の計測値が良好であると評価する。一方、第２評価部８は、ＲＭＳ及びＮＵＭが上記条件を満たさない場合には、第２の計測値が不良であると評価する。そして、第２評価部８は、第２の計測値の評価結果を誤差分散値算出部９に送信する。

誤差分散値算出部９は、第１評価部７の評価結果に基づいて、ＧＮＳＳセンサ１で計測された第１の計測値の誤差分散値Ｑ_１を算出する。例えば、誤差分散値算出部９は、第１評価部７の評価結果として、第１の計測値が良好であるという評価結果が得られた場合には、誤差分散値Ｑ_１を第１の値に設定し、第１の計測値が不良であるという評価結果が得られた場合には、誤差分散値Ｑ_１を第１の値よりも大きい第２の値に設定する。

一例として、誤差分散値算出部９は、図４に示すように、第１評価部７の評価結果として、第１の計測値が良好であるという評価結果が得られた場合には、現在位置（ｘｇ，ｙｇ）の計測精度３σを「０．０５」とし、方位θｇの計測精度３σを「０．５」に設定する。一方、誤差分散値算出部９は、第１評価部７の評価結果として、第２の計測値が不良であるという評価結果が得られた場合には、現在位置（ｘｇ，ｙｇ）の計測精度３σを「３．６」とし、方位θｇの計測精度３σを「５」に設定する。
そして、誤差分散値算出部９は、設定した現在位置（ｘｇ，ｙｇ）の計測精度３σと方位θｇの計測精度３σとに基づいて、それぞれの誤差分散値Ｑ_１（例えば、分散共分散行列）を算出する。

また、誤差分散値算出部９は、第２評価部８の評価結果に基づいて、スキャンマッチング部４で計測された第２の計測値の誤差分散値Ｑ_２を算出する。例えば、誤差分散値算出部９は、第２評価部８の評価結果として、第２の計測値が良好であるという評価結果が得られた場合には、誤差分散値Ｑ_２を第３の値に設定し、第２の計測値が不良であるという評価結果が得られた場合には、誤差分散値Ｑ_２を第３の値よりも大きい第４の値に設定する。

一例として、誤差分散値算出部９は、図５に示すように、第２評価部８の評価結果として、第２の計測値が良好であるという評価結果が得られた場合には、現在位置（ｘｓ，ｙｓ）の計測精度３σを「３．６」とし、方位θｓの計測精度３σを「０．１」に設定する。一方、誤差分散値算出部９は、第２評価部８の評価結果として、第２の計測値が不良であるという評価結果が得られた場合には、現在位置（ｘｓ，ｙｓ）の計測精度３σを「６」とし、方位θｓの計測精度３σを「５」に設定する。
そして、誤差分散値算出部９は、設定した現在位置（ｘｓ，ｙｓ）の計測精度３σと方位θｓの計測精度３σとに基づいて、それぞれの誤差分散値Ｑ_２（例えば、分散共分散行列）を算出する。

このように、誤差分散値算出部９は、第１の計測値及び第２の計測値のそれぞれの誤差分散値を一定値ではなく、各計測値の品質に応じて調整する。例えば、誤差分散値算出部９は、第１の計測値の品質が不良であれば、誤差分散値Ｑ_１を良好時よりも大きい値に調整する。また、誤差分散値算出部９は、第２の計測値の品質が不良であれば、誤差分散値Ｑ_２を良好時よりも大きい値に調整する。

したがって、統合部５は、第１の計測値や第２の計測値の品質が良好であれば、当該計測値の重みを上げてセンサフュージョンを実行し、第１の計測値や第２の計測値の品質が不良であれば、当該計測値の重みを下げてセンサフュージョンを実行することで、自己位置を推定する。これにより、第１の計測値や第２の計測値の計測精度が低下する場所であっても、安定して自己位置を推定することができる。

なお、自己位置推定部６で推定した自己位置（事後推定値）は、例えば、運搬車両Ｍの走行を制御する制御装置１００に送信される。そして、制御装置１００は、自己位置推定部６から得られた事後推定値に基づいて、運搬車両Ｍのアクセルやブレーキの操作量、及びステアリングの操舵角を制御することで、例えば、運搬車両Ｍの自律走行を行う。

次に、本発明の一実施形態に係るスキャンマッチング部４の動作の流れを説明する。図６は、本発明の一実施形態に係るスキャンマッチング部４の動作の流れを示す図である。

スキャンマッチング部４は、時刻ｋの運搬車両Ｍの自己位置（ｘｓ，ｙｓ，θｓ）を計測する計測処理を実行するにあたって、運搬車両Ｍが移動しているか否かを判定する（ステップＳ１０１）。なお、運搬車両Ｍが移動しているか否かを判定する方法は、特定に限定されないが、例えば、スキャンマッチング部４は、運搬車両Ｍの車速に基づいて運搬車両Ｍが移動しているか否かを判定することができる。

スキャンマッチング部４は、運搬車両Ｍが移動していると判定した場合には、時刻ｋでの距離センサ２の計測値の点群を取得する（ステップＳ１０２）。一方、スキャンマッチング部４は、運搬車両Ｍが移動していないと判定した場合には、時刻ｋの計測処理を終了して時刻ｋ＋１の計測処理に移行する（ステップＳ１０９）。

スキャンマッチング部４は、ステップＳ１０２の処理において取得した点群の点数が規定値を超えているか否かを判定する（ステップＳ１０３）。スキャンマッチング部４は、取得した点群の点数が規定値を超えていると判定した場合には、十分な点数の点群が得られたとして、時刻ｋの計測処理を継続する。一方、スキャンマッチング部４は、取得した点群の点数が規定値以下である場合には、十分な点数の点群が得られていないとして、時刻ｋの計測処理を停止して、時刻ｋ＋１の計測処理に移行する（ステップＳ１０９）。

スキャンマッチング部４は、十分な点数の点群が得られた場合には、マップデータとしてボクセルマップを取得する（ステップＳ１０４）。また、スキャンマッチング部４は、事前フィルタリングとして、テップＳ１０２で得られた点群が３Ｄである場合には、当該点群を２Ｄの点群に変換するとともに、ノイズを除去して入力点群を得る（ステップＳ１０５）。

例えば、スキャンマッチング部４は、最も水平レイヤーに近い点群であって、水平視野角３６０°のうち運搬車両Ｍの前方から左右に±９０°の範囲に限定した点群を抽出する。また、コンテナヤードでは、距離センサ２の計測距離（スキャンの距離）が４０ｍ以上の場合にはノイズが多くなってしまう傾向にあるため、スキャンマッチング部４は、ボクセルに対する点の時間占有度が高いグリッドのみをボクセルマップに反映させるフィルタによりノイズを低減させる。その後、スキャンマッチング部４は、おおまかな位置合わせを行うためにボクセルマップに対してｋｄ木による最近傍探索を行う（ステップＳ１０６）。なお、ステップＳ１０６の処理は必須ではない。

スキャンマッチング部４は、その最近傍探索の結果を以って、ボクセルマップと入力点群とを用いてＩＣＰアルゴリズムによるスキャンマッチングを行うことで、時刻ｋの運搬車両Ｍの自己位置（ｘｓ，ｙｓ，θｓ）を計測する（ステップＳ１０７）。さらに、スキャンマッチング部４は、第２の指標としてマップデータ及び入力点群の二乗平均値であるＲＭＳと、入力点群の点数であるＮＵＭとを算出する（ステップＳ１０８）。
そして、スキャンマッチング部４は、計測した時刻ｋの運搬車両Ｍの自己位置（第２の計測値）と、算出したＲＭＳ及びＮＵＭとを統合部５に送信する。その後、スキャンマッチング部４は、時刻ｋの計測処理を終了して時刻ｋ＋１の計測処理に移行する（ステップＳ１０９）。

次に、本発明の一実施形態に係る統合部５が第１の計測値と第２の計測値とを統合して、時刻ｋにおける運搬車両Ｍの自己位置を推定する方法について、図７を用いて説明する。

まず、統合部５は、デッドレコニングにより時刻ｋでの運搬車両Ｍの事前推定値Ｐｋを求める。次に、統合部５は、時刻ｋ又は時刻ｋと同視し得る範囲の時刻の第１の計測値及び第２の計測値が得られたならば、事前推定値Ｐｋに対して、その第１の計測値及び第２の計測値のうち、いずれか一方の計測値を、拡張カルマンフィルタを用いて反映させることで事前推定値Ｐｋを修正する。次に、統合部５は、修正した事前推定値Ｐｋに対して、他方の計測値を、拡張カルマンフィルタを用いて反映させて事後推定値Ｐ´ｋを算出する。これにより、統合部５は、第１の計測値及び第２の計測値を用いて運搬車両Ｍの自己位置（事後推定値Ｐ´ｋ）を推定することができる。

ここで、統合部５は、第１の計測値を事前推定値Ｐｋに反映させる度合いを第１の指標に応じて調整しながら事前推定値Ｐｋを修正する。具体的には、統合部５は、第１の計測値の誤差分散値Ｑ_１を第１の指標に応じて調整する。この誤差分散値Ｑ_１を調整することは、図７に示すように、第１の計測値の誤差楕円Ｅ_１の範囲を調整することを意味する。したがって、例えば、統合部５は、第１の指標により第１の計測値の品質を評価し、その評価の結果、当該品質が良好であるならば、誤差楕円Ｅ_１の範囲を所定の範囲に調整する。一方、統合部５は、その評価の結果、第１の計測値の品質が不良であるならば、誤差楕円Ｅ_１の範囲が良好時よりも広くなるように調整する。

同様に、統合部５は、第２の計測値を事前推定値Ｐｋに反映させる度合いを第２の指標に応じて調整しながら事前推定値Ｐｋを修正する。具体的には、統合部５は、第２の計測値の誤差分散値Ｑ_２を第２の指標に応じて調整する。この誤差分散値Ｑ_２を調整することは、図７に示すように、第２の計測値の誤差楕円Ｅ_２の範囲を調整することを意味する。したがって、統合部５は、第２の指標により第２の計測値の品質を評価し、その評価の結果、当該品質が良好であるならば、誤差楕円Ｅ_２の範囲を所定の範囲に調整する。一方、統合部５は、その評価の結果、第２の計測値の品質が不良であるならば、誤差楕円Ｅ_２の範囲が良好時よりも広くなるように調整する。

このように、自己位置推定装置Ａは、ＧＮＳＳセンサ１や距離センサ２等の外界センサにより得られた計測値を、当該計測値の誤算分散値に応じて内界センサにより得られた事前推定値に反映させて運搬車両Ｍの自己位置を推定するものである。この内界センサとは、運搬車両Ｍの状態を知るためのセンサであって、その内界センサで得られた値は、デッドレコニングに用いられる。そして、自己位置推定装置Ａは、上記計測値の品質を評価し、その評価結果に応じて、当該計測値の誤算分散値を調整する。すなわち、自己位置推定装置Ａは、上記計測値の品質を評価し、品質が良い計測値の重みを上げて、品質の悪い計測値の重みを下げてセンサフュージョンして、運搬車両Ｍの自己位置を推定する。これにより、運搬車両Ｍが、ＧＮＳＳセンサ１における自己位置の計測精度やスキャンマッチングにおける自己位置の計測精度が低下する場所（例えば、コンテナヤード）を走行する場合であっても、自己位置推定装置Ａは、安定した自己位置の推定を行うことができる。

以上、この発明の実施形態について図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない範囲の設計等も含まれる。

（変形例１）上記実施形態では、ＧＮＳＳセンサ１やスキャンマッチングによる自己位置の計測値（第１の計測値及び第２の計測値）を良好又は不良の２種類で評価を行ったが、本発明はこれに限定されず、３種類以上で評価してもよい。この場合には、誤差分散値算出部９は、計測値の品質の評価が高い、すなわち、品質が高くなるにつれて、当該計測値の誤差分散値が高くなるように当該誤差分散値を調整すればよい。

（変形例２）上記実施形態では、統合部５は、時刻ｋ又は時刻ｋと同視し得る範囲の時刻で第１の計測値及び第２の計測値の双方の計測値が得られた場合を例として説明したが、本発明はこれに限定されない。例えば、時刻ｋ又は時刻ｋと同視し得る範囲の時刻において、第１の計測値のみが得られた場合には、統合部５は、推定した事前推定値Ｐｋに対して、その第１の計測値を反映させることで事後推定値Ｐ´ｋを算出してもよい。すなわち、統合部５は、第１の計測値及び第２の計測値を同時刻に取得してセンサフュージョンする必要はなく、計測値が得られた時刻で当該計測値を逐次センサフュージョンしてその時刻の事前推定値を修正すればよい。

（変形例３）上記実施形態では、計測値としてＧＮＳＳセンサ１の第１の計測値とスキャンマッチングの第２の計測値の双方を用いたが、本発明はこれに限定されず、例えば、少なくともいずれかの計測値を用いればよい。ただし、スキャンマッチングは絶対精度を保証できないため、第１の計測値と第２の計測値のうち、いずれかの計測値を用いる場合には、第１の計測値を用いることが望ましい。

（変形例４）上記実施形態において、ＧＮＳＳセンサ１の品質はしきい値を使う第１の指標に基づいて評価しているが、ＧＮＳＳのＲＡＷデータから得られる、衛星数ｎｕｍや擬似距離psude_distance、搬送波位相phaseなども利用し、連続値として品質を評価してもよい。例えば、以下の式（１）を用いてもよい。
ＧＮＳＳの誤差分散値＝f(測位モード，num, psude_distance, phase) …（１）

（変形例５）上記実施形態において、距離センサ２として、ＬＩＤＡＲやＬＲＦの代わりにカメラなどの撮像装置を用いても良い。その場合には、当該撮像装置の計測データに誤差を与えて、当該誤差を撮像装置の計測データを統合する際の品質データとして用いてもよい。

（変形例６）上記実施形態において、計測値としてエンコーダの計測値を用いても良い。また、その計測値に誤差を与えて、センサフュージョンを行う際の品質データとして用いてもよい。

（変形例７）上記実施形態では、ＧＮＳＳセンサ１や距離センサ２は、運搬車両Ｍに設置されている場合について、説明したが、本発明はこれに限定されず、運搬車両Ｍ外に設置されていてもよい。

（変形例８）上記実施形態では、センサフュージョンのアルゴリズムとして、拡張カルマンフィルタを適用したが、本発明はこれに限定されない。すなわち、デッドレコニングにより推定された事前推定値に、外界センサを用いて計測した自己位置の計測値を反映させることで当該事前推定値を修正するアルゴリズムであれば、種々のアルゴリズムを適用可能である。

（変形例９）上記実施形態において、運搬車両Ｍの方位θをジャイロコンパス等の様々な種類のセンサを用いて取得してもよい。

（変形例１０）上記実施形態において、第２評価部８は、ＲＭＳが第１の閾値未満であること、及びＮＵＭが第２の閾値以上であることの少なくともいずれか一方が成立した場合に第２の計測値が良好であると評価してもよい。

（変形例１１）上記実施形態において、第２評価部８は、ＲＭＳとＮＵＭをしきい値で評価せず、関数を用いて連続値として評価しても良い。例えば以下の式（２）を用いてもうよい。
距離センサの誤差分散値= g(ＲＭＳ，ＮＵＭ) …（２）

以上、説明したように、本実施形態に係る推定部３は、自己位置の計測値の品質を逐次評価して、その品質の評価結果に応じて計測値を事前推定値に反映させる度合いを調整する。

このような構成によれば、自己位置の計測精度が低下する場所であっても、安定した自己位置の推定を行うことができる。例えば、コンテナヤードでは片側にしか構造物が無い場所や、そもそも構造物がない場所があるため、コンテナヤードでスキャンマッチングによる自己位置計測を実行すると、誤差が蓄積して計測精度が低下する場合がある。また、コンテナヤードでＧＮＳＳセンサによる自己位置計測を実行すると、コンテナやトランスファークレーンの影響により、観測可能な衛星数が減少したり、衛星信号のマルチパスが発生したりすると、ＧＮＳＳセンサの計測精度が低下する場合がある。本実施形態の自己位置推定装置Ａは、計測精度を評価して、その評価結果に応じて誤差分散値を調整するため、コンテナヤードであっても、安定した自己位置の推定を行うことができる。

また、推定部３は、自己位置の計測値として、ＧＮＳＳセンサ１により計測された第１の計測値と、スキャンマッチングにより計測された第２の計測値との双方を用いてもよい。この場合には、推定部３は、基本的には第１の計測値を用いて事前推定値を修正し、第１の計測値の品質が悪化した場合に、ＧＮＳＳセンサ１の補完として、第２の計測値を当該第２の計測値の品質に応じて事前推定値の修正に利用してもよい。

このような構成によれば、ＧＮＳＳセンサ１による自己位置の計測精度が低下する場所であっても、安定した自己位置の推定を行うことができる。

１ ＧＮＳＳセンサ
２ 距離センサ
３ 推定部
４ スキャンマッチング部
５ 統合部
６ 自己位置推定部
７ 第１評価部
８ 第２評価部
９ 誤差分散値算出部

Claims (7)

1. 移動体の自己位置を計測する計測部と、
   デッドレコニングにより推定された前記自己位置の推定値に、前記計測部で計測された前記自己位置の計測値を反映させることで当該推定値を修正する推定部と、
   を備え、
   前記推定部は、前記計測値の品質に応じて前記計測値を反映させる度合いを調整することを特徴とする、自己位置推定装置。
2. 前記計測部は、前記計測値の品質として、前記計測部の計測状態を示す指標を算出し、
   前記推定部は、前記計測部が算出した前記指標に応じて前記計測値を反映させる度合いを調整することを特徴とする、請求項１に記載の自己位置推定装置。
3. 前記計測部は、衛星からの信号に基づいて前記自己位置の第１の計測値を計測するとともに、前記第１の計測値の計測状態を示す第１の指標を算出するＧＮＳＳセンサを備え、
   前記推定部は、前記第１の計測値を前記推定値に反映させる度合いを前記第１の指標に応じて調整しながら前記推定値を修正することを特徴とする、請求項１又は２に記載の自己位置推定装置。
4. 前記計測部は、距離センサが前記移動体の周囲をスキャンすることで計測した計測点の点群を用いてＩＣＰアルゴリズムのスキャンマッチングを行うことにより前記自己位置の第２の計測値を計測するとともに、前記第２の計測値の計測状態を示す第２の指標を算出するスキャンマッチング部を更に備え、
   前記推定部は、前記第２の計測値を前記推定値に反映させる度合いを前記第２の指標に応じて調整しながら、前記第１の計測値と前記第２の計測値とを前記推定値に反映させて当該推定値を修正することを特徴とする、請求項３に記載の自己位置推定装置。
5. 前記推定部は、
   カルマンフィルタを用いて前記第１の計測値と前記第２の計測値とを前記推定値に反映させて前記推定値を修正する推定部と、
   前記カルマンフィルタにおける前記第１の計測値の誤差分散値を前記第１の指標に基づいて算出し、前記カルマンフィルタにおける前記第２の計測値の誤差分散値を前記第２の指標に基づいて算出する誤差分散値算出部と、
   を備えることを特徴とする、請求項４に記載の自己位置推定装置。
6. 前記ＧＮＳＳセンサは、前記ＧＮＳＳセンサの計測結果がＦｌｏａｔ解かＦｉｘ解かのいずれかであるかを判定し、その判定結果を前記第１の指標として前記推定部に送信し、
   前記誤差分散値算出部は、前記判定結果がＦｉｘ解である場合には前記第１の計測値の誤差分散値を第１の値に設定し、前記判定結果が前記Ｆｌｏａｔ解である場合には前記第１の計測値の誤差分散値を前記第１の値よりも大きい第２の値に設定することを特徴とする、請求項５に記載の自己位置推定装置。
7. 前記スキャンマッチング部は、前記スキャンマッチングに用いるマップデータ及び前記移動体の現在位置で前記距離センサが計測した計測点の点群の二乗平均値と、前記点群に含まれる点数とを算出し、
   前記誤差分散値算出部は、前記二乗平均値が第１の閾値未満であって、且つ、前記点数が第２の閾値以上である条件を満たす場合には、前記第２の計測値の誤差分散値を第３の値に設定し、前記条件を満たさない場合には、前記第２の計測値の誤差分散値を第３の値よりも大きい第４の値に設定することを特徴とする、請求項５又は６に記載の自己位置推定装置。

Images