JP5032953B2 - 自己位置認識システム - Google Patents

Description

本発明は、自律移動車両等で用いられる自己位置認識システムに関する。

従来から、車両やロボットなどの自律移動体の開発と実用化が行われている。自律移動体が自律的に移動するには、障害物の検出とその回避、および自己位置の認識が必要である。自己位置の認識によって、事前に地図情報を参照できるので、壁などの環境固定障害物の検出も容易となる。自己位置の認識には、自己の姿勢（向き）の認識と現在地の位置座標の認識、すなわち回転方向と並進方向の両方の認識が必要である。このような自己位置の認識には、例えば車両において、駆動輪のモータに取り付けられたエンコーダから求められる車輪の回転数や舵角などの内部データを用いて移動距離や移動方向を決定するいわゆるデッドレコニングの方法が多く用いられる。

上述のエンコーダ情報を用いる場合、車輪のスリップ発生等により誤差が累積して大きくなるので、通常、デッドレコニングにマップマッチングによる位置補正が組み合わされる。マップマッチングでは、例えば、入力済の地図情報を記憶した記憶媒体と水平レーザレーダとを備え、地図情報とレーザレーダで取得した障害物位置情報との差分に基づいて、デッドレコニングによって認識した位置を補正する。

レーザレーダは、反射光によって、物体表面までの距離データを取得する。レーザレーダによって取得される障害物位置情報は、測定に係る物体表面の位置情報を含んでおり、移動空間における自己以外の移動する障害物のほか、壁や柱などの環境を構成する物体の位置情報を含む。そこで、地図情報には、これらの位置情報と比較可能な情報を含めておく。人が介在するような環境では、予期せぬ障害物が置かれることが多く、入力済の地図情報に載っていない障害物が存在することも多い。

上述のマップマッチングを効率的かつ精度良く行うために、距離データの測定点列が直線となる平面壁などの表面位置を用いることが考えられる。自律移動体が屋内で廊下を走行中に廊下の曲がり角などを通過する場合、互いに直交する壁などが環境情報として存在する。レーザレーダによって広い水平視野範囲をスキャン方向の角度を変化させることにより水平にスキャンすると、スキャン平面内に直線状に配列された、直交する測定点列が得られる。このような状況のもとで、ヒストグラムを用いてデータ点列の特徴を効率的に抽出して利用することにより、直線状に配列した点列の方向を検出すると共に、その点列の配置を地図情報における壁の配置とマッチングさせる方法が知られている（例えば、非特許文献１参照）。

また、上述のヒストグラムを用いたマップマッチングの方法を電動車椅子の操縦補助システムに応用した例が知られている（例えば、非特許文献２参照）。
ワイス（Ｗｅｉｓｓ）、プットカマ（Ｐｕｔｔｋａｍｅｒ）著 「ア マップ ベイスト オン レーザスキャンズ ウィズアウト ジオメトリック インタプリテーション（Ａ ｍａｐ Ｂａｓｅｄ ｏｎ ｌａｓｅｒｓｃａｎｓ ｗｉｔｈｏｕｔ ｇｅｏｍｅｔｒｉｃ ｉｎｔｅｒｐｒｅｔａｔｉｏｎ）」、インテリジェント オートノマス システムズ（Ｉｎｔｅｌｌｉｇｅｎｔ Ａｕｔｏｎｏｍｏｕｓ Ｓｙｓｔｅｍｓ）４，４０３−４０７頁、１９９５年 後藤健志著 「レーザレンジファインダを用いた電動車椅子の操縦補助システム」、奈良先端科学技術大学院大学情報科学研究科情報システム学専攻修士論文、ＮＡＩＳＴ−ＩＳ−ＭＴ００５１０３８，２００２年

しかしながら、上述した非特許文献１，２に示されるようなマップマッチングにおいては、平行な壁が、複数、近距離に存在する場合に、間違った壁にマッチングするおそれがあり、間違いのより少ないマッチング方法が安全な自律移動のために求められている。この間違いが発生するのは、並進方向の補正距離を求めるために形成するヒストグラムが、地図情報に基づくデータと実測したデータのそれぞれについて互いに独立に形成されることに一因がある。

本発明は、上記課題を解消するものであって、平行な壁が、複数、近距離に存在する場合に間違った壁にマッチングするおそれを低減すると共に複雑な移動環境においてより低い演算負荷のもとで精度良い自己位置認識を実現できる自己位置認識システムを提供することを目的とする。

上記課題を達成するために、請求項１の発明は、予め入力された地図情報を記憶する地図情報記憶手段と、前記地図情報上で認識した自律移動体の自己の位置を記憶する自己位置記憶手段と、自律移動体の自己の周辺に存在する障害物をセンシングするセンサと、前記センサによって得られた障害物情報と前記自己位置記憶手段が現在記憶している自己位置における周辺の前記地図情報記憶手段に記憶された地図情報とを演算処理して自己位置を補正する演算手段と、を備えた自己位置認識システムにおいて、演算手段は、現在位置として認識している位置で前記センサを用いてセンシングされるはずの前記地図情報に基づく障害物情報を第１の距離データとし、前記センサによってセンシングされた実際の障害物情報を第２の距離データとしてこれらのデータを取得し、前記第１および第２の距離データのそれぞれについて、データ点を結ぶ線分の角度を求めると共に、得られた角度の出現頻度を求めて第１および第２の角度ヒストグラムを形成し、前記第１および第２の角度ヒストグラムを比較すると共にその比較結果に基づいて現在認識している自律移動体の回転方向を補正する補正角度を求め、その補正角度に基づいて前記第１または第２の距離データを相互に回転して回転方向を回転補正した新たな第１の距離データおよび新たな第２の距離データを取得し、前記新たな第１の距離データにおけるデータ点と前記新たな第２の距離データにおけるデータ点とについて前記センサによる検知方向が最も近いもの同士によりデータ点対を形成し、前記第１または第２の角度ヒストグラムのいずれかにおいて出現頻度の高い角度を抽出すると共にその角度に前記回転補正を施して第１の角度とし、前記第１の角度の方向に直交する軸である第１の並進軸および前記軸に直交する軸である第２の並進軸を設定し、前記第１の並進軸に関する前記各データ点対の座標値の差分の出現頻度を表す第１の差分ヒストグラムおよび前記第２の並進軸に関する前記各データ点対の座標値の差分の出現頻度を表す第２の差分ヒストグラムを形成し、前記第１の差分ヒストグラムにおけるピーク値を与える距離差分値と、前記第２の差分ヒストグラムにおけるピーク値を与える距離差分値と、前記補正角度とに基づいて、現在認識している自律移動体の並進方向および回転方向に関する補正を行って自己位置を補正するものである。

請求項２の発明は、請求項１記載の自己位置認識システムにおいて、前記第１の角度を設定するために用いた角度ヒストグラムにおいて出現頻度が高い角度であって前記抽出した角度とは異なる角度を抽出し、その角度に前記回転補正を施し、その回転補正された角度の方向に直交する軸として前記第２の並進軸を設定するものである。

請求項３の発明は、請求項１または請求項２記載の自己位置認識システムにおいて、前記差分ヒストグラムを作成する際に、前記新たな第１または第２の距離データのいずれかにおいて前記センサによる検知方向が互いに隣り合うデータ点における注目している並進軸に関する座標値が互いに所定値以上離れている場合には、そのデータ点によって構成されるデータ点対についての差分値を前記差分ヒストグラム作成に採用しないものである。

請求項４の発明は、請求項１乃至請求項３のいずれか一項に記載の自己位置認識システムにおいて、前記いずれかの差分ヒストグラムに複数のピークが存在する場合にはその差分ヒストグラムに対応する並進方向の補正を行わないものである。

請求項１の発明によれば、第１の距離データと第２の距離データとの間で互いに関連づけられたデータ点の組であるデータ点対について差分ヒストグラムを形成して並進方向の補正距離を求めるので、平行な壁が、複数、近距離に存在する場合であっても間違った壁にマッチングするおそれを低減できる。すなわち、従来の方法では、並進方向の補正距離を求めるためのヒストグラムを、第１の距離データに関するヒストグラムと第２の距離データに関するヒストグラムのように、それぞれ独立個別に求められていたので、距離データの局所的構造を考慮できなかったが、本発明の方法によればデータ点対を用いることにより、この点が改善されている。また、差分ヒストグラムを用いることにより、従来のヒストグラムを用いる場合と同様に、精度良く並進方向の補正距離を求めることができ、精度良い自己位置認識を実現できる。また、本発明によると、差分ヒストグラムが形成された時点で、そのピーク値により並進方向の補正距離の情報が得られるので、従来行っていたヒストグラム間の相互相関を計算する必要がなく、計算のための負担が低減される。

請求項２の発明によれば、Ｙ字交差点などの周辺における直交しない壁の情報を含む第１および第２の距離データであっても、これらのデータを有効活用して並進方向および回転方向に関する補正を行うことができ、周囲環境への依存性をより少なくして精度良い自己位置認識を実現できる。

請求項３の発明によれば、注目している並進軸方向の補正距離の取得に有効なデータ点対のみを用いて、その情報を反映させた差分ヒストグラムを形成することになるので、ノイズの少ない差分ヒストグラムとすることができ、精度と信頼性の高いマッチング、従って精度と信頼性の高い自己位置認識を実現できる。

請求項４の発明によれば、並進方向のマッチングにおける間違った位置補正を回避でき、間違った自己位置認識を回避できる。これにより、自律移動体の安全な移動を実現できる。

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

（第１の実施形態）
図１は自己位置認識システムのブロック構成を示し、図２は同システムにおける自己位置認識処理のフローチャートを示し、図３乃至図８（ａ）（ｂ）は各処理の内容を示す。まず、図１乃至図５によって自己位置認識システム１の概要を説明し、その後詳細を説明する。

自己位置認識システム１は、図１に示すように、予め入力された地図情報を記憶する地図情報記憶手段である地図情報記憶装置２と、地図情報上で認識した自律移動体の自己の位置を記憶する自己位置記憶手段である自己位置記憶装置３と、自律移動体の自己の周辺に存在する障害物をセンシングするセンサ４と、センサ４によって得られた障害物情報と自己位置記憶装置３が現在記憶している自己位置における周辺の地図情報記憶装置２に記憶された地図情報とを演算処理して自己位置を認識する演算処理装置５とを備えている。

センサ４は、自律移動体の略前方空間における所定の水平面において、所定角度置きに環境情報、広い意味で障害物情報を取得する。センサ４は、地図情報と比較できる環境情報を取得できるセンサであればよく、レーザレーダ、光センサ、超音波センサ、特に超音波アレイセンサ、レーダなどを用いることができる。また、超音波アレイセンサやＣＣＤ画像装置などを用いて距離画像を生成し、その距離画像から第２の距離データを取得するようにしてもよい。

上述のセンサの中でも、レーザレーダは空間分解能が高いのでセンサ４として好適である。例えば、一次元スキャン型のレーザレーダは、レーザビームが走行する面内に２次元分布するデータ点を、共に高い角度精度と位置精度のもとで測定することができる。以下では、センサ４としてこのようなレーザレーダを想定しているが、センサ４はこれに限るものではない。

演算処理装置５は、図２に示すように、ステップＳ１において、現在位置として認識して自己位置記憶装置３に記憶している位置で、センサ４を用いてセンシングされるはずの障害物情報（第１の距離データＡという）を地図情報記憶装置２に記憶した地図情報に基づいて取得する（図３参照）。さらに、演算処理装置５は、センサ４によってセンシングした結果に基づいて実際の障害物情報（第２の距離データＰという）を取得する（図４参照）。

上記に続いて演算処理装置５は、第１の距離データＡおよび第２の距離データＰのそれぞれについて、データ点を結ぶ線分の角度を求めると共に、得られた角度の出現頻度を求めて第１および第２の角度ヒストグラムＦ１，Ｆ２を形成する（Ｓ２、図８（ａ）（ｂ）参照）。

上記に続いて演算処理装置５は、第１および第２の角度ヒストグラムＦ１，Ｆ２を比較し、その比較結果に基づいて現在認識している自律移動体の回転方向を補正する補正角度θを求める（Ｓ３）。

さらに、演算処理装置５は、その補正角度θに基づいて第１の距離データＡ、第２の距離データＰを相互に回転して回転方向を回転補正した新たな第１の距離データおよび新たな第２の距離データを取得する（Ｓ４）。この回転は、相対的なものであり、いずれのデータを基準にするかについて自由度があり、一方だけ回転したり、両方回転したりすることができる。そこで、本実施形態では、例えば、第２の距離データＰを補正角度θ回転して、新たな第２の距離データＢとする（図７参照）。この状態で、第１および第２の距離データＡ，Ｂは、互いに回転のない並進移動のみの関係となっている。以下ではこの並進移動の距離が求められる。

上記に続いて演算処理装置５は、第１の距離データＡにおけるデータ点と第２の距離データＢにおけるデータ点とについてセンサ４による検知方向が最も近いもの同士によりデータ点対を形成する（Ｓ５）。例えば、後述の図７におけるデータ点ａ（ｉ），ｂ（ｊ）や、データ点ａ（ｉ＋１），ｂ（ｊ＋１）などがそれぞれデータ点対を形成している。

上記に続いて演算処理装置５は、第１または第２の角度ヒストグラムＦ１，Ｆ２のいずれかにおいて出現頻度の高い角度を抽出すると共にその角度に前記回転補正を施して第１の角度とし、第１の角度の方向に直交する軸である第１の並進軸と、この軸に直交する軸である第２の並進軸とを設定する（Ｓ６）。例えば、後述の図７における、第１の並進軸ｕおよび第２の並進軸ｖが設定される。

上記の場合、通常は、前記第１の角度ヒストグラムから最も出現頻度の高い角度を抽出する。これは、第１の距離データＡ、従って第１の角度ヒストグラムＦ１が、マップマッチングにとってノイズとなる移動する障害物などを含まない地図情報に基づくことによる。これにより、地図情報にない方向でマップマッチングをしてしまうという誤認識の発生を回避でき、従って、複雑な移動環境においても精度良い自己位置認識ができる。

また、上述のステップＳ６は、ステップＳ５の後に行うように説明したが、この２つのステップは、互いに処理順序に関する制約はない。そこで、処理の順番を入れ替えて、ステップＳ６の処理を行った後に、ステップＳ５の処理を行うようにしてもよい。

上記に続いて演算処理装置５は、第１の並進軸ｕに関する各データ点対の座標値の差分の出現頻度を表す第１の差分ヒストグラムおよび第２の並進軸に関する各データ点対の座標値の差分の出現頻度を表す第２の差分ヒストグラムを形成する（Ｓ７）。例えば、後述の図８（ａ）における第１の差分ヒストグラムＨ１、図８（ｂ）における第２の差分ヒストグラムＨ２が形成される。

上記に続いて演算処理装置５は、第１の差分ヒストグラムＨ１におけるピーク値を与える距離差分値、および第２の差分ヒストグラムＨ２におけるピーク値を与える距離差分値を求め、これらの距離差分値と補正角度θとから並進方向の補正距離を求める（Ｓ８）。例えば、後述の図８（ａ）における第１の補正距離Δｕ、図８（ｂ）における第２の補正距離Δｖが求められる。

上記に続いて演算処理装置５は、上述の並進方向の補正距離Δｕ，Δｖと補正角度θとに基づいて、現在認識している自律移動体の並進方向および回転方向に関する補正を行って自律移動体の自己位置を補正する（Ｓ９）。

すなわち、自己位置認識システム１の演算処理装置５は、上記のヒストグラムを用いるマップマッチングによって得られた補正角度θ、第１の補正距離Δｕ、および第２の補正距離Δｖに基づいて、現在認識している自律移動体の回転方向すなわち向き（または姿勢）と現在位置の座標とを補正でき、従って、自己位置認識ができる。向きの補正は補正角度θによって行われ、位置座標の補正は、図３、図５における（Δｘ，Δｙ）を、（θ，Δｕ，Δｖ）から座標変換によって求めることにより行われる。

なお、演算処理装置５は、自己位置認識システム１の稼働中において、上記のステップＳ１〜Ｓ９を所定の制御周期のもとで繰り返す。これにより、自己位置認識システム１を備えた自律移動体は、自己位置を補正しつつ正しく認識して自律的に移動することができる。

次に、自己位置認識システム１における処理について詳細に説明する。図３は自己位置認識システム１で処理される第１の距離データＡを示し、図４は第２の距離データＰを示し、図５は第１の距離データＡにセンサ４の位置を一致させて第２の距離データＰを重ねた状態を示し、図６（ａ）（ｂ）はそれぞれ第１および第２の距離データに関する角度ヒストグラムを示し、図７はセンサ４の位置を一致させて角度補正した第１および第２の距離データを重ねて示し、図８（ａ）（ｂ）はそれぞれ第１および第２の差分ヒストグラムを示す。

（第１および第２の距離データ）
図３は、自己位置認識システム１が保持している地図情報の一部を示し、始点終点によって定義された線分に基づく壁Ｗ０の位置が示されている。センサ４は、自律移動車両などの自律移動体に備えられて移動する。自律移動体が稼働する２次元領域における位置を定義する座標系として、グローバル座標系（不図示）が定義されており、地図情報は、そのグローバル座標系に基づいて記述される。

通常、自律移動体は複数のセンサ４を備えるが、ここでは、センサ４そのものが自律移動体であり、センサ４の自己位置認識が自律移動体の自己位置認識であるとする。センサ４には、その進行方向前方に向かうｙ軸と、右方向に向かうｘ軸とから成り、センサ４と共に移動するｘｙ座標系（ローカル座標系）が定義されている。グローバル座標系に対するｘｙ座標系の向きと位置を確定することが、すなわち自己位置認識である。

センサ４は、前方の水平面内における視野範囲を一定角度、例えば、３度毎に、レーザビームでスキャンして、所定距離範囲内における物体表面位置の距離データを取得する。演算処理装置５が現在位置として認識している位置が、図３に示すセンサ４の位置であるとされ、かつ、この位置において、センサ４を用いてセンシングされるはずの、いわば仮想の測定点が、白丸のデータ点ａ（第１の距離データＡ）であるとされている。これらのデータ点ａは、２つの壁ｗ０を有する地図情報記憶装置２に記憶された障害物上の点である。ｘｙ座標系において、角度をｘ軸の方向からｙ軸方向に測るものとする。

また、図３において、破線で示されたセンサ４０の位置は、地図情報上における実際のセンサ４の位置である。従って、センサ４が、自己位置を正しく認識する処理は、センサ４とセンサ４０の位置および方向のずれを表すΔｘ，Δｙ，θを求めることになる。

図４には、センサ４によって実際に測定された２つの壁Ｗを有する障害物上の測定点が、黒丸のデータ点ｐ（第２の距離データＰ）として示されている。図４におけるセンサ４の位置は図３にセンサ４０の位置として示されており、図３におけるセンサ４の位置は図４にセンサ３９の位置として示されている。

（各データの相互関係）
図５は、図４のデータを図３のデータに重ねたものである。なお、図５は、ｘｙ座標系が見易さのため、図面上に正置されている。図５において、ｘ軸に対する壁Ｗ０，Ｗを構成する線分の角度がα１，α２，α３、およびφ１，φ２，φ３として示されている。

また、第１の距離データＡの測定点ａ０，ａｎなどと、第２の距離データＰの測定点ｐ０，ｐｍなどとは、もし測定点が存在するなら、センサ４から放射状に伸びる線分上に２点ずつ並ぶことになる。

図５に示す状況において、確認前の想定した自己位置に基づいた距離データである第１の距離データＡと実測データである第２の距離データＰのデータ点の位置が異なっているので、現在の自己位置認識が間違っていることになる。そこで、例えば、第２の距離データＰを回転し、さらに並進移動して第１の距離データＡに重ねる操作、すなわちマップマッチングにより、正しい自己位置認識が行われる。

また、図５において、センサ４０の配置は、実測における第２の距離データＰに対するセンサ４の位置関係を、そのまま、第１の距離データＡに対するセンサ４の位置関係に焼き直して示したものとなっている。そして、上述したように、センサ４とセンサ４０の向きと位置の違い（θ，Δｘ，Δｙ）が、自己位置認識のずれを表している。このθ，Δｘ，Δｙなどを求める処理が自己位置認識の処理である。

（角度ヒストグラムの形成）
次に、角度ヒストグラムの形成と補正角度の算出について説明する。図５において、壁Ｗ０上の地図情報に基づくデータ点ａ０，ａｎ等は、障害物のモデル化の程度に従って、前記同様に略直線状に並んでいる。壁Ｗ上の実測によるデータ点ｐ０，ｐｍ等は、壁Ｗの平面性と距離測定の誤差の支配のもとで略直線状に並んでいる。

そこで、隣接するデータ点間を結ぶ線分の傾き角度を、例えば、データ点ａ０とその隣のデータ点間の線分は、傾き角度α１という具合に求めて、傾き角度の出現頻度、すなわち度数を求める。すると、第１の距離データＡについては、図５（ａ）に示す第１の角度ヒストグラムＦ１が得られ、第２の距離データＰについては、図５（ｂ）に示す第２の角度ヒストグラムＦ２が得られる。なお、これらの図において、角度が±πを超える部分については、２π周期の繰り返しにより表示されている。

（補正角度の決定）
上述の角度ヒストグラムＦ１，Ｆ２において、壁Ｗ０の各部に応じて角度α１，α２，α３に度数のピークが現れ、壁Ｗの各部に応じて角度φ１，φ２，φ３に度数のピークが現れる。ところで、各距離データＡ，Ｐにおいて、それぞれのデータを構成するデータ点間の相互配置は全体の回転や並進移動に対する不変量（保存量）である。また、第１の距離データＡがモデルデータであり、第２の距離データＰが実測データであるという違いはあるが、同じ障害物に対するデータである。

これらのことを反映して、各ヒストグラムＦ１，Ｆ２における各ピーク間の配置は、両ヒストグラム間の不変量となっている。すなわち、両ヒストグラムを角度方向にずらして互いに重ね合わすことができる。ヒストグラムを角度方向にずらすことは距離データを回転することに対応し、互いに重ね合わすことは２つの距離データに含まれる直線状のデータ点列を互いに平行な状態にすることに対応する。

上述のことから、ヒストグラムＦ１，Ｆ２を重ね合わせるために必要な移動角度が、自己位置認識のための補正角度そのものであることが分かる。このような補正角度は、両ヒストグラムＦ１，Ｆ２を比較して、例えば、対応するピーク間の差（α１−φ１），（α２−φ２），（α３−φ３）を求めて、それらの平均を求めればよい。

なお、実際の自律移動体の稼働環境では、傾きの異なる複数の平面からなる壁や、曲面を有する壁や、移動障害物などが存在するので、図６（ｂ）に示す第２のヒストグラムＦ２の波形はノイズを含んだ形状になる。また、図６（ａ）に示す第１のヒストグラムＦ１の波形も、通常、なにがしかのノイズを含む。このような２つの波形を最適重ね合わせ状態とする角度の導出は、いわゆる相互相関関数を用いて数値的に行われる。

角度変数をｉ，ｊとし、第１および第２のヒストグラムを関数Ｆ１（ｉ），Ｆ２（ｉ）とすると、相互相関関数はｋ（ｊ）は、
ｋ（ｊ）＝ΣＦ１（ｉ）×Ｆ２（ｉ＋ｊ）、
と表される。ここで、Σは、角度変数ｉについて和を求める記号である。この相互相関関数ｋ（ｊ）のピーク値、通常最大値を与える角度変数ｊにより補正角度θが決定される。

（距離データの回転）
図７における距離データＢは、第２の距離データＰをセンサ４の位置を回転中心として上述の補正角度θだけ回転した回転補正により得られた新たな第２の距離データである。

この図において、データ点が引数、いわゆるインデックスを添えて示されている。例えば、センサ４が、３゜置きに例えば、０゜から１８０゜まで測定することを想定すると、第１の距離データＡの場合、６１個のデータ点ａ（０）〜ａ（６０）が生成される。同様に、第２の距離データＰの場合、６１個のデータ点ｐ（０）〜ｐ（６０）、またはｂ（０）〜ｂ（６０）が測定される。

図７において、グループＧ１に属するデータ点ａ（０）〜ａ（ｎ）等とデータ点ｂ（０）〜ｂ（ｍ）等とは、互いに距離δｕを隔てて平行に並んでおり、グループＧ２に属するデータ点ａ（ｉ１），ａ（ｉ１＋１）等とデータ点ｂ（ｊ１），ｂ（ｊ１＋１）等とは、互いに距離δｖを隔てて平行に並んでおり、グループＧ３に属するデータ点ａ（ｉ２），ａ（ｉ２＋１）等とデータ点ｂ（ｊ２），ｂ（ｊ２＋１）等とは、互いに距離δｕを隔てて平行に並んでいる。これらのことは、壁Ｗ０や壁Ｗにおいて、その主たる壁面が互いに平行乃至直角に配置されている状況を反映している。

上述の各データ点毎の並進距離δｕやδｖに基づいて、データ全体の並進距離であるΔｕやΔｖを精度良く求めることができれば、これらを補正角度θに基づいて座標変換することにより、Δｕ，Δｖ，θの関数として、上述のΔｘ，Δｙを求めることができる。そこで、以下では、δｕ，δｖに基づいてΔｕ，Δｖを精度良く求めるための処理を説明する。

（データ点対の形成）
図７に示す状況において、第１の距離データＡにおけるデータ点と第２の距離データＢにおけるデータ点とについてセンサ４による検知方向が最も近いもの同士によりデータ点対を形成する。すなわち、図７において、データ点対［ａ（ｉ），ｂ（ｊ）］，［ａ（ｉ＋１），ｂ（ｊ＋１）］，・・，［ａ（ｉ１），ｂ（ｊ１）］，・・，［ａ（ｉ２），ｂ（ｊ２）］，・・等が形成される。

上記の引数、すなわちインデックスの設定方法の前提によると、各データ点対間のインデックスの差は一定である。すなわち、ｉ−ｊ＝ｉ１−ｊ１＝ｉ２−ｊ２＝Ｎ、である。この一定置Ｎは、補正角度θとインデックス毎の角度の増分、例えば上述の３゜を用いて、Ｎ＝θ／３（端数を四捨五入、または切り捨て）により決定される。

（並進軸の設定）
次に、図７に示すように、上述の角度ヒストグラムＦ１において出現頻度が最も高い角度である角度α１に対応する方向に直交する軸である第１の並進軸ｕを設定する。この場合、角度α１は第１の距離データＡのデータに属する角度であり、ｘｙ座標系のｘ座標軸に対して設定されている。従って、第１の並進軸ｕは、角度（α１＋π／２）、または（α１−π／２）の方向に設定すればよい。

なお、第１の並進軸ｕを設定するもとの角度として角度α１を選択したが、第２の距離データＰのデータに属する角度、例えば角度φ１を選択することもできる。この場合には、角度φ１に対して、補正角度θの回転を施した後、その角度の方向に第１の並進軸ｕを設定する。

すなわち、第１の並進軸ｕを設定する方法をより一般化すると、第１または第２の角度ヒストグラムのいずれかにおいて出現頻度の高い角度を抽出すると共にその角度に回転補正を施して第１の角度とし、その第１の角度の方向に直交する軸として第１の並進軸を設定すればよい、ということになる。通常は、上記のように、角度ヒストグラムＦ１において出現頻度が最も高い角度である角度α１に基づいて第１の並進軸ｕを設定する。

上記により第１の並進軸ｕを設定した後、この第１の並進軸ｕに直交する並進軸として、第２の並進軸ｖを設定する。このように直交する２つの並進軸を設定した状態が、図７に示されている。

（差分ヒストグラムの形成）
次に、図８（ａ）に示すように、第１の並進軸ｕに関する各データ点対の座標値の差分の出現頻度を表す第１の差分ヒストグラムＨ１を形成する。例えば、データ点ｂ（ｊ）のｕ座標値ｕｂｊと、そのデータ点と対を形成するデータ点ａ（ｉ）のｕ座標値ｕａｉとの差分、δｕ＝（ｕｂｊ−ｕａｉ）を計算し、その値に基づいて、図８（ａ）に示すδｕ軸に設けられた投票ビンに投票する。このような投票を全てのデータ点対について行うと、図８（ａ）に示すヒストグラムＨ１が形成される。

同様に、図８（ｂ）に示すように、第２の並進軸ｖに関する各データ点対の座標値の差分の出現頻度を表す第２の差分ヒストグラムＨ２を形成する。

（並進距離補正値）
差分ヒストグラムＨ１におけるピークｕ１は、図７におけるデータグループＧ１，Ｇ３に対応し、ピークｕ２は、データグループＧ２に対応する。ここで、欲しいデータは、ピークｕ１に対応する距離差分値Δｕであり、これがｕ軸方向の並進距離補正値である。

また、差分ヒストグラムＨ２におけるピークｖ１は、図７におけるデータグループＧ２に対応し、ピークｖ２は、データグループＧ１，Ｇ３に対応する。ここで、欲しいデータは、ピークｖ１に対応する距離差分値Δｖであり、これがｖ軸方向の並進距離補正値である。

上述の並進距離補正値Δｕ，Δｖは、各差分ヒストグラムＨ１，Ｈ２におけるピーク値から読み取ることができる。また、これらのヒストグラムの曲線をガウス関数などでフィッティングしてその中心値としてΔｕやΔｖを求めるようにしてもよい。

以上により、Δｕ，Δｖ，θが求められたので、自己位置認識システム１は、これらを用いて、一般的な既知の座標変換により、上述のΔｘ，Δｙを求めることができ、現在認識している自律移動体の並進方向および回転方向に関する補正を行って自己位置を正しく認識することができる。

本実施形態の自己位置認識システム１によれば、第１の距離データＡと第２の距離データＰ，Ｂとの間で互いに関連づけられたデータ点の組であるデータ点対について差分ヒストグラムＨ１，Ｈ２を形成して並進方向の補正距離Δｕ，Δｖを求めるので、平行な壁が、複数、近距離に存在する場合であっても間違った壁にマッチングするおそれを低減できる。すなわち、従来の方法では、並進方向の補正距離を求めるためのヒストグラムを、第１の距離データに関するヒストグラムと第２の距離データに関するヒストグラムのように、それぞれ独立個別に求められていたので、距離データの局所的構造を考慮できなかったが、本発明の方法によればデータ点対を用いることにより、この点が改善されている。

また、差分ヒストグラムＨ１，Ｈ２を用いることにより、精度良く並進方向の補正距離を求めることができ、精度良い自己位置認識を実現できる。また、自己位置認識システム１によると、差分ヒストグラムＨ１，Ｈ２が形成された時点で、そのピーク値により並進方向の補正距離Δｕ，Δｖの情報が得られるので、従来行っていたヒストグラム間の相互相関を計算する必要がなく、計算のための負担が低減される。

（第２の実施形態）
図９（ａ）（ｂ）は第２の実施形態に係る自己位置認識システムにおける第１および第２の差分ヒストグラムを示す。本実施形態は、上述の第１の実施形態における図８（ａ）（ｂ）に現れるピークｕ２やｖ２を低くする方法に関する。

すなわち、自己位置認識システム１において、差分ヒストグラムＨ１，Ｈ２を作成する際に、例えば、第１の距離データＡにおいてセンサ４による検知方向が互いに隣り合うデータ点における注目している並進軸に関する座標値が互いに所定値以上離れている場合には、そのデータ点によって構成されるデータ点対についての差分値を採用しないという制限を設けるものである。

上述の制限を、図７を参照して説明する。例えば、注目している並進軸が並進軸ｕであるとする。第１の距離データＡにおける検知方向が互いに隣り合う２つのデータ点のｕ軸に関する座標値の差を見た場合、グループＧ１に属するデータ点等は、他のグループＧ２，Ｇ３に属するデータ点等よりも大きな差を有する。このような差の大小に基づいて、図８（ａ）に現れるピークｕ２を低減する。

上述の差の大小を弁別する比較基準として所定値εｕを設定すると、グループＧ２の場合、例えば、｜ａ（ｉ１）−ａ（ｉ１＋１）｜＞εｕ、であり、グループＧ１の場合、例えば、｜ａ（ｉ）−ａ（ｉ＋１）｜＜εｕ、である。

従って、データ点対［ａ（ｉ１），ｂ（ｊ１）］，［ａ（ｉ１＋１），ｂ（ｊ１＋１）］についての差分値を採用しないこととする。差分ヒストグラムＨ１の作成時にこのような弁別と取捨選択を行うことにより、グループＧ２に属するデータ点等からの寄与を排除して、図９（ａ）に示すように、ピークｕ２を低くして改善された差分ヒストグラムＨ１が作成される。

また、同様の方法により、並進軸ｖに対して所定値εｖを設定すると、図９（ｂ）に示すように、ピークｖ２を低くして改善された差分ヒストグラムＨ２が作成される。

なお、上述の弁別と取捨選択を行う対象データ点は、第１の距離データＡのデータ点に限らず、第２の距離データＢのデータ点について行うこともできる。また、所定値εｕ，εｖは、センサ４による空間分解能や、自律移動体の稼動環境などに応じて、適宜決めることができる。

本実施形態の自己位置認識システム１によれば、並進軸ｕ，ｖ方向の補正距離の取得に有効なデータ点対のみを用いて、その情報を反映させた差分ヒストグラムＨ１，Ｈ２を形成することになるので、ノイズの少ない差分ヒストグラムとすることができ、精度と信頼性の高いマッチング、従って精度と信頼性の高い自己位置認識を実現できる。

（第３の実施形態）
図１０（ａ）は第３の実施形態に係る自己位置認識システムにおける第１および第２の距離データを角度補正した状態で重ねて示し、図１０（ｂ）は回転前の第２の距離データの例を示し、図１１は図１０（ａ）における第１の並進軸に対する第１の差分ヒストグラムを示す。

本実施形態は、自己位置認識処理において、予期せぬ事態が発生したときの事態を安全側に回避する処理に関する。予期せぬ事態とは、例えば、センサ４が実測を行った際に、図１０（ｂ）に示すように、第２の距離データＰにおいて、地図情報記憶装置２に記憶された地図情報に含まれていない壁Ｗｒを有する障害物が検知された場合などである。

図１０（ｂ）の例では、壁Ｗｒが壁Ｗの一部と平行である。なお、第１の距離データＡは、前出の図３に示したものとする。すると、これらの距離データを回転補正して重ねた状態において、図１０（ａ）に示すように、データ点間の並進軸ｕに関する差分値がδｕ１となるデータ点のグループＧ４が現れる。

上述の状況のもとで、並進軸ｕに関する差分ヒストグラムＨ１には、図１１に示すように、ピークｕ１に加えて、さらにグループＧ４に対応するピークｕ３が現れる。

そこで、本実施形態の自己位置認識システム１においては、差分ヒストグラムＨ１，Ｈ２のいずれかに複数のピークが存在する場合にはその差分ヒストグラムに対応する並進方向の補正を行わないこととしている。上述の場合、ｕ軸方向の補正は行われない。

本実施形態の自己位置認識システム１によれば、並進方向のマッチングにおける間違った位置補正を回避でき、間違った自己位置認識を回避できる。これにより、自律移動体の安全な移動を実現できる。

（第４の実施形態）
本実施形態は、第２の並進軸ｖの設定の仕方に関するものである。上述の各実施形態１，２，３においては、第２の並進軸ｖは、第１の並進軸ｕに直交する方向に設定されていた。そして、このような設定のもとでは、グループＧ２のデータ点のように、第１の並進軸ｕに平行に分布したデータ点に対して、有効に第１の並進軸ｖ方向の補正距離を求めることができる。すなわち、直交するｕｖ座標系は、環境障害物における直交する壁に対して有効である。

本実施形態は、直交しない壁に対しても有効となる一般化した第２の並進軸ｖの設定について示す。角度ヒストグラムを形成するまでは上述の各実施形態１，２，３と同様なので、第１の実施形態における図６（ａ）を参照する。

すなわち、本実施形態の自己位置認識システム１は、第１の角度α１を設定するために用いた図６（ａ）の角度ヒストグラムＦ１において、出現頻度が高い角度であって抽出した角度α１とは異なる角度、すなわち、後述する理由により角度α２を抽出し、その角度α２に回転補正を施し（後述）、その回転補正された角度の方向に直交する軸として第２の並進軸ｖを設定する。

上述のα２を選ぶ理由を説明する。角度α３における出現頻度は、角度α２における出現頻度よりも高いが、角度の２πによる周期性を考えると、角度α３は角度α１と同等と考えられるので、次に出現頻度の高い角度α２が選ばれることになる。これを別の面から見ると、「２次元平面における位置を精度良く定めるには、なるべく直交する座標軸系に基づくのが好ましいと考えられるから、角度α１に対して略π／２の角度の隔たりのある角度α２を選択する」と根拠付けできる。

また、上述の第２の並進軸ｖを設定する際に角度α２に回転補正をすること述べたが、この回転補正は行わないことも有り得る。すなわち、図２における処理フロー（Ｓ４）に関して説明したように、補正角度θに基づいて第１の距離データＡ、第２の距離データＰを相互に回転して回転方向を回転補正した新たな第１および第２の距離データを取得する回転は相対的なものであり、いずれのデータを基準にするかについて自由度があり、一方だけ回転したり、両方回転したりすることができる。図７に示すように、第２の距離データＰだけが回転補正される場合、角度α２の方向は、角度α１の方向と同様に、回転補正が行われない。

上述のように、本実施形態では、１つの角度ヒストグラムから、出現頻度が高く、角度差がπ／２に近い２つの角度を選択して、これらに基づいて第１および第２の並進軸ｕ，ｖを設定する。また、採用する角度ヒストグラムは、一般に、第１の距離データＡ、または第２の距離データＰのいずれに関するものでもよい。場合によって、両方の角度ヒストグラムから、個別に角度を１ずつ抽出して用いるようにしてもよい。

本実施形態の自己位置認識システム１によれば、Ｙ字交差点などのように、周辺における直交しない壁の情報を含む第１および第２の距離データであっても、これらのデータを有効活用して並進方向および回転方向に関する補正を行うことができ、周囲環境への依存性をより少なくして精度良い自己位置認識を実現できる。

なお、以上に述べた第１乃至第４の実施形態における自己位置認識システム１の地図情報記憶装置２、自己位置記憶装置３、演算処理装置５は、ＣＰＵやメモリや外部記憶装置や表示装置や入力装置などを備えた一般的な構成を備えた電子計算機、および、その上のプロセス又は機能の集合として構成することができる。また、本発明は、上記構成に限られることなく種々の変形が可能である。また、図２に示したフローチャートにおいて、並列処理が可能な処理は並列に処理を行ってもよく、また処理の順番を替えてもよい。

また、本発明は、上記構成に限られることなく種々の変形が可能である。例えば、上述した各実施形態の構成を矛盾のない範囲で互いに組み合わせた構成（例えば、第３の実施形態において、第４の実施形態を組合せてｕ軸方向の補正を行う）とすることができ、そのような組合せ可能な構成の実施形態は明記されていなくても当然に本発明に含まれる。

本発明の第１の実施形態に係る自己位置認識システムのブロック構成図。 同上システムにおける自己位置認識処理を説明するフローチャート。 同上システムで処理される第１の距離データの例を示す平面図。 同上システムで処理される第２の距離データの例を示す平面図。 同上第１の距離データにセンサの位置を一致させて同上第２の距離データを重ねて示した平面図。 （ａ）は同上第１の距離データに関する角度ヒストグラムの図、（ｂ）は同上第２の距離データに関する角度ヒストグラムの図。 同上第１および第２の距離データを角度補正した状態で重ねて示す平面図。 （ａ）は第１の差分ヒストグラムの図、（ｂ）は第２の差分ヒストグラムの図。 （ａ）は第２の実施形態に係る自己位置認識システムにおける第１の差分ヒストグラムの図、（ｂ）は同第２の差分ヒストグラムの図。 （ａ）は第３の実施形態に係る自己位置認識システムにおける第１および第２の距離データを角度補正した状態で重ねて示す平面図、（ｂ）は回転前の第２の距離データの例を示す平面図。 第１の並進軸に対する第１の差分ヒストグラムの図。

符号の説明

１ 自己位置認識システム
２ 地図情報記憶装置
３ 自己位置記憶装置
３９，４，４０，４１ センサ
５ 演算処理装置
ａ，ｂ，ｐ データ点
ｕ 第１の並進軸
ｖ 第２の並進軸
Ａ 第１の距離データ
Ｂ，Ｐ 第２の距離データ
Ｆ１，Ｆ２ 角度ヒストグラム
Ｈ１，Ｈ２ 差分ヒストグラム
α１ 第１の角度
α２ 第２の角度
θ 補正角度

Claims (4)

1. 予め入力された地図情報を記憶する地図情報記憶手段と、前記地図情報上で認識した自律移動体の自己の位置を記憶する自己位置記憶手段と、自律移動体の自己の周辺に存在する障害物をセンシングするセンサと、前記センサによって得られた障害物情報と前記自己位置記憶手段が現在記憶している自己位置における周辺の前記地図情報記憶手段に記憶された地図情報とを演算処理して自己位置を補正する演算手段と、を備えた自己位置認識システムにおいて、
   前記演算手段は、
   現在位置として認識している位置で前記センサを用いてセンシングされるはずの前記地図情報に基づく障害物情報を第１の距離データとし、前記センサによってセンシングされた実際の障害物情報を第２の距離データとしてこれらのデータを取得し、
   前記第１および第２の距離データのそれぞれについて、データ点を結ぶ線分の角度を求めると共に、得られた角度の出現頻度を求めて第１および第２の角度ヒストグラムを形成し、
   前記第１および第２の角度ヒストグラムを比較すると共にその比較結果に基づいて現在認識している自律移動体の回転方向を補正する補正角度を求め、その補正角度に基づいて前記第１または第２の距離データを相互に回転して回転方向を回転補正した新たな第１の距離データおよび新たな第２の距離データを取得し、
   前記新たな第１の距離データにおけるデータ点と前記新たな第２の距離データにおけるデータ点とについて前記センサによる検知方向が最も近いもの同士によりデータ点対を形成し、
   前記第１または第２の角度ヒストグラムのいずれかにおいて出現頻度の高い角度を抽出すると共にその角度に前記回転補正を施して第１の角度とし、前記第１の角度の方向に直交する軸である第１の並進軸および前記軸に直交する軸である第２の並進軸を設定し、
   前記第１の並進軸に関する前記各データ点対の座標値の差分の出現頻度を表す第１の差分ヒストグラムおよび前記第２の並進軸に関する前記各データ点対の座標値の差分の出現頻度を表す第２の差分ヒストグラムを形成し、
   前記第１の差分ヒストグラムにおけるピーク値を与える距離差分値と、前記第２の差分ヒストグラムにおけるピーク値を与える距離差分値と、前記補正角度とに基づいて、現在認識している自律移動体の並進方向および回転方向に関する補正を行って自己位置を補正することを特徴とする自己位置認識システム。
2. 前記第１の角度を設定するために用いた角度ヒストグラムにおいて出現頻度が高い角度であって前記抽出した角度とは異なる角度を抽出し、その角度に前記回転補正を施し、その回転補正された角度の方向に直交する軸として前記第２の並進軸を設定することを特徴とする請求項１記載の自己位置認識システム。
3. 前記差分ヒストグラムを作成する際に、前記新たな第１または第２の距離データのいずれかにおいて前記センサによる検知方向が互いに隣り合うデータ点における注目している並進軸に関する座標値が互いに所定値以上離れている場合には、そのデータ点によって構成されるデータ点対についての差分値を前記差分ヒストグラム作成に採用しないことを特徴とする請求項１または請求項２記載の自己位置認識システム。
4. 前記いずれかの差分ヒストグラムに複数のピークが存在する場合にはその差分ヒストグラムに対応する並進方向の補正を行わないことを特徴とする請求項１乃至請求項３のいずれか一項に記載の自己位置認識システム。

Images