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

Description

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

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

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

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

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

また、上述のヒストグラムを用いたマップマッチングの方法を電動車椅子の操縦補助システムに応用した例が知られている（例えば、非特許文献２参照）。

しかしながら、上述した非特許文献１，２に示されるようなマップマッチングにおいては、壁が直交することが前提とされており、直交しない壁の情報を活用する点について述べられていない。自律移動体が移動する実際の環境においては、壁が直交しない環境も数多くあるので、このような直交しない壁情報をマップマッチングに有効に用いて自己位置認識における位置補正を行えるようにすることが、複雑な移動環境における精度良い自己位置認識にとって重要である。

本発明は、上記課題を解消するものであって、直交しない壁情報を自己位置認識に用いて位置補正を行うことができ、複雑な移動環境における演算負荷を低減した精度良い自己位置認識を実現できる自己位置認識システムを提供することを目的とする。

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

請求項２の発明は、請求項１記載の自己位置認識システムにおいて、前記演算手段は、前記新たな第１の距離データおよび前記第２の距離データのそれぞれについて、前記第１の並進軸にデータ点を投影して第１の並進軸に対するデータ点の出現頻度から成る第１の位置ヒストグラムおよび第２の位置ヒストグラムを形成すると共に、前記第２の並進軸にデータ点を投影して第２の並進軸に対するデータ点の出現頻度から成る第１の位置ヒストグラムおよび第２の位置ヒストグラムを形成し、前記第１の並進軸に対する第１および第２の位置ヒストグラムの相互相関関数を算出しその結果に基づいて現在認識している自律移動体の第１の並進軸方向の位置を補正する距離データを求めると共に、前記第２の並進軸に対する第１および第２の位置ヒストグラムの相互相関関数を算出しその結果に基づいて現在認識している自律移動体の第２の並進軸方向の位置を補正する距離データを求めることにより自己位置を認識するものである。

請求項３の発明は、請求項１または請求項２記載の自己位置認識システムにおいて、前記第１または第２の角度の方向が、自己位置認識に用いる座標系の座標軸の１つと平行になるように、前記新たな第１の距離データおよび前記第２の距離データの両方を回転し、これらの回転した距離データを用いて前記第１および第２の並進軸方向の補正距離を求めるものである。

請求項４の発明は、請求項１乃至請求項３のいずれか一項記載の自己位置認識システムにおいて、前記第１および第２の角度は、前記第１の角度ヒストグラムから抽出するものである。

請求項１の発明によれば、角度ヒストグラムから出現頻度の高い２つの角度を抽出し、それらの角度の方向に直交する方向の軸に沿って距離データを並進移動させることにより現在認識している自律移動体の各並進軸方向の位置を補正するので、直交する壁の情報がない複雑な移動環境においても、直交しないが向きの異なる２つの壁の情報を含む障害物情報（距離データ）を用いて精度良く自己位置を認識することが可能である。

請求項２の発明によれば、２つの壁が直交するという条件を前提とすることなく、一般的な異なる方向を向いた２つの壁の情報を含む距離データに対して、ヒストグラムと相互相関関数を用いる数値的処理を施すことにより、精度良く自己位置認識ができる。

請求項３の発明によれば、１つの並進軸の方向が自己位置認識に用いている座標系の座標軸の方向に一致するので、位置ヒストグラム形成のための演算が容易となり、自己位置認識のための演算負荷を低減できる。

請求項４の発明によれば、第１の角度ヒストグラムが、マップマッチングにとってノイズデータとなる移動する障害物などを含まない地図情報に基づくので、地図情報にない２方向でマップマッチングをしてしまうという誤認識の発生を回避でき、複雑な移動環境においても精度良い自己位置認識ができる。

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

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

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

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

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

上記に続いて演算手段５は、第１および第２の角度ヒストグラムの相互相関関数を算出すると共にその結果に基づいて現在認識している自律移動体の回転方向を補正する補正角度Δθを求め（Ｓ３）、さらに、第１の距離データＡを補正角度Δθだけ回転した新たな第１の距離データＢを取得する（Ｓ４、図５参照）。

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

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

上記に続いて演算手段５は、新たな第１の距離データＢおよび第２の距離データＰのそれぞれについて、第１の角度の方向に直交する方向の軸（第１の並進軸ｕという）にデータ点を投影して第１の並進軸ｕに対するデータ点の出現頻度から成る第１の位置ヒストグラムおよび第２の位置ヒストグラムを形成すると共に、第２の角度の方向に直交する方向の軸（第２の並進軸ｖという）にデータ点を投影して第２の並進軸ｖに対するデータ点の出現頻度から成る第１の位置ヒストグラムおよび第２の位置ヒストグラムを形成する（Ｓ６、図６参照）。

上記に続いて演算手段５は、第１の並進軸ｕに対する第１および第２の位置ヒストグラムの相互相関関数を算出しその結果に基づいて現在認識している自律移動体の第１の並進軸方向の位置を補正する第１の補正距離Δｕを求めると共に（Ｓ７）、第２の並進軸ｖに対する第１および第２の位置ヒストグラムの相互相関関数を算出しその結果に基づいて現在認識している自律移動体の第２の並進軸方向の位置を補正する第２の補正距離Δｖを求める（Ｓ８）。

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

以下、詳細説明する。図３は、自己位置認識システム１で処理される第１の距離データＡ、第２の距離データＰ、及びレーザレーダ４の配置例を示す。前提条件を述べる。レーザレーダ４は、自律移動車両などの自律移動体に備えられて移動する。自律移動体が稼働する２次元領域における位置を定義する座標系として、グローバル座標系が定義されており、地図情報は、そのグローバル座標系に基づいて記述される。通常、自律移動体は複数のレーザレーダを備えるが、ここでは、レーザレーダ４そのものが自律移動体であり、レーザレーダ４の自己位置認識が自律移動体の自己位置認識であるとする。レーザレーダ４には、その進行方向前方にｙ軸を有し、右方向にｘ軸を有して、レーザレーダ４と共に移動するｘｙ座標系（ローカル座標系）が定義されている。グローバル座標系に対するｘｙ座標系の向きと位置を確定することが、すなわち自己位置認識である。

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

また、図３には、レーザレーダ４によって実際に測定された２つの壁ｗ１，ｗ２を有する障害物上の測定点が、データ点ｐ１〜ｐ１１（第２の距離データＰ）として示されている。ｘｙ座標系において、実測された壁ｗ１は角度φ１の方向、壁ｗ２は角度φ２の方向にある。

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

また、図３において、レーザレーダ４０の配置は、実測による第２の距離データＰに対するレーザレーダ４の位置に対応させて、第１の距離データＡに対するレーザレーダの位置に示したものである。従って、レーザレーダ４とレーザレーダ４０の向きと位置の違い（Δθ，Δｘ１，Δｙ１）が、自己位置認識のずれを表している。このΔθ，Δｘ１，Δｙ１、またはその相当量（Δｘ，Δｙ）などを求める処理が自己位置認識の処理である。

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

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

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

角度を表す変数をｉとし、第１および第２のヒストグラムをｈ１（ｉ），ｈ２（ｉ）とすると、相互相関関数はｋ（ｊ）は、
ｋ（ｊ）＝Σｈ１（ｉ）ｈ２（ｉ＋ｊ）、
と表される。ここで、Σは、角度変数ｉについて和を求める記号である。この相互相関関数ｋ（ｊ）のピーク値（通常最大値、以下同様）を与える角度変数ｊが、補正角度Δθである。なお、図３、図４（ｂ）におけるΔθ，Δθ１，Δθ２は、一般には誤差を含んでいるので互いに等しいとは限らず、Δθ≒Δθ１≒Δθ２である。

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

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

補正距離Δｕ，Δｖを求めるため、まず、前述の図４（ａ）に示す第１のヒストグラムから、出現頻度の高い第１の角度α１と第２の角度α２の２つの角度を抽出する。本例では、簡略化された例が示されており、ピークは２つしかないが、一般に２以上存在する可能性がある。また、第１の角度ヒストグラムを用いるのは、第１のヒストグラムが、マップマッチングにとってノイズとなる移動する障害物などを含まない地図情報に基づくので、誤認識の発生を回避でき、複雑な移動環境においても精度良い自己位置認識ができるからである。

上述の第１および第２の角度α１，α２の差が９０度に近いときは、２つの角度のいずれか一方の角度を他方の角度の方向に直交する方向の角度とする。これは、通常、互いに９０度に近い角度関係で異なる方向を向いている２つの壁は、互いに直交して配置されていると考えられることによる。この場合に、現実に即して、互いに直交する２方向を並進方向とすることにより、角度決定の誤差を排除して精度良い自己位置認識ができる。

この第１および第２の角度α１，α２は、第１の距離データＡを補正角度Δθだけ回転して得られた新たな第１の距離データＢにおいては、図６に示すように、それぞれ、β１，β２となる。ここで、β１＝α１＋Δθ、β２＝α２＋Δθである。これらの角度α１，α２従って、β１，β２は、上述の距離Δｕ，Δｖを求めるために用いられる。すなわち、この２角度によって、図６に示すように、第１の角度β１の方向（矢印ｓの方向）に直交する方向の軸（第１の並進軸ｕ）と第２の角度β２の方向（矢印ｔの方向）に直交する方向の軸（第２の並進軸ｖ）が決定される。第１の距離データＢは、これらの並進軸ｕ，ｖに沿って、第２の距離データＰに重なるように平行移動（並進移動）され、その移動距離により、現在認識している自律移動体の各並進軸方向の位置を補正する第１および第２の補正距離Δｕ，Δｖが求められる。

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

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

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

次に、図７を参照して、上述の補正距離Δｕ，Δｖ，Δｘ，Δｙの相互関係を求める。図７において、直線ｂｄは補助線である。角度δは、２つの壁ｗ１，ｗ２、従ってこれらの壁に沿った、好ましくは、第１の距離データＡ，Ｂに基づくデータ点列の成す角度であり、δ＝β２−β１＝α２−α１、である。図７における幾何学的考察により、Δｘ，Δｙを表す式が以下のように求められる。
Δｘ＝（（Δｖ・ｃｏｓ（β１）＋Δｕ・ｃｏｓ（π−β２））／ｓｉｎ（δ）、
Δｙ＝（（Δｕ・ｓｉｎ（π−β２）−Δｖ・ｓｉｎ（β１））／ｓｉｎ（δ）。

（第２の実施形態）
次に、自己位置認識システム１における自己位置認識処理の他の例を説明する。図８はこの処理のフローチャートを示し、図９は距離データ及びレーザレーダの配置例を示す。この実施形態における自己位置認識システム１のブロック構成は、図１に示した第１の実施形態のものと同様である。また、図８に示すフローチャートにおいて、ステップ＃１は、図２に示した第１の実施形態におけるフローチャートのステップＳ１〜Ｓ３、およびステップＳ５と同様であり、その説明は省略する。なお、前述したように、ステップＳ４，Ｓ５の順序は入れ替え可能である。

本実施形態は、ステップ＃２に特徴があり、このステップでは、図８におけるステップＳ４の回転処理に続けて、両方の距離データＡ，Ｐを更に回転する。このステップ＃２の回転によって、図９に示すように、第１の角度α１の方向（矢印ｓの方向）が自己位置認識に用いる座標系であるｘｙ座標系のｙ軸と平行になるように、第１の距離データＡおよび第２の距離データＰの両方が回転される。レーザレーダ４は、この追加の回転によりその場回転してレーザレーダ４ａの配置となる。また、第１の距離データＡに対応しているレーザレーダ４１は、この追加の回転によりレーザレーダ４１ａの配置となる。

第１の距離データＡは、角度ω１＝π／２−α１の回転により新たな第１の距離データＢ１とされる。また、第２の距離データＰは、角度ω２＝π／２−α１−Δθの回転により新たな第２の距離データＰ１とされる。この回転により、第１の並進軸がｘ軸方向となる。また、第２の並進軸（図９におけるＹ軸）は、ｘ軸から角度δ＝α２−α１、だけ回転した方向となる。

ステップ＃３において、図９に示した回転された第１および第２の距離データＢ１，Ｐ１から、第１の並進軸ｘ、および第２の並進軸Ｙに関するヒストグラムが形成される。この処理は、第１の実施形態におけるステップＳ６、および図６の説明と同様である。

また、ステップ＃４において、第１の実施形態におけるステップＳ７，Ｓ８と同様に、各並進軸ｘ，Ｙについて、それぞれ相互相関関数を算出して、第１および第２の補正距離が求められる。すなわち、図９に示すように、第１の並進軸ｘ方向の補正距離Δｘ２、第２の並進軸Ｙ方向の補正距離ΔＹが求められる。また、これらの補正距離Δｘ２，ΔＹ、および、ステップ＃１で求めた補正角度Δθを用いて、現在認識している自律移動体の回転方向すなわち向き（または姿勢）と現在位置の座標とを補正して、自己位置認識が行われる。

次に、上述の補正距離Δｘ２，ΔＹと、上述の追加の回転の前の補正距離、すなわちΔｘ，Δｙとの相互関係を求める。ここで、第１の距離データＢ１と第２の距離データＰ１のｙ軸方向の並進差をΔｙ２とすると、δ＝α２−α１，ΔＹ，Δｘ２を用いて、
Δｙ２＝（ΔＹ−Δｘ２・ｃｏｓδ）／ｓｉｎδ、
となる。ところで、第１の距離データＢ１と第２の距離データＰ１は、前述の図５において、互いの並進差から補正距離Δｘ，Δｙを生成している第１の距離データＢと第２の距離データＰを、共に角度（π−α１）回転したものであるから、補正距離（Δｘ，Δｙ）は、補正距離（Δｘ２，Δｙ２）を角度（π−α１）だけ逆回転することによって得られる。従って、
Δｘ＝Δｘ２・ｃｏｓ（α１）−Δｙ２・ｓｉｎ（α１）、
Δｙ＝Δｘ２・ｓｉｎ（α１）＋Δｙ２・ｃｏｓ（α１）、
となる。

なお、上記において、第１および第２の角度α１，α２のうち何れの方向を、何れの座標系の座標軸に平行にするかは、任意に選択できる。図９の例では、２つの角度α１，α２のうち、小さい角度α１（α１＜α２）の方向をｙ軸に平行にしている。また、上述の最初の回転と追加の回転とは、実際は、２段階に分けることなく、１回の回転で済ませることができる。

上述の回転を行う本実施形態によれば、１つの並進軸の方向が自己位置認識に用いている座標系の座標軸の１つ（ｘ軸）の方向に一致するので、位置ヒストグラム形成のための演算が容易となり、自己位置認識のための演算負荷を低減できる。

（第３の実施形態）
図１０乃至図１２（ａ）（ｂ）（ｃ）を参照して、第３の実施形態を説明する。この実施形態では、上述の第２の実施形態で説明した自己位置認識システム１を実環境に適用した場合のデータ例を示す。図１０は自己位置認識システムを備えた自律移動体１０が移動中に自己位置認識を行う様子を示し、図１１（ａ）（ｂ）（ｃ）、図１２（ａ）（ｂ）（ｃ）はヒストグラムと相互相関関数の例を示す。

略直方体形状の自律移動体１０は、自己位置認識システム１（不図示）のレーザレーダ４を前面左隅部に備えて、壁Ｗによって左右を囲まれた通路を移動中である。左方の壁Ｗには、直交しない壁面から成る凹所が存在する。自律移動体１０の自己位置認識システム１は、現在認識している自己位置に基づいて地図情報から第１の距離データを取得し、レーザレーダ４を用いた実測によって、そのスキャン可能角度範囲における前方および左方の障害物の距離データ（第２の距離データ）を取得する。なお、レーザ光の入射角が９０゜に近い場合（壁面に平行に近くなる場合）には、測定データが得られない。

上述の状況のもとで取得された第１および第２の距離データに基づいて得られた角度ヒストグラム、距離ヒストグラムは、図１１（ａ）（ｂ）（ｃ）に示すようになる。第１の距離データ（地図情報から求めたもの）に対応する第１の角度ヒストグラムｍ０と、第２の距離データ（実測値）に対応する第２の角度ヒストグラムｒ０との角度座標軸方向のずれが補正角度である。また、角度ヒストグラムｍ０の最初の２つのピークに対する角度から第１軸方向および第２軸方向が定められ、これらの軸に関する距離ヒストグラムから補正距離が求められる。距離ヒストグラムｍ１，ｍ２が第１の距離データから得られ、距離ヒストグラムｒ１，ｒ２が第２の距離データから得られる。

上述の角度ヒストグラムｍ０，ｒ０、距離ヒストグラムｍ１，ｒ１、および距離ヒストグラムｍ２，ｒ２について、それぞれ相互相関関数ｋを算出すると、図１２（ａ）（ｂ）（ｃ）に示すようになる。それぞれピークｃ０，ｃ１，ｃ２の位置に対応する角度差、および距離差の値が、補正角度および補正距離となる。自己位置認識システム１における自動処理に際して、閾値ＴＨを変化させることにより所定のピークを検出する。

なお、以上に述べた第１乃至第３の実施形態における自己位置認識システム１の地図情報記憶手段２、自己位置記憶手段３、演算手段５は、ＣＰＵやメモリや外部記憶装置や表示装置や入力装置などを備えた一般的な構成を備えた電子計算機、および、その上のプロセス又は機能の集合として構成することができる。また、本発明は、上記構成に限られることなく種々の変形が可能である。例えば、環境情報である第２の距離データを取得する手段として、レーザレーダ４を説明したが、レーザレーダに限らず、超音波アレイセンサやＣＣＤ画像装置などを用いて距離画像を生成し、その距離画像から第２の距離データを取得するようにしてもよい。また、図２、図８に示したフローチャートにおいて、並列処理が可能な処理は並列に処理を行ってもよく、また処理の順番を替えてもよい。

また、上記において、第１の距離データＡを、補正角度Δθ、または、ω１＝π／２−α１−Δθだけ回転して、新たな第１の距離データＢ、Ｂ１を取得しているが、この場合の回転処理について２通りの方法で処理を行って第１の距離データＢ、Ｂ１を取得することができる。１つは、各データ点の座標を、回転変換して新たな座標値を有するデータ点を生成する方法である。他の１つは、第１の距離データを取得するもともとの方法に基づく方法である。すなわち、現在位置として認識している位置についてΔθ、または、ω１に基づいて方向の補正を行った後に、回転後の新たな、現在位置として認識している位置においてレーザレーダ４を用いてセンシングされるはずの地図情報に基づく障害物情報（第１の距離データという）を取得する方法である。第２の方法によると、角度補正されている分、実測データに近くなり、２つの距離ヒストグラムの一致精度が向上すると考えられる。

本発明の第１の実施形態に係る自己位置認識システムのブロック構成図。 同上システムにおける自己位置認識処理を説明するフローチャート。 同上システムで処理される第１および第２の距離データのデータ点配置例を示す平面図。 （ａ）は図３に示した第１の距離データに関する角度ヒストグラムの図、（ｂ）は図３に示した第２の距離データに関する角度ヒストグラムの図。 図３における第１の距離データを補正角度分回転した状態の平面図。 図５に示した第１および第２の距離データから位置ヒストグラムを形成する様子を示す平面図。 同上システムにおける補正距離の導出を説明するための幾何学配置図。 第２の実施形態に係る自己位置認識システムにおける自己位置認識処理を説明するフローチャート。 同上フローチャートに従って処理される第１および第２の距離データのデータ点配置例を示す平面図。 第３の実施形態に係る自己位置認識システムを備えた自律移動体が自己位置認識を行う様子を示す斜視図。 （ａ）は角度ヒストグラムの例を示す図、（ｂ）（ｃ）は位置ヒストグラムの例を示す図。 （ａ）は図１１（ａ）の角度ヒストグラムについての相互相関関数の図、（ｂ）は図１１（ｂ）の距離ヒストグラムについての相互相関関数の図、（ｃ）は図１１（ｃ）の距離ヒストグラムについての相互相関関数の図。

符号の説明

１ 自己位置認識システム
２ 地図情報記憶手段
３ 自己位置記憶手段
４，４ａ，４０，４１，４１ａ レーザレーダ
５ 演算手段
ａ１〜ａ１２，ｂ１〜ｂ１２，ｐ１〜ｐ１１ データ点
ｋ 相互相関関数
ｕ，ｘ 第１の並進軸
ｖ，Ｙ 第２の並進軸
Ａ，Ｂ，Ｂ１ 第１の距離データ
Ｆ （角度の）出現頻度
Ｇ，Ｇ１，Ｇ２ （位置の）出現頻度
Ｐ，Ｐ１ 第２の距離データ
α１，β１ 第１の角度
α２，β２ 第２の角度
Δθ 補正角度
Δｘ 第１の補正距離
Δｙ 第２の補正距離

Claims (4)

1. 予め入力された地図情報を記憶する地図情報記憶手段と、前記地図情報上で認識した自
   律移動体の自己の位置を記憶する自己位置記憶手段と、自律移動体の自己の周辺に存在す
   る障害物をセンシングするレーザレーダと、前記レーザレーダによって得られた障害物情
   報と前記自己位置記憶手段が現在記憶している自己位置における周辺の前記地図情報記憶
   手段に記憶された地図情報とを演算処理して自己位置を補正する演算手段と、を備えた自
   己位置認識システムにおいて、
   前記演算手段は、
   現在位置として認識している位置で前記レーザレーダを用いてセンシングされるはずの
   前記地図情報に基づく障害物情報（第１の距離データという）と、前記レーザレーダによ
   ってセンシングされた実際の障害物情報（第２の距離データという）とを取得し、
   前記第１の距離データおよび第２の距離データのそれぞれについて、データ点を結ぶ線
   分の角度を求めると共に、得られた角度の出現頻度を求めて第１の角度ヒストグラムおよ
   び第２の角度ヒストグラムを形成し、
   前記第１および第２の角度ヒストグラムの相互相関関数を算出すると共にその結果に基
   づいて現在認識している自律移動体の回転方向を補正する補正角度を求めて前記第１の距
   離データを前記補正角度だけ回転した新たな第１の距離データを取得し、
   前記第１の角度ヒストグラムまたは第２の角度ヒストグラムのいずれかにおいて出現頻
   度の高い２つの角度（これらを第１の角度と第２の角度という）を抽出し、
   前記新たな第１の距離データを、そのデータが前記第２の距離データに重なるように、
   前記第１の角度の方向に直交する方向の軸（第１の並進軸という）と前記第２の角度の方
   向に直交する方向の軸（第２の並進軸という）とに沿って並進移動させることにより現在
   認識している自律移動体の前記第１および第２の並進軸方向の位置を補正することを特徴
   とする自己位置認識システム。
2. 前記演算手段は、前記新たな第１の距離データおよび前記第２の距離データのそれぞれ
   について、前記第１の並進軸にデータ点を投影して第１の並進軸に対するデータ点の出現
   頻度から成る第１の位置ヒストグラムおよび第２の位置ヒストグラムを形成すると共に、
   前記第２の並進軸にデータ点を投影して第２の並進軸に対するデータ点の出現頻度から成
   る第１の位置ヒストグラムおよび第２の位置ヒストグラムを形成し、
   前記第１の並進軸に対する第１および第２の位置ヒストグラムの相互相関関数を算出し
   その結果に基づいて現在認識している自律移動体の第１の並進軸方向の位置を補正する距
   離データを求めると共に、前記第２の並進軸に対する第１および第２の位置ヒストグラム
   の相互相関関数を算出しその結果に基づいて現在認識している自律移動体の第２の並進軸
   方向の位置を補正する距離データを求めることを特徴とする請求項１記載の自己位置認識
   システム。
3. 前記第１または第２の角度の方向が、自己位置認識に用いる座標系の座標軸の１つと平
   行になるように、前記新たな第１の距離データおよび前記第２の距離データの両方を回転
   し、これらの回転した距離データを用いて前記第１および第２の並進軸方向の補正距離を
   求めることを特徴とする請求項１または請求項２記載の自己位置認識システム。
4. 前記第１および第２の角度は、前記第１の角度ヒストグラムから抽出することを特徴と
   する請求項１乃至請求項３のいずれか一項記載の自己位置認識システム。

Images