JP4697127B2 - 自己位置認識方法 - Google Patents

Description

本発明は、自律的に移動する移動体における自己位置認識方法に関する。

従来、自律的に移動する車両が記憶保持している地図上における自己位置を決定する自己位置認識方法として、デッドレコニングの方法やマップマッチングの方法がある。デッドレコニングでは、車輪の回転数やジャイロなどの走行開始時から蓄積されたデータに基づく移動距離によって自己位置を認識する。マップマッチングでは、走行環境に存在する壁や障害物などの目印とされる物体の位置を障害物センサで検出してその検出結果と地図との比較（重ねてマッチングさせる比較）から自己位置を認識する。デッドレコニングだけでは、位置認識の誤差が蓄積され、その誤差を解消できない。そこで、誤差が問題になる場合は、デッドレコニングと組合せて、または単独で、蓄積データに依らずに自己位置を確定できるマップマッチングが用いられる。

上述のマップマッチングを効率的かつ精度良く行うために、距離データの測定点列が直線となる平面壁などの表面位置がマッチング用のデータとして用いられる。レーザレーダによって広い視野範囲を水平にスキャンすると、直線状に配列された測定点列が得られる。その測定点のヒストグラムにより、点列の配列方向、従って壁の方向を検出して地図情報における壁の配置とマッチングさせれば自己位置を認識できる。このようなマップマッチングの方法を電動車椅子の操縦補助システムに応用した例が知られている（例えば、特許文献１参照）。
後藤健志著 「レーザレンジファインダを用いた電動車椅子の操縦補助システム」、奈良先端科学技術大学院大学情報科学研究科情報システム学専攻修士論文、ＮＡＩＳＴ−ＩＳ−ＭＴ００５１０３８，２００２年

しかしながら、上述したようなマップマッチングを含めて、一般にマップマッチングによる自己位置認識方法においては、以下に示すような問題がある。通常、センサの死角をなくすために自律移動体、例えば車両の前方左右や後方左右などのように、複数台の障害物センサが配置される。このような複数の障害物センサを用いて障害物情報を取得し、マップマッチングを行うとき発生する問題を、図８（ａ）（ｂ）、図９（ａ）（ｂ）を参照して説明する。

図８（ａ）に示すように、自律移動体１が、前面右方に備えた視野角ξ１の障害物センサ２１によって左右の壁Ｗを検出し、その測定点列が図中の黒丸の点列のように得られたとする。これらの点列の位置と、壁Ｗの位置のずれは、自律移動体１が認識している自己位置（位置座標と姿勢で決まる）が、正しくないことを意味する。この場合、地図情報と測定点列とをマッチングさせることにより、壁Ｗに直交する方向のずれと壁Ｗに対する回転のずれ（すなわち姿勢のずれ）とを解消する移動量と回転量とを求めて姿勢を含む自己位置を補正し、これにより、自己位置を認識できる。同様に、図８（ｂ）に示すように、前面左方に備えた視野角ξ２の障害物センサ２２によって左右の壁Ｗを検出し、地図情報と測定点列とをマッチングさせて自己位置を認識できる。

ところが、障害物センサ２１，２２によって取得された障害物情報に基づくマップマッチング結果が、互いに整合性がとれていない場合に、マップマッチングの結果に基づいて左右の回転補正を連続して繰り返すと、路面の摩擦力や種々の不可逆的要因によって、例えば、図９（ａ）（ｂ）に示すように、自律移動体１が前方へ移動してしまうという事態が発生する。さらに、図８（ａ）（ｂ）に示すように、自律移動体１の左右に壁Ｗが存在するが前後方向に位置決め用の目標物が存在しない場合には、図９（ａ）（ｂ）に示したような、前後方向の位置ずれを補正することができない。

上述のような自己位置認識結果とその後の位置補正における不安定性は、複数の障害物センサの取付位置や取付角度に誤差があって各障害物センサ間の整合性が損なわれている場合や、測定しようとした壁Ｗの前面に他の移動物や障害物が介在したりして、距離データにノイズが混入してマップマッチングの信頼性が損なわれた場合などに発生する。

本発明は、上記課題を解消するものであって、簡単な構成により、複数の障害物センサを用いる場合に、信頼性と位置精度とを確保して自己位置を安定に認識できる自己位置認識方法を提供することを目的とする。

上記課題を達成するために、請求項１の発明は、障害物情報と地図情報とのマップマッチングに基づいて地図上の自己位置を認識する自己位置認識方法において、複数の障害物センサを用いて自己の周辺に存在する障害物を検知して複数の障害物情報を取得する障害物情報取得工程と、地図記憶装置に記憶された地図情報と前記障害物情報取得工程によって取得された複数の障害物情報の各々とを自己の移動の自由度毎にマップマッチングさせるための移動量を地図情報に基づく位置データのヒストグラムと障害物情報に基づく位置データのヒストグラムとの相互相関関数を計算することにより相互相関関数のピーク位置から求めるマッチング演算工程と、前記マッチング演算工程によって求めた各移動量をマッチング度合いについて前記ピークの高さによって評価するマッチング評価工程と、前記マッチング評価工程による各障害物情報毎および各自由度毎の評価結果を総合的に評価すると共にその結果に基づいて前記移動の自由度毎の移動量を決定して地図上の自己位置を決定する総合評価工程と、を備えたものである。

請求項２の発明は、請求項１に記載の自己位置認識方法において、前記マッチング評価工程では、前記マッチング度合いの信頼性の有無を評価結果とするものである。

請求項３の発明は、請求項２に記載の自己位置認識方法において、前記総合評価工程では、前記マッチング評価工程によりマッチングが信頼性有りとされた移動量の各自由度毎の平均値を各自由度毎の移動量とするものである。

請求項４の発明は、請求項２に記載の自己位置認識方法において、前記総合評価工程では、各自由度毎に所定の順番で前記マッチング評価工程の複数の評価結果を評価し、前記順番に行う評価の際に最初に信頼性ありとされた移動量をその自由度に対する移動量とするものである。

請求項５の発明は、請求項２に記載の自己位置認識方法において、前記総合評価工程では、１つの障害物センサの障害物情報に基づく全自由度についての移動量が全て信頼性有りとされている場合、当該各移動量を全自由度に対する各移動量とするものである。

請求項６の発明は、請求項１に記載の自己位置認識方法において、前記マッチング評価工程では、前記マッチング度合いを数値化して評価結果とするものである。

請求項７の発明は、請求項６に記載の自己位置認識方法において、前記総合評価工程では、前記マッチング評価工程により各自由度毎に最もマッチング度合いが高いとされた移動量を各自由度に対する移動量とするものである。

請求項８の発明は、請求項６に記載の自己位置認識方法において、前記総合評価工程では、前記数値化されたマッチング度合いの全自由度に対する総和が最も高い障害物情報に基づいて求められた各移動量を各自由度に対する移動量とするものである。

請求項９の発明は、請求項１に記載の自己位置認識方法において、前記障害物情報取得工程では、較正用の障害物と前記各障害物センサとを用いて較正用の障害物情報を取得し、前記総合評価工程では、前記較正用の障害物情報から求められた移動量を比較して各障害物センサ間の補正値を求めるものである。

請求項１０の発明は、請求項９に記載の自己位置認識方法において、前記総合評価工程では、前記補正値が所定の値を超える障害物センサをセンサ異常とするものである。

請求項１の発明によれば、各障害物情報毎および各自由度毎の評価結果を総合的に評価すると共にその結果に基づいて移動の自由度毎の移動量を決定して自己位置を決定するので、各自由度毎に最良の移動量を決定して、全体として、信頼性と位置精度とが確保された状況のもとで、自己位置を安定に認識できる。つまり、複数の障害物情報を相互に補完させると共に、複数の障害物情報によるマッチング結果の無秩序な適用を回避して自己位置を認識できるので、自律移動体が移動する種々の環境のもとで、最良の自己位置認識が可能となる。

請求項２の発明によれば、信頼性の有りなしによって、マッチング度合いを２値化するので、評価が単純明快であり、その後の総合評価が容易である。

請求項３の発明によれば、総合評価の計算が比較的簡単となる。

請求項４の発明によれば、全ての障害物センサによるマッチング演算とその後の評価結果を待つことなく、信頼性有りの結果が出た時点で最終の移動量を決定でき、自己位置認識処理が速くなる。

請求項５の発明によれば、全自由度について、１つの障害物センサに基づく各移動量を用いるので、統一性の取れた自己位置認識ができる。

請求項６の発明によれば、数値によって信頼性の比較ができるので、信頼性有無の２値化された評価結果を用いる場合よりもきめ細かい自己位置認識を実現できる。

請求項７の発明によれば、信頼度のもっとも高いマップマッチングによって自己位置認識ができる。

請求項８の発明によれば、総合的に信頼性の高い１つの障害物センサによるマップマッチングの結果に基づいて、信頼性の高い自己位置認識ができる。

請求項９の発明によれば、各障害物センサ間の補正値によって、その後の各移動量を補正することができるので、各障害物センサによる自己位置認識結果に整合性を持たせることができ、異なる障害物センサによる自己位置認識結果の不安定性を解消できる。つまり、各障害物センサの特性の違いによる測定誤差や、障害物センサの取付位置の誤差などを、補正値によりソフトウエア的に自動修正でき、信頼度の高いマップマッチングによる自己位置認識が可能となる。

請求項１０の発明によれば、障害物センサの故障を検知して、その後の自己位置認識の信頼性を確保できる。

以下、本発明の実施形態に係る自己位置認識方法について、図面を参照して説明する。

（第１の実施形態）
図１は第１の実施形態に係る自己位置認識方法のフローチャートを示し、図２はこの方法が適用される自律移動体を示す。この自己位置認識方法は、図１に示すように、障害物情報を取得する障害物情報取得工程（Ｓ１）と、地図情報と障害物情報とから位置補正用の複数の移動量を求めるマッチング演算工程（Ｓ２）と、求めた移動量を評価するマッチング評価工程（Ｓ３）と、評価結果を総合的に評価して自己位置を決定する総合評価工程（Ｓ４）と、を備えている。ここでは、図２に示すように、４つの障害物センサ２１〜２４を備えた自律移動体１が自己位置認識を行う例を説明する。なお、本発明は、複数の障害物センサであれば自己位置認識に適用できる。

自律移動体１は、前後左右に、それぞれ検出の視野角ξ１〜ξ４を有する障害物センサ２１〜２４を備えて、自己位置を認識しながら前方矢印Ｚ方向に自律的に移動している。自律移動体１の走行経路の両側には、障害物として壁Ｗが存在する。自律移動体１は、このような走行経路における障害物情報を地図情報として地図記憶装置に記憶している。地図記憶装置に記憶されている地図情報は、走行領域全体を表すグローバル座標系Ｘ−Ｙのもとで記述されている。自律移動体１は、グローバル座標系Ｘ−Ｙにおける自己位置と姿勢とを確認しつつ、すなわち自己位置認識を行いつつ移動する。

上述の確認の作業は、通常、前回確認した地図上の位置から移動した現在位置であると推定される位置（姿勢も含む、以下同じ）を、障害物情報に基づいて確認し、この確認によって得られた補正量を用いて位置補正するという作業になる。また、自己位置認識とは、グローバル座標系Ｘ−Ｙにおける、自律移動体１の基準点、例えば回転中心ｇの位置座標と、基準点回りの回転量Ｒとを決定することである。回転量Ｒは、例えばＸ軸に対する自律移動体１の姿勢（前方方向）の傾きを表す。

各障害物センサ２１〜２４は、それぞれのセンサ毎に定義された、自律移動体１と共に移動する移動座標系である座標系ｘ１−ｙ１，ｘ２−ｙ２，ｘ３−ｙ３，ｘ４−ｙ４を有している。これらの移動座標系と、自律移動体１上の基準点、およびグローバル座標系Ｘ−Ｙとの関係は、原理的に数式によって互いに一意的に関連づけることができる。そこで、十分な障害物情報が得られるならば、各障害物センサ２１〜２４のいずれによってでも、自律移動体１の自己位置認識が可能である。

上述のように、原理的に何れの障害物センサ２１〜２４を用いても、自己位置認識できるのであるが、各障害物センサの取付精度や測定精度、または測定時のノイズの混入などによって、マップマッチングの結果に信頼性の差が出てくるので、上述の各工程Ｓ１〜Ｓ４を経て、自己位置を認識する。

まず、障害物情報取得工程（Ｓ１）において、複数の障害物センサを用いて自己の周辺に存在する障害物を検知して障害物情報を取得する。ここでは、簡単のため、２つの障害物センサ２１，２２を用いて、前方左右の壁Ｗの位置を取得するものとする。

次に、マッチング演算工程（Ｓ２）において、地図記憶装置に記憶された地図情報と前記障害物情報取得工程（Ｓ１）によって取得された２つの障害物情報とを移動の自由度毎にマッチングさせるための移動量（すなわち補正量）を求める。つまり、今の場合、例えば、障害物センサ２１による障害物情報に基づくＸ方向、Ｙ方向、およびＸ軸からの回転の３つの自由度に対する移動量、および、障害物センサ２２による障害物情報に基づくＸ方向、Ｙ方向、およびＸ軸からの回転の３つの自由度に対する移動量、すなわち補正量（Δθ，ΔＸ，ΔＹ）を求める。この工程で行われるマッチング（マップマッチング）の方法については後述する。

次に、マッチング評価工程（Ｓ３）において、前記マッチング演算工程（Ｓ２）によって求めた各移動量をマッチング度合いについて評価する。今の場合、３つの移動の自由度に対する、それぞれ２つずつの移動量についての評価がなされる。

最後に、総合評価工程（Ｓ４）において、前記マッチング評価工程（Ｓ３）による各障害物情報毎および各自由度毎の評価結果を総合的に評価すると共にその結果に基づいて前記移動の自由度毎の移動量を決定して地図上の自己位置を決定する。つまり、最終的に、Ｘ方向、Ｙ方向、およびＸ軸からの回転の３つの自由度に対する、それぞれ最良とされる移動量を１つずつ決定して、これらの移動量により、グローバル座標系Ｘ−Ｙにおける、自律移動体１の位置と姿勢が決定され、自己位置認識が終了する。

上述のように、本発明の自己位置認識方法によれば、各障害物情報毎および各自由度毎の評価結果を総合的に評価すると共にその結果に基づいて移動の自由度毎の移動量を決定して自己位置を決定するので、各自由度毎に最良の移動量を決定して、全体として、信頼性と位置精度とが確保された状況のもとで、自己位置を安定に認識できる。つまり、複数の障害物情報を相互に補完させると共に、複数の障害物情報によるマッチング結果の無秩序な適用を回避して自己位置を認識できるので、自律移動体が移動する種々の環境のもとで、最良の自己位置認識が可能となる。

（マップマッチング）
次に、図３乃至図６を参照して、上述のマッチング演算工程（Ｓ２）で行われるマップマッチングの例を説明する。ここで説明する方法は、距離データの測定点列が測定空間において直線状となる平面壁などの表面位置を、ヒストグラムとヒストグラム間の相互相関関数とを用いて地図データ上において検出する方法である。ここでは簡単のために、水平面内におけるマップマッチングを考える。この場合、測定データを地図にマッチングさせることにより、平面内での回転の自由度と、平面内での２方向の並進自由度とに対する、３つの移動量、すなわち、１つの回転角度（補正角度Δθとする）および２つの並進移動距離（補正距離Δｘ，Δｙとする）が求められる。

図３、図４（ａ）（ｂ）は上述の補正角度Δθを求める方法を示し、図５は上述の補正距離Δｘ，Δｙを求める方法を示す。ここに示すマッチングの方法は、直線状の測定点列をある方向から見ると集中して見えることをヒストグラムに反映させて利用する。このようなヒストグラムは、予め推定されている地図上の位置における壁の位置データからも生成され、これらは互いに比較される。なお、障害物センサとして水平スキャンするレーザレーダ２を用いる。

このようなヒストグラムは、各移動の自由度毎に定義され、各自由度毎に、測定データのヒストグラムと地図データのヒストグラムとが得られる。そこで、各自由度毎に、測定データのヒストグラムと地図データのヒストグラムとの相互相関関数ｋを計算することにより、マッチングさせるために必要な移動量を相互相関関数ｋのピーク位置から求め、マッチング度合いをそのピークの高さから求める。以下、具体的に説明する。

図３には、レーザレーダ２が実際に測定した、２つの壁Ｗ１，Ｗ２を有する障害物上の測定点が、データ点ｐ１〜ｐ１１（実測データＰ）として示され、レーザレーダ２を用いてセンシングされるはずの、いわば仮想の測定点が、データ点ａ１〜ａ１２（地図データＡ）として示されている。地図データＡは、地図情報として記憶された障害物上の点である。ｘ−ｙ座標系は、レーザレーダ２と共に移動するローカル座標系である。ｘ−ｙ座標系において、実測された壁Ｗ１は角度φ１の方向、壁Ｗ２は角度φ２の方向にあり、地図上の壁ｗ１は角度α１の方向（矢印ｓの方向）にあり、地図上の壁ｗ２は角度α２の方向（矢印ｔの方向）にある。

上述の状況において、現在認識している自己位置に基づいた地図データＡと実測データＰの位置が異なっているので、地図データＡを回転し、さらに並進移動して実測データＰに重ねる操作（すなわちマップマッチング）により、自己位置認識が行われる。

また、図３において、レーザレーダ２ａの配置は、実測データＰに対するレーザレーダ２の位置に対応させて、地図データＡに対するレーザレーダの位置に示したものである。従って、レーザレーダ２，２ａの向きと位置の違い（Δθ，Δｘ，Δｙ）が、自己位置認識のずれであり補正量である。

次に、角度ヒストグラムの形成と補正角度の算出について説明する。図３において、壁Ｗ１，Ｗ２上の実測データ点ｐ１〜ｐ１１は、壁の平面性と距離測定の誤差の支配のもとで略直線状に並んでいる。地図情報に基づくデータ点ａ１〜ａ１２は、障害物のモデル化の程度に従って、前記同様に略直線状に並んでいる。そこで、隣接するデータ点間の線分の傾き角度を、例えば、データ点ａ１，ａ２間の線分は、傾き角度α１という具合に求めて、傾き角度の出現頻度Ｆ１、すなわち度数を求める。すると、地図データＡ、実測データＰについて、それぞれ図４（ａ）、図４（ｂ）に示す角度ヒストグラムＦ１，Ｆ２が得られる。

角度ヒストグラムＦ１において、壁ｗ１，ｗ２の方向に対応して、角度α１，α２に度数のピークが現れる。角度ヒストグラムＦ２において、壁Ｗ１，Ｗ２の方向に対応して、角度φ１，φ２に度数のピークが現れる。ところで、地図データＡ、実測データＰにおいて、データ点間の相互配置は全体の回転や並進移動に対する不変量（保存量）である。また、地図データＡ、実測データＰがそれぞれ、モデルデータ、実測データという違いはあるが、同じ障害物に対する距離データである。そこで、両ヒストグラムを角度方向にずらして（距離データの回転に対応）互いに重ね合わすことができる。ヒストグラムを角度方向にずらすことにより、２つの距離データに含まれる直線状のデータ点列は互いに平行になる（図５参照）。

上述のことから、ヒストグラムを重ね合わせて最適重ね合わせ状態とする角度の導出は、相互相関関数ｋを用いて数値的に行われる。角度を表す変数をｉとし、各ヒストグラムＦ１，Ｆ２をｈ１（ｉ），ｈ２（ｉ）とすると、相互相関関数ｋ（ｊ）は、
ｋ（ｊ）＝Σｈ１（ｉ）・ｈ２（ｉ＋ｊ）、
と表される。ここで、Σは、角度変数ｉについて和を求める記号である。この相互相関関数ｋ（ｊ）のピーク値（通常最大値、以下同様）を与える角度変数ｊが、補正角度Δθに対応する。

図５は、地図データＡを、レーザレーダ２の位置を回転中心として上述の相互相関関数ｋを算出して求めた補正角度Δθだけ回転し、これにより、回転補正して得られた新たな地図データＢを示す。この図において、壁ｗ１に対応するデータ点ｂ１〜ｂ４と、壁Ｗ１に対応するデータ点ｐ１〜ｐ７とは、互いに距離Δｕを隔てて平行に並んでおり、壁ｗ２に対応するデータ点ｂ５〜ｂ１２と、壁Ｗ２に対応するデータ点ｐ８〜ｐ１１とは、互いに距離Δｖを隔てて平行に並んでいる。

また、補正角度Δθの回転によってレーザレーダ２ａがレーザレーダ２ｂの位置に移動する。このレーザレーダ２とレーザレーダ２ｂのｘｙ方向の位置ずれが、補正距離δｘ，δｙであり、図５の状態における自己位置認識のずれである。この補正距離δｘ，δｙを求めるために、上述の補正距離Δｕ，Δｖを求める。

補正距離Δｕ，Δｖを求めるため、まず、図４（ａ）に示すヒストグラムＦ１から、角度α１，α２を求める。角度α１，α２は、地図データＡを補正角度Δθだけ回転して得られた新たな地図データＢにおいては、それぞれ、β１＝α１＋Δθ、β２＝α２＋Δθとなる。これらの角度β１，β２は、上述の距離Δｕ，Δｖを求めるために用いられる。すなわち、この２角度によって、角度β１の方向（矢印ｓの方向）に直交する方向の軸（並進軸ｕ）と角度β２の方向（矢印ｔの方向）に直交する方向の軸（並進軸ｖ）が決定される。地図データＢは、これらの並進軸ｕ，ｖに沿って、実測データＰに重なるように平行移動（並進移動）され、その移動距離により、現在認識している各並進軸方向の位置を補正する補正距離Δｕ，Δｖが求められる。

上述の補正距離Δｕ，Δｖは、補正角度Δθを求めた処理と同様に、ヒストグラムと相互相関関数を用いて求められる。地図データＢおよび実測データＰのそれぞれについて、並進軸ｕにデータ点を投影して並進軸ｕに対する、データ点ｂ１〜ｂ１２の出現頻度Ｇ１から成る位置ヒストグラム、およびデータ点ｐ１〜ｐ１１の出現頻度Ｇ２から成る位置ヒストグラムを形成する。

同様に、並進軸ｖにデータ点を投影して並進軸ｖに対するデータ点ｂ１〜ｂ１２の出現頻度Ｈ１から成る位置ヒストグラム、およびデータ点ｐ１〜ｐ１１の出現頻度Ｈ２から成る位置ヒストグラムを形成する。

並進軸ｕに対する各位置ヒストグラムＧ１，Ｇ２の相互相関関数ｋを算出し、相互相関関数ｋのピーク値を与える位置変数から、現在認識している並進軸ｕ方向の位置を補正するための補正距離Δｕを求める。また、並進軸ｖに対する各位置ヒストグラムＨ１，Ｈ２の相互相関関数ｋを算出し、相互相関関数ｋのピーク値を与える位置変数から、現在認識している並進軸ｖ方向の位置を補正するための補正距離Δｖを求める。

上記により、ヒストグラムＦ１，Ｆ２，Ｇ１，Ｇ２，Ｈ１，Ｈ２を用いるマップマッチングによって補正角度Δθと補正距離Δｕ，Δｖが得られる。また、これらのΔθ，Δｕ，Δｖを用いて、δｘ，δｙ、従って、Δｘ，Δｙが求められる。そこで、上述のレーザレーダ２を、図２における、例えば障害物センサ２１に対応させると、これらの量（Δθ，Δｘ，Δｙ）に基づいて、図２におけるグローバル座標系Ｘ−Ｙに対する自律移動体１の自己位置認識における移動量、すなわち補正量（Δθ，ΔＸ，ΔＹ）を、周知の座標変換などの式を用いて、求めることができる。

次に、マッチング評価工程（Ｓ３）における、各移動量、つまり補正量のマッチング度合いの評価について説明する。図６（ａ）（ｂ）（ｃ）は、相互相関関数ｋの計算例を示す。それぞれピークｃ０，ｃ１，ｃ２の位置における角度差、および距離差の値が、補正角度Δθ、および補正距離Δｘ，Δｙに対応する。相互相関関数ｋを規格化しておくことによって、各ピークｃ０，ｃ１，ｃ２の高さを、各補正量のマッチング度合いの評価に用いることができる。例えば、すなわちピークの高さが高いほどマッチング度合いが良い、との評価をすることができる。

上述のマップマッチングの方法は、障害物センサ２１と同様に、他の障害物センサ２２，２３，２４に対しても適用できる。従って、各障害物情報毎および各自由度毎の各補正量と各評価結果を求めることができる。そこで、総合評価工程（Ｓ４）において、総合的に各評価結果を評価すると共にその結果に基づいて前記移動の自由度毎の移動量（補正量）を決定して地図上の自己位置を決定できる。

（第２の実施形態）
次に第２の実施形態を説明する。この実施形態の自己位置認識方法は、上述の第１の実施形態におけるマッチング評価工程（Ｓ３）において、マッチング度合いの信頼性の有無を評価結果とするものである。これは、例えば、前出の図６（ａ）（ｂ）（ｃ）に示した相互相関関数ｋにおける各ピークｃ０，ｃ１，ｃ２に対して、それぞれ所定の閾値ＴＨを定めておき、この閾値ＴＨを超えるピークについては信頼性ありとし、超えないピークについては信頼性なしとする方法である。このような方法によれば、信頼性の有りなしによって、マッチング度合いを２値化するので、評価が単純明快であり、その後の総合評価が容易となる。

（第３の実施形態）
次に第３の実施形態を説明する。この実施形態の自己位置認識方法は、上述の第２の実施形態において、マッチング評価工程（Ｓ３）によりマッチング度合いを信頼性の有無により評価すると共に、総合評価工程（Ｓ４）において、前記マッチング評価工程（Ｓ３）によりマッチングが信頼性有りとされた移動量の各自由度毎の平均値を各自由度毎の移動量とするものである。これを、以下の表１に示すように、２つの障害物センサを用いた場合について説明する。

表１は、センサ１，２を用いて、３つの移動の自由度ｕ，ｖ，θについて得られた、相互相関関数ｋの最大相関値（ピーク値）つまりマッチング度合いと、各補正量Δｕ，Δｖ，Δθとを示している。また、各センサ毎、7各自由度毎の判定は、最大相関値が閾値を超えた場合は信頼性ありとして○で示し、閾値を超えない場合は信頼性なしとして×で示している。

上述の表１に示した評価結果の場合、本実施形態の自己位置認識方法では、Δｕについてはセンサ１の結果ｕ１を採用し、Δｖについてはセンサ２の結果ｖ２を採用し、Δθについてはセンサ１の結果θ１とセンサ２の結果θ２の平均値（θ１＋θ２）／２、を採用する。このような自己位置認識方法によれば、総合評価の計算が比較的簡単となる。

（第４の実施形態）
次に第４の実施形態を説明する。この実施形態の自己位置認識方法は、上述の第２の実施形態において、マッチング評価工程（Ｓ３）によりマッチング度合いを信頼性の有無により評価すると共に、総合評価工程（Ｓ４）において、各自由度毎に所定の順番で前記マッチング評価工程（Ｓ３）の複数の評価結果を評価し、前記順番に行う評価の際に最初に信頼性ありとされた移動量をその自由度に対する移動量とするものである。これを、以下の表２に示すように、２つの障害物センサを用いた場合について説明する。

表２は、表１と同様にセンサ１，２による結果を示す。本実施形態では、マッチング評価工程（Ｓ３）と総合評価工程（Ｓ４）とで行う評価は、各自由度毎に、まずセンサ１の結果を評価し、その後、センサ２の結果を評価するように評価の順番を定めている。

上述の表２に示した結果は、Δｕについてはセンサ１の結果ｕ１を採用し、センサ２については評価を行わず、Δｖについてはセンサ１の結果が採用できなかったので、採用可能なセンサ２の結果ｖ２を採用し、Δθについてはセンサ１の結果θ１を採用してセンサ２については評価を行わないことを示している。このような自己位置認識方法によれば、全ての障害物センサによるマッチング演算とその後の評価結果を待つことなく、信頼性有りの結果が出た時点で最終の移動量を決定でき、自己位置認識処理が速くなる。

（第５の実施形態）
次に第５の実施形態を説明する。この実施形態の自己位置認識方法は、上述の第２の実施形態において、マッチング評価工程（Ｓ３）によりマッチング度合いを信頼性の有無により評価すると共に、総合評価工程（Ｓ４）において、１つの障害物センサの障害物情報に基づく全自由度についての移動量が全て信頼性有りとされている場合、当該各移動量を全自由度に対する各移動量とするものである。これを、以下の表３に示すように、２つの障害物センサを用いた場合について説明する。

表３は、表１同様にセンサ１，２を用いた結果を示す。センサ１のΔｖに信頼性なしとされており、センサ２については３自由度について信頼性ありとされている。そこで、本実施形態では、センサ２による結果ｕ２，ｖ２，θ２を全て採用する。このような自己位置認識方法によれば、全自由度について、１つの障害物センサに基づく各移動量を用いるので、統一性の取れた自己位置認識ができる。

（第６の実施形態）
次に第６の実施形態を説明する。この実施形態の自己位置認識方法は、上述の第１の実施形態におけるマッチング評価工程（Ｓ３）において、マッチング度合いを数値化して評価結果とするものである。これは、例えば、前出の図６（ａ）（ｂ）（ｃ）に示した相互相関関数ｋにおける各ピークｃ０，ｃ１，ｃ２の値から、各自由度毎に定めた所定の閾値ＴＨを差し引いた量を評価値とするものである。これを、以下の表４に示すように、２つの障害物センサを用いた場合について説明する。

表４において、自由度ｕに対する閾値をｋｕ０、自由度ｖに対する閾値をｋｖ０、自由度θに対する閾値をｋθ０としている。また、評価値は、例えばセンサ１の自由度ｕの場合、Ｃｕ１＝ｋ（ｕ１）−ｋｕ０、としており、他も同様である。このように、評価値を数値化して表すことにより、数値によって信頼性の比較ができるので、信頼性有無を２値化した評価結果を用いる場合よりもきめ細かい自己位置認識を実現できる。

（第７の実施形態）
次に第７の実施形態を説明する。この実施形態の自己位置認識方法は、上述の第６の実施形態において、マッチング評価工程（Ｓ３）によりマッチング度合いを数値化して評価すると共に、総合評価工程（Ｓ４）において、マッチング評価工程（Ｓ３）により各自由度毎に最もマッチング度合いが高いとされた移動量を各自由度に対する移動量とするものである。これを、上述の表４を参照して説明する。表４の評価値に対して、次式（１）が成り立つものとする。
Ｃｕ１＞Ｃｕ２，Ｃｖ１＜Ｃｖ２，Ｃθ１＞Ｃθ２ ・・・（１）。

上式（１）が成り立つ場合、自由度ｕについては、Δｕとしてセンサ１の結果ｕ１を採用し、自由度ｖについては、Δｖとしてセンサ２の結果ｖ２を採用し、自由度θについては、Δθとしてセンサ１の結果θ１を採用する。このような自己位置認識方法によれば、各自由度毎に信頼度のもっとも高いマップマッチングによって自己位置認識ができる。

（第８の実施形態）
次に第８の実施形態を説明する。この実施形態の自己位置認識方法は、上述の第６の実施形態において、マッチング評価工程（Ｓ３）によりマッチング度合いを数値化して評価すると共に、総合評価工程（Ｓ４）において、数値化されたマッチング度合いの全自由度に対する総和が最も高い障害物情報に基づいて求められた各移動量を各自由度に対する移動量とするものである。これを、上述の表４を参照して説明する。表４の評価値に対して、次式（２）が成り立つものとする。
Ｃｕ１＋Ｃｖ１＋Ｃθ１＞Ｃｕ２＋Ｃｖ２＋Ｃθ２ ・・・（２）。

上式（２）が成り立つ場合、全ての自由度ｕ，ｖ，θに対してセンサ１の結果ｕ１，ｖ１，θ１を採用し、センサ２の結果ｕ２，ｖ２，θ２は採用しない。このような自己位置認識方法によれば、総合的に信頼性の高い１つの障害物センサによるマップマッチングの結果に基づいて、信頼性の高い自己位置認識ができる。

なお、表４の結果に対して前式（１）が成り立つ場合、Ｃｖ１＜Ｃｖ２なので、Ｃｖ１＜０の可能性がある。このとき、Ｃｖ２＞０であれば、第７の実施形態の方法と第８の実施形態の方法とを併用して、自由度ｖについてセンサ２の結果ｖ２を採用するようにしてもよい。

（第９の実施形態）
次に第９の実施形態を、図７を参照して、説明する。この自己位置認識方法においては、障害物情報取得工程（Ｓ１）では、図７に示すように、較正用の障害物３と各障害物センサ２１，２２とを用いて較正用の障害物情報を取得し、総合評価工程（Ｓ４）では、較正用の障害物情報から求められた移動量を比較して各障害物センサ２１，２２間の補正値を求めるものである。

ここでは、自律移動体１が、オドメトリ機能、すなわち、移動用車輪の回転数の累積により走行距離や旋回角度を求めて自己位置推定を行う機能を備えているものとする。また、障害物センサ２１，２２は自律移動体１の前方左右に対称に備えられ、較正用の障害物３も、図７の配置で、自律移動体１に対して左右対称構造を有するものとする。すなわち、ｃ１＝ｃ２，ｄ１＝ｄ２，ｅ１＝ｅ２，Ｌ１＝Ｌ２とする。また、距離Ｌ０は、自律移動体１の前面とこれに対向する障害物３の対向面との距離であるが、左右対称であれば、Ｌ０が一定である必要はない。なお、各センサの視野角ξ１，ξ２は、特に等しい必要はない。ここで、ｃ１とｃ２，ｄ１とｄ２，ｅ１とｅ２のいずれかが、互いに等しくない場合、障害物センサ２１，２２に取付誤差があることになる。本実施形態では、その誤差を修正するための補正値が得られる。

自律移動体１は、オドメトリ調整開始スイッチを有し、ユーザが、自律移動体１を障害物３の内部に左右対称に配置してこのスイッチを押すと、自律移動体１がオドメトリ調整モードとなる。オドメトリ調整モードにおいて、自律移動体１は障害物３に対してマップマッチングを行って、各障害物センサ２１，２２間の補正値を求める。なお、ユーザが前記開始スイッチを押すことなく、所定設定条件のもとで、自律移動体１が自律的に障害物３の位置に移動して、上述の補正値を求めるマップマッチングを行うようにしてもよい。

上述の補正値の求め方を説明する。一方の障害物センサ、例えば、障害物センサ２１によって測定した障害物情報を自律移動体１の左右対称軸に関して左右反転して、前述した第１の実施形態のマップマッチングの方法における地図データＡとし、障害物センサ２２によって測定した障害物情報を実測データＰとする。自律移動体１は、これらの地図データＡと実測データＰとによって、移動量（補正量）Δθ，Δｘ，Δｙ等を求めて、これらの数値、またはこれらに基づいて求めた数値を各障害物センサ２１，２２間の補正値とする。自律移動体１は、これらの補正値を記憶しておき、各障害物センサ２１，２２による測定結果にに基づく移動量（補正量）Δθ，Δｘ，Δｙ等を補正することができる。

本実施形態によれば、上述のように、各障害物センサ間の補正値によって、各移動量を補正することができるので、各障害物センサによる自己位置認識結果に整合性を持たせることができ、異なる障害物センサによる自己位置認識結果の不安定性を解消できる。つまり、各障害物センサの特性の違いによる測定誤差や、障害物センサの取付位置の誤差などを、補正値によりソフトウエア的に容易に自動修正でき、信頼度の高いマップマッチングによる自己位置認識が可能となる。

（第１０の実施形態）
次に第１０の実施形態を説明する。この実施形態の自己位置認識方法は、上述の第９の実施形態において、総合評価工程（Ｓ４）では、前記補正値が所定の値を超える障害物センサをセンサ異常とするものである。このような自己位置認識方法によれば、障害物センサの故障を検知して、その後の自己位置認識の信頼性や安定性を確保できる。

なお、本発明は、上記構成に限られることなく種々の変形が可能である。例えば、上記では、直線状配列の測定点に基づくヒストグラムと相互相関関数とを用いるマップマッチングを用いる自己位置認識方法について説明したが、各移動の自由度毎に移動量（補正量）を決定できてマッチング度合いを評価できるマップマッチング方法であれば、そのマップマッチング方法を用いて本発明を適用することができる。

本発明の第１の実施形態に係る自己位置認識方法における自己位置認識処理を説明するフローチャート。 同上自己位置認識方法を適用する状況を示す自律移動体の平面図。 同上自己位置認識方法におけるマップマッチングを説明するデータ点配置例を示す平面図。 （ａ）は図３に示した地図データに関する角度ヒストグラムの図、（ｂ）は図３に示した実測データに関する角度ヒストグラムの図。 図３における地図データを補正角度分回転した状態において距離ヒストグラムを形成する様子を示す平面図。 （ａ）は角度ヒストグラムについての相互相関関数の図、（ｂ）（ｃ）は距離ヒストグラムについての相互相関関数の図。 第９の実施形態に係る自己位置認識方法における障害物センサの較正のための処理を説明する平面図。 （ａ）（ｂ）は従来の自己位置認識の問題点を説明するための自己位置認識の様子を示す平面図。 （ａ）（ｂ）は従来の自己位置認識の問題点を説明する自己位置補正の様子を示す平面図。

符号の説明

２，２ａ，２ｂ，２１〜２４ 障害物センサ
３ 障害物（較正用）
Ｗ，Ｗ１，Ｗ２，ｗ１，ｗ２ 壁（障害物）

Claims (10)

1. 障害物情報と地図情報とのマップマッチングに基づいて地図上の自己位置を認識する自己位置認識方法において、
   複数の障害物センサを用いて自己の周辺に存在する障害物を検知して複数の障害物情報を取得する障害物情報取得工程と、
   地図記憶装置に記憶された地図情報と前記障害物情報取得工程によって取得された複数の障害物情報の各々とを自己の移動の自由度毎にマップマッチングさせるための移動量を地図情報に基づく位置データのヒストグラムと障害物情報に基づく位置データのヒストグラムとの相互相関関数を計算することにより相互相関関数のピーク位置から求めるマッチング演算工程と、
   前記マッチング演算工程によって求めた各移動量をマッチング度合いについて前記ピークの高さによって評価するマッチング評価工程と、
   前記マッチング評価工程による各障害物情報毎および各自由度毎の評価結果を総合的に評価すると共にその結果に基づいて前記移動の自由度毎の移動量を決定して地図上の自己位置を決定する総合評価工程と、を備えたことを特徴とする自己位置認識方法。
2. 前記マッチング評価工程では、前記マッチング度合いの信頼性の有無を評価結果とすることを特徴とする請求項１に記載の自己位置認識方法。
3. 前記総合評価工程では、前記マッチング評価工程によりマッチングが信頼性有りとされた移動量の各自由度毎の平均値を各自由度毎の移動量とすることを特徴とする請求項２に記載の自己位置認識方法。
4. 前記総合評価工程では、各自由度毎に所定の順番で前記マッチング評価工程の複数の評価結果を評価し、前記順番に行う評価の際に最初に信頼性ありとされた移動量をその自由度に対する移動量とすることを特徴とする請求項２に記載の自己位置認識方法。
5. 前記総合評価工程では、１つの障害物センサの障害物情報に基づく全自由度についての移動量が全て信頼性有りとされている場合、当該各移動量を全自由度に対する各移動量とすることを特徴とする請求項２に記載の自己位置認識方法。
6. 前記マッチング評価工程では、前記マッチング度合いを数値化して評価結果とすることを特徴とする請求項１に記載の自己位置認識方法。
7. 前記総合評価工程では、前記マッチング評価工程により各自由度毎に最もマッチング度合いが高いとされた移動量を各自由度に対する移動量とすることを特徴とする請求項６に記載の自己位置認識方法。
8. 前記総合評価工程では、前記数値化されたマッチング度合いの全自由度に対する総和が最も高い障害物情報に基づいて求められた各移動量を各自由度に対する移動量とすることを特徴とする請求項６に記載の自己位置認識方法。
9. 前記障害物情報取得工程では、較正用の障害物と前記各障害物センサとを用いて較正用の障害物情報を取得し、
   前記総合評価工程では、前記較正用の障害物情報から求められた移動量を比較して各障害物センサ間の補正値を求めることを特徴とする請求項１に記載の自己位置認識方法。
10. 前記総合評価工程では、前記補正値が所定の値を超える障害物センサをセンサ異常とすることを特徴とする請求項９に記載の自己位置認識方法。

Images