JP2013170906A - 移動体運動推定装置 - Google Patents

Abstract

【課題】簡易なハードウエア構成で、移動体の運動推定の実時間処理を実現することができるようにする。
【解決手段】複数の演算器２０によって、レジスタＢに順次保持されるレーザの１走査分の反射点座標の各々について、レジスタＡ０〜Ａ６３に保持された複数の反射点座標の各々との差分を演算して、移動ベクトルを出力する。複数のデコーダ２２によって、複数の演算器２０から出力される移動ベクトルについて、当該移動ベクトルの度数を計数するカウンタＣを指定する。これによって、レーザの１走査分に対する、移動ベクトルに関する複数の値の度数がカウンタＣ０〜Ｃ２５５によって計数される。比較器ツリー２４によって、最大度数を計数したカウンタＣを特定し、出力部２６によって、比較器ツリー２４によって特定されたカウンタＣに対応する移動ベクトルを、自車運動の推定結果として出力する。
【選択図】図２

Description

本発明は、移動体運動推定装置に係り、特に、レーダ装置を用いて、移動体の運動を推定する移動体運動推定装置に関する。

特許文献１には、自車運動推定について、レーザレーダにより異なる時刻で検出された物体の位置の変化に基づいて推定することが記載されているが、具体的な演算法が開示されていない。

また、従来技術として、図１７に示すように、レーザレーダにより得られた過去の反射点マップから自車運動を仮定して現時点の予測マップを複数作成し、実際に得られた現時点の反射点マップと一致度の高い予測マップを探索し、最も一致度の高い予測マップを作成するときに仮定した運動量を推定結果として採用する方法が知られている。

特開２０１０−１６０７７７号公報

しかしながら、上記の従来方法では、演算量が多く、実時間処理の実現が困難である、という問題がある。特に、自車運動推定の精度を上げるためには、予測マップのステップを細かくする必要があるが、予測マップを細かく作成すると、予測マップ数が増え、それに対して演算量が増大してしまう。

そこで、専用のハードウエアによる高速化も選択肢としては考えられるが、以下の理由により、そのままのアルゴリズムではハードウエア実現は困難である。

予測マップを候補移動量にしたがって作成するため、演算データとして大量の予測マップを保存しておく必要があり、ハードウエアの構成要素としてはメモリを使用せざるを得ない。メモリを使用するとアクセス競合の問題があるため、並列処理を採用しても処理時間短縮効果は限られている。

また、別の方法として、予測マップごとにＰＥ（Ｐｒｏｃｅｓｓｉｎｇ Ｅｌｅｍｅｎｔ：演算器とメモリ）を個別に割り当てれば、ＰＥ数だけの並列度が得られて処理時間の短縮も期待できるが、この場合は、複数のＰＥ全体をまとめる方法や各ＰＥへのデータの配信方法など、解決しなければならない課題が多く、ハードウエアを複雑化してしまう。

その点、ＧＰＵの様な既に実用化されているＳＩＭＤプロセッサを採用して、ソフトウエアでアルゴリズムを実現する方法も考えられるが、現状のＧＰＵは消費電力が大きく、レーザレーダに組み込むのは困難である。

本発明は、上記問題を解決すべく成されたもので、簡易なハードウエア構成で、移動体の運動推定の実時間処理を実現することができる移動体運動推定装置を提供することを目的とする。

上記目的を達成するために本発明に係る移動体運動推定装置は、移動体に搭載され、かつ、レーザを走査して前記レーザの各反射点座標を計測するレーザ装置から順次得られる推定対象時点のレーザの走査に対する前記反射点座標を保持する第１レジスタと、前記推定対象時点より前に得られた前記レーザの１走査分の複数の前記反射点座標を保持する複数の第２レジスタと、前記第１レジスタに順次保持される前記レーザの１走査分の反射点座標の各々について、前記反射点座標と、前記複数の第２レジスタに保持された前記複数の反射点座標の各々との差分を各々演算する演算器と、前記レーザの１走査分に対する、前記演算器の出力値に関する複数の値の度数を計数するための複数のカウンタと、前記演算器の各出力値について、前記演算器の出力値に対応する度数を計数する、前記複数のカウンタのうちの何れか１つのカウンタを指定するデコーダと、前記複数のカウンタのうち、最大度数を計数した前記カウンタを特定する比較器と、前記比較器によって特定された前記カウンタに対応する前記演算器の出力値を、前記移動体の運動の推定結果とする運動推定部と、を含んで構成されている。

本発明に係る移動体運動推定装置では、レーザ装置から順次得られる推定対象時点のレーザの走査に対する前記反射点座標が第１レジスタに保持され、前記推定対象時点より前に得られた前記レーザの１走査分の複数の前記反射点座標が複数の第２レジスタに保持される。

そして、演算器によって、前記第１レジスタに順次保持される前記レーザの１走査分の反射点座標の各々について、前記反射点座標と、前記複数の第２レジスタに保持された前記複数の反射点座標の各々との差分を各々演算する。デコーダによって、前記演算器の各出力値について、前記演算器の出力値に対応する度数を計数する、前記複数のカウンタのうちの何れか１つのカウンタを指定する。これによって、レーザの１走査分に対する、前記演算器の出力値に関する複数の値の度数が複数のカウンタによって計数される。

そして、比較器によって、前記複数のカウンタのうち、最大度数を計数した前記カウンタを特定し、運動推定部によって、前記比較器によって特定された前記カウンタに対応する前記演算器の出力値を、前記移動体の運動の推定結果とする。

このように、レーザ装置から順次得られる推定対象時点のレーザの走査に対する前記反射点座標と、推定対象時点より前に得られたレーザの１走査分の複数の前記反射点座標とについて、それぞれの差分を演算して、演算器の出力値の各値の度数を計数し、最大度数を計数したカウンタに基づいて、移動体の運動を推定することにより、簡易なハードウエア構成で、移動体の運動推定の実時間処理を実現することができる。

本発明に係る前記演算器は、複数の演算器であって、前記複数の演算器は、前記第１レジスタに順次保持される前記レーザの１走査分の反射点座標の各々について、前記反射点座標と、前記複数の第２レジスタに保持された前記複数の反射点座標との差分を並列に演算するようにすることができる。また、本発明に係る前記デコーダは、複数のデコーダであって、前記複数のデコーダは、前記複数の演算器の出力値に対応する度数を計数する前記カウンタを並列に指定するようにすることができる。これによって、処理の高速化を図ることができる。

本発明に係る移動体運動推定装置は、仮定する前記移動体の複数の回転量の候補の各々について、前記レーザ装置から順次得られる前記推定対象時点のレーザの走査に対する反射点座標から、前記回転量の候補を差し引いた座標に順次変換する座標変換部を更に含み、前記第１レジスタは、前記座標変換部によって順次変換された座標を保持し、前記複数のカウンタは、前記演算器の出力値に関する複数の値及び前記回転量の候補の各組み合わせに対する度数を計数するためのものであって、前記デコーダは、前記演算器の出力値及び前記出力値の演算に用いられた前記反射点座標の変換に用いられた前記回転量の候補の組み合わせに対応する度数を計数する前記カウンタを指定し、前記運動推定部は、前記比較器によって特定された前記カウンタに対応する前記演算器の出力値及び前記回転量の候補を、前記移動体の運動の推定結果とするようにすることができる。これによって、回転量を含めた移動体の運動を推定することができる。

本発明に係る移動体運動推定装置は、前記演算器の出力値が所定範囲内であるか否かを判定する判定器を更に含み、前記デコーダは、前記判定器によって肯定判定された前記演算器の出力値に対応する度数を計数する前記カウンタを指定するようにすることができる。これによって、カウント対象とする運動量に予め範囲を設け、無用のカウント処理を排除すると共に、推定結果の信頼性を向上させることができる。

上記の判定器を含む本発明に係る前記演算器は、複数の演算器であって、前記複数の演算器は、前記第１レジスタに順次保持される前記レーザの１走査分の反射点座標の各々について、前記反射点座標と、前記複数の第２レジスタに保持された前記複数の反射点座標との差分を並列に演算し、前記デコーダは、複数のデコーダであって、前記判定器は、複数の判定器であって、前記複数の判定器は、前記複数の演算器の出力値が所定範囲内であるか否かを並列に判定し、前記複数のデコーダは、前記複数の演算器のうち、前記判定器によって肯定判定された前記演算器の出力値に対応する度数を計数する前記カウンタを並列に指定するようにすることができる。これによって、処理の高速化を図ることができる。

本発明に係る前記複数のカウンタは、前記レーザの１走査分に対する、前記演算器の出力値の指定された部分桁に関する複数の値の度数を計数するためのものであって、前記デコーダは、前記演算器の出力値の前記指定された部分桁の値に対応する度数を計数する前記カウンタを指定し、前記運動推定部は、前記比較器によって特定されたカウンタに対応する前記演算器の出力値の部分桁の値に、予め求められた前記部分桁より上位桁の値を結合して、前記移動体の運動の推定結果とするようにすることができる。これによって、指定した部分桁をカウント対象とすることで、必要な推定精度に合わせた部分桁を指定することが可能となる。

以上説明したように本発明によれば、レーザ装置から順次得られる推定対象時点のレーザの走査に対する前記反射点座標と、推定対象時点より前に得られたレーザの１走査分の複数の前記反射点座標とについて、それぞれの差分を演算して、演算器の出力値の各値の度数を計数し、最大度数を計数したカウンタに基づいて、移動体の運動を推定することにより、簡易なハードウエア構成で、移動体の運動推定の実時間処理を実現することができる、という効果が得られる。

第１の実施の形態の自車運動推定装置およびレーザレーダの配置を示す図である。 第１の実施の形態の自車運動推定装置の構成を示す図である。 移動ベクトルのヒストグラムを作成する方法を説明するための図である。 第１の実施の形態の自車運動推定装置の動作を説明するためのタイミングチャートである。 第２の実施の形態の自車運動推定装置の構成を示す図である。 第２の実施の形態の自車運動推定装置の動作を説明するためのタイミングチャートである。 第３の実施の形態の自車運動推定装置の構成を示す図である。 移動ベクトルのヒストグラムを作成する方法を説明するための図である。 第３の実施の形態の自車運動推定装置の動作を説明するためのタイミングチャートである。 第４の実施の形態の自車運動推定装置の構成を示す図である。 第５の実施の形態の自車運動推定装置の構成を示す図である。 第６の実施の形態の自車運動推定装置の構成を示す図である。 第７の実施の形態の自車運動推定装置の構成を示す図である。 第７の実施の形態の自車運動推定装置の制御部による部分桁指定処理ルーチンの内容を示すフローチャートである。 第８の実施の形態の自車運動推定装置の構成を示す図である。 第９の実施の形態の自車運動推定装置の構成を示す図である。 自車の運動量を推定する従来方法を説明するための図である。

以下、図面を参照して本発明の実施の形態を詳細に説明する。図１に示すように、第１の実施の形態の自車運動推定装置１０は、自車両に搭載されたレーザレーダ１２に接続されている。

レーザレーダ１２は、自車両の前方に対してレーザを１次元（水平方向）に走査しながら照射し、レーザの各反射点座標を計測する。

レーザレーダ１２は、車両前方に設置され、装置を基準とする車両前方に存在する物体までの距離を検出する装置であり、出力するレーザを水平方向に走査することで、レーザの反射により自車両前方に存在する複数の物体表面上の複数の点の位置を検出することができる。レーザレーダ１２は、レーザの各反射点の位置を表す２次元座標を自車運動推定装置１０に逐次出力する。

自車運動推定装置１０は、図２に示すように、レーザレーダ１２の１スキャン分の反射点座標（ｘ、ｚ）を保持する複数のレジスタＡ０〜Ａ６３と、現時点の１つの反射点座標（ｘ、ｚ）を保持するレジスタＢと、複数のレジスタＡ０〜Ａ６３とレジスタＢとの間で座標値の引き算を行い２点間の移動ベクトル（ｄｘ、ｄｚ）を求める複数の演算器２０と、移動ベクトル（ｄｘ、ｄｚ）に関する複数の値のカウント値（度数）を計数するためのカウンタＣ０〜Ｃ２５５と、複数の演算器２０から出力された移動ベクトル（ｄｘ、ｄｚ）の度数を計数するカウンタＣを、カウンタＣ０〜Ｃ２５５の中から特定し、特定されたカウンタＣのカウント値をカウントアップする複数のデコーダ２２と、カウンタＣ０〜Ｃ２５５の中から、最も大きいカウント値を保持するカウンタを特定し、当該カウンタに対応する移動ベクトル（ｄｘ、ｄｚ）及びカウント値を出力する比較器ツリー２４と、比較器ツリーの出力に基づいて、自車運動（並進運動）の推定値を出力する出力部２６と、デコーダ２２、カウンタＣ０〜Ｃ２５５、及び比較器ツリー２４の動作を制御する制御部２８とを備えている。なお、複数のレジスタＡ０〜Ａ６３は、複数の第２レジスタの一例であり、レジスタＢは、第１レジスタの一例である。また、比較器ツリー２４は、比較器の一例であり、出力部２６が、運動推定部の一例である。

複数のレジスタＡ０〜Ａ６３には、レジスタ制御部(図示省略)によって、１時刻前の１スキャン分の反射点座標が格納される。

レジスタＢには、現在のレーザの走査により順次得られる１つの反射点座標（ｘ、ｚ）が、レジスタ制御部によって格納される。

演算器２０の個数は、レジスタＡ０〜Ａ６３と同数であり、複数の演算器２０と複数のレジスタＡ０〜Ａ６３とが一対一に対応して接続されており、各演算器２０は、対応するレジスタＡが保持する座標とレジスタＢが保持している座標との間で引き算をし、２点間の移動ベクトルを求めて出力する。複数の演算器２０は、１スキャン分の移動ベクトルを並列に求める。ここでの１スキャン分の移動ベクトルは、同じ値を持つものはない。なぜならレジスタＡ０〜Ａ６３の値は全て異なるからである。

また、１スキャン分の移動ベクトルの分布を求めるため、移動ベクトルの値ごとにカウンタＣ０〜Ｃ２５５が設けられている。例えば、移動ベクトル（１５、１５）がカウンタＣ２５５に対応する、という具合に、移動ベクトルの各値と、カウンタＣ０〜Ｃ２５５との対応関係を予め決めておく（実際には、正負を考慮する必要がある）。また、新しいスキャンの処理開始時点で、全てのカウンタＣ０〜Ｃ２５５の内容は０にリセットされる。

デコーダ２２の個数は、演算器２０と同数であり、複数のデコーダ２２と複数の演算器２０とが一対一に対応して接続されており、各デコーダ２２は、対応する演算器２０で求めた移動ベクトルに対応するカウンタＣを特定し、特定したカウンタＣのカウント値をカウントアップする。複数のデコーダ２２による１スキャン分のカウントアップが並列に行われる。

レーザレーダ１２から次の反射点座標が入力されると、レジスタ制御部は、入力された反射点座標で、レジスタＢを更新する。

制御部２８は、レーザレーダ１２から反射点座標が入力される毎に、複数のデコーダ２２による並列処理が行われるようにタイミング制御する。これにより、レーザレーダ１２から反射点座標が入力される毎に、各演算器２０による演算、複数のデコーダ２２による並列処理、及びカウンタＣ０〜Ｃ２５５のカウントアップが繰り返し行われる。これを１スキャン分の反射点座標（例えば６４点）について繰り返すことにより、図３に示すように、１時刻前ｔ0の各反射点から現時刻ｔ1の各反射点への全組み合せ分の移動ベクトルを求めて、カウンタＣ０〜Ｃ２５５のカウントアップを行い、カウンタＣ０〜Ｃ２５５のカウント値により、移動ベクトルのヒストグラムが作成される。

比較器ツリー２４は、２つの入力値から大きい方を選択出力する機能(比較器)を基本ユニットとし、この基本ユニットをツリー状に組み合わせ、ｎ個の入力から、最大値を持つ入力を選択出力するように構成したものである。図２においてはｎ＝４の場合をひとつの長方形で表している。比較のキーはカウント値であるが、基本ユニットの出力はカウント値と移動ベクトル（ｄｘ、ｄｚ）（つまりカウンタ番号）のセットである。

制御部２８は、１スキャン分の反射点座標についてのカウント処理が終わったところで、最大のカウント値をもつカウンタＣを探索するため、比較器ツリー２４を動作させる。これによって、１スキャン分の反射点座標についてのカウント処理の結果から、最大カウント値を有するカウンタＣに対応する移動ベクトル（ｄｘ、ｄｚ）が出力される。

また、制御部２８は、比較器ツリー２４の動作が終了すると、カウンタＣ０〜Ｃ２５５の初期化を行う。

出力部２６は、比較器ツリー２４から出力される移動ベクトルを、自車運動の推定値として出力する。

次に、第１の実施の形態の自車運動推定装置１０の動作について説明する。図４に示すタイミングチャートを用いて説明する。なお、上記図４では、反射点座標を「xz-○□」と表している。○はi, j, kでラインの順番を表し、□は０〜６３の数値で反射点の入力されてくる順番を表している。例えば、左の反射点から右の反射点へと順番に入力される。

まず、レジスタＡ０〜Ａ６３レジスタには、レジスタ制御部によって、１時刻前に得られた１走査分の反射点座標が格納される。レジスタＢには、次の走査（現時点の走査）の反射点座標が１点ずつ、レジスタ制御部によって格納される。なお、処理開始の指示は、制御部２８によって行われる。

そして、複数の演算器２０によって、レジスタＢの座標と、レジスタＡ０〜Ａ６３が保持している座標との間で移動ベクトルが求められる。６４個の移動ベクトルが同時並列に求められる。複数のデコーダ２２によって、複数の演算器２０から求められた６４個の移動ベクトルから、それぞれの投票先（ヒストグラムの対応するビン）のカウンタＣを特定し、投票先の指示があったカウンタＣのカウント値を＋１する。

制御部２８によって、現時刻のレーザ走査による１ライン分の反射点座標を用いて求められる移動ベクトルについて、カウント値のカウントアップが終了したと判断されると、制御部２８は、比較器ツリー２４に対して、最大のカウント値を有するカウンタＣを特定し、特定されたカウンタＣに対応するカウント値及び移動ベクトルを出力するように指示する。

そして、出力部２６により、比較器ツリー２４から出力された移動ベクトルを、自車運動の推定値として出力する。

上記の動作が、レーザレーダ１２によるレーザの走査毎に、繰り返し行われ、自車運動の推定値が繰り返し求められる。

以上説明したように、第１の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標と、推定対象時点より１時刻前に得られたレーザの１走査分の複数の反射点座標とについて、それぞれの差分を演算して移動ベクトルを求め、演算器から出力される移動ベクトルの各値の度数を計数し、最大度数を計数したカウンタに基づいて、自車運動を推定することにより、簡易なハードウエア構成で、自車運動推定の実時間処理を実現することができる。

また、異なる時刻のレーザ走査で得られた反射点座標から移動ベクトルを算出し、移動ベクトルの投票処理を行うことで、全ての候補移動ベクトルについて演算する必要がなくなり、演算量を削減できる。また、移動ベクトルの算出から投票までの処理の並列化が簡素な構成で実現可能となり、処理時間を短縮できる。

次に、本発明の第２の実施の形態について説明する。なお、第１の実施の形態と同様の構成となる部分については、第１の実施の形態と同様の符号を付して説明を省略する。

第２の実施の形態では、レジスタＡ０〜Ａ６３にデータをセットする方法が、第１の実施の形態と異なっている。

図５に示すように、第２の実施の形態に係る自車運動推定装置２１０は、レジスタＡ０〜Ａ６３と、レジスタＢＢと、レーザレーダ１２の１スキャン分の反射点座標（ｘ、ｚ）を保持する複数のレジスタＢ０〜Ｂ６３と、複数の演算器２０と、カウンタＣ０〜Ｃ２５５と、複数のデコーダ２２と、比較器ツリー２４と、出力部２６と、制御部２８とを備えている。なお、レジスタＢＢが、第１レジスタの一例である。

複数のレジスタＡ０〜Ａ６３には、レジスタ制御部(図示省略)によって、１時刻前の１スキャン分の反射点座標が格納される。

レジスタＢＢには、現在のレーザの走査により順次得られる反射点座標（ｘ、ｚ）の１つが、レジスタ制御部によって格納される。

１スキャン分の演算処理が終わるまで、レジスタＡ０〜Ａ６３のデータは移動させることができないため、データ移動用にレジスタＢ０〜Ｂ６３を設ける。

反射点座標は、レーザレーダ１２から時系列に入力されて来るので、新しい反射点座標が入力されるつど、制御部２８によって、座標データをレジスタＢＢ、Ｂ６３〜Ｂ０内でシフトさせ（ＢＢ→Ｂ６３→Ｂ６２…Ｂ１→Ｂ０）、レジスタＢ０〜Ｂ６３に１スキャン分の反射点座標をデータセットする。制御部２８は、１スキャンの終了または開始のタイミングでレジスタＢ０〜Ｂ６３の内容をレジスタＡ０〜Ａ６３にコピーし、レジスタＡ０〜Ａ６３に必要な反射点座標のデータをセットする。

次に、第２の実施の形態の自車運動推定装置１０の動作について説明する。図６に示すタイミングチャートを用いて説明する。

レーザレーダ１２から出力されるスキャンタイミング信号は、１ライン（レーザの１走査に相当）の間“１”となる信号で、ラインとラインの間で“０”となる。また、レーザレーダ１２から出力される反射点タイミング信号は、反射点座標が入力されるタイミングを表す信号で、この信号が“１”のとき、入力バス（図示省略）上の反射点座標のデータが有効であることを保証している。制御部２８は、レーザレーダ１２からのタイミング信号を受けて、レジスタをはじめとする各部の制御を行う。

レーザレーダ１２から入力される反射点座標は、反射点タイミング信号が“１”のときに、レジスタＢＢに取り込まれる。

そして、複数の演算器２０によって、レジスタＢＢの座標と、レジスタＡ０〜Ａ６３が保持している座標との間で移動ベクトルが求められる。６４個の移動ベクトルが同時並列に求められる。次に、複数のデコーダ２２によって、複数の演算器２０から求められた６４個の移動ベクトルから、それぞれの投票先（ヒストグラムの対応するビン）のカウンタＣを特定し、投票先の指示があったカウンタＣのカウント値を＋１する。

スキャンタイミング信号が“０”になったことで、制御部２８は、現時刻のレーザ走査による１ライン分の反射点座標を用いて求められる移動ベクトルについて、カウント値のカウントアップが終了したと判断すると、制御部２８は、比較器ツリー２４に対して、最大のカウント値を有するカウンタＣを特定して、特定されたカウンタＣに対応するカウント値及び移動ベクトルを出力するように指示する。

そして、出力部２６により、比較器ツリー２４から出力された移動ベクトルを、自車運動の推定値として出力する。

一方、一旦レジスタＢＢに保持された反射点座標は、レジスタＢ６３、レジスタＢ６２・・・と順次移動して行き、１ラインの終わりの時点では、レジスタＢ０からレジスタＢ６３に1ライン分の反射点座標が保持されている。

スキャンタイミング信号が“０”になったこと（ラインとラインの切れ目）で、制御部２８は、現時刻のレーザ走査による１ライン分の反射点座標を用いて求められる移動ベクトルについて、カウント値のカウントアップが終了したと判断すると、制御部２８は、レジスタＢ０〜Ｂ６３の内容をレジスタＡ０〜Ａ６３にコピーするとともに、カウンタＣ０〜Ｃ２５５をクリア（カウント値を０にリセット）する。

上記の動作が、レーザレーダ１２によるレーザの走査毎に、繰り返し行われ、自車運動の推定値が繰り返し求められる。

このように、反射点座標の入力部分のダブルバッファ化により、レジスタＡ０〜Ａ６３にデータをセットすることができる。

次に、本発明の第３の実施の形態について説明する。なお、第１の実施の形態及び第２の実施の形態と同様の構成となる部分については、第１の実施の形態と同様の符号を付して説明を省略する。

第３の実施の形態では、並進運動だけでなく、回転運動も推定している点が、第１の実施の形態と異なっている。

図７に示すように、第３の実施の形態に係る自車運動推定装置３１０は、複数のレジスタＡ０〜Ａ６３と、レジスタＢＢと、複数のレジスタＢ０〜Ｂ６３と、仮定した回転運動量の候補の分だけ差し引いた座標に変換する座標変換部３２０と、複数の演算器２０と、カウンタＣ０〜Ｃ２５５と、複数のデコーダ２２と、比較器ツリー２４と、出力部２６と、制御部２８とを備えている。

座標変換部３２０は、制御部２８の制御により、レジスタＢＢの反射点座標を、仮定した回転運動量θの複数候補の各々について、当該候補の分だけ差し引いた座標に変換する。例えば、回転運動量θの候補が±１０°の範囲を１°刻みに２１候補ある場合（回転運動量０°を含む）、レジスタＢＢの反射点座標を、２１個の反射点座標に順次変換する。

各演算器２０は、回転運動量θの複数候補の各々について、対応するレジスタＡが保持する座標と、座標変換部３２０によって変換された当該候補の分だけ差し引いた座標との間で引き算をし、２点間の移動ベクトル（ｄｘ、ｄｚ）を求めて出力する。

また、カウンタＣ０〜Ｃ２５５は、回転運動量を含めた移動ベクトル（ｄｘ、ｄｚ、θ）の値ごとに設けられている。例えば、移動ベクトル（１５、１５、１０°）がカウンタＣ２５５に対応する、という具合に、移動ベクトル（ｄｘ、ｄｚ、θ）の各値と、カウンタＣ０〜Ｃ２５５とが対応付けられている。

各デコーダ２２は、対応する演算器２０で求めた移動ベクトル（ｄｘ、ｄｚ）と、その移動ベクトルを求めるために用いられた反射点座標の変換に用いられた回転運動量θの候補との組み合わせに対応するカウンタＣを特定し、特定したカウンタＣの値をカウントアップする。

制御部２８は、レーザレーダ１２から反射点座標が入力され、回転運動量の候補を差し引いた反射点座標に変換される毎に、複数のデコーダ２２による並列処理が行われるようにタイミング制御する。これにより、レーザレーダ１２から反射点座標が入力され、回転運動量の候補を差し引いた反射点座標に変換される毎に、各演算器２０による演算、複数のデコーダ２２による並列処理、及びカウンタＣ０〜Ｃ２５５のカウントアップが繰り返し行われる。これを１スキャン分の反射点座標（例えば６４点）について繰り返すことにより、図８に示すように、１時刻前ｔ0の各反射点から、回転運動量の各候補を差し引いて変換された現時刻ｔ1の各反射点への全組み合せ分の移動ベクトルを求め、カウンタＣ０〜Ｃ２５５のカウント値により、回転運動量を含む移動ベクトルのヒストグラムが作成される。

比較器ツリー２４における基本ユニットの出力はカウント値と移動ベクトル（ｄｘ、ｄｚ、θ）（つまりカウンタ番号）のセットである。

制御部２８は、１スキャン分の反射点座標についての回転運動量の各候補を用いた移動ベクトルに対して、カウント処理が終わったところで、最大のカウント値をもつカウンタＣを探索するため、比較器ツリー２４を動作させる。これによって、１スキャン分の反射点座標についての回転運動量の各候補を用いた移動ベクトルに対するカウント処理の結果から、最大カウント値を有するカウンタＣに対応する移動ベクトル（ｄｘ、ｄｚ、θ）が出力される。

次に、第３の実施の形態の自車運動推定装置１０の動作について説明する。図９に示すタイミングチャートを用いて説明する。

レーザレーダ１２から入力される反射点座標は、反射点タイミング信号が“１”のときに、レジスタＢＢに取り込まれる。

レジスタＢＢの反射点座標は、座標変換部３２０によって回転運動量θの各候補を差し引いた座標に各々変換される。回転運動量θの各候補は制御部２８から指定される。

そして、複数の演算器２０によって、レジスタＢＢの座標が変換される毎に、変換された座標と、レジスタＡ０〜Ａ６３が保持している座標との間で移動ベクトルが求められる。６４個の移動ベクトルが同時並列に求められる。複数のデコーダ２２によって、複数の演算器２０から求められた、回転運動量を含めた６４個の移動ベクトル（ｄｘ、ｄｚ、θ）から、それぞれの投票先（ヒストグラムの対応するビン）のカウンタＣを特定し、投票先の指示があったカウンタＣのカウント値を＋１する。

スキャンタイミング信号が“０”になったことで、制御部２８は、現時刻のレーザ走査による１ライン分の反射点座標の各々に対する変換後の座標の各々を用いて求められる移動ベクトルについて、カウント値のカウントアップが終了したと判断すると、制御部２８は、比較器ツリー２４に対して、最大のカウント値を有するカウンタＣを特定して、特定されたカウンタＣに対応するカウント値及び移動ベクトル（ｄｘ、ｄｚ、θ）を出力するように指示する。

そして、出力部２６により、比較器ツリー２４から出力された移動ベクトル（ｄｘ、ｄｚ、θ）を、自車運動の推定値として出力する。

以上説明したように、第３の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標から、仮定した回転運動量の候補を差し引いた反射点座標と、推定対象時点より１時刻前に得られたレーザの１走査分の複数の反射点座標とについて、それぞれの差分を演算して移動ベクトルを求め、演算器から出力される移動ベクトルの値及び回転運動量の候補の組み合わせの度数を計数し、最大度数を計数したカウンタに基づいて、回転運動量を含む自車運動を推定することにより、簡易なハードウエア構成で、回転運動量を含む自車運動推定の実時間処理を実現することができる。

次に、本発明の第４の実施の形態について説明する。なお、第１の実施の形態と同様の構成となる部分については、第１の実施の形態と同様の符号を付して説明を省略する。

第４の実施の形態では、演算器により求められた移動ベクトルが所定範囲内である場合のみ、移動ベクトルに対応するカウンタをカウントアップしている点が、第１の実施の形態と異なっている。

図１０に示すように、第４の実施の形態に係る自車運動推定装置４１０は、レジスタＡ０〜Ａ６３と、レジスタＢと、複数の演算器２０と、移動ベクトルの範囲を指定するデータを保持する範囲指定レジスタ４１８と、演算器２０の出力が、指定された移動ベクトルの範囲内であるか否かを判定する複数の比較器４２０と、カウンタＣ０〜Ｃ２５５と、複数のデコーダ４２２と、比較器ツリー２４と、出力部２６と、制御部２８とを備えている。なお、比較器４２０が、判定器の一例である。

範囲指定レジスタ４１８には、移動ベクトル（ｄｘ、ｄｚ）のｄｚ成分の最大値及び最小値、及びｄｙ成分の最大値及び最小値が格納され、移動ベクトルの範囲が指定される。

比較器４２０の個数は、演算器２０と同数であり、複数の比較器４２０と複数の演算器２０とが一対一に対応して接続されており、各比較器４２０は、対応する演算器２０で求めた移動ベクトルが、範囲指定レジスタ４１８に指定された範囲内であるかを判定し、範囲指定レジスタ４１８に指定された範囲内である場合に、対応するデコーダ４２２が動作するように動作許可信号を出力する。複数の比較器４２０による１スキャン分の比較判定が並列に行われる。

各デコーダ４２２は、対応する比較器４２０から動作許可信号が入力された場合に、対応する演算器２０で求められた移動ベクトルに対応するカウンタＣを特定し、特定したカウンタＣの値をカウントアップする。複数のデコーダ２２による１スキャン分のカウントアップが並列に行われる。デコーダ４２２は、対応する比較器４２０から動作許可信号が入力されない場合には、求めた移動ベクトルが所定の範囲内に入っていないため、カウントアップ動作を行わない。

制御部２８は、現時点の自車運動推定値の信頼度が高い場合に、次スキャンでの移動ベクトルの指定範囲を狭く設定するように、範囲指定レジスタ４１８を更新し、現時点の自車運動推定値の信頼度が低い場合に、次スキャンでの移動ベクトルの指定範囲を広く設定するように、範囲指定レジスタ４１８を更新する。

例えば、以下のように、現時点の自車運動推定値の信頼度をもとに、次スキャンでの移動ベクトルの指定範囲を設定する。

まず、制御部２８は、レーザの１スキャン分の反射点座標について複数の演算器２０により求めた全移動ベクトルの分散（標準偏差）を求める。その分散が小さければ信頼度は高いと考えられるため、制御部２８は、処理対象の移動ベクトルの範囲を、現在の自車運動推定値から２σ（２×標準偏差）の範囲内に設定するように、範囲指定レジスタ４１８を更新する。

なお、分散を求めるときに、複数の演算器２０により求められる全移動ベクトルを対象とせず、ヒストグラムの度数（カウンタＣのカウント値）の大きい方から10個程度を選択し、選択された１０個の移動ベクトルにおける度数から、移動ベクトルの分散を求めるようにしてもよい。

また、制御部２８は、レーザレーダ１２から反射点座標が入力される毎に、複数のデコーダ２２による並列処理が行われるようにタイミング制御する。これにより、レーザレーダ１２から反射点座標が入力される毎に、各演算器２０による演算、各比較器４２０による比較判定、複数のデコーダ２２による並列処理、及びカウンタＣ０〜Ｃ２５５のカウントアップが繰り返し行われる。これを１スキャン分の反射点座標（例えば６４点）について繰り返すことにより、１時刻前ｔ0の各反射点から現時刻ｔ1の各反射点への全組み合せ分の移動ベクトルを求めると共に、指定範囲外の移動ベクトルを排除し、カウンタＣ０〜Ｃ２５５のカウント値により、指定範囲内の移動ベクトルのヒストグラムが作成される。

次に、第４の実施の形態の自車運動推定装置４１０の動作について説明する。

まず、レジスタＡ０〜Ａ６３レジスタには、レジスタ制御部によって、１時刻前に得られた１走査分の反射点座標が格納される。レジスタＢには、次の走査（現時刻の走査）で得られる反射点座標が１点ずつ、レジスタ制御部によって格納される。なお、処理開始の指示は、制御部２８によって行われる。

そして、複数の演算器２０によって、レジスタＢの座標と、レジスタＡ０〜Ａ６３が保持している座標との間で移動ベクトルが求められる。６４個の移動ベクトルが同時並列に求められる。複数の比較器４２０によって、範囲指定レジスタ４１８の内容に基づいて、複数の演算器２０から求められた６４個の移動ベクトルが、指定された範囲内であるか否かを判定し、肯定判定された場合に、動作許可信号を対応するデコーダ４２２に出力する。

動作許可信号が入力された複数のデコーダ４２２によって、対応する複数の演算器２０から求められた複数の移動ベクトルから、それぞれの投票先（ヒストグラムの対応するビン）のカウンタＣを特定し、投票先の指示があったカウンタＣのカウント値を＋１する。

制御部２８によって、現時刻のレーザ走査による１ライン分の反射点座標について、カウント値のカウントアップが終了したと判断されると、制御部２８は、比較器ツリー２４に対して、最大のカウント値を有するカウンタＣを特定して、特定されたカウンタＣに対応するカウント値及び移動ベクトルを出力するように指示する。

そして、出力部２６により、比較器ツリー２４から出力された移動ベクトルを、自車運動の推定値として出力する。

また、制御部２８によって、レーザの１スキャン分の反射点座標について複数の演算器２０により求めた全移動ベクトルの分散（標準偏差）を求め、自車運動の推定値および当該分散に基づ処理対象の移動ベクトルの範囲を求め、範囲指定レジスタ４１８を更新する。

上記の動作が、レーザレーダ１２によるレーザの走査毎に、繰り返し行われ、自車運動の推定値が繰り返し求められる。

以上説明したように、第４の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標と、推定対象時点より１時刻前に得られたレーザの１走査分の複数の反射点座標とについて、それぞれの差分を演算して移動ベクトルを求め、所定範囲内の移動ベクトルの各値の度数を計数し、最大度数を計数したカウンタに基づいて、自車運動を推定することにより、簡易なハードウエア構成で、自車運動推定の実時間処理を実現することができる。

また、予め分かっている自車運動の範囲内の移動ベクトルのみで投票処理を実施することにより、ノイズを除去することで自車運動推定の信頼度が向上し、演算量も削減できる。

予め自車運動のおよその値が分かっていれば、そこから外れる運動量は考慮する必要がない。むしろ、外れた運動量をカウント対象とすることでノイズが増え、誤った値が最大カウント値を示す可能性がある。本実施の形態では、カウント対象とする運動量にあらかじめ範囲を設けることで、無用のカウント処理を排除するとともに、推定結果の信頼度を向上させる。

また、範囲制限によりカウンタの個数が削減できることに加え、比較器ツリーの規模も小さくなり、ハードウエア量の削減効果もある。

なお、上記の実施の形態では、複数の演算器２０により求めた１スキャン分の全移動ベクトルの分散を用いて、移動ベクトルの制限範囲を設定する場合を例に説明したが、これに限定されるものではない。

例えば、スキャン毎に出力される自車運動推定値の時系列での変化が小さければ、信頼度は高いと考えられる。また、自車運動は急激には変化しないため、推定値の時系列変化が小さければ、正しく推定されていると考えられる。また、自車運動の変化の割合は物理的な上限がある（１秒間に時速が０ｋｍ→１００ｋｍという加速は通常はありえない）。そこで、制御部２８は、スキャン毎に出力される自車運動推定値の時系列での変化が小さい場合に、処理対象のベクトルの制限範囲を、現在の自車運動推定値から起こり得る範囲に設定するようにしてもよい。

また、ヒストグラムの最大度数と２番目の度数（カウント値）の差が大きければ、信頼度は高いと考えられる。逆に度数の差が小さければ、１番目の移動ベクトルと２番目の移動ベクトルのどちらが真値に近いのか判断できず、信頼度は低い。そこで、制御部２８は、カウンタの最大度数と２番目の度数との差が大きく、信頼度が高いと判断できる場合に、処理対象のベクトルの範囲を、現在の自車運動推定値から起こり得る範囲に設定するようにしてもよい。

次に、本発明の第５の実施の形態について説明する。なお、第１の実施の形態及び第４の実施の形態と同様の構成となる部分については、第１の実施の形態及び第４の実施の形態と同様の符号を付して説明を省略する。

第５の実施の形態では、ベース移動ベクトルを差し引いた現時点の反射点座標を用いて、差分移動ベクトルを求めている点が、第４の実施の形態と異なっている。

図１１に示すように、第５の実施の形態に係る自車運動推定装置５１０は、レジスタＡ０〜Ａ６３と、レジスタＢＢと、ベース移動ベクトルを保持するレジスタＢＶと、レジスタＢと、レジスタＢＢに保持された座標からレジスタＢＶに保持されたベース移動ベクトルを差し引いてレジスタＢに格納する演算器５１２と、複数の演算器２０と、範囲指定レジスタ４１８と、複数の比較器４２０と、カウンタＣ０〜Ｃ２５５と、複数のデコーダ４２２と、比較器ツリー２４と、演算器５２４と、出力部２６と、制御部２８と、を備えている。

レジスタＢＢには、現在の反射点座標（ｘ、ｚ）が保持される。レジスタＢＶには、仮定されるベース移動ベクトルが保持される。本実施の形態では、直前に推定された自車運動量の推定値をベース移動ベクトルとしてレジスタＢＶに格納する。

演算器５１２は、レジスタＢＢに保持された反射点座標から、ベース移動ベクトルを差し引いて、１時刻前の反射点座標を推定し、レジスタＢに保存する。

各演算器２０は、対応するレジスタＡが保持する座標とレジスタＢが保持している座標との間で引き算をし、２点間の差分移動ベクトルを求めて出力する。

範囲指定レジスタ４１８には、差分移動ベクトル（ｄｘ、ｄｚ）のｄｘ成分の絶対値の最大値、及びｄｚ成分の絶対値の最大値が格納され、差分移動ベクトルの範囲が指定される。ここで、差分移動ベクトルの範囲の最小値が指定されていないのは、１時刻前の推定座標からの移動ベクトルを演算しているため、自車運動に変化がなければ、差分移動ベクトルは０となるからである。

各比較器４２０は、対応する演算器２０で求めた移動ベクトルの各成分の絶対値が、範囲指定レジスタ４１８に指定された範囲内であるかを判定し、範囲指定レジスタ４１８に指定された範囲内である場合に、対応するデコーダ４２２が動作するように動作許可信号を出力する。

比較器ツリー２４から出力される差分移動ベクトルは、レジスタＢ（１時刻前を推定した座標）までの移動ベクトルであるため、推定に用いたベース移動ベクトルに当該差分移動ベクトルを加えたものが自車運動の推定値となる。したがって、演算器５２４は、比較器ツリー２４から出力される差分移動ベクトルに、１時刻前の自車運動の推定値である移動ベクトル（ベース移動ベクトル）を加算して、求められた移動ベクトルを出力する。

出力部２６は、演算器５２４から出力される移動ベクトルを、自車運動の推定値として出力する。

次に、第５の実施の形態の自車運動推定装置５１０の動作について説明する。

まず、レジスタＡ０〜Ａ６３レジスタには、レジスタ制御部によって、１時刻前に得られた１走査分の反射点座標が格納される。また、レジスタ制御部によって、１時刻前に推定された自車運動の推定値である移動ベクトルが、レジスタＢＶに格納される。レジスタＢＢには、次の走査（現時刻の走査）の反射点座標が１点ずつ、レジスタ制御部によって格納される。そして、演算器５１２によって、レジスタＢＢの反射点座標から、レジスタＢＶの移動ベクトルを差し引いた座標を求め、レジスタＢに格納する。

そして、複数の演算器２０によって、レジスタＢの座標と、レジスタＡ０〜Ａ６３が保持している座標との間で差分移動ベクトルが求められる。６４個の差分移動ベクトルが同時並列に求められる。複数の比較器４２０によって、範囲指定レジスタ４１８の内容に基づいて、複数の演算器２０から求められた６４個の差分移動ベクトルが、指定された範囲内であるか否かを判定し、肯定判定された場合に、動作許可信号をデコーダ４２２に出力する。

動作許可信号が入力された複数のデコーダ４２２によって、対応する複数の演算器２０から求められた複数の差分移動ベクトルから、それぞれの投票先（ヒストグラムの対応するビン）のカウンタＣを特定し、投票先の指示があったカウンタＣのカウント値を＋１する。

制御部２８によって、現時刻のレーザ走査による１ライン分の反射点座標を用いて求められた差分移動ベクトルについて、カウント値のカウントアップが終了したと判断されると、制御部２８は、比較器ツリー２４に対して、最大のカウント値を有するカウンタＣを特定して、特定されたカウンタＣに対応するカウント値及び差分移動ベクトルを出力するように指示する。

そして、演算器５２４により、比較器ツリー２４から出力された差分移動ベクトルと、現時点のベース移動ベクトルとして用いられている１時刻前の移動ベクトルとを加算して求めた移動ベクトルを出力する。

出力部２６により、演算器５２４から出力された移動ベクトルを、自車運動の推定値として出力する。

以上説明したように、第５の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標から、ベース移動ベクトルを差し引いた反射点座標と、推定対象時点より１時刻前に得られたレーザの１走査分の複数の反射点座標とについて、それぞれの差分を演算して差分移動ベクトルを求め、所定範囲内の差分移動ベクトルの値の度数を計数し、最大度数を計数したカウンタに基づいて、自車運動を推定することにより、簡易なハードウエア構成で、自車運動推定の実時間処理を実現することができる。

また、差分移動ベクトルを求めるようにすることにより、差分移動ベクトルの制限範囲を小さく設定することが可能となり、予想される自車運動の制限範囲から外れる差分移動ベクトルは、カウント処理の対象外とすることで、ノイズ除去の効果が得られ、自車運動推定結果の信頼度を向上させることが可能となる。また、範囲制限によりカウンタの個数が削減できることに加え、比較器ツリーの規模も小さくなり、ハードウエア量の削減効果もある。

次に、本発明の第６の実施の形態について説明する。なお、第１の実施の形態、第４の実施の形態、及び第５の実施の形態と同様の構成となる部分については、上記の実施の形態と同様の符号を付して説明を省略する。

第６の実施の形態では、並進運動だけでなく、回転運動も推定している点と、レジスタＡ０〜Ａ６３にデータをセットする方法とが、第５の実施の形態と異なっている。

図１２に示すように、第６の実施の形態に係る自車運動推定装置６１０は、レジスタＡ０〜Ａ６３と、レジスタＢＢと、複数のレジスタＢ０〜Ｂ６３と、レジスタＢＶＡと、レジスタＢと、レジスタＢＢに保持された座標からレジスタＢＶＡに保持されたベース移動ベクトル（並進・回転成分）を差し引くと共に、さらに仮定した回転運動量の候補の分だけ差し引いた座標に変換する座標変換部６１２と、複数の演算器２０と、範囲指定レジスタ４１８と、複数の比較器４２０と、カウンタＣ０〜Ｃ２５５と、複数のデコーダ４２２と、比較器ツリー２４と、演算器６２４と、出力部２６と、制御部２８と、を備えている。

座標変換部６１２は、レジスタＢＢに保持された反射点座標から、ベース移動ベクトル（ｄｘ、ｄｚ、θ）を差し引いて、１時刻前の反射点座標を推定すると共に、制御部２８の制御により、推定された１時刻前の反射点座標を、仮定した回転運動量θの複数候補の各々について、当該候補の分だけ差し引いた座標に変換し、レジスタＢに保持する。

各演算器２０は、回転運動量θの複数候補の各々について、対応するレジスタＡが保持する座標と、レジスタＢ（座標変換部３２０によって変換された当該候補の分だけ差し引いた座標）との間で引き算をし、２点間の差分移動ベクトル（ｄｘ、ｄｚ）を求めて出力する。

また、カウンタＣ０〜Ｃ２５５は、差分移動ベクトル（ｄｘ、ｄｚ）及び回転運動量θの組み合わせごとに設けられている。

比較器４２０から動作許可信号が入力された各デコーダ２２は、対応する演算器２０で求めた差分移動ベクトル（ｄｘ、ｄｚ）と、その差分移動ベクトルを演算するのに用いられた反射点座標の変換に用いられた回転運動量θの候補との組み合わせに対応するカウンタＣを特定し、特定したカウンタＣのカウント値をカウントアップする。

制御部２８は、レーザレーダ１２から反射点座標が入力されて、１時刻前の反射点座標が推定され、更に回転運動量の候補を差し引いた反射点座標に変換される毎に、複数のデコーダ２２による並列処理が行われるようにタイミング制御する。これにより、レーザレーダ１２から反射点座標が入力されて１時刻前の反射点座標が推定され、更に回転運動量の候補を差し引いた反射点座標に変換される毎に、各演算器２０による演算、複数の比較器４２０による比較判定、複数のデコーダ４２２による並列処理、及びカウンタＣ０〜Ｃ２５５のカウントアップが繰り返し行われる。これを１スキャン分の反射点座標（例えば６４点）について繰り返すことにより、１時刻前ｔ0の各反射点から、１時刻前の反射点座標の推定値から回転運動量の各候補を差し引いて変換された現時刻ｔ1の各反射点への全組み合せ分の差分移動ベクトルを求めると共に、指定範囲外の差分移動ベクトルを排除し、カウンタＣ０〜Ｃ２５５のカウント値により、指定範囲内の差分移動ベクトル及び回転運動量の組み合わせのヒストグラムが作成される。

比較器ツリー２４における基本ユニットの出力はカウント値と差分移動ベクトル（ｄｘ、ｄｚ）及び回転運動量θの組み合わせ（つまりカウンタ番号）とのセットである。

制御部２８は、１スキャン分の反射点座標についての回転運動量の各候補を用いた移動ベクトルに対して、カウント処理が終わったところで、最大のカウント値をもつカウンタＣを探索するため、比較器ツリー２４を動作させる。これによって、１スキャン分の反射点座標についての回転運動量の各候補を用いた移動ベクトルに対するカウント処理の結果から、最大カウント値を有するカウンタＣに対応する差分移動ベクトル（ｄｘ、ｄｚ）及び回転運動量θの組み合わせが出力される。

演算器６２４は、比較器ツリー２４から出力される差分移動ベクトル（ｄｘ、ｄｚ）に、１時刻前の自車運動の推定値である移動ベクトル（ｄｘ、ｄｚ）を加算して、求められた並進成分（ｄｘ、ｄｚ）と、同じく比較器ツリー２４から出力された回転運動量θと１時刻前の自車運動の推定値であるベース移動ベクトルの回転運動量θを加算して求められる回転運動量θからなる移動ベクトル（ｄｘ、ｄｚ、θ）を出力する。

出力部２６は、演算器５２４から出力される移動ベクトル（ｄｘ、ｄｚ、θ）を、自車運動の推定値として出力する。

反射点座標は、レーザレーダ１２から時系列に入力されて来るので、新しい反射点座標が入力されるつど、制御部２８によって、座標データをレジスタＢＢ、Ｂ６３〜Ｂ０内でシフトさせ（ＢＢ→Ｂ６３→Ｂ６２…Ｂ１→Ｂ０）、レジスタＢ０〜Ｂ６３に１スキャン分の座標をデータセットする。制御部２８は、１スキャンの終了または開始のタイミングでレジスタＢ０〜Ｂ６３の内容をレジスタＡ０〜Ａ６３にコピーし、レジスタＡ０〜Ａ６３に必要な反射点座標のデータをセットする。

次に、第６の実施の形態の自車運動推定装置６１０の動作について説明する。

まず、制御部２８によって、レジスタＡ０〜Ａ６３レジスタには、レジスタＢ０〜Ｂ６３の内容がコピーされ、１時刻前に得られた１走査分の反射点座標が格納される。また、レジスタ制御部によって、１時刻前に推定された自車運動の推定値である移動ベクトルの並進成分と回転成分が、レジスタＢＶＡに格納される。レジスタＢＢには、次の走査（現時刻の走査）の反射点座標が１点ずつ、制御部２８によって格納される。

そして、座標変換部６１２によって、レジスタＢＢの反射点座標から、レジスタＢＶＡの移動ベクトル（並進・回転成分）を差し引くと共に、さらに回転運動量θの候補の各々について、回転運動量θの候補を差し引いた座標に各々変換する。回転運動量θの各候補は制御部２８から指定される。

そして、複数の演算器２０によって、レジスタＢＢの座標が変換される毎に、変換された座標と、レジスタＡ０〜Ａ６３が保持している座標との間で差分移動ベクトルが求められる。６４個の差分移動ベクトルが同時並列に求められる。複数の比較器４２０によって、範囲指定レジスタ４１８の内容に基づいて、複数の演算器２０から求められた６４個の差分移動ベクトルが、指定された範囲内であるか否かを判定し、肯定判定された場合に、動作許可信号をデコーダ４２２に出力する。

動作許可信号が入力された複数のデコーダ４２２によって、対応する複数の演算器２０から求められた複数の差分移動ベクトル及び回転運動量の組み合わせから、それぞれの投票先（ヒストグラムの対応するビン）のカウンタＣを決定し、投票先の指示があったカウンタＣのカウント値を＋１する。

制御部２８は、現時刻のレーザ走査による１ライン分の反射点座標の各々に対する変換後の座標の各々から求められる差分移動ベクトルについて、カウント値のカウントアップが終了したと判断すると、制御部２８は、比較器ツリー２４に対して、最大のカウント値を有するカウンタＣを特定して、特定されたカウンタＣに対応するカウント値、差分移動ベクトル（ｄｘ、ｄｚ）、及び回転運動量θを出力するように指示する。

そして、演算器６２４により、比較器ツリー２４から出力された差分移動ベクトル（ｄｘ、ｄｚ）と、現時点のベース移動ベクトルとして用いられている１時刻前の移動ベクトルの並進成分（ｄｘ、ｄｚ）とを加算して求めた並進成分（ｄｘ、ｄｚ）と、同じく、比較器ツリー２４から出力された回転運動量θと１時刻前の移動ベクトルの回転成分（θ）とを加算して求めた回転成分とからなる移動ベクトル（ｄｘ、ｄｚ、θ）を出力する。

出力部２６により、演算器６２４から出力された移動ベクトルを、自車運動の推定値として出力する。

以上説明したように、第６の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標から、ベース移動ベクトル（並進・回転成分）を差し引くと共に、仮定した回転運動量の候補を差し引いた反射点座標と、推定対象時点より１時刻前に得られたレーザの１走査分の複数の反射点座標とについて、それぞれの差分を演算して差分移動ベクトルを求め、所定範囲内の差分移動ベクトルの値及び回転運動量の候補の組み合わせの度数を計数し、最大度数を計数したカウンタに基づいて、回転運動量を含む自車運動を推定することにより、簡易なハードウエア構成で、回転運動量を含む自車運動推定の実時間処理を実現することができる。

次に、本発明の第７の実施の形態について説明する。なお、第１の実施の形態と同様の構成となる部分については、第１の実施の形態と同様の符号を付して説明を省略する。

第７の実施の形態では、複数のカウンタが、移動ベクトルの指定された部分桁について、部分桁の値毎の度数を計数している点が、第１の実施の形態と異なっている。

図１３に示すように、第７の実施の形態に係る自車運動推定装置７１０は、レジスタＡ０〜Ａ６３と、レジスタＢと、複数の演算器２０と、移動ベクトルの上位桁の範囲を指定するデータを保持する範囲指定レジスタ７１８と、演算器２０の出力が、指定された移動ベクトルの範囲内であるか否かを判定する複数の比較器７２０と、カウンタＣ０〜Ｃ２５５と、複数のデコーダ７２２と、比較器ツリー２４と、結合部７２４と、出力部２６と、制御部２８とを備えている。なお、比較器７２０が、判定器の一例である。

範囲指定レジスタ７１８には、移動ベクトル（ｄｘ、ｄｚ）のｄｘ成分の上位桁（例えば、上位３桁）の最大値及び最小値、及びｄｙ成分の上位桁（例えば、上位３桁）の最大値及び最小値が格納され、移動ベクトルの上位桁の範囲が指定される。

比較器７２０の個数は、演算器２０と同数であり、複数の比較器７２０と複数の演算器２０とが一対一に対応して接続されており、各比較器７２０は、対応する演算器２０で求めた移動ベクトルの上位桁が、範囲指定レジスタ７１８に指定された範囲内であるかを判定し、範囲指定レジスタ７１８に指定された範囲内である場合に、対応するデコーダ７２２が動作するように動作許可信号を出力する。複数の比較器７２０による１スキャン分の比較判定が並列に行われる。

また、カウンタＣ０〜Ｃ２５５は、移動ベクトル（ｄｘ、ｄｚ）の部分桁の値ごとに設けられている。移動ベクトルの部分桁の各値とカウンタＣ０〜Ｃ２５５との対応関係は、固定的に決めておくのではなく、指定できるようにしておく。例えば、設定１では移動ベクトル（０，０）をカウンタにＣ０に、移動ベクトル（０，１）をカウンタＣ１に、移動ベクトル（０，２）をカウンタＣ２・・・と対応させ、設定２では移動ベクトル（０，４）と（０，５）をカウンタＣ２・・・と対応させておき、設定１を使用するか設定２を使用するかは、実行時に選択できるように構成しておく。

各デコーダ７２２は、対応する比較器７２０から動作許可信号が入力された場合に、対応する演算器２０で求められた移動ベクトルの指定された部分桁の値に対応するカウンタＣを特定し、特定したカウンタＣの値をカウントアップする。複数のデコーダ２２による１スキャン分のカウントアップが並列に行われる。デコーダ７２２は、対応する比較器７２０から動作許可信号が入力されない場合には、求めた移動ベクトルの上位桁が所定の範囲内に入っていないため、カウントアップ動作を行わない。

デコーダ７２２には、制御部２８から部分桁が指定される。例えば、移動ベクトル（ｄｘ、ｄｚ）の指定された部分桁（例えば、ｄｘの上位３桁とｄｚの上位３桁）を採用する。これによって、カウンタＣ０〜Ｃ２５５により、移動ベクトルの上位３桁の値に関するヒストグラムが作成される。また、部分桁の指定は、上位桁だけに限らず、中間部分（例えば下位４桁目〜３桁目の２桁）や下位部分が指定される。

比較器ツリー２４における基本ユニットの出力はカウント値と移動ベクトルの指定された部分桁（ｄｘ、ｄｚ）とのセットである。

結合部７２４は、比較器ツリー２４から出力される移動ベクトルの部分桁（ｄｘ、ｄｚ）に、指定された部分桁より上位桁の予め求められた値を結合して、結合された移動ベクトルを出力する。

出力部２６は、結合部７２４から出力される移動ベクトルを、自車運動の推定値として出力する。

また、制御部２８は、以下に説明するように、指定する移動ベクトルの部分桁、及びカウンタＣと移動ベクトルの部分桁の値との対応関係を適応的に設定する。

例えば、自車運動（簡単のため車速のみで説明）の範囲が０〜４０ｍ/ｓとし、真値が３．７ｍ/ｓであったとする。また、簡単のため、１０進数での部分桁を指定するものとする。この前提で、０．１ｍ/ｓの桁まで推定しようとすると、ヒストグラムの分割が０．１ｍ/ｓ（より細かい）刻みでなければならない。０〜４０を０．１刻みで分割すると、ヒストグラムのビン数は４０１必要である。

ところが、１ｍ/ｓの桁までの推定で良ければ、１刻みで分割し、ビン数は４１でよい。

そこで、まず制御部２８は、移動ベクトルの部分桁として、上位桁（上位２桁）を指定すると共に、ビン数４１のヒストグラム（０，１，２，・・・４０）に対し、移動ベクトルが示す車速を投票するように、カウンタＣと移動ベクトルの部分桁の値との対応関係を設定する。上記の例では、最大度数の車速を探索すると３ｍ/ｓ（真値を３．７と仮定しているので）という結果が得られる。

この段階では、真値は３．０〜３．９ ｍ/ｓの範囲にあるものと考えられる。

数回のスキャンにわたって、毎回３ｍ/ｓという同じ自車運動推定結果が得られる場合に、その精度での信頼度は高いと考えられるため、制御部２８は、移動ベクトルの部分桁として、中位桁（０．１の位の１桁）を指定すると共に、ヒストグラムの分割を変更し３．０〜３．９の範囲を１０分割したビン構成で投票処理を行うように、カウンタＣと移動ベクトルの部分桁の値との対応関係を設定する。

このとき、最大度数となるのは、０．７のビンであり、移動ベクトルの上位桁（基礎運動量）である３ｍ/ｓと結合して、３．７ｍ/ｓという自車運動推定結果が得られる。この段階では、真値は３．７０〜３．７９ｍ/ｓの範囲にあるものと考えられる。

以降、制御部２８が同様に処理することにより、順次、精度を高めることができる。

また、上記のように、移動ベクトルの部分桁の値に結合する上位桁の値は、当該部分桁より上位桁の値であって、数回のスキャンにわたって同じ値が得られた自車運動推定結果から求められる。

なお、移動ベクトルの部分桁として中位桁を指定する場合のビン構成は３．０〜３．９を含んでいればよく、２．０〜５．０の範囲を０．１単位で分割したビン構成で投票処理を行うように、カウンタＣと移動ベクトルの部分桁の値との対応関係を設定しても良い。この場合、移動ベクトルの上位桁（基礎運動量）としては２．０ｍ/ｓに設定するのが良く、ビン構成は０．０〜３．０の３１ビンとなる。

以上、１０進数で説明したが、ハードウエア構成を考慮すると、２進数で切りの良い数値（１０ではなく、２のベキ乗数）で分割するのが良い。

自車の運動量は急激に変化しないため、上記のように部分桁の指定を使って、最初は粗い自車運動推定を行い、順次推定精度を上げるように、指定する移動ベクトルの部分桁、及びカウンタＣと移動ベクトルの部分桁の値との対応関係を設定する。このとき、精度を上げるためにカウンタの数を増やす必要がない。

次に、第７の実施の形態の自車運動推定装置７１０の動作について説明する。

まず、レジスタＡ０〜Ａ６３レジスタには、レジスタ制御部によって、１時刻前に得られた１走査分の反射点座標が格納される。レジスタＢには、次の走査（現時刻の走査）の反射点座標が１点ずつ、レジスタ制御部によって格納される。なお、処理開始の指示は、制御部２８によって行われる。

そして、複数の演算器２０によって、レジスタＢの座標と、レジスタＡ０〜Ａ６３が保持している座標との間で移動ベクトルが求められる。６４個の移動ベクトルが同時並列に求められる。複数の比較器７２０によって、範囲指定レジスタ７１８の内容に基づいて、複数の演算器２０から求められた６４個の移動ベクトルの上位桁が、指定された範囲内であるか否かを判定し、肯定判定された場合に、動作許可信号をデコーダ７２２に出力する。

動作許可信号が入力された複数のデコーダ７２２によって、対応する複数の演算器２０から求められた複数の移動ベクトルの、制御部２８より指定された部分桁の値から、それぞれの投票先（ヒストグラムの対応するビン）のカウンタＣを特定し、投票先の指示があったカウンタＣのカウント値を＋１する。

制御部２８によって、現時刻のレーザ走査による１ライン分の反射点座標から求められる移動ベクトルについて、カウント値のカウントアップが終了したと判断されると、制御部２８は、比較器ツリー２４に対して、最大のカウント値を有するカウンタＣを特定して、特定されたカウンタＣに対応するカウント値及び移動ベクトル（部分桁）を出力するように指示する。

そして、結合部７２４により、比較器ツリー２４から出力された移動ベクトル（部分桁）と、当該部分桁より上位の上位桁の値とを結合して求めた移動ベクトルを出力する。

そして、出力部２６により、結合部７２４から出力された移動ベクトルを、自車運動の推定値として出力する。

このとき、制御部２８によって、図１４に示す部分桁指定処理ルーチンが実行される。まず、ステップＳ７００において、複数のデコーダ７２２に対して上位の部分桁を指定すると共に、当該上位の部分桁に応じたカウンタＣと部分桁の値との対応関係を設定する。このとき、結合部７２４に対しては、結合する上位桁がないものとして設定される。

そして、ステップＳ７０２において、結合部７２４からの複数回の出力に基づいて、所定回数連続して同じ出力であるか否かを判定する。同じ出力が所定回数連続してなかった場合には、自車運動推定結果の信頼度が高くないと判断し、上記ステップＳ７００に戻るが、一方、同じ出力が所定回数連続した場合には、自車運動推定結果の信頼度が高いと判断し、ステップＳ７０４へ移行する。

ステップＳ７０４では、複数のデコーダ７２２に対して中位の部分桁を指定すると共に、当該中位の部分桁に応じたカウンタＣと部分桁の値との対応関係を設定する。このとき、結合部７２４に対しては、上記ステップＳ７０２で得られた結合部７２４からの出力が、結合する上位桁として設定される。

そして、ステップＳ７０６において、結合部７２４からの複数回の出力に基づいて、所定回数連続して同じ出力であるか否かを判定する。同じ出力が所定回数連続してなかった場合には、自車運動推定結果の信頼度が高くないと判断し、上記ステップＳ７００に戻るが、一方、同じ出力が所定回数連続した場合には、自車運動推定結果の信頼度が高いと判断し、ステップＳ７０８へ移行する。

ステップＳ７０８では、複数のデコーダ７２２に対して下位の部分桁を指定すると共に、当該下位の部分桁に応じたカウンタＣの対応関係を設定する。このとき、結合部７２４に対しては、上記ステップＳ７０６で得られた結合部７２４からの出力が、結合する上位桁として設定される。

そして、ステップＳ７１０において、結合部７２４から所定個の出力を得たか否かを判定し、所定個の出力を得たと判定されると、上記ステップＳ７００へ戻り、部分桁としての上位桁の指定から再度開始する。

以上説明したように、第７の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標と、推定対象時点より１時刻前に得られたレーザの１走査分の複数の反射点座標とについて、それぞれの差分を演算して移動ベクトルを求め、上位桁が所定範囲内の移動ベクトルの部分桁の各値の度数を計数し、最大度数を計数したカウンタに基づいて、自車運動を推定することにより、簡易なハードウエア構成で、自車運動推定の実時間処理を実現することができる。

また、移動ベクトルの指定した部分桁をカウント対象とすることで、必要な精度に合わせた部分桁を指定することが可能となる。このことにより、最初は、上位の部分桁をカウント対象に指定することで、低い精度の自車運動推定を行い、次スキャンでは下位の部分桁をカウント対象に指定することで、順次精度を高める自車運動推定が可能となり、自車運動推定結果の演算精度を向上させることができる。

次に、本発明の第８の実施の形態について説明する。なお、第１の実施の形態、第５の実施の形態、及び第７の実施の形態と同様の構成となる部分については、上記の実施の形態と同様の符号を付して説明を省略する。

第８の実施の形態では、ベース移動ベクトルを差し引いた現時点の反射点座標を用いて、差分移動ベクトルを求めている点が、第７の実施の形態と異なっている。

図１５に示すように、第８の実施の形態に係る自車運動推定装置８１０は、レジスタＡ０〜Ａ６３と、レジスタＢＢと、ベース移動ベクトルを保持するレジスタＢＶと、演算器５１２と、レジスタＢと、複数の演算器２０と、範囲指定レジスタ４１８と、複数の比較器８２０と、カウンタＣ０〜Ｃ２５５と、複数のデコーダ７２２と、比較器ツリー２４と、演算器５２４と、出力部２６と、制御部２８と、を備えている。なお、比較器８２０が、判定器の一例である。演算器５２４が、結合部の一例である。

レジスタＢＢには、現在の反射点座標（ｘ、ｚ）が保持される。レジスタＢＶには、仮定されるベース移動ベクトルが保持される。

演算器５１２は、レジスタＢＢに保持された反射点座標から、ベース移動ベクトルを差し引いて、１時刻前の反射点座標を推定し、レジスタＢに保存する。

各演算器２０は、対応するレジスタＡが保持する座標とレジスタＢが保持している座標との間で引き算をし、２点間の差分移動ベクトルを求めて出力する。

各比較器８２０は、対応する演算器２０で求めた差分移動ベクトルの上位桁（例えば、上位３桁）が、０であるかを判定し、差分移動ベクトルの上位桁が、０である場合に、対応するデコーダ７２２が動作するように動作許可信号を出力する。ここで０とは、ヒストグラム対象外の上位桁に有意の桁が含まれていないことを意味し、差分移動ベクトルが正数の場合は上位桁が２進数で“０００・・・”であるかを判定することであるが、負数の場合は（２の補数表現であるとして）２進数で“１１１・・・”であるかを判定することに注意されたい。また、複数の比較器８２０による１スキャン分の比較判定が並列に行われる。なお、ヒストグラム対象外の上位桁の指定は、制御部２８により行われる。

また、カウンタＣ０〜Ｃ２５５は、差分移動ベクトル（ｄｘ、ｄｚ）の部分桁の値ごとに設けられている。差分移動ベクトルの部分桁の各値とカウンタＣ０〜Ｃ２５５との対応関係は、固定的に決めておくのではなく、指定できるようにしておく。例えば、設定１では差分移動ベクトル（０，０）をカウンタにＣ０に、差分移動ベクトル（０，１）をカウンタＣ１に、差分移動ベクトル（０，２）をカウンタＣ２・・・と対応させ、設定２では差分移動ベクトル（０，４）と（０，５）をカウンタＣ２・・・と対応させておき、設定１を使用するか設定２を使用するかは、実行時に選択できるように構成しておく。

各デコーダ７２２は、対応する比較器８２０から動作許可信号が入力された場合に、対応する演算器２０で求められた差分移動ベクトルの指定された部分桁の値に対応するカウンタＣを特定し、特定したカウンタＣの値をカウントアップする。複数のデコーダ２２による１スキャン分のカウントアップが並列に行われる。デコーダ７２２は、対応する比較器８２０から動作許可信号が入力されない場合には、求めた差分移動ベクトルの上位桁が０でないため、カウントアップ動作を行わない。

デコーダ７２２には、制御部２８から部分桁が指定される。例えば、差分移動ベクトル（ｄｘ、ｄｚ）の指定された部分桁（例えば、ｄｘの上位４桁目〜６桁目の中位３桁とｄｚの上位４桁目〜６桁目の中位３桁）を採用する。これによって、カウンタＣ０〜Ｃ２５５により、差分移動ベクトルの中位３桁の値に関するヒストグラムが作成される。また、部分桁の指定は、中位桁だけに限らず、当該中位桁より下位部分（例えば下位４桁目〜３桁目の２桁や、更に下位桁）も指定される。

比較器ツリー２４から出力される差分移動ベクトルは、レジスタＢ（１時刻前を推定した座標）までの移動ベクトルであるため、推定に用いたベース移動ベクトルに差分移動ベクトルを加えたものが自車運動の推定値となる。したがって、演算器５２４は、比較器ツリー２４から出力される差分移動ベクトルの部分桁に、１時刻前の自車運動の推定値である移動ベクトルを加算して、求められた移動ベクトルを出力する。これによって、差分移動ベクトルの部分桁の値に、部分桁より上位桁の値が結合された移動ベクトルが求められる。

また、制御部２８は、指定する差分移動ベクトルの部分桁、及びカウンタＣと部分桁の値との対応関係を適応的に設定する。

例えば、制御部２８は、差分移動ベクトルの部分桁として、中位桁（上位４桁目〜６桁の３桁）を指定すると共に、カウンタＣと部分桁の値との対応関係を設定する。数回のスキャンにわたって、毎回同じ自車運動推定結果が得られる場合に、制御部２８は、差分移動ベクトルの部分桁として、上記中位桁より下位の部分桁（上位７桁目〜９桁の３桁）を指定すると共に、カウンタＣと部分桁の値との対応関係を設定する。

また、数回のスキャンにわたって、毎回同じ自車運動推定結果が得られる場合に、制御部２８は、差分移動ベクトルの部分桁として、さらに下位の部分桁（上位１０桁目〜１２桁の３桁）を指定すると共に、カウンタＣと部分桁の値との対応関係を設定する。

次に、第８の実施の形態の自車運動推定装置８１０の動作について説明する。

まず、レジスタＡ０〜Ａ６３レジスタには、レジスタ制御部によって、１時刻前に得られた１走査分の反射点座標が格納される。また、レジスタ制御部によって、１時刻前に推定された自車運動の推定値である移動ベクトルが、レジスタＢＶに格納される。レジスタＢＢには、次の走査（現時刻の走査）の反射点座標が１点ずつ、レジスタ制御部によって格納される。そして、演算器５１２によって、レジスタＢＢの反射点座標から、レジスタＢＶの移動ベクトルを差し引いた座標を求め、レジスタＢに格納する。

そして、複数の演算器２０によって、レジスタＢの座標と、レジスタＡ０〜Ａ６３が保持している座標との間で差分移動ベクトルが求められる。６４個の差分移動ベクトルが同時並列に求められる。複数の比較器８２０によって、複数の演算器２０から求められた６４個の差分移動ベクトルの指定された上位桁が、０であるか否かを判定し、肯定判定された場合に、動作許可信号を対応するデコーダ７２２に出力する。

動作許可信号が入力された複数のデコーダ７２２によって、対応する複数の演算器２０から求められた複数の差分移動ベクトルの、制御部２８より指定された部分桁の値から、それぞれの投票先（ヒストグラムの対応するビン）のカウンタＣを特定し、投票先の指示があったカウンタＣのカウント値を＋１する。

制御部２８によって、現時刻のレーザ走査による１ライン分の反射点座標を用いて求められる差分移動ベクトルについて、カウント値のカウントアップが終了したと判断されると、制御部２８は、比較器ツリー２４に対して、最大のカウント値を有するカウンタＣを特定して、特定されたカウンタＣに対応するカウント値及び差分移動ベクトルの部分桁の値を出力するように指示する。

そして、演算器５２４により、比較器ツリー２４から出力された差分移動ベクトルの部分桁と、現時点のベース移動ベクトルとして用いられている１時刻前の移動ベクトルとを加算して求めた移動ベクトルを出力する。

出力部２６により、演算器５２４から出力された移動ベクトルを、自車運動の推定値として出力する。

このとき、制御部２８によって、上記第７の実施の形態と同様に部分桁指定処理ルーチンが実行され、複数のデコーダ７２２に対して、中位の部分桁から、順次、下位の部分桁が指定される。

以上説明したように、第８の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標から、ベース移動ベクトルを差し引いた反射点座標と、推定対象時点より１時刻前に得られたレーザの１走査分の複数の反射点座標とについて、それぞれの差分を演算して差分移動ベクトルを求め、所定範囲内の差分移動ベクトルの部分桁の値の度数を計数し、最大度数を計数したカウンタに基づいて、自車運動を推定することにより、簡易なハードウエア構成で、自車運動推定の実時間処理を実現することができる。

なお、上記の第５の実施の形態及び第８の実施の形態では、ベース移動ベクトルとして、直前に推定された自車運動量を用いる場合を例に説明したが、これに限定されるものではない。例えば、予め定められた基礎運動量を、ベース移動ベクトルとして用いてもよい。この場合には、演算器５２４により、比較器ツリー２４から出力された差分移動ベクトルと、ベース移動ベクトルとして用いられる基礎運動量を加算して求めた移動ベクトルを出力すればよい。

次に、本発明の第９の実施の形態について説明する。なお、第１の実施の形態、第６の実施の形態、第７の実施の形態、及び第８の実施の形態と同様の構成となる部分については、上記の実施の形態と同様の符号を付して説明を省略する。

第９の実施の形態では、並進運動だけでなく、回転運動も推定している点と、レジスタＡ０〜Ａ６３にデータをセットする方法とが、第８の実施の形態と異なっている。

図１６に示すように、第９の実施の形態に係る自車運動推定装置９１０は、レジスタＡ０〜Ａ６３と、レジスタＢＢと、複数のレジスタＢ０〜Ｂ６３と、レジスタＢＶＡと、レジスタＢと、座標変換部６１２と、複数の演算器２０と、複数の比較器８２０と、カウンタＣ０〜Ｃ２５５と、複数のデコーダ７２２と、比較器ツリー２４と、演算器６２４と、出力部２６と、制御部２８と、を備えている。演算器６２４が、結合部の一例である。

座標変換部６１２は、レジスタＢＢに保持された反射点座標から、ベース移動ベクトルの並進・回転成分（ｄｘ、ｄｚ、θ）を差し引いて、１時刻前の反射点座標を推定すると共に、制御部２８の制御により、推定された１時刻前の反射点座標を、仮定した回転運動量θの複数候補の各々について、当該候補の分だけ差し引いた座標に変換し、レジスタＢに保持する。

各演算器２０は、回転運動量θの複数候補の各々について、対応するレジスタＡが保持する座標と、レジスタＢ（座標変換部３２０によって変換された当該候補の分だけ差し引いた座標）との間で引き算をし、２点間の差分移動ベクトル（ｄｘ、ｄｚ）を求めて出力する。

各比較器８２０は、対応する演算器２０で求めた差分移動ベクトルの上位桁（例えば、上位３桁）が、０であるかを判定し、差分移動ベクトルの上位桁が、０である場合に、対応するデコーダ７２２が動作するように動作許可信号を出力する。

また、カウンタＣ０〜Ｃ２５５は、差分移動ベクトル（ｄｘ、ｄｚ）の部分桁の値及び回転運動量θの値の組み合わせ毎に設けられている。差分移動ベクトルの部分桁及び回転運動量の各組み合わせとカウンタＣ０〜Ｃ２５５との対応関係は、固定的に決めておくのではなく、指定できるようにしておく。

各デコーダ７２２は、対応する比較器７２０から動作許可信号が入力された場合に、対応する演算器２０で求められた差分移動ベクトル（ｄｘ、ｄｚ）の指定された部分桁の値と、その差分移動ベクトルを求めるのに用いられた反射点座標の変換に用いられた回転運動量θの候補との組み合わせに対応するカウンタＣを特定し、特定したカウンタＣの値をカウントアップする。デコーダ７２２は、対応する比較器８２０から動作許可信号が入力されない場合には、求めた差分移動ベクトルの上位桁が０でないため、カウントアップ動作を行わない。

デコーダ７２２には、制御部２８から部分桁、及びカウンタＣと部分桁の値との対応関係が指定される。

制御部２８は、レーザレーダ１２から反射点座標が入力されて、１時刻前の反射点座標が推定され、更に回転運動量の候補を差し引いた反射点座標に変換される毎に、複数のデコーダ７２２による並列処理が行われるようにタイミング制御する。これにより、レーザレーダ１２から反射点座標が入力されて１時刻前の反射点座標が推定され、更に回転運動量の候補を差し引いた反射点座標に変換される毎に、各演算器２０による演算、複数の比較器８２０による上位桁の判定、複数のデコーダ７２２による並列処理、及びカウンタＣ０〜Ｃ２５５のカウントアップが繰り返し行われる。これを１スキャン分の反射点座標（例えば６４点）について繰り返すことにより、１時刻前ｔ0の各反射点から、１時刻前の反射点座標の推定値から回転運動量の各候補を差し引いて変換された現時刻ｔ1の各反射点への全組み合せ分の差分移動ベクトルを求めると共に、指定範囲外の差分移動ベクトルを排除し、カウンタＣ０〜Ｃ２５５のカウント値により、指定範囲内の差分移動ベクトル及び回転運動量の組み合わせのヒストグラムが作成される。

比較器ツリー２４における基本ユニットの出力は、カウント値と、差分移動ベクトル（ｄｘ、ｄｚ）の部分桁の値および回転運動量θの組み合わせとのセットである。

制御部２８は、１スキャン分の反射点座標についての回転運動量の各候補を用いて求められた差分移動ベクトルに対するカウント処理が終わったところで、最大のカウント値をもつカウンタＣを探索するため、比較器ツリー２４を動作させる。これによって、１スキャン分の反射点座標についての回転運動量の各候補を用いて求められた差分移動ベクトルに対するカウント処理の結果から、最大カウント値を有するカウンタＣに対応する差分移動ベクトル（ｄｘ、ｄｚ）の部分桁の値および回転運動量θが出力される。

演算器６２４は、比較器ツリー２４から出力される差分移動ベクトル（ｄｘ、ｄｚ、θ）の部分桁の値に、１時刻前の自車運動の推定値である移動ベクトルの並進成分（ｄｘ、ｄｚ、θ）を加算して、求められた並進成分（ｄｘ、ｄｚ、θ）と、比較器ツリー２４から出力された回転運動量θとからなる移動ベクトル（ｄｘ、ｄｚ、θ）を出力する。これによって、差分移動ベクトルの部分桁の値に、部分桁より上位桁の値が結合された移動ベクトルが求められる。

出力部２６は、演算器６２４から出力される移動ベクトル（ｄｘ、ｄｚ、θ）を、自車運動の推定値として出力する。

反射点座標は、レーザレーダ１２から時系列に入力されて来るので、新しい反射点座標が入力されるつど、制御部２８によって、座標データをレジスタＢＢ、Ｂ６３〜Ｂ０内でシフトさせ（ＢＢ→Ｂ６３→Ｂ６２…Ｂ１→Ｂ０）、レジスタＢ０〜Ｂ６３に１スキャン分の座標をデータセットする。制御部２８は、１スキャンの終了または開始のタイミングでレジスタＢ０〜Ｂ６３の内容をレジスタＡ０〜Ａ６３にコピーし、レジスタＡ０〜Ａ６３に必要な反射点座標のデータをセットする。

また、制御部２８は、上記第８の実施の形態と同様に、指定する差分移動ベクトルの部分桁、及びカウンタＣと部分桁の値との対応関係を適応的に設定する。

次に、第９の実施の形態の自車運動推定装置９１０の動作について説明する。

まず、制御部２８によって、レジスタＡ０〜Ａ６３には、レジスタＢ０〜Ｂ６３の内容がコピーされ、１時刻前に得られた１走査分の反射点座標が格納される。また、レジスタ制御部によって、１時刻前に推定された自車運動の推定値である移動ベクトルの並進・回転成分が、レジスタＢＶＡに格納される。レジスタＢＢには、次の走査（現時刻の走査）の反射点座標が１点ずつ、制御部２８によって格納される。

そして、座標変換部６１２によって、レジスタＢＢの反射点座標から、レジスタＢＶＡの移動ベクトルを差し引くと共に、回転運動量θの候補の各々について、回転運動量θの候補を差し引いた座標に各々変換する。回転運動量θの各候補は制御部２８から指定される。

そして、複数の演算器２０によって、レジスタＢＢの座標が変換される毎に、変換された座標と、レジスタＡ０〜Ａ６３が保持している座標との間で差分移動ベクトルが求められる。６４個の差分移動ベクトルが同時並列に求められる。複数の比較器８２０によって、複数の演算器２０から求められた６４個の差分移動ベクトルの指定された上位桁が、０であるか否かを判定し、肯定判定された場合に、動作許可信号をデコーダ７２２に出力する。

動作許可信号が入力された複数のデコーダ７２２によって、対応する複数の演算器２０から求められた複数の差分移動ベクトルの部分桁の値及び回転運動量の組み合わせから、それぞれの投票先（ヒストグラムの対応するビン）のカウンタＣを決定し、投票先の指示があったカウンタＣのカウント値を＋１する。

制御部２８は、現時刻のレーザ走査による１ライン分の反射点座標の各々に対する変換後の座標の各々を用いて求められた差分移動ベクトルについて、カウント値のカウントアップが終了したと判断すると、制御部２８は、比較器ツリー２４に対して、最大のカウント値を有するカウンタＣを特定して、特定されたカウンタＣに対応するカウント値、差分移動ベクトル（ｄｘ、ｄｚ）の部分桁の値、及び回転運動量θを出力するように指示する。

そして、演算器６２４により、比較器ツリー２４から出力された差分移動ベクトル（ｄｘ、ｄｚ）と、現時点のベース移動ベクトルとして用いられている１時刻前の移動ベクトルの並進成分（ｄｘ、ｄｚ）とを加算して求めた並進成分（ｄｘ、ｄｚ）と、同じく、比較器ツリー２４から出力された回転運動量θと１時刻前の移動ベクトルの回転成分θとを加算して求めた回転成分とからなる移動ベクトル（ｄｘ、ｄｚ、θ）を出力する。

出力部２６により、演算器６２４から出力された移動ベクトルを、自車運動の推定値として出力する。

このとき、制御部２８によって、上記第７の実施の形態と同様に部分桁指定処理ルーチンが実行され、複数のデコーダ７２２に対して、中位の部分桁から、順次、下位の部分桁が指定される。

以上説明したように、第９の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標から、ベース移動ベクトルを差し引くと共に、仮定した回転運動量の候補を差し引いた反射点座標と、推定対象時点より１時刻前に得られたレーザの１走査分の複数の反射点座標とについて、それぞれの差分を演算して差分移動ベクトルを求め、所定範囲内の差分移動ベクトルの部分桁の値及び回転運動量の候補の組み合わせの度数を計数し、最大度数を計数したカウンタに基づいて、回転運動量を含む自車運動を推定することにより、簡易なハードウエア構成で、回転運動量を含む自車運動推定の実時間処理を実現することができる。

なお、上記の第６の実施の形態及び第９の実施の形態では、ベース移動ベクトルとして、直前に推定された自車運動量を用いる場合を例に説明したが、これに限定されるものではない。例えば、予め定められた基礎運動量を、ベース移動ベクトルとして用いてもよい。この場合には、演算器６２４により、比較器ツリー２４から出力された差分移動ベクトルと、ベース移動ベクトルとして用いられる基礎運動量を加算して、移動ベクトルの並進成分を求めればよい。

なお、上記の第１の実施の形態〜第９の実施の形態において、１スキャン分の反射点座標の数だけ演算器を設ける場合を例に説明したが、これに限定されるものではなく、演算器を少なくとも１つ設けるようにすればよい。この場合には、少なくとも１つの演算器により、現時点の反射点座標と、レジスタＡ０〜Ａ６３の反射点座標との各組み合わせについて移動ベクトル又は差分移動ベクトルを順次求めるようにすればよい。更に、デコーダを少なくとも１つ設けるようにしてもよい。この場合には、少なくとも１つのデコーダにより、演算器により順次演算される移動ベクトル又は差分移動ベクトルに対応するカウンタを順次特定して、カウンタＣの値をカウントアップするようにすればよい。
また、１スキャン分の全ての反射点座標を用いて演算する場合を例に説明したが、これに限定されるものではなく、１スキャン分の一部の反射点座標を用いて演算するようにしてもよい。例えば、レーザレーダが１走査あたり１２８点の反射点座標を出力する場合に、そのうちの１００点の反射点座標のみを演算に用いるようにしてもよい。

１０、２１０、３１０、４１０、５１０、６１０、７１０、８１０、９１０ 自車運動推定装置
１２ レーザレーダ
２０、５１２、５２４、６２４演算器
２２、４２２、７２２ デコーダ
２４ 比較器ツリー
２６ 出力部
２８ 制御部
３２０、６１２座標変換部
４１８、７１８範囲指定レジスタ
４２０、７２０、８２０ 比較器
７２４ 結合部
Ａ０〜Ａ６３ レジスタ
Ｂ、ＢＢ レジスタ
Ｂ０〜Ｂ６３ レジスタ
ＢＶ レジスタ
Ｃ０〜Ｃ２５５カウンタ

Claims (7)

1. 移動体に搭載され、かつ、レーザを走査して前記レーザの各反射点座標を計測するレーザ装置から順次得られる推定対象時点のレーザの走査に対する前記反射点座標を保持する第１レジスタと、
   前記推定対象時点より前に得られた前記レーザの１走査分の複数の前記反射点座標を保持する複数の第２レジスタと、
   前記第１レジスタに順次保持される前記レーザの１走査分の反射点座標の各々について、前記反射点座標と、前記複数の第２レジスタに保持された前記複数の反射点座標の各々との差分を各々演算する演算器と、
   前記レーザの１走査分に対する、前記演算器の出力値に関する複数の値の度数を計数するための複数のカウンタと、
   前記演算器の各出力値について、前記演算器の出力値に対応する度数を計数する、前記複数のカウンタのうちの何れか１つのカウンタを指定するデコーダと、
   前記複数のカウンタのうち、最大度数を計数した前記カウンタを特定する比較器と、
   前記比較器によって特定された前記カウンタに対応する前記演算器の出力値を、前記移動体の運動の推定結果とする運動推定部と、
   を含む移動体運動推定装置。
2. 前記演算器は、複数の演算器であって、
   前記複数の演算器は、前記第１レジスタに順次保持される前記レーザの１走査分の反射点座標の各々について、前記反射点座標と、前記複数の第２レジスタに保持された前記複数の反射点座標との差分を並列に演算する請求項１記載の移動体運動推定装置。
3. 前記デコーダは、複数のデコーダであって、
   前記複数のデコーダは、前記複数の演算器の出力値に対応する度数を計数する前記カウンタを並列に指定する請求項２記載の移動体運動推定装置。
4. 仮定する前記移動体の複数の回転量の候補の各々について、前記レーザ装置から順次得られる前記推定対象時点のレーザの走査に対する反射点座標から、前記回転量の候補を差し引いた座標に順次変換する座標変換部を更に含み、
   前記第１レジスタは、前記座標変換部によって順次変換された座標を保持し、
   前記複数のカウンタは、前記演算器の出力値に関する複数の値及び前記回転量の候補の各組み合わせに対する度数を計数するためのものであって、
   前記デコーダは、前記演算器の出力値及び前記出力値の演算に用いられた前記反射点座標の変換に用いられた前記回転量の候補の組み合わせに対応する度数を計数する前記カウンタを指定し、
   前記運動推定部は、前記比較器によって特定された前記カウンタに対応する前記演算器の出力値及び前記回転量の候補を、前記移動体の運動の推定結果とする請求項１〜請求項３の何れか１項記載の移動体運動推定装置。
5. 前記演算器の出力値が所定範囲内であるか否かを判定する判定器を更に含み、
   前記デコーダは、前記判定器によって肯定判定された前記演算器の出力値に対応する度数を計数する前記カウンタを指定する請求項１〜請求項４の何れか１項記載の移動体運動推定装置。
6. 前記演算器は、複数の演算器であって、
   前記複数の演算器は、前記第１レジスタに順次保持される前記レーザの１走査分の反射点座標の各々について、前記反射点座標と、前記複数の第２レジスタに保持された前記複数の反射点座標との差分を並列に演算し、
   前記デコーダは、複数のデコーダであって、
   前記判定器は、複数の判定器であって、
   前記複数の判定器は、前記複数の演算器の出力値が所定範囲内であるか否かを並列に判定し、
   前記複数のデコーダは、前記複数の演算器のうち、前記判定器によって肯定判定された前記演算器の出力値に対応する度数を計数する前記カウンタを並列に指定する請求項５記載の移動体運動推定装置。
7. 前記複数のカウンタは、前記レーザの１走査分に対する、前記演算器の出力値の指定された部分桁に関する複数の値の度数を計数するためのものであって、
   前記デコーダは、前記演算器の出力値の前記指定された部分桁の値に対応する度数を計数する前記カウンタを指定し、
   前記運動推定部は、前記比較器によって特定されたカウンタに対応する前記演算器の出力値の部分桁の値に、予め求められた前記部分桁より上位桁の値を結合して、前記移動体の運動の推定結果とする請求項１〜請求項６記載の移動体運動推定装置。