JP5935377B2 - Mobile body motion estimation device - Google Patents

Mobile body motion estimation device Download PDF

Info

Publication number
JP5935377B2
JP5935377B2 JP2012034511A JP2012034511A JP5935377B2 JP 5935377 B2 JP5935377 B2 JP 5935377B2 JP 2012034511 A JP2012034511 A JP 2012034511A JP 2012034511 A JP2012034511 A JP 2012034511A JP 5935377 B2 JP5935377 B2 JP 5935377B2
Authority
JP
Japan
Prior art keywords
counter
reflection point
movement vector
point coordinates
register
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Expired - Fee Related
Application number
JP2012034511A
Other languages
Japanese (ja)
Other versions
JP2013170906A (en
Inventor
太田 充彦
充彦 太田
清澄 城殿
清澄 城殿
峰樹 曽我
峰樹 曽我
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Toyota Central R&D Labs Inc
Original Assignee
Toyota Central R&D Labs Inc
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Toyota Central R&D Labs Inc filed Critical Toyota Central R&D Labs Inc
Priority to JP2012034511A priority Critical patent/JP5935377B2/en
Publication of JP2013170906A publication Critical patent/JP2013170906A/en
Application granted granted Critical
Publication of JP5935377B2 publication Critical patent/JP5935377B2/en
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Landscapes

  • Optical Radar Systems And Details Thereof (AREA)
  • Traffic Control Systems (AREA)

Description

本発明は、移動体運動推定装置に係り、特に、レーダ装置を用いて、移動体の運動を推定する移動体運動推定装置に関する。   The present invention relates to a mobile body motion estimation apparatus, and more particularly to a mobile body motion estimation apparatus that estimates a motion of a mobile body using a radar apparatus.

特許文献1には、自車運動推定について、レーザレーダにより異なる時刻で検出された物体の位置の変化に基づいて推定することが記載されているが、具体的な演算法が開示されていない。   Japanese Patent Application Laid-Open No. 2004-228561 describes that the own vehicle motion estimation is estimated based on a change in the position of an object detected at different times by a laser radar, but a specific calculation method is not disclosed.

また、従来技術として、図17に示すように、レーザレーダにより得られた過去の反射点マップから自車運動を仮定して現時点の予測マップを複数作成し、実際に得られた現時点の反射点マップと一致度の高い予測マップを探索し、最も一致度の高い予測マップを作成するときに仮定した運動量を推定結果として採用する方法が知られている。   As a conventional technique, as shown in FIG. 17, a plurality of current prediction maps are created by assuming own vehicle motion from past reflection point maps obtained by a laser radar. A method is known in which a prediction map having a high degree of coincidence with a map is searched, and the momentum assumed when the prediction map having the highest degree of coincidence is created is adopted as an estimation result.

特開2010−160777号公報JP 2010-160777 A

しかしながら、上記の従来方法では、演算量が多く、実時間処理の実現が困難である、という問題がある。特に、自車運動推定の精度を上げるためには、予測マップのステップを細かくする必要があるが、予測マップを細かく作成すると、予測マップ数が増え、それに対して演算量が増大してしまう。   However, the above-described conventional method has a problem that the amount of calculation is large and real-time processing is difficult to realize. In particular, in order to increase the accuracy of the vehicle motion estimation, it is necessary to make the step of the prediction map fine. However, if the prediction map is made finely, the number of prediction maps increases, and the amount of calculation increases.

そこで、専用のハードウエアによる高速化も選択肢としては考えられるが、以下の理由により、そのままのアルゴリズムではハードウエア実現は困難である。   Therefore, speeding up with dedicated hardware can be considered as an option, but for the following reasons, it is difficult to implement hardware with the algorithm as it is.

予測マップを候補移動量にしたがって作成するため、演算データとして大量の予測マップを保存しておく必要があり、ハードウエアの構成要素としてはメモリを使用せざるを得ない。メモリを使用するとアクセス競合の問題があるため、並列処理を採用しても処理時間短縮効果は限られている。   In order to create a prediction map according to the amount of candidate movement, it is necessary to store a large amount of the prediction map as operation data, and a memory must be used as a hardware component. Since there is a problem of access contention when using a memory, even if parallel processing is employed, the effect of reducing the processing time is limited.

また、別の方法として、予測マップごとにPE(Processing Element:演算器とメモリ)を個別に割り当てれば、PE数だけの並列度が得られて処理時間の短縮も期待できるが、この場合は、複数のPE全体をまとめる方法や各PEへのデータの配信方法など、解決しなければならない課題が多く、ハードウエアを複雑化してしまう。   As another method, if a PE (Processing Element: a computing unit and a memory) is individually assigned for each prediction map, a degree of parallelism corresponding to the number of PEs can be obtained, and a reduction in processing time can be expected. There are many problems that need to be solved, such as a method for grouping a plurality of PEs and a method for distributing data to each PE, which complicates the hardware.

その点、GPUの様な既に実用化されているSIMDプロセッサを採用して、ソフトウエアでアルゴリズムを実現する方法も考えられるが、現状のGPUは消費電力が大きく、レーザレーダに組み込むのは困難である。   In that respect, it is possible to adopt a software-implemented SIMD processor such as a GPU, but the current GPU consumes a lot of power and is difficult to incorporate into a laser radar. is there.

本発明は、上記問題を解決すべく成されたもので、簡易なハードウエア構成で、移動体の運動推定の実時間処理を実現することができる移動体運動推定装置を提供することを目的とする。   The present invention has been made to solve the above-described problem, and an object thereof is to provide a mobile body motion estimation apparatus that can realize real-time processing of mobile body motion estimation with a simple hardware configuration. To do.

上記目的を達成するために本発明に係る移動体運動推定装置は、移動体に搭載され、かつ、レーザを走査して前記レーザの各反射点座標を計測するレーザ装置から順次得られる推定対象時点のレーザの走査に対する前記反射点座標を保持する第1レジスタと、前記推定対象時点より前に得られた前記レーザの1走査分の複数の前記反射点座標を保持する複数の第2レジスタと、前記第1レジスタに順次保持される前記レーザの1走査分の反射点座標の各々について、前記反射点座標と、前記複数の第2レジスタに保持された前記複数の反射点座標の各々との差分を各々演算する演算器と、前記演算器の出力値との対応関係が予め定められた複数のカウンタであって、前記カウンタ毎に、前記レーザの1走査分に対する、前記カウンタに対応する前記演算器の出力値の度数を計数するための複数のカウンタと、前記演算器の各出力値について、前記演算器の出力値に対応する度数を計数する、前記複数のカウンタのうちの何れか1つのカウンタを指定するデコーダと、前記複数のカウンタのうち、最大度数を計数した前記カウンタを特定する比較器と、前記比較器によって特定された前記カウンタに対応する前記演算器の出力値を、前記移動体の運動の推定結果とする運動推定部と、を含んで構成されている。 In order to achieve the above object, a moving object motion estimation device according to the present invention is mounted on a moving object, and is sequentially estimated from a laser device that scans a laser and measures each reflection point coordinate of the laser. A first register for holding the reflection point coordinates for the laser scanning of the laser, a plurality of second registers for holding a plurality of reflection point coordinates for one scan of the laser obtained before the estimation target time point, For each of the reflection point coordinates for one scan of the laser sequentially held in the first register, the difference between the reflection point coordinates and each of the plurality of reflection point coordinates held in the plurality of second registers. each and calculator for calculating a corresponding relation between the output value of the arithmetic unit comprising: a plurality of counters predetermined in said counter each, for one scan of the laser, to correspond to the counter A plurality of counters for counting the frequency of the output value of the arithmetic unit, for each output value of the arithmetic unit, counts the frequency corresponding to the output value of the arithmetic unit, one of the plurality of counters A decoder for designating one counter, a comparator for identifying the counter that has counted the maximum frequency among the plurality of counters, and an output value of the arithmetic unit corresponding to the counter identified by the comparator, A motion estimator that serves as a motion estimation result of the moving body.

本発明に係る移動体運動推定装置では、レーザ装置から順次得られる推定対象時点のレーザの走査に対する前記反射点座標が第1レジスタに保持され、前記推定対象時点より前に得られた前記レーザの1走査分の複数の前記反射点座標が複数の第2レジスタに保持される。   In the moving body motion estimation apparatus according to the present invention, the reflection point coordinates for the laser scanning at the estimation target time points sequentially obtained from the laser device are held in a first register, and the laser of the laser obtained before the estimation target time point is stored. A plurality of reflection point coordinates for one scan are held in a plurality of second registers.

そして、演算器によって、前記第1レジスタに順次保持される前記レーザの1走査分の反射点座標の各々について、前記反射点座標と、前記複数の第2レジスタに保持された前記複数の反射点座標の各々との差分を各々演算する。デコーダによって、前記演算器の各出力値について、前記演算器の出力値に対応する度数を計数する、前記複数のカウンタのうちの何れか1つのカウンタを指定する。これによって、レーザの1走査分に対する、前記演算器の出力値の度数が複数のカウンタによって計数される。 Then, for each of the reflection point coordinates for one scan of the laser sequentially held in the first register by the arithmetic unit, the reflection point coordinates and the plurality of reflection points held in the plurality of second registers. The difference from each of the coordinates is calculated. The decoder designates one of the plurality of counters that counts the frequency corresponding to the output value of the computing unit for each output value of the computing unit. Thereby, the frequency of the output value of the computing unit for one scan of the laser is counted by a plurality of counters.

そして、比較器によって、前記複数のカウンタのうち、最大度数を計数した前記カウンタを特定し、運動推定部によって、前記比較器によって特定された前記カウンタに対応する前記演算器の出力値を、前記移動体の運動の推定結果とする。   Then, the comparator specifies the counter that has counted the maximum frequency among the plurality of counters, and the motion estimation unit outputs the output value of the arithmetic unit corresponding to the counter specified by the comparator, It is assumed that the motion of the moving object is estimated.

このように、レーザ装置から順次得られる推定対象時点のレーザの走査に対する前記反射点座標と、推定対象時点より前に得られたレーザの1走査分の複数の前記反射点座標とについて、それぞれの差分を演算して、演算器の出力値の各値の度数を計数し、最大度数を計数したカウンタに基づいて、移動体の運動を推定することにより、簡易なハードウエア構成で、移動体の運動推定の実時間処理を実現することができる。   As described above, each of the reflection point coordinates for the scanning of the laser at the estimation target time obtained sequentially from the laser device and the plurality of reflection point coordinates for one scan of the laser obtained before the estimation target time are respectively By calculating the difference, counting the frequency of each value of the output value of the computing unit, and estimating the movement of the moving body based on the counter that has counted the maximum frequency, with a simple hardware configuration, Real-time processing of motion estimation can be realized.

本発明に係る前記演算器は、複数の演算器であって、前記複数の演算器は、前記第1レジスタに順次保持される前記レーザの1走査分の反射点座標の各々について、前記反射点座標と、前記複数の第2レジスタに保持された前記複数の反射点座標との差分を並列に演算するようにすることができる。また、本発明に係る前記デコーダは、複数のデコーダであって、前記複数のデコーダは、前記複数の演算器の出力値に対応する度数を計数する前記カウンタを並列に指定するようにすることができる。これによって、処理の高速化を図ることができる。   The computing unit according to the present invention is a plurality of computing units, and the plurality of computing units are configured to reflect the reflection point for each of the reflection point coordinates for one scan of the laser sequentially held in the first register. Differences between the coordinates and the plurality of reflection point coordinates held in the plurality of second registers can be calculated in parallel. The decoder according to the present invention may be a plurality of decoders, and the plurality of decoders may specify the counters for counting the frequencies corresponding to the output values of the plurality of arithmetic units in parallel. it can. As a result, the processing speed can be increased.

本発明に係る移動体運動推定装置は、仮定する前記移動体の複数の回転量の候補の各々について、前記レーザ装置から順次得られる前記推定対象時点のレーザの走査に対する反射点座標から、前記回転量の候補を差し引いた座標に順次変換する座標変換部を更に含み、前記第1レジスタは、前記座標変換部によって順次変換された座標を保持し、前記複数のカウンタは、前記演算器の出力値及び前記回転量の候補の組み合わせとの対応関係が予め定められ、前記カウンタは、前記カウンタに対応する前記演算器の出力値及び前記回転量の候補の組み合わせの度数を計数するためのものであって、前記デコーダは、前記演算器の出力値及び前記出力値の演算に用いられた前記反射点座標の変換に用いられた前記回転量の候補の組み合わせに対応する度数を計数する前記カウンタを指定し、前記運動推定部は、前記比較器によって特定された前記カウンタに対応する前記演算器の出力値及び前記回転量の候補を、前記移動体の運動の推定結果とするようにすることができる。これによって、回転量を含めた移動体の運動を推定することができる。 The moving body motion estimation apparatus according to the present invention is configured to calculate, based on a reflection point coordinate for each of a plurality of rotation amount candidates of the moving body to be assumed, a reflection point coordinate with respect to a laser scan at the estimation target time point sequentially obtained from the laser apparatus. A coordinate conversion unit that sequentially converts the coordinates into the coordinates obtained by subtracting the quantity candidates; the first register holds the coordinates that are sequentially converted by the coordinate conversion unit; and the plurality of counters are output values of the computing unit. correspondence between the combination of beauty the rotation amount candidates predetermined said counter, for counting the frequency of combinations of output values and the rotation amount of the candidate of the arithmetic unit corresponding to the counter The decoder corresponds to the combination of the output value of the calculator and the candidate for the amount of rotation used for the conversion of the reflection point coordinates used for the calculation of the output value. The motion estimation unit is configured to estimate the motion value of the moving object based on the output value of the computing unit and the rotation amount candidate corresponding to the counter specified by the comparator. As a result. Thereby, it is possible to estimate the motion of the moving body including the rotation amount.

本発明に係る移動体運動推定装置は、前記演算器の出力値が所定範囲内であるか否かを判定する判定器を更に含み、前記デコーダは、前記判定器によって肯定判定された前記演算器の出力値に対応する度数を計数する前記カウンタを指定するようにすることができる。これによって、カウント対象とする運動量に予め範囲を設け、無用のカウント処理を排除すると共に、推定結果の信頼性を向上させることができる。   The mobile body motion estimation apparatus according to the present invention further includes a determiner that determines whether or not an output value of the calculator is within a predetermined range, and the decoder is the calculator that has been determined to be positive by the determiner. The counter that counts the frequency corresponding to the output value can be designated. As a result, a range is provided in advance for the amount of exercise to be counted, and unnecessary count processing can be eliminated, and the reliability of the estimation result can be improved.

上記の判定器を含む本発明に係る前記演算器は、複数の演算器であって、前記複数の演算器は、前記第1レジスタに順次保持される前記レーザの1走査分の反射点座標の各々について、前記反射点座標と、前記複数の第2レジスタに保持された前記複数の反射点座標との差分を並列に演算し、前記デコーダは、複数のデコーダであって、前記判定器は、複数の判定器であって、前記複数の判定器は、前記複数の演算器の出力値が所定範囲内であるか否かを並列に判定し、前記複数のデコーダは、前記複数の演算器のうち、前記判定器によって肯定判定された前記演算器の出力値に対応する度数を計数する前記カウンタを並列に指定するようにすることができる。これによって、処理の高速化を図ることができる。   The computing unit according to the present invention including the determining unit is a plurality of computing units, and the plurality of computing units include reflection point coordinates for one scan of the laser sequentially held in the first register. For each, the difference between the reflection point coordinates and the plurality of reflection point coordinates held in the plurality of second registers is calculated in parallel, the decoder is a plurality of decoders, A plurality of determiners, wherein the plurality of determiners determine in parallel whether the output values of the plurality of computing units are within a predetermined range; and the plurality of decoders Of these, the counters that count the frequency corresponding to the output value of the arithmetic unit that has been affirmed by the determiner can be designated in parallel. As a result, the processing speed can be increased.

本発明に係る前記複数のカウンタは、前記演算器の出力値の指定された部分桁の値との対応関係が予め定められ、前記カウンタは、前記レーザの1走査分に対する、前記カウンタに対応する前記演算器の出力値の指定された部分桁の値の度数を計数するためのものであって、前記デコーダは、前記演算器の出力値の前記指定された部分桁の値に対応する度数を計数する前記カウンタを指定し、前記運動推定部は、前記比較器によって特定されたカウンタに対応する前記演算器の出力値の部分桁の値に、予め求められた前記部分桁より上位桁の値を結合して、前記移動体の運動の推定結果とするようにすることができる。これによって、指定した部分桁をカウント対象とすることで、必要な推定精度に合わせた部分桁を指定することが可能となる。 In the plurality of counters according to the present invention, a correspondence relationship with a value of a specified partial digit of the output value of the arithmetic unit is predetermined , and the counter corresponds to the counter for one scan of the laser. A counter for counting the frequency of a specified partial digit value of the output value of the arithmetic unit, wherein the decoder calculates a frequency corresponding to the value of the specified partial digit of the output value of the arithmetic unit. The counter for counting is designated, and the motion estimation unit sets the value of the higher digit than the partial digit obtained in advance to the value of the partial digit of the output value of the computing unit corresponding to the counter specified by the comparator. Can be combined to obtain an estimation result of the movement of the moving body. Thus, by specifying the designated partial digit as a count target, it is possible to designate the partial digit according to the required estimation accuracy.

以上説明したように本発明によれば、レーザ装置から順次得られる推定対象時点のレーザの走査に対する前記反射点座標と、推定対象時点より前に得られたレーザの1走査分の複数の前記反射点座標とについて、それぞれの差分を演算して、演算器の出力値の各値の度数を計数し、最大度数を計数したカウンタに基づいて、移動体の運動を推定することにより、簡易なハードウエア構成で、移動体の運動推定の実時間処理を実現することができる、という効果が得られる。   As described above, according to the present invention, the reflection point coordinates for the scanning of the laser at the estimation target time sequentially obtained from the laser device and the plurality of reflections for one scan of the laser obtained before the estimation target time are obtained. By calculating the respective differences with respect to the point coordinates, counting the frequency of each value of the output value of the computing unit, and estimating the movement of the moving body based on the counter counting the maximum frequency, a simple hardware The effect that real-time processing of motion estimation of a moving object can be realized with the wear configuration is obtained.

第1の実施の形態の自車運動推定装置およびレーザレーダの配置を示す図である。It is a figure which shows arrangement | positioning of the own vehicle motion estimation apparatus and laser radar of 1st Embodiment. 第1の実施の形態の自車運動推定装置の構成を示す図である。It is a figure which shows the structure of the own vehicle motion estimation apparatus of 1st Embodiment. 移動ベクトルのヒストグラムを作成する方法を説明するための図である。It is a figure for demonstrating the method of producing the histogram of a movement vector. 第1の実施の形態の自車運動推定装置の動作を説明するためのタイミングチャートである。It is a timing chart for demonstrating operation | movement of the own vehicle motion estimation apparatus of 1st Embodiment. 第2の実施の形態の自車運動推定装置の構成を示す図である。It is a figure which shows the structure of the own vehicle motion estimation apparatus of 2nd Embodiment. 第2の実施の形態の自車運動推定装置の動作を説明するためのタイミングチャートである。It is a timing chart for demonstrating operation | movement of the own vehicle motion estimation apparatus of 2nd Embodiment. 第3の実施の形態の自車運動推定装置の構成を示す図である。It is a figure which shows the structure of the own vehicle motion estimation apparatus of 3rd Embodiment. 移動ベクトルのヒストグラムを作成する方法を説明するための図である。It is a figure for demonstrating the method of producing the histogram of a movement vector. 第3の実施の形態の自車運動推定装置の動作を説明するためのタイミングチャートである。It is a timing chart for demonstrating operation | movement of the own vehicle motion estimation apparatus of 3rd Embodiment. 第4の実施の形態の自車運動推定装置の構成を示す図である。It is a figure which shows the structure of the own vehicle motion estimation apparatus of 4th Embodiment. 第5の実施の形態の自車運動推定装置の構成を示す図である。It is a figure which shows the structure of the own vehicle motion estimation apparatus of 5th Embodiment. 第6の実施の形態の自車運動推定装置の構成を示す図である。It is a figure which shows the structure of the own vehicle motion estimation apparatus of 6th Embodiment. 第7の実施の形態の自車運動推定装置の構成を示す図である。It is a figure which shows the structure of the own vehicle motion estimation apparatus of 7th Embodiment. 第7の実施の形態の自車運動推定装置の制御部による部分桁指定処理ルーチンの内容を示すフローチャートである。It is a flowchart which shows the content of the partial digit designation | designated processing routine by the control part of the own vehicle motion estimation apparatus of 7th Embodiment. 第8の実施の形態の自車運動推定装置の構成を示す図である。It is a figure which shows the structure of the own vehicle motion estimation apparatus of 8th Embodiment. 第9の実施の形態の自車運動推定装置の構成を示す図である。It is a figure which shows the structure of the own vehicle motion estimation apparatus of 9th Embodiment. 自車の運動量を推定する従来方法を説明するための図である。It is a figure for demonstrating the conventional method of estimating the momentum of the own vehicle.

以下、図面を参照して本発明の実施の形態を詳細に説明する。図1に示すように、第1の実施の形態の自車運動推定装置10は、自車両に搭載されたレーザレーダ12に接続されている。   Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. As shown in FIG. 1, the own vehicle motion estimation device 10 of the first embodiment is connected to a laser radar 12 mounted on the own vehicle.

レーザレーダ12は、自車両の前方に対してレーザを1次元(水平方向)に走査しながら照射し、レーザの各反射点座標を計測する。   The laser radar 12 irradiates the front of the host vehicle while scanning the laser in one dimension (horizontal direction), and measures each reflection point coordinate of the laser.

レーザレーダ12は、車両前方に設置され、装置を基準とする車両前方に存在する物体までの距離を検出する装置であり、出力するレーザを水平方向に走査することで、レーザの反射により自車両前方に存在する複数の物体表面上の複数の点の位置を検出することができる。レーザレーダ12は、レーザの各反射点の位置を表す2次元座標を自車運動推定装置10に逐次出力する。   The laser radar 12 is a device that is installed in front of the vehicle and detects a distance to an object existing in front of the vehicle with respect to the device. The laser radar 12 scans the output laser in the horizontal direction, thereby reflecting the own vehicle by reflecting the laser. It is possible to detect the positions of a plurality of points on the front surfaces of a plurality of objects. The laser radar 12 sequentially outputs two-dimensional coordinates representing the position of each reflection point of the laser to the own vehicle motion estimation device 10.

自車運動推定装置10は、図2に示すように、レーザレーダ12の1スキャン分の反射点座標(x、z)を保持する複数のレジスタA0〜A63と、現時点の1つの反射点座標(x、z)を保持するレジスタBと、複数のレジスタA0〜A63とレジスタBとの間で座標値の引き算を行い2点間の移動ベクトル(dx、dz)を求める複数の演算器20と、移動ベクトル(dx、dz)に関する複数の値のカウント値(度数)を計数するためのカウンタC0〜C255と、複数の演算器20から出力された移動ベクトル(dx、dz)の度数を計数するカウンタCを、カウンタC0〜C255の中から特定し、特定されたカウンタCのカウント値をカウントアップする複数のデコーダ22と、カウンタC0〜C255の中から、最も大きいカウント値を保持するカウンタを特定し、当該カウンタに対応する移動ベクトル(dx、dz)及びカウント値を出力する比較器ツリー24と、比較器ツリーの出力に基づいて、自車運動(並進運動)の推定値を出力する出力部26と、デコーダ22、カウンタC0〜C255、及び比較器ツリー24の動作を制御する制御部28とを備えている。なお、複数のレジスタA0〜A63は、複数の第2レジスタの一例であり、レジスタBは、第1レジスタの一例である。また、比較器ツリー24は、比較器の一例であり、出力部26が、運動推定部の一例である。   As shown in FIG. 2, the own vehicle motion estimation apparatus 10 includes a plurality of registers A0 to A63 that hold reflection point coordinates (x, z) for one scan of the laser radar 12 and one current reflection point coordinate ( x, z), a plurality of arithmetic units 20 for subtracting coordinate values between the plurality of registers A0 to A63 and the register B to obtain a movement vector (dx, dz) between two points; Counters C0 to C255 for counting a plurality of count values (frequency) regarding the movement vector (dx, dz), and a counter for counting the frequency of the movement vector (dx, dz) output from the plurality of computing units 20 C is specified from the counters C0 to C255, and a plurality of decoders 22 for counting up the count value of the specified counter C and the largest counter among the counters C0 to C255 are counted. A counter holding the current value, a movement vector (dx, dz) corresponding to the counter and a count value are output, and the vehicle movement (translational movement) based on the output of the comparator tree , An output unit 26 that outputs the estimated value of the above, a decoder 22, counters C0 to C255, and a control unit 28 that controls the operation of the comparator tree 24. The plurality of registers A0 to A63 are examples of a plurality of second registers, and the register B is an example of a first register. The comparator tree 24 is an example of a comparator, and the output unit 26 is an example of a motion estimation unit.

複数のレジスタA0〜A63には、レジスタ制御部(図示省略)によって、1時刻前の1スキャン分の反射点座標が格納される。   In a plurality of registers A0 to A63, the reflection point coordinates for one scan before one time are stored by a register control unit (not shown).

レジスタBには、現在のレーザの走査により順次得られる1つの反射点座標(x、z)が、レジスタ制御部によって格納される。   In the register B, one reflection point coordinate (x, z) sequentially obtained by the current laser scanning is stored by the register control unit.

演算器20の個数は、レジスタA0〜A63と同数であり、複数の演算器20と複数のレジスタA0〜A63とが一対一に対応して接続されており、各演算器20は、対応するレジスタAが保持する座標とレジスタBが保持している座標との間で引き算をし、2点間の移動ベクトルを求めて出力する。複数の演算器20は、1スキャン分の移動ベクトルを並列に求める。ここでの1スキャン分の移動ベクトルは、同じ値を持つものはない。なぜならレジスタA0〜A63の値は全て異なるからである。   The number of arithmetic units 20 is the same as that of the registers A0 to A63, and a plurality of arithmetic units 20 and a plurality of registers A0 to A63 are connected in a one-to-one correspondence, and each arithmetic unit 20 has a corresponding register. Subtraction is performed between the coordinates held by A and the coordinates held by register B, and a movement vector between two points is obtained and output. The plurality of computing units 20 obtain a movement vector for one scan in parallel. Here, the movement vectors for one scan do not have the same value. This is because the values of the registers A0 to A63 are all different.

また、1スキャン分の移動ベクトルの分布を求めるため、移動ベクトルの値ごとにカウンタC0〜C255が設けられている。例えば、移動ベクトル(15、15)がカウンタC255に対応する、という具合に、移動ベクトルの各値と、カウンタC0〜C255との対応関係を予め決めておく(実際には、正負を考慮する必要がある)。また、新しいスキャンの処理開始時点で、全てのカウンタC0〜C255の内容は0にリセットされる。   In addition, in order to obtain the movement vector distribution for one scan, counters C0 to C255 are provided for each movement vector value. For example, the correspondence between each value of the movement vector and the counters C0 to C255 is determined in advance such that the movement vector (15, 15) corresponds to the counter C255 (actually, it is necessary to consider positive / negative) There). Also, at the start of a new scan process, the contents of all the counters C0 to C255 are reset to zero.

デコーダ22の個数は、演算器20と同数であり、複数のデコーダ22と複数の演算器20とが一対一に対応して接続されており、各デコーダ22は、対応する演算器20で求めた移動ベクトルに対応するカウンタCを特定し、特定したカウンタCのカウント値をカウントアップする。複数のデコーダ22による1スキャン分のカウントアップが並列に行われる。   The number of decoders 22 is the same as that of the arithmetic units 20, and a plurality of decoders 22 and a plurality of arithmetic units 20 are connected in a one-to-one correspondence, and each decoder 22 is obtained by a corresponding arithmetic unit 20. The counter C corresponding to the movement vector is specified, and the count value of the specified counter C is counted up. Count-up for one scan by the plurality of decoders 22 is performed in parallel.

レーザレーダ12から次の反射点座標が入力されると、レジスタ制御部は、入力された反射点座標で、レジスタBを更新する。   When the next reflection point coordinate is input from the laser radar 12, the register control unit updates the register B with the input reflection point coordinate.

制御部28は、レーザレーダ12から反射点座標が入力される毎に、複数のデコーダ22による並列処理が行われるようにタイミング制御する。これにより、レーザレーダ12から反射点座標が入力される毎に、各演算器20による演算、複数のデコーダ22による並列処理、及びカウンタC0〜C255のカウントアップが繰り返し行われる。これを1スキャン分の反射点座標(例えば64点)について繰り返すことにより、図3に示すように、1時刻前t0の各反射点から現時刻t1の各反射点への全組み合せ分の移動ベクトルを求めて、カウンタC0〜C255のカウントアップを行い、カウンタC0〜C255のカウント値により、移動ベクトルのヒストグラムが作成される。   The control unit 28 performs timing control so that parallel processing by a plurality of decoders 22 is performed every time reflection point coordinates are input from the laser radar 12. Thereby, every time the reflection point coordinates are input from the laser radar 12, the calculation by each calculator 20, the parallel processing by the plurality of decoders 22, and the count-up of the counters C0 to C255 are repeatedly performed. By repeating this for reflection point coordinates (for example, 64 points) for one scan, as shown in FIG. 3, the movement vectors for all combinations from each reflection point at time t1 to each reflection point at time t1 as shown in FIG. The counters C0 to C255 are counted up, and a movement vector histogram is created based on the count values of the counters C0 to C255.

比較器ツリー24は、2つの入力値から大きい方を選択出力する機能(比較器)を基本ユニットとし、この基本ユニットをツリー状に組み合わせ、n個の入力から、最大値を持つ入力を選択出力するように構成したものである。図2においてはn=4の場合をひとつの長方形で表している。比較のキーはカウント値であるが、基本ユニットの出力はカウント値と移動ベクトル(dx、dz)(つまりカウンタ番号)のセットである。   The comparator tree 24 has a function (comparator) for selecting and outputting the larger one of two input values as a basic unit, and this basic unit is combined in a tree shape to select and output the input having the maximum value from n inputs. It is comprised so that it may do. In FIG. 2, the case of n = 4 is represented by one rectangle. The key for comparison is the count value, but the output of the basic unit is a set of the count value and the movement vector (dx, dz) (that is, the counter number).

制御部28は、1スキャン分の反射点座標についてのカウント処理が終わったところで、最大のカウント値をもつカウンタCを探索するため、比較器ツリー24を動作させる。これによって、1スキャン分の反射点座標についてのカウント処理の結果から、最大カウント値を有するカウンタCに対応する移動ベクトル(dx、dz)が出力される。   When the counting process for the reflection point coordinates for one scan is completed, the control unit 28 operates the comparator tree 24 to search for the counter C having the maximum count value. As a result, the movement vector (dx, dz) corresponding to the counter C having the maximum count value is output from the result of the counting process for the reflection point coordinates for one scan.

また、制御部28は、比較器ツリー24の動作が終了すると、カウンタC0〜C255の初期化を行う。   In addition, when the operation of the comparator tree 24 is completed, the control unit 28 initializes the counters C0 to C255.

出力部26は、比較器ツリー24から出力される移動ベクトルを、自車運動の推定値として出力する。   The output unit 26 outputs the movement vector output from the comparator tree 24 as an estimated value of the own vehicle motion.

次に、第1の実施の形態の自車運動推定装置10の動作について説明する。図4に示すタイミングチャートを用いて説明する。なお、上記図4では、反射点座標を「xz-○□」と表している。○はi, j, kでラインの順番を表し、□は0〜63の数値で反射点の入力されてくる順番を表している。例えば、左の反射点から右の反射点へと順番に入力される。   Next, operation | movement of the own vehicle motion estimation apparatus 10 of 1st Embodiment is demonstrated. This will be described with reference to the timing chart shown in FIG. In FIG. 4 above, the reflection point coordinates are represented as “xz- ◯ □”. O represents the order of lines with i, j, k, and □ represents the order in which reflection points are input with numerical values of 0 to 63. For example, the input is sequentially performed from the left reflection point to the right reflection point.

まず、レジスタA0〜A63レジスタには、レジスタ制御部によって、1時刻前に得られた1走査分の反射点座標が格納される。レジスタBには、次の走査(現時点の走査)の反射点座標が1点ずつ、レジスタ制御部によって格納される。なお、処理開始の指示は、制御部28によって行われる。   First, in the registers A0 to A63, the reflection point coordinates for one scan obtained one time before by the register control unit are stored. The register B stores the reflection point coordinates of the next scan (current scan) one by one in the register B. The processing start instruction is given by the control unit 28.

そして、複数の演算器20によって、レジスタBの座標と、レジスタA0〜A63が保持している座標との間で移動ベクトルが求められる。64個の移動ベクトルが同時並列に求められる。複数のデコーダ22によって、複数の演算器20から求められた64個の移動ベクトルから、それぞれの投票先(ヒストグラムの対応するビン)のカウンタCを特定し、投票先の指示があったカウンタCのカウント値を+1する。   Then, a plurality of arithmetic units 20 obtain a movement vector between the coordinates of the register B and the coordinates held by the registers A0 to A63. 64 movement vectors are obtained in parallel. The plurality of decoders 22 identify counters C of the respective vote destinations (corresponding bins of the histogram) from the 64 movement vectors obtained from the plurality of computing units 20, and the counter C for which the vote destination is instructed is specified. Add 1 to the count value.

制御部28によって、現時刻のレーザ走査による1ライン分の反射点座標を用いて求められる移動ベクトルについて、カウント値のカウントアップが終了したと判断されると、制御部28は、比較器ツリー24に対して、最大のカウント値を有するカウンタCを特定し、特定されたカウンタCに対応するカウント値及び移動ベクトルを出力するように指示する。   When the control unit 28 determines that the count-up of the count value has been completed for the movement vector obtained using the reflection point coordinates for one line by the laser scanning at the current time, the control unit 28 displays the comparator tree 24. The counter C having the maximum count value is specified and the count value and the movement vector corresponding to the specified counter C are output.

そして、出力部26により、比較器ツリー24から出力された移動ベクトルを、自車運動の推定値として出力する。   Then, the output unit 26 outputs the movement vector output from the comparator tree 24 as an estimated value of the own vehicle motion.

上記の動作が、レーザレーダ12によるレーザの走査毎に、繰り返し行われ、自車運動の推定値が繰り返し求められる。   The above operation is repeated every time the laser radar 12 scans the laser, and the estimated value of the own vehicle motion is repeatedly obtained.

以上説明したように、第1の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標と、推定対象時点より1時刻前に得られたレーザの1走査分の複数の反射点座標とについて、それぞれの差分を演算して移動ベクトルを求め、演算器から出力される移動ベクトルの各値の度数を計数し、最大度数を計数したカウンタに基づいて、自車運動を推定することにより、簡易なハードウエア構成で、自車運動推定の実時間処理を実現することができる。   As described above, according to the own vehicle motion estimation device according to the first embodiment, the reflection point coordinates for the laser scanning at the estimation target time points sequentially obtained from the laser radar, and one time before the estimation target time point. With respect to the plurality of reflection point coordinates for one scanning of the obtained laser, the respective differences are calculated to obtain a movement vector, the frequency of each value of the movement vector output from the calculator is counted, and the maximum frequency is counted. By estimating the vehicle movement based on the counter, the real-time processing of the vehicle movement estimation can be realized with a simple hardware configuration.

また、異なる時刻のレーザ走査で得られた反射点座標から移動ベクトルを算出し、移動ベクトルの投票処理を行うことで、全ての候補移動ベクトルについて演算する必要がなくなり、演算量を削減できる。また、移動ベクトルの算出から投票までの処理の並列化が簡素な構成で実現可能となり、処理時間を短縮できる。   Further, by calculating the movement vector from the reflection point coordinates obtained by laser scanning at different times and performing the movement vector voting process, it is not necessary to calculate all the candidate movement vectors, and the amount of calculation can be reduced. In addition, parallel processing from calculation of movement vectors to voting can be realized with a simple configuration, and processing time can be shortened.

次に、本発明の第2の実施の形態について説明する。なお、第1の実施の形態と同様の構成となる部分については、第1の実施の形態と同様の符号を付して説明を省略する。   Next, a second embodiment of the present invention will be described. In addition, about the part which becomes the structure similar to 1st Embodiment, the code | symbol similar to 1st Embodiment is attached | subjected and description is abbreviate | omitted.

第2の実施の形態では、レジスタA0〜A63にデータをセットする方法が、第1の実施の形態と異なっている。   In the second embodiment, the method for setting data in the registers A0 to A63 is different from that in the first embodiment.

図5に示すように、第2の実施の形態に係る自車運動推定装置210は、レジスタA0〜A63と、レジスタBBと、レーザレーダ12の1スキャン分の反射点座標(x、z)を保持する複数のレジスタB0〜B63と、複数の演算器20と、カウンタC0〜C255と、複数のデコーダ22と、比較器ツリー24と、出力部26と、制御部28とを備えている。なお、レジスタBBが、第1レジスタの一例である。   As shown in FIG. 5, the own vehicle motion estimation device 210 according to the second embodiment uses the register A0 to A63, the register BB, and the reflection point coordinates (x, z) for one scan of the laser radar 12. A plurality of registers B0 to B63, a plurality of arithmetic units 20, counters C0 to C255, a plurality of decoders 22, a comparator tree 24, an output unit 26, and a control unit 28 are provided. The register BB is an example of the first register.

複数のレジスタA0〜A63には、レジスタ制御部(図示省略)によって、1時刻前の1スキャン分の反射点座標が格納される。   In a plurality of registers A0 to A63, the reflection point coordinates for one scan before one time are stored by a register control unit (not shown).

レジスタBBには、現在のレーザの走査により順次得られる反射点座標(x、z)の1つが、レジスタ制御部によって格納される。   In the register BB, one of the reflection point coordinates (x, z) sequentially obtained by the current laser scanning is stored by the register control unit.

1スキャン分の演算処理が終わるまで、レジスタA0〜A63のデータは移動させることができないため、データ移動用にレジスタB0〜B63を設ける。   Since the data in the registers A0 to A63 cannot be moved until the calculation process for one scan is completed, the registers B0 to B63 are provided for data movement.

反射点座標は、レーザレーダ12から時系列に入力されて来るので、新しい反射点座標が入力されるつど、制御部28によって、座標データをレジスタBB、B63〜B0内でシフトさせ(BB→B63→B62…B1→B0)、レジスタB0〜B63に1スキャン分の反射点座標をデータセットする。制御部28は、1スキャンの終了または開始のタイミングでレジスタB0〜B63の内容をレジスタA0〜A63にコピーし、レジスタA0〜A63に必要な反射点座標のデータをセットする。   Since the reflection point coordinates are input from the laser radar 12 in time series, each time a new reflection point coordinate is input, the control unit 28 shifts the coordinate data in the registers BB and B63 to B0 (BB → B63). → B62... B1 → B0), and the reflection point coordinates for one scan are set in the registers B0 to B63. The control unit 28 copies the contents of the registers B0 to B63 to the registers A0 to A63 at the end or start timing of one scan, and sets the necessary reflection point coordinate data in the registers A0 to A63.

次に、第2の実施の形態の自車運動推定装置10の動作について説明する。図6に示すタイミングチャートを用いて説明する。   Next, operation | movement of the own vehicle motion estimation apparatus 10 of 2nd Embodiment is demonstrated. This will be described with reference to the timing chart shown in FIG.

レーザレーダ12から出力されるスキャンタイミング信号は、1ライン(レーザの1走査に相当)の間“1”となる信号で、ラインとラインの間で“0”となる。また、レーザレーダ12から出力される反射点タイミング信号は、反射点座標が入力されるタイミングを表す信号で、この信号が“1”のとき、入力バス(図示省略)上の反射点座標のデータが有効であることを保証している。制御部28は、レーザレーダ12からのタイミング信号を受けて、レジスタをはじめとする各部の制御を行う。   The scan timing signal output from the laser radar 12 is a signal that is “1” during one line (corresponding to one scan of the laser), and is “0” between lines. The reflection point timing signal output from the laser radar 12 is a signal indicating the timing at which the reflection point coordinates are input. When this signal is “1”, the reflection point coordinate data on the input bus (not shown). Is guaranteed to be effective. The control unit 28 receives the timing signal from the laser radar 12 and controls each unit including the register.

レーザレーダ12から入力される反射点座標は、反射点タイミング信号が“1”のときに、レジスタBBに取り込まれる。   The reflection point coordinates input from the laser radar 12 are taken into the register BB when the reflection point timing signal is “1”.

そして、複数の演算器20によって、レジスタBBの座標と、レジスタA0〜A63が保持している座標との間で移動ベクトルが求められる。64個の移動ベクトルが同時並列に求められる。次に、複数のデコーダ22によって、複数の演算器20から求められた64個の移動ベクトルから、それぞれの投票先(ヒストグラムの対応するビン)のカウンタCを特定し、投票先の指示があったカウンタCのカウント値を+1する。   Then, a plurality of arithmetic units 20 obtain a movement vector between the coordinates of the register BB and the coordinates held by the registers A0 to A63. 64 movement vectors are obtained in parallel. Next, the counters C of the respective vote destinations (corresponding bins of the histogram) are identified from the 64 movement vectors obtained from the plurality of arithmetic units 20 by the plurality of decoders 22 and the vote destinations are instructed. The count value of the counter C is incremented by 1.

スキャンタイミング信号が“0”になったことで、制御部28は、現時刻のレーザ走査による1ライン分の反射点座標を用いて求められる移動ベクトルについて、カウント値のカウントアップが終了したと判断すると、制御部28は、比較器ツリー24に対して、最大のカウント値を有するカウンタCを特定して、特定されたカウンタCに対応するカウント値及び移動ベクトルを出力するように指示する。   When the scan timing signal becomes “0”, the control unit 28 determines that the count value has been counted up for the movement vector obtained using the reflection point coordinates for one line by the laser scanning at the current time. Then, the control unit 28 instructs the comparator tree 24 to specify the counter C having the maximum count value and output the count value and the movement vector corresponding to the specified counter C.

そして、出力部26により、比較器ツリー24から出力された移動ベクトルを、自車運動の推定値として出力する。   Then, the output unit 26 outputs the movement vector output from the comparator tree 24 as an estimated value of the own vehicle motion.

一方、一旦レジスタBBに保持された反射点座標は、レジスタB63、レジスタB62・・・と順次移動して行き、1ラインの終わりの時点では、レジスタB0からレジスタB63に1ライン分の反射点座標が保持されている。   On the other hand, the reflection point coordinates once held in the register BB sequentially move in the order of the register B63, the register B62..., And at the end of one line, the reflection point coordinates for one line from the register B0 to the register B63. Is held.

スキャンタイミング信号が“0”になったこと(ラインとラインの切れ目)で、制御部28は、現時刻のレーザ走査による1ライン分の反射点座標を用いて求められる移動ベクトルについて、カウント値のカウントアップが終了したと判断すると、制御部28は、レジスタB0〜B63の内容をレジスタA0〜A63にコピーするとともに、カウンタC0〜C255をクリア(カウント値を0にリセット)する。   When the scan timing signal becomes “0” (line-to-line break), the control unit 28 sets the count value for the movement vector obtained using the reflection point coordinates for one line by laser scanning at the current time. When determining that the count-up is completed, the control unit 28 copies the contents of the registers B0 to B63 to the registers A0 to A63 and clears the counters C0 to C255 (resets the count value to 0).

上記の動作が、レーザレーダ12によるレーザの走査毎に、繰り返し行われ、自車運動の推定値が繰り返し求められる。   The above operation is repeated every time the laser radar 12 scans the laser, and the estimated value of the own vehicle motion is repeatedly obtained.

このように、反射点座標の入力部分のダブルバッファ化により、レジスタA0〜A63にデータをセットすることができる。   As described above, data can be set in the registers A0 to A63 by forming the reflection point coordinate input portion as a double buffer.

次に、本発明の第3の実施の形態について説明する。なお、第1の実施の形態及び第2の実施の形態と同様の構成となる部分については、第1の実施の形態と同様の符号を付して説明を省略する。   Next, a third embodiment of the present invention will be described. In addition, about the part which becomes the structure similar to 1st Embodiment and 2nd Embodiment, the code | symbol similar to 1st Embodiment is attached | subjected and description is abbreviate | omitted.

第3の実施の形態では、並進運動だけでなく、回転運動も推定している点が、第1の実施の形態と異なっている。   The third embodiment is different from the first embodiment in that not only translational motion but also rotational motion is estimated.

図7に示すように、第3の実施の形態に係る自車運動推定装置310は、複数のレジスタA0〜A63と、レジスタBBと、複数のレジスタB0〜B63と、仮定した回転運動量の候補の分だけ差し引いた座標に変換する座標変換部320と、複数の演算器20と、カウンタC0〜C255と、複数のデコーダ22と、比較器ツリー24と、出力部26と、制御部28とを備えている。   As shown in FIG. 7, the own vehicle motion estimation apparatus 310 according to the third embodiment includes a plurality of registers A0 to A63, a register BB, a plurality of registers B0 to B63, and assumed rotational momentum candidates. A coordinate conversion unit 320 that converts the coordinates into the subtracted amount, a plurality of computing units 20, counters C0 to C255, a plurality of decoders 22, a comparator tree 24, an output unit 26, and a control unit 28 are provided. ing.

座標変換部320は、制御部28の制御により、レジスタBBの反射点座標を、仮定した回転運動量θの複数候補の各々について、当該候補の分だけ差し引いた座標に変換する。例えば、回転運動量θの候補が±10°の範囲を1°刻みに21候補ある場合(回転運動量0°を含む)、レジスタBBの反射点座標を、21個の反射点座標に順次変換する。   Under the control of the control unit 28, the coordinate conversion unit 320 converts the reflection point coordinates of the register BB into coordinates obtained by subtracting the candidate for each of a plurality of assumed rotational momentum θ. For example, when there are 21 candidates for the rotational momentum θ in the range of ± 10 ° in increments of 1 ° (including the rotational momentum 0 °), the reflection point coordinates of the register BB are sequentially converted into 21 reflection point coordinates.

各演算器20は、回転運動量θの複数候補の各々について、対応するレジスタAが保持する座標と、座標変換部320によって変換された当該候補の分だけ差し引いた座標との間で引き算をし、2点間の移動ベクトル(dx、dz)を求めて出力する。   Each computing unit 20 subtracts, for each of a plurality of candidates for rotational momentum θ, the coordinates held by the corresponding register A and the coordinates subtracted by the candidates converted by the coordinate conversion unit 320, A movement vector (dx, dz) between two points is obtained and output.

また、カウンタC0〜C255は、回転運動量を含めた移動ベクトル(dx、dz、θ)の値ごとに設けられている。例えば、移動ベクトル(15、15、10°)がカウンタC255に対応する、という具合に、移動ベクトル(dx、dz、θ)の各値と、カウンタC0〜C255とが対応付けられている。   Further, the counters C0 to C255 are provided for each value of the movement vector (dx, dz, θ) including the rotational momentum. For example, each value of the movement vector (dx, dz, θ) and the counters C0 to C255 are associated with each other such that the movement vector (15, 15, 10 °) corresponds to the counter C255.

各デコーダ22は、対応する演算器20で求めた移動ベクトル(dx、dz)と、その移動ベクトルを求めるために用いられた反射点座標の変換に用いられた回転運動量θの候補との組み合わせに対応するカウンタCを特定し、特定したカウンタCの値をカウントアップする。   Each decoder 22 combines the movement vector (dx, dz) obtained by the corresponding computing unit 20 with the candidate of the rotational momentum θ used for conversion of the reflection point coordinates used for obtaining the movement vector. The corresponding counter C is specified, and the value of the specified counter C is counted up.

制御部28は、レーザレーダ12から反射点座標が入力され、回転運動量の候補を差し引いた反射点座標に変換される毎に、複数のデコーダ22による並列処理が行われるようにタイミング制御する。これにより、レーザレーダ12から反射点座標が入力され、回転運動量の候補を差し引いた反射点座標に変換される毎に、各演算器20による演算、複数のデコーダ22による並列処理、及びカウンタC0〜C255のカウントアップが繰り返し行われる。これを1スキャン分の反射点座標(例えば64点)について繰り返すことにより、図8に示すように、1時刻前t0の各反射点から、回転運動量の各候補を差し引いて変換された現時刻t1の各反射点への全組み合せ分の移動ベクトルを求め、カウンタC0〜C255のカウント値により、回転運動量を含む移動ベクトルのヒストグラムが作成される。   The control unit 28 controls the timing so that parallel processing by the plurality of decoders 22 is performed each time the reflection point coordinates are input from the laser radar 12 and converted to the reflection point coordinates obtained by subtracting the rotational momentum candidates. Thus, each time the reflection point coordinates are input from the laser radar 12 and converted to the reflection point coordinates obtained by subtracting the rotational momentum candidates, the calculation by each computing unit 20, the parallel processing by the plurality of decoders 22, and the counters C0 to C0. C255 is counted up repeatedly. By repeating this with respect to the reflection point coordinates (for example, 64 points) for one scan, as shown in FIG. 8, the current time t1 converted by subtracting each candidate for the rotational momentum from each reflection point one time before t0. The movement vectors for all combinations of the reflection points are obtained, and a histogram of the movement vectors including the rotational momentum is created based on the count values of the counters C0 to C255.

比較器ツリー24における基本ユニットの出力はカウント値と移動ベクトル(dx、dz、θ)(つまりカウンタ番号)のセットである。   The output of the basic unit in the comparator tree 24 is a set of count values and movement vectors (dx, dz, θ) (that is, counter numbers).

制御部28は、1スキャン分の反射点座標についての回転運動量の各候補を用いた移動ベクトルに対して、カウント処理が終わったところで、最大のカウント値をもつカウンタCを探索するため、比較器ツリー24を動作させる。これによって、1スキャン分の反射点座標についての回転運動量の各候補を用いた移動ベクトルに対するカウント処理の結果から、最大カウント値を有するカウンタCに対応する移動ベクトル(dx、dz、θ)が出力される。   The control unit 28 searches for the counter C having the maximum count value when the counting process is completed for the movement vector using each rotational momentum candidate for the reflection point coordinates for one scan. The tree 24 is operated. As a result, the movement vector (dx, dz, θ) corresponding to the counter C having the maximum count value is output from the result of the counting process for the movement vector using each candidate of the rotational momentum for the reflection point coordinates for one scan. Is done.

次に、第3の実施の形態の自車運動推定装置10の動作について説明する。図9に示すタイミングチャートを用いて説明する。   Next, operation | movement of the own vehicle motion estimation apparatus 10 of 3rd Embodiment is demonstrated. This will be described with reference to the timing chart shown in FIG.

レーザレーダ12から入力される反射点座標は、反射点タイミング信号が“1”のときに、レジスタBBに取り込まれる。   The reflection point coordinates input from the laser radar 12 are taken into the register BB when the reflection point timing signal is “1”.

レジスタBBの反射点座標は、座標変換部320によって回転運動量θの各候補を差し引いた座標に各々変換される。回転運動量θの各候補は制御部28から指定される。   The reflection point coordinates of the register BB are converted into coordinates obtained by subtracting each candidate of the rotational momentum θ by the coordinate conversion unit 320. Each candidate of the rotational momentum θ is designated by the control unit 28.

そして、複数の演算器20によって、レジスタBBの座標が変換される毎に、変換された座標と、レジスタA0〜A63が保持している座標との間で移動ベクトルが求められる。64個の移動ベクトルが同時並列に求められる。複数のデコーダ22によって、複数の演算器20から求められた、回転運動量を含めた64個の移動ベクトル(dx、dz、θ)から、それぞれの投票先(ヒストグラムの対応するビン)のカウンタCを特定し、投票先の指示があったカウンタCのカウント値を+1する。   Each time the coordinates of the register BB are converted by the plurality of arithmetic units 20, a movement vector is obtained between the converted coordinates and the coordinates held by the registers A0 to A63. 64 movement vectors are obtained in parallel. From the 64 movement vectors (dx, dz, θ) obtained from the plurality of computing units 20 by the plurality of decoders 22 and including the rotational momentum, the counter C of each voting destination (the bin corresponding to the histogram) is calculated. The count value of the counter C that is specified and designated by the vote destination is incremented by one.

スキャンタイミング信号が“0”になったことで、制御部28は、現時刻のレーザ走査による1ライン分の反射点座標の各々に対する変換後の座標の各々を用いて求められる移動ベクトルについて、カウント値のカウントアップが終了したと判断すると、制御部28は、比較器ツリー24に対して、最大のカウント値を有するカウンタCを特定して、特定されたカウンタCに対応するカウント値及び移動ベクトル(dx、dz、θ)を出力するように指示する。   When the scan timing signal becomes “0”, the control unit 28 counts the movement vector obtained using each of the coordinates after conversion with respect to each of the reflection point coordinates for one line by the laser scanning at the current time. If it is determined that the value count-up is completed, the control unit 28 specifies the counter C having the maximum count value with respect to the comparator tree 24, and the count value and movement vector corresponding to the specified counter C are determined. Instructs to output (dx, dz, θ).

そして、出力部26により、比較器ツリー24から出力された移動ベクトル(dx、dz、θ)を、自車運動の推定値として出力する。   Then, the output unit 26 outputs the movement vector (dx, dz, θ) output from the comparator tree 24 as an estimated value of the own vehicle motion.

以上説明したように、第3の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標から、仮定した回転運動量の候補を差し引いた反射点座標と、推定対象時点より1時刻前に得られたレーザの1走査分の複数の反射点座標とについて、それぞれの差分を演算して移動ベクトルを求め、演算器から出力される移動ベクトルの値及び回転運動量の候補の組み合わせの度数を計数し、最大度数を計数したカウンタに基づいて、回転運動量を含む自車運動を推定することにより、簡易なハードウエア構成で、回転運動量を含む自車運動推定の実時間処理を実現することができる。   As described above, according to the own vehicle motion estimation device according to the third embodiment, the assumed rotational momentum candidate is subtracted from the reflection point coordinates for the laser scanning at the estimation target time points sequentially obtained from the laser radar. The difference between the reflected point coordinates and a plurality of reflected point coordinates for one scan of the laser obtained one hour before the estimation target time is calculated to obtain a movement vector, and the movement output from the calculator Counting the number of combinations of vector values and rotational momentum candidates, and including the rotational momentum with a simple hardware configuration by estimating the vehicle movement including the rotational momentum based on the counter counting the maximum frequency Real-time processing of self-vehicle motion estimation can be realized.

次に、本発明の第4の実施の形態について説明する。なお、第1の実施の形態と同様の構成となる部分については、第1の実施の形態と同様の符号を付して説明を省略する。   Next, a fourth embodiment of the present invention will be described. In addition, about the part which becomes the structure similar to 1st Embodiment, the code | symbol similar to 1st Embodiment is attached | subjected and description is abbreviate | omitted.

第4の実施の形態では、演算器により求められた移動ベクトルが所定範囲内である場合のみ、移動ベクトルに対応するカウンタをカウントアップしている点が、第1の実施の形態と異なっている。   The fourth embodiment is different from the first embodiment in that the counter corresponding to the movement vector is counted up only when the movement vector obtained by the arithmetic unit is within a predetermined range. .

図10に示すように、第4の実施の形態に係る自車運動推定装置410は、レジスタA0〜A63と、レジスタBと、複数の演算器20と、移動ベクトルの範囲を指定するデータを保持する範囲指定レジスタ418と、演算器20の出力が、指定された移動ベクトルの範囲内であるか否かを判定する複数の比較器420と、カウンタC0〜C255と、複数のデコーダ422と、比較器ツリー24と、出力部26と、制御部28とを備えている。なお、比較器420が、判定器の一例である。   As shown in FIG. 10, the own vehicle motion estimation device 410 according to the fourth embodiment holds registers A0 to A63, a register B, a plurality of arithmetic units 20, and data specifying a range of movement vectors. A range designation register 418, a plurality of comparators 420 for determining whether the output of the arithmetic unit 20 is within the range of the designated movement vector, counters C0 to C255, a plurality of decoders 422, and comparison A container tree 24, an output unit 26, and a control unit 28 are provided. The comparator 420 is an example of a determiner.

範囲指定レジスタ418には、移動ベクトル(dx、dz)のdz成分の最大値及び最小値、及びdy成分の最大値及び最小値が格納され、移動ベクトルの範囲が指定される。   The range designation register 418 stores the maximum value and minimum value of the dz component of the movement vector (dx, dz), and the maximum value and minimum value of the dy component, and specifies the range of the movement vector.

比較器420の個数は、演算器20と同数であり、複数の比較器420と複数の演算器20とが一対一に対応して接続されており、各比較器420は、対応する演算器20で求めた移動ベクトルが、範囲指定レジスタ418に指定された範囲内であるかを判定し、範囲指定レジスタ418に指定された範囲内である場合に、対応するデコーダ422が動作するように動作許可信号を出力する。複数の比較器420による1スキャン分の比較判定が並列に行われる。   The number of the comparators 420 is the same as that of the computing units 20, and the plurality of comparators 420 and the plurality of computing units 20 are connected in a one-to-one correspondence, and each comparator 420 is associated with the corresponding computing unit 20. It is determined whether or not the movement vector obtained in step 4 is within the range specified in the range specification register 418. If the movement vector is within the range specified in the range specification register 418, the operation is permitted so that the corresponding decoder 422 operates. Output a signal. Comparison determination for one scan by the plurality of comparators 420 is performed in parallel.

各デコーダ422は、対応する比較器420から動作許可信号が入力された場合に、対応する演算器20で求められた移動ベクトルに対応するカウンタCを特定し、特定したカウンタCの値をカウントアップする。複数のデコーダ22による1スキャン分のカウントアップが並列に行われる。デコーダ422は、対応する比較器420から動作許可信号が入力されない場合には、求めた移動ベクトルが所定の範囲内に入っていないため、カウントアップ動作を行わない。   Each decoder 422 specifies the counter C corresponding to the movement vector obtained by the corresponding arithmetic unit 20 when the operation permission signal is input from the corresponding comparator 420, and counts up the value of the specified counter C. To do. Count-up for one scan by the plurality of decoders 22 is performed in parallel. When the operation permission signal is not input from the corresponding comparator 420, the decoder 422 does not perform the count-up operation because the obtained movement vector is not within the predetermined range.

制御部28は、現時点の自車運動推定値の信頼度が高い場合に、次スキャンでの移動ベクトルの指定範囲を狭く設定するように、範囲指定レジスタ418を更新し、現時点の自車運動推定値の信頼度が低い場合に、次スキャンでの移動ベクトルの指定範囲を広く設定するように、範囲指定レジスタ418を更新する。   When the reliability of the current vehicle motion estimation value is high, the control unit 28 updates the range specification register 418 so as to set the specified range of the movement vector in the next scan so as to estimate the current vehicle motion estimation. When the reliability of the value is low, the range designation register 418 is updated so that the designated range of the movement vector in the next scan is set wide.

例えば、以下のように、現時点の自車運動推定値の信頼度をもとに、次スキャンでの移動ベクトルの指定範囲を設定する。   For example, as shown below, the designated range of the movement vector in the next scan is set based on the reliability of the current vehicle motion estimation value.

まず、制御部28は、レーザの1スキャン分の反射点座標について複数の演算器20により求めた全移動ベクトルの分散(標準偏差)を求める。その分散が小さければ信頼度は高いと考えられるため、制御部28は、処理対象の移動ベクトルの範囲を、現在の自車運動推定値から2σ(2×標準偏差)の範囲内に設定するように、範囲指定レジスタ418を更新する。   First, the control unit 28 obtains the variance (standard deviation) of all the movement vectors obtained by the plurality of computing units 20 for the reflection point coordinates for one scan of the laser. Since the reliability is considered to be high if the variance is small, the control unit 28 sets the range of the movement vector to be processed within the range of 2σ (2 × standard deviation) from the current vehicle motion estimation value. In addition, the range designation register 418 is updated.

なお、分散を求めるときに、複数の演算器20により求められる全移動ベクトルを対象とせず、ヒストグラムの度数(カウンタCのカウント値)の大きい方から10個程度を選択し、選択された10個の移動ベクトルにおける度数から、移動ベクトルの分散を求めるようにしてもよい。   When obtaining the variance, not all the movement vectors obtained by the plurality of computing units 20 are considered, but about 10 from the larger histogram frequency (count value of the counter C) are selected, and the selected 10 The variance of the movement vector may be obtained from the frequency of the movement vector.

また、制御部28は、レーザレーダ12から反射点座標が入力される毎に、複数のデコーダ22による並列処理が行われるようにタイミング制御する。これにより、レーザレーダ12から反射点座標が入力される毎に、各演算器20による演算、各比較器420による比較判定、複数のデコーダ22による並列処理、及びカウンタC0〜C255のカウントアップが繰り返し行われる。これを1スキャン分の反射点座標(例えば64点)について繰り返すことにより、1時刻前t0の各反射点から現時刻t1の各反射点への全組み合せ分の移動ベクトルを求めると共に、指定範囲外の移動ベクトルを排除し、カウンタC0〜C255のカウント値により、指定範囲内の移動ベクトルのヒストグラムが作成される。   Further, the control unit 28 performs timing control so that parallel processing by the plurality of decoders 22 is performed every time the reflection point coordinates are input from the laser radar 12. Thereby, every time the reflection point coordinates are input from the laser radar 12, the calculation by each calculator 20, the comparison judgment by each comparator 420, the parallel processing by the plurality of decoders 22, and the count up of the counters C0 to C255 are repeated. Done. By repeating this with respect to the reflection point coordinates (for example, 64 points) for one scan, the movement vectors for all combinations from each reflection point at t0 one hour before to each reflection point at the current time t1 are obtained and out of the specified range And a histogram of the movement vectors in the specified range is created based on the count values of the counters C0 to C255.

次に、第4の実施の形態の自車運動推定装置410の動作について説明する。   Next, operation | movement of the own vehicle motion estimation apparatus 410 of 4th Embodiment is demonstrated.

まず、レジスタA0〜A63レジスタには、レジスタ制御部によって、1時刻前に得られた1走査分の反射点座標が格納される。レジスタBには、次の走査(現時刻の走査)で得られる反射点座標が1点ずつ、レジスタ制御部によって格納される。なお、処理開始の指示は、制御部28によって行われる。   First, in the registers A0 to A63, the reflection point coordinates for one scan obtained one time before by the register control unit are stored. In the register B, the reflection point coordinates obtained in the next scan (scan at the current time) are stored one by one by the register control unit. The processing start instruction is given by the control unit 28.

そして、複数の演算器20によって、レジスタBの座標と、レジスタA0〜A63が保持している座標との間で移動ベクトルが求められる。64個の移動ベクトルが同時並列に求められる。複数の比較器420によって、範囲指定レジスタ418の内容に基づいて、複数の演算器20から求められた64個の移動ベクトルが、指定された範囲内であるか否かを判定し、肯定判定された場合に、動作許可信号を対応するデコーダ422に出力する。   Then, a plurality of arithmetic units 20 obtain a movement vector between the coordinates of the register B and the coordinates held by the registers A0 to A63. 64 movement vectors are obtained in parallel. Based on the contents of the range specification register 418, the plurality of comparators 420 determine whether or not the 64 movement vectors obtained from the plurality of computing units 20 are within the specified range, and an affirmative determination is made. In the case of an error, the operation permission signal is output to the corresponding decoder 422.

動作許可信号が入力された複数のデコーダ422によって、対応する複数の演算器20から求められた複数の移動ベクトルから、それぞれの投票先(ヒストグラムの対応するビン)のカウンタCを特定し、投票先の指示があったカウンタCのカウント値を+1する。   The plurality of decoders 422 to which the operation permission signals are input specify the counters C of the respective voting destinations (corresponding bins of the histogram) from the plurality of movement vectors obtained from the corresponding arithmetic units 20, and the voting destinations 1 is added to the count value of the counter C instructed to

制御部28によって、現時刻のレーザ走査による1ライン分の反射点座標について、カウント値のカウントアップが終了したと判断されると、制御部28は、比較器ツリー24に対して、最大のカウント値を有するカウンタCを特定して、特定されたカウンタCに対応するカウント値及び移動ベクトルを出力するように指示する。   When the control unit 28 determines that the count-up of the count value has been completed for the reflection point coordinates for one line by the laser scanning at the current time, the control unit 28 determines the maximum count for the comparator tree 24. A counter C having a value is specified, and an instruction is given to output a count value and a movement vector corresponding to the specified counter C.

そして、出力部26により、比較器ツリー24から出力された移動ベクトルを、自車運動の推定値として出力する。   Then, the output unit 26 outputs the movement vector output from the comparator tree 24 as an estimated value of the own vehicle motion.

また、制御部28によって、レーザの1スキャン分の反射点座標について複数の演算器20により求めた全移動ベクトルの分散(標準偏差)を求め、自車運動の推定値および当該分散に基づ処理対象の移動ベクトルの範囲を求め、範囲指定レジスタ418を更新する。   Further, the control unit 28 obtains the variance (standard deviation) of all the movement vectors obtained by the plurality of computing units 20 for the reflection point coordinates for one scan of the laser, and performs processing based on the estimated value of the own vehicle motion and the variance. The range of the target movement vector is obtained, and the range designation register 418 is updated.

上記の動作が、レーザレーダ12によるレーザの走査毎に、繰り返し行われ、自車運動の推定値が繰り返し求められる。   The above operation is repeated every time the laser radar 12 scans the laser, and the estimated value of the own vehicle motion is repeatedly obtained.

以上説明したように、第4の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標と、推定対象時点より1時刻前に得られたレーザの1走査分の複数の反射点座標とについて、それぞれの差分を演算して移動ベクトルを求め、所定範囲内の移動ベクトルの各値の度数を計数し、最大度数を計数したカウンタに基づいて、自車運動を推定することにより、簡易なハードウエア構成で、自車運動推定の実時間処理を実現することができる。   As described above, according to the own vehicle motion estimation device according to the fourth embodiment, the reflection point coordinates for the laser scanning at the estimation target time points sequentially obtained from the laser radar, and one time before the estimation target time point. A counter that counts the frequency of each value of the movement vector within a predetermined range, counts the frequency of each value of the plurality of reflection point coordinates for one scan of the obtained laser, calculates the movement vector, counts the maximum frequency By estimating the vehicle motion based on the above, real-time processing of the vehicle motion estimation can be realized with a simple hardware configuration.

また、予め分かっている自車運動の範囲内の移動ベクトルのみで投票処理を実施することにより、ノイズを除去することで自車運動推定の信頼度が向上し、演算量も削減できる。   Further, by performing the voting process only with the movement vector within the range of the own vehicle motion that is known in advance, the reliability of the own vehicle motion estimation is improved by removing the noise, and the calculation amount can be reduced.

予め自車運動のおよその値が分かっていれば、そこから外れる運動量は考慮する必要がない。むしろ、外れた運動量をカウント対象とすることでノイズが増え、誤った値が最大カウント値を示す可能性がある。本実施の形態では、カウント対象とする運動量にあらかじめ範囲を設けることで、無用のカウント処理を排除するとともに、推定結果の信頼度を向上させる。   If the approximate value of the vehicle movement is known in advance, there is no need to consider the amount of movement that deviates from that value. Rather, there is a possibility that the noise increases due to the deviating momentum being counted, and an incorrect value indicates the maximum count value. In the present embodiment, by providing a range in advance for the amount of exercise to be counted, unnecessary count processing is eliminated and the reliability of the estimation result is improved.

また、範囲制限によりカウンタの個数が削減できることに加え、比較器ツリーの規模も小さくなり、ハードウエア量の削減効果もある。   In addition to reducing the number of counters by limiting the range, the size of the comparator tree is reduced, and the amount of hardware can be reduced.

なお、上記の実施の形態では、複数の演算器20により求めた1スキャン分の全移動ベクトルの分散を用いて、移動ベクトルの制限範囲を設定する場合を例に説明したが、これに限定されるものではない。   In the above-described embodiment, the case where the movement vector limit range is set using the variance of all movement vectors for one scan obtained by the plurality of computing units 20 has been described as an example. However, the present invention is not limited to this. It is not something.

例えば、スキャン毎に出力される自車運動推定値の時系列での変化が小さければ、信頼度は高いと考えられる。また、自車運動は急激には変化しないため、推定値の時系列変化が小さければ、正しく推定されていると考えられる。また、自車運動の変化の割合は物理的な上限がある(1秒間に時速が0km→100kmという加速は通常はありえない)。そこで、制御部28は、スキャン毎に出力される自車運動推定値の時系列での変化が小さい場合に、処理対象のベクトルの制限範囲を、現在の自車運動推定値から起こり得る範囲に設定するようにしてもよい。   For example, if the change in the time series of the own vehicle motion estimation value output for each scan is small, the reliability is considered high. In addition, since the vehicle movement does not change abruptly, if the time series change of the estimated value is small, it is considered that the vehicle is correctly estimated. In addition, the rate of change of the own vehicle motion has a physical upper limit (acceleration of 0 km to 100 km per hour is not normally possible). Therefore, when the change in the time series of the own vehicle motion estimation value output for each scan is small, the control unit 28 sets the limit range of the processing target vector to a range that can occur from the current own vehicle motion estimation value. You may make it set.

また、ヒストグラムの最大度数と2番目の度数(カウント値)の差が大きければ、信頼度は高いと考えられる。逆に度数の差が小さければ、1番目の移動ベクトルと2番目の移動ベクトルのどちらが真値に近いのか判断できず、信頼度は低い。そこで、制御部28は、カウンタの最大度数と2番目の度数との差が大きく、信頼度が高いと判断できる場合に、処理対象のベクトルの範囲を、現在の自車運動推定値から起こり得る範囲に設定するようにしてもよい。   Further, if the difference between the maximum frequency of the histogram and the second frequency (count value) is large, the reliability is considered high. On the other hand, if the frequency difference is small, it cannot be determined which of the first movement vector and the second movement vector is closer to the true value, and the reliability is low. Therefore, when the difference between the maximum frequency of the counter and the second frequency is large and it can be determined that the reliability is high, the control unit 28 can determine the range of the vector to be processed from the current own vehicle motion estimation value. You may make it set to a range.

次に、本発明の第5の実施の形態について説明する。なお、第1の実施の形態及び第4の実施の形態と同様の構成となる部分については、第1の実施の形態及び第4の実施の形態と同様の符号を付して説明を省略する。   Next, a fifth embodiment of the present invention will be described. In addition, about the part which becomes the structure similar to 1st Embodiment and 4th Embodiment, the code | symbol similar to 1st Embodiment and 4th Embodiment is attached | subjected and description is abbreviate | omitted. .

第5の実施の形態では、ベース移動ベクトルを差し引いた現時点の反射点座標を用いて、差分移動ベクトルを求めている点が、第4の実施の形態と異なっている。   The fifth embodiment is different from the fourth embodiment in that the difference movement vector is obtained using the current reflection point coordinates obtained by subtracting the base movement vector.

図11に示すように、第5の実施の形態に係る自車運動推定装置510は、レジスタA0〜A63と、レジスタBBと、ベース移動ベクトルを保持するレジスタBVと、レジスタBと、レジスタBBに保持された座標からレジスタBVに保持されたベース移動ベクトルを差し引いてレジスタBに格納する演算器512と、複数の演算器20と、範囲指定レジスタ418と、複数の比較器420と、カウンタC0〜C255と、複数のデコーダ422と、比較器ツリー24と、演算器524と、出力部26と、制御部28と、を備えている。   As shown in FIG. 11, the own vehicle motion estimation apparatus 510 according to the fifth embodiment includes a register A0 to A63, a register BB, a register BV that holds a base movement vector, a register B, and a register BB. A calculator 512 that subtracts the base movement vector held in the register BV from the held coordinates and stores the result in the register B, a plurality of calculators 20, a range specification register 418, a plurality of comparators 420, and counters C0 to C0. C255, a plurality of decoders 422, a comparator tree 24, an arithmetic unit 524, an output unit 26, and a control unit 28 are provided.

レジスタBBには、現在の反射点座標(x、z)が保持される。レジスタBVには、仮定されるベース移動ベクトルが保持される。本実施の形態では、直前に推定された自車運動量の推定値をベース移動ベクトルとしてレジスタBVに格納する。   The register BB holds the current reflection point coordinates (x, z). The assumed base movement vector is held in the register BV. In the present embodiment, the estimated value of the own vehicle momentum estimated immediately before is stored in the register BV as a base movement vector.

演算器512は、レジスタBBに保持された反射点座標から、ベース移動ベクトルを差し引いて、1時刻前の反射点座標を推定し、レジスタBに保存する。   The arithmetic unit 512 subtracts the base movement vector from the reflection point coordinates held in the register BB, estimates the reflection point coordinates one hour before, and stores them in the register B.

各演算器20は、対応するレジスタAが保持する座標とレジスタBが保持している座標との間で引き算をし、2点間の差分移動ベクトルを求めて出力する。   Each computing unit 20 performs subtraction between the coordinates held by the corresponding register A and the coordinates held by the register B, and obtains and outputs a difference movement vector between the two points.

範囲指定レジスタ418には、差分移動ベクトル(dx、dz)のdx成分の絶対値の最大値、及びdz成分の絶対値の最大値が格納され、差分移動ベクトルの範囲が指定される。ここで、差分移動ベクトルの範囲の最小値が指定されていないのは、1時刻前の推定座標からの移動ベクトルを演算しているため、自車運動に変化がなければ、差分移動ベクトルは0となるからである。   The range designation register 418 stores the maximum value of the absolute value of the dx component and the maximum value of the absolute value of the dz component of the differential motion vector (dx, dz), and designates the range of the differential motion vector. Here, the minimum value of the range of the difference movement vector is not specified because the movement vector from the estimated coordinates one time before is calculated, and therefore, if there is no change in the own vehicle motion, the difference movement vector is 0. Because it becomes.

各比較器420は、対応する演算器20で求めた移動ベクトルの各成分の絶対値が、範囲指定レジスタ418に指定された範囲内であるかを判定し、範囲指定レジスタ418に指定された範囲内である場合に、対応するデコーダ422が動作するように動作許可信号を出力する。   Each comparator 420 determines whether the absolute value of each component of the movement vector obtained by the corresponding computing unit 20 is within the range specified in the range specification register 418, and the range specified in the range specification register 418. If it is within the range, an operation permission signal is output so that the corresponding decoder 422 operates.

比較器ツリー24から出力される差分移動ベクトルは、レジスタB(1時刻前を推定した座標)までの移動ベクトルであるため、推定に用いたベース移動ベクトルに当該差分移動ベクトルを加えたものが自車運動の推定値となる。したがって、演算器524は、比較器ツリー24から出力される差分移動ベクトルに、1時刻前の自車運動の推定値である移動ベクトル(ベース移動ベクトル)を加算して、求められた移動ベクトルを出力する。   Since the difference movement vector output from the comparator tree 24 is a movement vector up to the register B (coordinates estimated one time before), the difference movement vector obtained by adding the difference movement vector to the base movement vector used for the estimation is itself. Estimated value of car movement. Therefore, the arithmetic unit 524 adds the movement vector (base movement vector) that is the estimated value of the host vehicle movement one hour before to the difference movement vector output from the comparator tree 24, and obtains the obtained movement vector. Output.

出力部26は、演算器524から出力される移動ベクトルを、自車運動の推定値として出力する。   The output unit 26 outputs the movement vector output from the calculator 524 as an estimated value of the own vehicle motion.

次に、第5の実施の形態の自車運動推定装置510の動作について説明する。   Next, the operation of the own vehicle motion estimation device 510 according to the fifth embodiment will be described.

まず、レジスタA0〜A63レジスタには、レジスタ制御部によって、1時刻前に得られた1走査分の反射点座標が格納される。また、レジスタ制御部によって、1時刻前に推定された自車運動の推定値である移動ベクトルが、レジスタBVに格納される。レジスタBBには、次の走査(現時刻の走査)の反射点座標が1点ずつ、レジスタ制御部によって格納される。そして、演算器512によって、レジスタBBの反射点座標から、レジスタBVの移動ベクトルを差し引いた座標を求め、レジスタBに格納する。   First, in the registers A0 to A63, the reflection point coordinates for one scan obtained one time before by the register control unit are stored. In addition, a movement vector that is an estimated value of the own vehicle motion estimated one time before by the register control unit is stored in the register BV. In the register BB, the reflection point coordinates of the next scan (scan at the current time) are stored one by one by the register control unit. Then, the arithmetic unit 512 obtains coordinates obtained by subtracting the movement vector of the register BV from the reflection point coordinates of the register BB, and stores them in the register B.

そして、複数の演算器20によって、レジスタBの座標と、レジスタA0〜A63が保持している座標との間で差分移動ベクトルが求められる。64個の差分移動ベクトルが同時並列に求められる。複数の比較器420によって、範囲指定レジスタ418の内容に基づいて、複数の演算器20から求められた64個の差分移動ベクトルが、指定された範囲内であるか否かを判定し、肯定判定された場合に、動作許可信号をデコーダ422に出力する。   Then, a difference movement vector is obtained between the coordinates of the register B and the coordinates held by the registers A0 to A63 by the plurality of arithmetic units 20. 64 differential motion vectors are obtained in parallel. Based on the contents of the range specification register 418, the plurality of comparators 420 determine whether or not the 64 differential movement vectors obtained from the plurality of computing units 20 are within the specified range, and an affirmative determination is made. If it is, an operation permission signal is output to the decoder 422.

動作許可信号が入力された複数のデコーダ422によって、対応する複数の演算器20から求められた複数の差分移動ベクトルから、それぞれの投票先(ヒストグラムの対応するビン)のカウンタCを特定し、投票先の指示があったカウンタCのカウント値を+1する。   The plurality of decoders 422 to which the operation permission signals are input specify the counters C of the respective voting destinations (corresponding bins of the histogram) from the plurality of differential movement vectors obtained from the corresponding plurality of computing units 20, and vote The count value of the counter C that has been instructed earlier is incremented by one.

制御部28によって、現時刻のレーザ走査による1ライン分の反射点座標を用いて求められた差分移動ベクトルについて、カウント値のカウントアップが終了したと判断されると、制御部28は、比較器ツリー24に対して、最大のカウント値を有するカウンタCを特定して、特定されたカウンタCに対応するカウント値及び差分移動ベクトルを出力するように指示する。   When the control unit 28 determines that the count-up of the count value has been completed for the differential movement vector obtained using the reflection point coordinates for one line by the laser scanning at the current time, the control unit 28 The tree 24 is instructed to specify the counter C having the maximum count value and output the count value and the differential movement vector corresponding to the specified counter C.

そして、演算器524により、比較器ツリー24から出力された差分移動ベクトルと、現時点のベース移動ベクトルとして用いられている1時刻前の移動ベクトルとを加算して求めた移動ベクトルを出力する。   Then, the arithmetic unit 524 outputs a movement vector obtained by adding the difference movement vector output from the comparator tree 24 and the movement vector one hour before used as the current base movement vector.

出力部26により、演算器524から出力された移動ベクトルを、自車運動の推定値として出力する。   The output unit 26 outputs the movement vector output from the calculator 524 as an estimated value of the own vehicle motion.

以上説明したように、第5の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標から、ベース移動ベクトルを差し引いた反射点座標と、推定対象時点より1時刻前に得られたレーザの1走査分の複数の反射点座標とについて、それぞれの差分を演算して差分移動ベクトルを求め、所定範囲内の差分移動ベクトルの値の度数を計数し、最大度数を計数したカウンタに基づいて、自車運動を推定することにより、簡易なハードウエア構成で、自車運動推定の実時間処理を実現することができる。   As described above, according to the own vehicle motion estimation device according to the fifth embodiment, the reflection point obtained by subtracting the base movement vector from the reflection point coordinates for the laser scanning at the estimation target time points sequentially obtained from the laser radar. The difference movement vector is obtained by calculating the difference between the coordinates and a plurality of reflection point coordinates for one scan of the laser obtained one time before the estimation target time point, and the value of the difference movement vector within a predetermined range By counting the frequency of the vehicle and estimating the vehicle motion based on the counter counting the maximum frequency, real-time processing of the vehicle motion estimation can be realized with a simple hardware configuration.

また、差分移動ベクトルを求めるようにすることにより、差分移動ベクトルの制限範囲を小さく設定することが可能となり、予想される自車運動の制限範囲から外れる差分移動ベクトルは、カウント処理の対象外とすることで、ノイズ除去の効果が得られ、自車運動推定結果の信頼度を向上させることが可能となる。また、範囲制限によりカウンタの個数が削減できることに加え、比較器ツリーの規模も小さくなり、ハードウエア量の削減効果もある。   In addition, by obtaining the difference movement vector, it becomes possible to set the restriction range of the difference movement vector to be small, and the difference movement vector that is out of the expected restriction range of the own vehicle motion is excluded from the count processing target. By doing so, the effect of noise removal is obtained, and the reliability of the own vehicle motion estimation result can be improved. In addition to reducing the number of counters by limiting the range, the size of the comparator tree is reduced, and the amount of hardware can be reduced.

次に、本発明の第6の実施の形態について説明する。なお、第1の実施の形態、第4の実施の形態、及び第5の実施の形態と同様の構成となる部分については、上記の実施の形態と同様の符号を付して説明を省略する。   Next, a sixth embodiment of the present invention will be described. In addition, about the part which becomes the structure similar to 1st Embodiment, 4th Embodiment, and 5th Embodiment, the code | symbol similar to said embodiment is attached | subjected and description is abbreviate | omitted. .

第6の実施の形態では、並進運動だけでなく、回転運動も推定している点と、レジスタA0〜A63にデータをセットする方法とが、第5の実施の形態と異なっている。   The sixth embodiment is different from the fifth embodiment in that not only translational motion but also rotational motion is estimated and a method of setting data in the registers A0 to A63.

図12に示すように、第6の実施の形態に係る自車運動推定装置610は、レジスタA0〜A63と、レジスタBBと、複数のレジスタB0〜B63と、レジスタBVAと、レジスタBと、レジスタBBに保持された座標からレジスタBVAに保持されたベース移動ベクトル(並進・回転成分)を差し引くと共に、さらに仮定した回転運動量の候補の分だけ差し引いた座標に変換する座標変換部612と、複数の演算器20と、範囲指定レジスタ418と、複数の比較器420と、カウンタC0〜C255と、複数のデコーダ422と、比較器ツリー24と、演算器624と、出力部26と、制御部28と、を備えている。   As shown in FIG. 12, the own vehicle motion estimation device 610 according to the sixth embodiment includes a register A0 to A63, a register BB, a plurality of registers B0 to B63, a register BVA, a register B, and a register. A coordinate conversion unit 612 that subtracts the base movement vector (translation / rotation component) held in the register BVA from the coordinates held in the BB, and further converts them into coordinates obtained by subtracting only the assumed rotational momentum candidates; An arithmetic unit 20, a range designation register 418, a plurality of comparators 420, counters C0 to C255, a plurality of decoders 422, a comparator tree 24, an arithmetic unit 624, an output unit 26, and a control unit 28 It is equipped with.

座標変換部612は、レジスタBBに保持された反射点座標から、ベース移動ベクトル(dx、dz、θ)を差し引いて、1時刻前の反射点座標を推定すると共に、制御部28の制御により、推定された1時刻前の反射点座標を、仮定した回転運動量θの複数候補の各々について、当該候補の分だけ差し引いた座標に変換し、レジスタBに保持する。   The coordinate conversion unit 612 subtracts the base movement vector (dx, dz, θ) from the reflection point coordinates held in the register BB to estimate the reflection point coordinates one hour ago, and is controlled by the control unit 28. The estimated reflection point coordinates one hour before are converted into coordinates obtained by subtracting the estimated number of candidates of the rotational momentum θ by the amount corresponding to the candidates, and stored in the register B.

各演算器20は、回転運動量θの複数候補の各々について、対応するレジスタAが保持する座標と、レジスタB(座標変換部320によって変換された当該候補の分だけ差し引いた座標)との間で引き算をし、2点間の差分移動ベクトル(dx、dz)を求めて出力する。   For each of a plurality of candidates for rotational momentum θ, each computing unit 20 is between a coordinate held by the corresponding register A and a register B (coordinates subtracted by the candidate converted by the coordinate conversion unit 320). Subtraction is performed, and a difference movement vector (dx, dz) between two points is obtained and output.

また、カウンタC0〜C255は、差分移動ベクトル(dx、dz)及び回転運動量θの組み合わせごとに設けられている。   Further, the counters C0 to C255 are provided for each combination of the differential movement vector (dx, dz) and the rotational momentum θ.

比較器420から動作許可信号が入力された各デコーダ22は、対応する演算器20で求めた差分移動ベクトル(dx、dz)と、その差分移動ベクトルを演算するのに用いられた反射点座標の変換に用いられた回転運動量θの候補との組み合わせに対応するカウンタCを特定し、特定したカウンタCのカウント値をカウントアップする。   Each decoder 22 to which the operation permission signal is input from the comparator 420, calculates the difference movement vector (dx, dz) obtained by the corresponding calculator 20 and the reflection point coordinates used to calculate the difference movement vector. The counter C corresponding to the combination with the candidate of the rotational momentum θ used for the conversion is specified, and the count value of the specified counter C is counted up.

制御部28は、レーザレーダ12から反射点座標が入力されて、1時刻前の反射点座標が推定され、更に回転運動量の候補を差し引いた反射点座標に変換される毎に、複数のデコーダ22による並列処理が行われるようにタイミング制御する。これにより、レーザレーダ12から反射点座標が入力されて1時刻前の反射点座標が推定され、更に回転運動量の候補を差し引いた反射点座標に変換される毎に、各演算器20による演算、複数の比較器420による比較判定、複数のデコーダ422による並列処理、及びカウンタC0〜C255のカウントアップが繰り返し行われる。これを1スキャン分の反射点座標(例えば64点)について繰り返すことにより、1時刻前t0の各反射点から、1時刻前の反射点座標の推定値から回転運動量の各候補を差し引いて変換された現時刻t1の各反射点への全組み合せ分の差分移動ベクトルを求めると共に、指定範囲外の差分移動ベクトルを排除し、カウンタC0〜C255のカウント値により、指定範囲内の差分移動ベクトル及び回転運動量の組み合わせのヒストグラムが作成される。   The control unit 28 receives the reflection point coordinates from the laser radar 12, estimates the reflection point coordinates one hour before, and further converts the reflection point coordinates to the reflection point coordinates obtained by subtracting the rotational momentum candidates. The timing is controlled so that parallel processing is performed. Thus, each time the reflection point coordinates are input from the laser radar 12 and the reflection point coordinates one time before are estimated and converted to the reflection point coordinates obtained by subtracting the rotational momentum candidate, Comparison determination by the plurality of comparators 420, parallel processing by the plurality of decoders 422, and counting up of the counters C0 to C255 are repeatedly performed. By repeating this for the reflection point coordinates (for example, 64 points) for one scan, conversion is performed by subtracting each rotational momentum candidate from the reflection point coordinate estimation value one time ago from each reflection point t0 one time ago. In addition, the difference movement vector for all combinations to each reflection point at the current time t1 is obtained, the difference movement vector outside the specified range is excluded, and the difference movement vector and rotation within the specified range are determined by the count values of the counters C0 to C255. A histogram of momentum combinations is created.

比較器ツリー24における基本ユニットの出力はカウント値と差分移動ベクトル(dx、dz)及び回転運動量θの組み合わせ(つまりカウンタ番号)とのセットである。   The output of the basic unit in the comparator tree 24 is a set of a count value, a difference movement vector (dx, dz), and a combination of rotational momentum θ (that is, a counter number).

制御部28は、1スキャン分の反射点座標についての回転運動量の各候補を用いた移動ベクトルに対して、カウント処理が終わったところで、最大のカウント値をもつカウンタCを探索するため、比較器ツリー24を動作させる。これによって、1スキャン分の反射点座標についての回転運動量の各候補を用いた移動ベクトルに対するカウント処理の結果から、最大カウント値を有するカウンタCに対応する差分移動ベクトル(dx、dz)及び回転運動量θの組み合わせが出力される。   The control unit 28 searches for the counter C having the maximum count value when the counting process is completed for the movement vector using each rotational momentum candidate for the reflection point coordinates for one scan. The tree 24 is operated. As a result, the difference movement vector (dx, dz) and the rotational momentum corresponding to the counter C having the maximum count value are obtained from the result of the counting process for the movement vector using each candidate of the rotational momentum for the reflection point coordinates for one scan. A combination of θ is output.

演算器624は、比較器ツリー24から出力される差分移動ベクトル(dx、dz)に、1時刻前の自車運動の推定値である移動ベクトル(dx、dz)を加算して、求められた並進成分(dx、dz)と、同じく比較器ツリー24から出力された回転運動量θと1時刻前の自車運動の推定値であるベース移動ベクトルの回転運動量θを加算して求められる回転運動量θからなる移動ベクトル(dx、dz、θ)を出力する。   The computing unit 624 is obtained by adding the movement vector (dx, dz), which is an estimated value of the own vehicle motion one hour before, to the difference movement vector (dx, dz) output from the comparator tree 24. The rotational momentum θ obtained by adding the translational component (dx, dz), the rotational momentum θ similarly output from the comparator tree 24, and the rotational momentum θ of the base movement vector, which is an estimated value of the own vehicle motion one time before. A movement vector (dx, dz, θ) consisting of is output.

出力部26は、演算器524から出力される移動ベクトル(dx、dz、θ)を、自車運動の推定値として出力する。   The output unit 26 outputs the movement vector (dx, dz, θ) output from the calculator 524 as an estimated value of the own vehicle motion.

反射点座標は、レーザレーダ12から時系列に入力されて来るので、新しい反射点座標が入力されるつど、制御部28によって、座標データをレジスタBB、B63〜B0内でシフトさせ(BB→B63→B62…B1→B0)、レジスタB0〜B63に1スキャン分の座標をデータセットする。制御部28は、1スキャンの終了または開始のタイミングでレジスタB0〜B63の内容をレジスタA0〜A63にコピーし、レジスタA0〜A63に必要な反射点座標のデータをセットする。   Since the reflection point coordinates are input from the laser radar 12 in time series, each time a new reflection point coordinate is input, the control unit 28 shifts the coordinate data in the registers BB and B63 to B0 (BB → B63). → B62... B1 → B0), and coordinates for one scan are set in the registers B0 to B63. The control unit 28 copies the contents of the registers B0 to B63 to the registers A0 to A63 at the end or start timing of one scan, and sets the necessary reflection point coordinate data in the registers A0 to A63.

次に、第6の実施の形態の自車運動推定装置610の動作について説明する。   Next, the operation of the own vehicle motion estimation device 610 according to the sixth embodiment will be described.

まず、制御部28によって、レジスタA0〜A63レジスタには、レジスタB0〜B63の内容がコピーされ、1時刻前に得られた1走査分の反射点座標が格納される。また、レジスタ制御部によって、1時刻前に推定された自車運動の推定値である移動ベクトルの並進成分と回転成分が、レジスタBVAに格納される。レジスタBBには、次の走査(現時刻の走査)の反射点座標が1点ずつ、制御部28によって格納される。   First, the control unit 28 copies the contents of the registers B0 to B63 into the registers A0 to A63, and stores the reflection point coordinates for one scan obtained one time before. In addition, the translation control component and the rotation component of the movement vector, which are the estimated values of the vehicle motion estimated one time before by the register control unit, are stored in the register BVA. The register BB stores the reflection point coordinates of the next scan (scan at the current time) one by one by the control unit 28.

そして、座標変換部612によって、レジスタBBの反射点座標から、レジスタBVAの移動ベクトル(並進・回転成分)を差し引くと共に、さらに回転運動量θの候補の各々について、回転運動量θの候補を差し引いた座標に各々変換する。回転運動量θの各候補は制御部28から指定される。   Then, the coordinate conversion unit 612 subtracts the movement vector (translation / rotation component) of the register BVA from the reflection point coordinates of the register BB, and further subtracts the rotation momentum θ candidate for each of the rotation momentum θ candidates. Respectively. Each candidate of the rotational momentum θ is designated by the control unit 28.

そして、複数の演算器20によって、レジスタBBの座標が変換される毎に、変換された座標と、レジスタA0〜A63が保持している座標との間で差分移動ベクトルが求められる。64個の差分移動ベクトルが同時並列に求められる。複数の比較器420によって、範囲指定レジスタ418の内容に基づいて、複数の演算器20から求められた64個の差分移動ベクトルが、指定された範囲内であるか否かを判定し、肯定判定された場合に、動作許可信号をデコーダ422に出力する。   Each time the coordinates of the register BB are converted by the plurality of arithmetic units 20, a difference movement vector is obtained between the converted coordinates and the coordinates held in the registers A0 to A63. 64 differential motion vectors are obtained in parallel. Based on the contents of the range specification register 418, the plurality of comparators 420 determine whether or not the 64 differential movement vectors obtained from the plurality of computing units 20 are within the specified range, and an affirmative determination is made. If it is, an operation permission signal is output to the decoder 422.

動作許可信号が入力された複数のデコーダ422によって、対応する複数の演算器20から求められた複数の差分移動ベクトル及び回転運動量の組み合わせから、それぞれの投票先(ヒストグラムの対応するビン)のカウンタCを決定し、投票先の指示があったカウンタCのカウント値を+1する。   A counter C for each voting destination (corresponding bin of the histogram) from a combination of a plurality of differential movement vectors and rotational momentum obtained from a plurality of corresponding computing units 20 by a plurality of decoders 422 to which an operation permission signal is input. And the count value of the counter C, which is instructed by the vote destination, is incremented by one.

制御部28は、現時刻のレーザ走査による1ライン分の反射点座標の各々に対する変換後の座標の各々から求められる差分移動ベクトルについて、カウント値のカウントアップが終了したと判断すると、制御部28は、比較器ツリー24に対して、最大のカウント値を有するカウンタCを特定して、特定されたカウンタCに対応するカウント値、差分移動ベクトル(dx、dz)、及び回転運動量θを出力するように指示する。   When the control unit 28 determines that the count-up of the count value is completed for the difference movement vector obtained from each of the coordinates after conversion with respect to each of the reflection point coordinates for one line by the laser scanning at the current time, the control unit 28 Identifies the counter C having the maximum count value and outputs the count value corresponding to the identified counter C, the difference movement vector (dx, dz), and the rotational momentum θ to the comparator tree 24. To instruct.

そして、演算器624により、比較器ツリー24から出力された差分移動ベクトル(dx、dz)と、現時点のベース移動ベクトルとして用いられている1時刻前の移動ベクトルの並進成分(dx、dz)とを加算して求めた並進成分(dx、dz)と、同じく、比較器ツリー24から出力された回転運動量θと1時刻前の移動ベクトルの回転成分(θ)とを加算して求めた回転成分とからなる移動ベクトル(dx、dz、θ)を出力する。   Then, the difference movement vector (dx, dz) output from the comparator tree 24 by the computing unit 624, and the translation component (dx, dz) of the movement vector one hour before used as the current base movement vector, , And the rotational component obtained by adding the rotational momentum θ output from the comparator tree 24 and the rotational component (θ) of the moving vector one time ago. A movement vector (dx, dz, θ) consisting of

出力部26により、演算器624から出力された移動ベクトルを、自車運動の推定値として出力する。   The output unit 26 outputs the movement vector output from the calculator 624 as an estimated value of the own vehicle motion.

以上説明したように、第6の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標から、ベース移動ベクトル(並進・回転成分)を差し引くと共に、仮定した回転運動量の候補を差し引いた反射点座標と、推定対象時点より1時刻前に得られたレーザの1走査分の複数の反射点座標とについて、それぞれの差分を演算して差分移動ベクトルを求め、所定範囲内の差分移動ベクトルの値及び回転運動量の候補の組み合わせの度数を計数し、最大度数を計数したカウンタに基づいて、回転運動量を含む自車運動を推定することにより、簡易なハードウエア構成で、回転運動量を含む自車運動推定の実時間処理を実現することができる。   As described above, according to the own vehicle motion estimation device according to the sixth embodiment, the base movement vector (translation / rotation component) is obtained from the reflection point coordinates with respect to the laser scanning at the estimation target time sequentially obtained from the laser radar. ), And the difference between the reflection point coordinates obtained by subtracting the assumed rotational momentum candidates and a plurality of reflection point coordinates for one scan of the laser obtained one hour before the estimation target time point is calculated. The difference movement vector is obtained, the difference movement vector value within a predetermined range and the number of rotation candidate combinations are counted, and the own vehicle movement including the rotation momentum is estimated based on the counter that has counted the maximum degree. Thus, real-time processing for estimating the own vehicle motion including the rotational momentum can be realized with a simple hardware configuration.

次に、本発明の第7の実施の形態について説明する。なお、第1の実施の形態と同様の構成となる部分については、第1の実施の形態と同様の符号を付して説明を省略する。   Next, a seventh embodiment of the present invention will be described. In addition, about the part which becomes the structure similar to 1st Embodiment, the code | symbol similar to 1st Embodiment is attached | subjected and description is abbreviate | omitted.

第7の実施の形態では、複数のカウンタが、移動ベクトルの指定された部分桁について、部分桁の値毎の度数を計数している点が、第1の実施の形態と異なっている。   The seventh embodiment is different from the first embodiment in that a plurality of counters count the frequency for each partial digit value for the partial digit designated by the movement vector.

図13に示すように、第7の実施の形態に係る自車運動推定装置710は、レジスタA0〜A63と、レジスタBと、複数の演算器20と、移動ベクトルの上位桁の範囲を指定するデータを保持する範囲指定レジスタ718と、演算器20の出力が、指定された移動ベクトルの範囲内であるか否かを判定する複数の比較器720と、カウンタC0〜C255と、複数のデコーダ722と、比較器ツリー24と、結合部724と、出力部26と、制御部28とを備えている。なお、比較器720が、判定器の一例である。   As shown in FIG. 13, the own vehicle motion estimation device 710 according to the seventh embodiment specifies registers A0 to A63, a register B, a plurality of arithmetic units 20, and a range of upper digits of the movement vector. A range designation register 718 that holds data, a plurality of comparators 720 that determine whether the output of the arithmetic unit 20 is within the range of the designated movement vector, counters C0 to C255, and a plurality of decoders 722 A comparator tree 24, a combining unit 724, an output unit 26, and a control unit 28. The comparator 720 is an example of a determiner.

範囲指定レジスタ718には、移動ベクトル(dx、dz)のdx成分の上位桁(例えば、上位3桁)の最大値及び最小値、及びdy成分の上位桁(例えば、上位3桁)の最大値及び最小値が格納され、移動ベクトルの上位桁の範囲が指定される。   The range designation register 718 includes a maximum value and a minimum value of upper digits (eg, upper 3 digits) of a dx component of a movement vector (dx, dz), and a maximum value of an upper digit (eg, upper 3 digits) of a dy component. And the minimum value are stored, and the range of the upper digit of the movement vector is designated.

比較器720の個数は、演算器20と同数であり、複数の比較器720と複数の演算器20とが一対一に対応して接続されており、各比較器720は、対応する演算器20で求めた移動ベクトルの上位桁が、範囲指定レジスタ718に指定された範囲内であるかを判定し、範囲指定レジスタ718に指定された範囲内である場合に、対応するデコーダ722が動作するように動作許可信号を出力する。複数の比較器720による1スキャン分の比較判定が並列に行われる。   The number of comparators 720 is the same as that of the arithmetic units 20, and a plurality of comparators 720 and a plurality of arithmetic units 20 are connected in a one-to-one correspondence, and each comparator 720 has a corresponding arithmetic unit 20. It is determined whether the high-order digit of the movement vector obtained in step 4 is within the range specified in the range specification register 718, and when it is within the range specified in the range specification register 718, the corresponding decoder 722 operates. An operation permission signal is output to. Comparison determination for one scan by the plurality of comparators 720 is performed in parallel.

また、カウンタC0〜C255は、移動ベクトル(dx、dz)の部分桁の値ごとに設けられている。移動ベクトルの部分桁の各値とカウンタC0〜C255との対応関係は、固定的に決めておくのではなく、指定できるようにしておく。例えば、設定1では移動ベクトル(0,0)をカウンタにC0に、移動ベクトル(0,1)をカウンタC1に、移動ベクトル(0,2)をカウンタC2・・・と対応させ、設定2では移動ベクトル(0,4)と(0,5)をカウンタC2・・・と対応させておき、設定1を使用するか設定2を使用するかは、実行時に選択できるように構成しておく。   Counters C0 to C255 are provided for each partial digit value of the movement vector (dx, dz). The correspondence between each value of the partial digits of the movement vector and the counters C0 to C255 is not fixedly determined but can be specified. For example, in the setting 1, the movement vector (0, 0) corresponds to the counter C0, the movement vector (0, 1) corresponds to the counter C1, the movement vector (0, 2) corresponds to the counter C2,. The movement vectors (0, 4) and (0, 5) are made to correspond to the counter C2..., So that whether to use setting 1 or setting 2 can be selected at the time of execution.

各デコーダ722は、対応する比較器720から動作許可信号が入力された場合に、対応する演算器20で求められた移動ベクトルの指定された部分桁の値に対応するカウンタCを特定し、特定したカウンタCの値をカウントアップする。複数のデコーダ22による1スキャン分のカウントアップが並列に行われる。デコーダ722は、対応する比較器720から動作許可信号が入力されない場合には、求めた移動ベクトルの上位桁が所定の範囲内に入っていないため、カウントアップ動作を行わない。   Each decoder 722 specifies the counter C corresponding to the value of the specified partial digit of the movement vector obtained by the corresponding arithmetic unit 20 when the operation permission signal is input from the corresponding comparator 720, and specifies The value of the counter C is counted up. Count-up for one scan by the plurality of decoders 22 is performed in parallel. When the operation permission signal is not input from the corresponding comparator 720, the decoder 722 does not perform the count-up operation because the upper digit of the obtained movement vector is not within the predetermined range.

デコーダ722には、制御部28から部分桁が指定される。例えば、移動ベクトル(dx、dz)の指定された部分桁(例えば、dxの上位3桁とdzの上位3桁)を採用する。これによって、カウンタC0〜C255により、移動ベクトルの上位3桁の値に関するヒストグラムが作成される。また、部分桁の指定は、上位桁だけに限らず、中間部分(例えば下位4桁目〜3桁目の2桁)や下位部分が指定される。   A partial digit is designated in the decoder 722 from the control unit 28. For example, the designated partial digits (for example, the upper 3 digits of dx and the upper 3 digits of dz) of the movement vector (dx, dz) are adopted. Thus, a histogram relating to the upper three digits of the movement vector is created by the counters C0 to C255. Also, the designation of the partial digits is not limited to the upper digits, but the intermediate portion (for example, the lower fourth digit to the second digit, the second digit) or the lower portion is designated.

比較器ツリー24における基本ユニットの出力はカウント値と移動ベクトルの指定された部分桁(dx、dz)とのセットである。   The output of the basic unit in the comparator tree 24 is a set of count values and designated partial digits (dx, dz) of the movement vector.

結合部724は、比較器ツリー24から出力される移動ベクトルの部分桁(dx、dz)に、指定された部分桁より上位桁の予め求められた値を結合して、結合された移動ベクトルを出力する。   The combining unit 724 combines the value obtained in advance of the designated partial digits with the partial digits (dx, dz) of the movement vector output from the comparator tree 24, and outputs the combined movement vector. Output.

出力部26は、結合部724から出力される移動ベクトルを、自車運動の推定値として出力する。   The output unit 26 outputs the movement vector output from the combining unit 724 as an estimated value of the own vehicle motion.

また、制御部28は、以下に説明するように、指定する移動ベクトルの部分桁、及びカウンタCと移動ベクトルの部分桁の値との対応関係を適応的に設定する。   Further, as described below, the control unit 28 adaptively sets the correspondence between the partial digit of the movement vector to be designated and the value of the counter C and the partial digit of the movement vector.

例えば、自車運動(簡単のため車速のみで説明)の範囲が0〜40m/sとし、真値が3.7m/sであったとする。また、簡単のため、10進数での部分桁を指定するものとする。この前提で、0.1m/sの桁まで推定しようとすると、ヒストグラムの分割が0.1m/s(より細かい)刻みでなければならない。0〜40を0.1刻みで分割すると、ヒストグラムのビン数は401必要である。   For example, it is assumed that the range of own vehicle motion (explained only by the vehicle speed for simplicity) is 0 to 40 m / s, and the true value is 3.7 m / s. For simplicity, it is assumed that partial digits in decimal numbers are designated. Under this premise, when trying to estimate to the order of 0.1 m / s, the division of the histogram must be in increments of 0.1 m / s (finer). If 0 to 40 are divided in increments of 0.1, 401 histogram bins are required.

ところが、1m/sの桁までの推定で良ければ、1刻みで分割し、ビン数は41でよい。   However, if estimation up to a digit of 1 m / s is sufficient, division is performed in increments of 41 and the number of bins may be 41.

そこで、まず制御部28は、移動ベクトルの部分桁として、上位桁(上位2桁)を指定すると共に、ビン数41のヒストグラム(0,1,2,・・・40)に対し、移動ベクトルが示す車速を投票するように、カウンタCと移動ベクトルの部分桁の値との対応関係を設定する。上記の例では、最大度数の車速を探索すると3m/s(真値を3.7と仮定しているので)という結果が得られる。   Therefore, first, the control unit 28 designates the upper digit (upper two digits) as a partial digit of the movement vector, and the movement vector is set to the histogram (0, 1, 2,... 40) having 41 bins. Correspondence between the counter C and the value of the partial digit of the movement vector is set so as to vote the indicated vehicle speed. In the above example, searching for the maximum vehicle speed yields a result of 3 m / s (because the true value is assumed to be 3.7).

この段階では、真値は3.0〜3.9 m/sの範囲にあるものと考えられる。   At this stage, the true value is considered to be in the range of 3.0 to 3.9 m / s.

数回のスキャンにわたって、毎回3m/sという同じ自車運動推定結果が得られる場合に、その精度での信頼度は高いと考えられるため、制御部28は、移動ベクトルの部分桁として、中位桁(0.1の位の1桁)を指定すると共に、ヒストグラムの分割を変更し3.0〜3.9の範囲を10分割したビン構成で投票処理を行うように、カウンタCと移動ベクトルの部分桁の値との対応関係を設定する。   When the same vehicle motion estimation result of 3 m / s is obtained every time over several scans, it is considered that the reliability in the accuracy is high. The counter C and the movement vector are designated so that the voting process is performed with a bin configuration in which the digit (one digit of 0.1) is specified and the division of the histogram is changed and the range from 3.0 to 3.9 is divided into ten. Set the correspondence with the value of the partial digit.

このとき、最大度数となるのは、0.7のビンであり、移動ベクトルの上位桁(基礎運動量)である3m/sと結合して、3.7m/sという自車運動推定結果が得られる。この段階では、真値は3.70〜3.79m/sの範囲にあるものと考えられる。   At this time, the maximum frequency is a bin of 0.7, which is combined with 3 m / s, which is the upper digit (basic momentum) of the movement vector, to obtain a vehicle motion estimation result of 3.7 m / s. It is done. At this stage, the true value is considered to be in the range of 3.70 to 3.79 m / s.

以降、制御部28が同様に処理することにより、順次、精度を高めることができる。   Thereafter, the control unit 28 performs the same processing, so that the accuracy can be improved sequentially.

また、上記のように、移動ベクトルの部分桁の値に結合する上位桁の値は、当該部分桁より上位桁の値であって、数回のスキャンにわたって同じ値が得られた自車運動推定結果から求められる。   Further, as described above, the value of the upper digit combined with the value of the partial digit of the movement vector is a value of the higher digit than the partial digit, and the same value is obtained over several scans. Calculated from the results.

なお、移動ベクトルの部分桁として中位桁を指定する場合のビン構成は3.0〜3.9を含んでいればよく、2.0〜5.0の範囲を0.1単位で分割したビン構成で投票処理を行うように、カウンタCと移動ベクトルの部分桁の値との対応関係を設定しても良い。この場合、移動ベクトルの上位桁(基礎運動量)としては2.0m/sに設定するのが良く、ビン構成は0.0〜3.0の31ビンとなる。   In addition, the bin configuration in the case of designating the middle digit as the partial digit of the movement vector only needs to include 3.0 to 3.9, and the range of 2.0 to 5.0 is divided by 0.1 unit. The correspondence between the counter C and the value of the partial digit of the movement vector may be set so that the voting process is performed in the bin configuration. In this case, the upper digit (basic momentum) of the movement vector is preferably set to 2.0 m / s, and the bin configuration is 31 bins of 0.0 to 3.0.

以上、10進数で説明したが、ハードウエア構成を考慮すると、2進数で切りの良い数値(10ではなく、2のベキ乗数)で分割するのが良い。   As described above, the decimal number is used. However, in consideration of the hardware configuration, it is preferable to divide by a numerical value that is a good number in binary (not a 10 but a power of 2).

自車の運動量は急激に変化しないため、上記のように部分桁の指定を使って、最初は粗い自車運動推定を行い、順次推定精度を上げるように、指定する移動ベクトルの部分桁、及びカウンタCと移動ベクトルの部分桁の値との対応関係を設定する。このとき、精度を上げるためにカウンタの数を増やす必要がない。   Since the amount of movement of the own vehicle does not change abruptly, the partial digit of the movement vector to be specified is specified so as to perform rough estimation of the own vehicle motion at first by using the designation of the partial digit as described above, and to sequentially increase the estimation accuracy. The correspondence between the counter C and the value of the partial digit of the movement vector is set. At this time, it is not necessary to increase the number of counters in order to increase accuracy.

次に、第7の実施の形態の自車運動推定装置710の動作について説明する。   Next, operation | movement of the own vehicle motion estimation apparatus 710 of 7th Embodiment is demonstrated.

まず、レジスタA0〜A63レジスタには、レジスタ制御部によって、1時刻前に得られた1走査分の反射点座標が格納される。レジスタBには、次の走査(現時刻の走査)の反射点座標が1点ずつ、レジスタ制御部によって格納される。なお、処理開始の指示は、制御部28によって行われる。   First, in the registers A0 to A63, the reflection point coordinates for one scan obtained one time before by the register control unit are stored. The register B stores the reflection point coordinates of the next scan (scan at the current time) one by one in the register B. The processing start instruction is given by the control unit 28.

そして、複数の演算器20によって、レジスタBの座標と、レジスタA0〜A63が保持している座標との間で移動ベクトルが求められる。64個の移動ベクトルが同時並列に求められる。複数の比較器720によって、範囲指定レジスタ718の内容に基づいて、複数の演算器20から求められた64個の移動ベクトルの上位桁が、指定された範囲内であるか否かを判定し、肯定判定された場合に、動作許可信号をデコーダ722に出力する。   Then, a plurality of arithmetic units 20 obtain a movement vector between the coordinates of the register B and the coordinates held by the registers A0 to A63. 64 movement vectors are obtained in parallel. The plurality of comparators 720 determine whether or not the upper digits of the 64 movement vectors obtained from the plurality of computing units 20 are within the designated range based on the contents of the range designation register 718. When an affirmative determination is made, an operation permission signal is output to the decoder 722.

動作許可信号が入力された複数のデコーダ722によって、対応する複数の演算器20から求められた複数の移動ベクトルの、制御部28より指定された部分桁の値から、それぞれの投票先(ヒストグラムの対応するビン)のカウンタCを特定し、投票先の指示があったカウンタCのカウント値を+1する。   From the partial digit values designated by the control unit 28 of the plurality of movement vectors obtained from the plurality of corresponding computing units 20 by the plurality of decoders 722 to which the operation permission signals are input, The counter C of the corresponding bin) is specified, and the count value of the counter C at which the vote destination is instructed is incremented by one.

制御部28によって、現時刻のレーザ走査による1ライン分の反射点座標から求められる移動ベクトルについて、カウント値のカウントアップが終了したと判断されると、制御部28は、比較器ツリー24に対して、最大のカウント値を有するカウンタCを特定して、特定されたカウンタCに対応するカウント値及び移動ベクトル(部分桁)を出力するように指示する。   When the control unit 28 determines that the count value has been counted up for the movement vector obtained from the reflection point coordinates for one line by the laser scanning at the current time, the control unit 28 The counter C having the maximum count value is specified, and an instruction is given to output the count value and movement vector (partial digit) corresponding to the specified counter C.

そして、結合部724により、比較器ツリー24から出力された移動ベクトル(部分桁)と、当該部分桁より上位の上位桁の値とを結合して求めた移動ベクトルを出力する。   The combining unit 724 outputs a movement vector obtained by combining the movement vector (partial digit) output from the comparator tree 24 and the value of the upper digit higher than the partial digit.

そして、出力部26により、結合部724から出力された移動ベクトルを、自車運動の推定値として出力する。   Then, the output unit 26 outputs the movement vector output from the combining unit 724 as an estimated value of the own vehicle motion.

このとき、制御部28によって、図14に示す部分桁指定処理ルーチンが実行される。まず、ステップS700において、複数のデコーダ722に対して上位の部分桁を指定すると共に、当該上位の部分桁に応じたカウンタCと部分桁の値との対応関係を設定する。このとき、結合部724に対しては、結合する上位桁がないものとして設定される。   At this time, the partial digit designation processing routine shown in FIG. First, in step S700, the upper partial digits are designated for the plurality of decoders 722, and the correspondence between the counter C and the partial digit values corresponding to the upper partial digits is set. At this time, the combination unit 724 is set as having no upper digit to be combined.

そして、ステップS702において、結合部724からの複数回の出力に基づいて、所定回数連続して同じ出力であるか否かを判定する。同じ出力が所定回数連続してなかった場合には、自車運動推定結果の信頼度が高くないと判断し、上記ステップS700に戻るが、一方、同じ出力が所定回数連続した場合には、自車運動推定結果の信頼度が高いと判断し、ステップS704へ移行する。   In step S <b> 702, based on a plurality of outputs from the combining unit 724, it is determined whether or not the outputs are the same continuously for a predetermined number of times. If the same output has not been repeated for a predetermined number of times, it is determined that the reliability of the own vehicle motion estimation result is not high, and the process returns to step S700. It is determined that the reliability of the vehicle motion estimation result is high, and the process proceeds to step S704.

ステップS704では、複数のデコーダ722に対して中位の部分桁を指定すると共に、当該中位の部分桁に応じたカウンタCと部分桁の値との対応関係を設定する。このとき、結合部724に対しては、上記ステップS702で得られた結合部724からの出力が、結合する上位桁として設定される。   In step S704, a middle partial digit is designated for the plurality of decoders 722, and the correspondence between the counter C and the partial digit value corresponding to the middle partial digit is set. At this time, for the combining unit 724, the output from the combining unit 724 obtained in step S702 is set as the upper digit to be combined.

そして、ステップS706において、結合部724からの複数回の出力に基づいて、所定回数連続して同じ出力であるか否かを判定する。同じ出力が所定回数連続してなかった場合には、自車運動推定結果の信頼度が高くないと判断し、上記ステップS700に戻るが、一方、同じ出力が所定回数連続した場合には、自車運動推定結果の信頼度が高いと判断し、ステップS708へ移行する。   In step S706, it is determined based on the plurality of outputs from the combining unit 724 whether or not the outputs are the same for a predetermined number of times. If the same output has not been repeated for a predetermined number of times, it is determined that the reliability of the own vehicle motion estimation result is not high, and the process returns to step S700. It is determined that the reliability of the vehicle motion estimation result is high, and the process proceeds to step S708.

ステップS708では、複数のデコーダ722に対して下位の部分桁を指定すると共に、当該下位の部分桁に応じたカウンタCの対応関係を設定する。このとき、結合部724に対しては、上記ステップS706で得られた結合部724からの出力が、結合する上位桁として設定される。   In step S708, the lower partial digits are designated for the plurality of decoders 722, and the correspondence relationship of the counter C corresponding to the lower partial digits is set. At this time, for the combining unit 724, the output from the combining unit 724 obtained in step S706 is set as the upper digit to be combined.

そして、ステップS710において、結合部724から所定個の出力を得たか否かを判定し、所定個の出力を得たと判定されると、上記ステップS700へ戻り、部分桁としての上位桁の指定から再度開始する。   In step S710, it is determined whether or not a predetermined number of outputs have been obtained from the combining unit 724. If it is determined that the predetermined number of outputs have been obtained, the process returns to step S700, and the upper digit is designated as a partial digit. Start again.

以上説明したように、第7の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標と、推定対象時点より1時刻前に得られたレーザの1走査分の複数の反射点座標とについて、それぞれの差分を演算して移動ベクトルを求め、上位桁が所定範囲内の移動ベクトルの部分桁の各値の度数を計数し、最大度数を計数したカウンタに基づいて、自車運動を推定することにより、簡易なハードウエア構成で、自車運動推定の実時間処理を実現することができる。   As described above, according to the own vehicle motion estimation device according to the seventh embodiment, the reflection point coordinates for the laser scanning at the estimation target time points sequentially obtained from the laser radar, and one time before the estimation target time point. With respect to a plurality of reflection point coordinates for one scanning of the obtained laser, the respective differences are calculated to obtain a movement vector, and the upper digit counts the frequency of each value of the partial digits of the movement vector within a predetermined range, By estimating the own vehicle motion based on the counter that counts the maximum frequency, real-time processing of the own vehicle motion estimation can be realized with a simple hardware configuration.

また、移動ベクトルの指定した部分桁をカウント対象とすることで、必要な精度に合わせた部分桁を指定することが可能となる。このことにより、最初は、上位の部分桁をカウント対象に指定することで、低い精度の自車運動推定を行い、次スキャンでは下位の部分桁をカウント対象に指定することで、順次精度を高める自車運動推定が可能となり、自車運動推定結果の演算精度を向上させることができる。   In addition, by making the partial digits designated by the movement vector to be counted, it becomes possible to designate partial digits that match the required accuracy. As a result, first, the higher-order partial digits are designated as count targets, and low-accuracy vehicle motion estimation is performed. In the next scan, the lower-order partial digits are designated as count targets, and the accuracy is sequentially increased. The own vehicle motion estimation is possible, and the calculation accuracy of the own vehicle motion estimation result can be improved.

次に、本発明の第8の実施の形態について説明する。なお、第1の実施の形態、第5の実施の形態、及び第7の実施の形態と同様の構成となる部分については、上記の実施の形態と同様の符号を付して説明を省略する。   Next, an eighth embodiment of the present invention will be described. In addition, about the part which becomes the structure similar to 1st Embodiment, 5th Embodiment, and 7th Embodiment, the code | symbol similar to said embodiment is attached | subjected, and description is abbreviate | omitted. .

第8の実施の形態では、ベース移動ベクトルを差し引いた現時点の反射点座標を用いて、差分移動ベクトルを求めている点が、第7の実施の形態と異なっている。   The eighth embodiment is different from the seventh embodiment in that the difference movement vector is obtained using the current reflection point coordinates obtained by subtracting the base movement vector.

図15に示すように、第8の実施の形態に係る自車運動推定装置810は、レジスタA0〜A63と、レジスタBBと、ベース移動ベクトルを保持するレジスタBVと、演算器512と、レジスタBと、複数の演算器20と、範囲指定レジスタ418と、複数の比較器820と、カウンタC0〜C255と、複数のデコーダ722と、比較器ツリー24と、演算器524と、出力部26と、制御部28と、を備えている。なお、比較器820が、判定器の一例である。演算器524が、結合部の一例である。   As shown in FIG. 15, the own vehicle motion estimation apparatus 810 according to the eighth embodiment includes registers A0 to A63, a register BB, a register BV that holds a base movement vector, a calculator 512, and a register B. A plurality of computing units 20, a range designation register 418, a plurality of comparators 820, counters C0 to C255, a plurality of decoders 722, a comparator tree 24, a computing unit 524, an output unit 26, And a control unit 28. The comparator 820 is an example of a determiner. The calculator 524 is an example of a coupling unit.

レジスタBBには、現在の反射点座標(x、z)が保持される。レジスタBVには、仮定されるベース移動ベクトルが保持される。   The register BB holds the current reflection point coordinates (x, z). The assumed base movement vector is held in the register BV.

演算器512は、レジスタBBに保持された反射点座標から、ベース移動ベクトルを差し引いて、1時刻前の反射点座標を推定し、レジスタBに保存する。   The arithmetic unit 512 subtracts the base movement vector from the reflection point coordinates held in the register BB, estimates the reflection point coordinates one hour before, and stores them in the register B.

各演算器20は、対応するレジスタAが保持する座標とレジスタBが保持している座標との間で引き算をし、2点間の差分移動ベクトルを求めて出力する。   Each computing unit 20 performs subtraction between the coordinates held by the corresponding register A and the coordinates held by the register B, and obtains and outputs a difference movement vector between the two points.

各比較器820は、対応する演算器20で求めた差分移動ベクトルの上位桁(例えば、上位3桁)が、0であるかを判定し、差分移動ベクトルの上位桁が、0である場合に、対応するデコーダ722が動作するように動作許可信号を出力する。ここで0とは、ヒストグラム対象外の上位桁に有意の桁が含まれていないことを意味し、差分移動ベクトルが正数の場合は上位桁が2進数で“000・・・”であるかを判定することであるが、負数の場合は(2の補数表現であるとして)2進数で“111・・・”であるかを判定することに注意されたい。また、複数の比較器820による1スキャン分の比較判定が並列に行われる。なお、ヒストグラム対象外の上位桁の指定は、制御部28により行われる。   Each comparator 820 determines whether or not the upper digit (for example, the upper three digits) of the difference movement vector obtained by the corresponding computing unit 20 is 0, and when the upper digit of the difference movement vector is 0. The operation permission signal is output so that the corresponding decoder 722 operates. Here, 0 means that no significant digit is included in the upper digits that are not included in the histogram. If the difference movement vector is a positive number, is the upper digit a binary number “000...” Note that in the case of a negative number, it is determined whether it is “111...” In a binary number (assuming that it is a two's complement expression). Further, the comparison determination for one scan by the plurality of comparators 820 is performed in parallel. Note that the control unit 28 designates the upper digits that are not subject to histogram.

また、カウンタC0〜C255は、差分移動ベクトル(dx、dz)の部分桁の値ごとに設けられている。差分移動ベクトルの部分桁の各値とカウンタC0〜C255との対応関係は、固定的に決めておくのではなく、指定できるようにしておく。例えば、設定1では差分移動ベクトル(0,0)をカウンタにC0に、差分移動ベクトル(0,1)をカウンタC1に、差分移動ベクトル(0,2)をカウンタC2・・・と対応させ、設定2では差分移動ベクトル(0,4)と(0,5)をカウンタC2・・・と対応させておき、設定1を使用するか設定2を使用するかは、実行時に選択できるように構成しておく。   Counters C0 to C255 are provided for each partial digit value of the differential movement vector (dx, dz). The correspondence between each value of the partial digits of the differential movement vector and the counters C0 to C255 is not fixedly determined but can be specified. For example, in setting 1, the differential movement vector (0, 0) is associated with the counter C0, the differential movement vector (0, 1) is associated with the counter C1, the differential movement vector (0, 2) is associated with the counter C2,. In the setting 2, the difference movement vectors (0, 4) and (0, 5) are made to correspond to the counter C2,..., So that whether to use the setting 1 or the setting 2 can be selected at the time of execution. Keep it.

各デコーダ722は、対応する比較器820から動作許可信号が入力された場合に、対応する演算器20で求められた差分移動ベクトルの指定された部分桁の値に対応するカウンタCを特定し、特定したカウンタCの値をカウントアップする。複数のデコーダ22による1スキャン分のカウントアップが並列に行われる。デコーダ722は、対応する比較器820から動作許可信号が入力されない場合には、求めた差分移動ベクトルの上位桁が0でないため、カウントアップ動作を行わない。   Each decoder 722 specifies a counter C corresponding to the value of the designated partial digit of the differential movement vector obtained by the corresponding computing unit 20 when the operation permission signal is input from the corresponding comparator 820, The value of the specified counter C is counted up. Count-up for one scan by the plurality of decoders 22 is performed in parallel. When the operation permission signal is not input from the corresponding comparator 820, the decoder 722 does not perform the count-up operation because the upper digit of the obtained difference movement vector is not 0.

デコーダ722には、制御部28から部分桁が指定される。例えば、差分移動ベクトル(dx、dz)の指定された部分桁(例えば、dxの上位4桁目〜6桁目の中位3桁とdzの上位4桁目〜6桁目の中位3桁)を採用する。これによって、カウンタC0〜C255により、差分移動ベクトルの中位3桁の値に関するヒストグラムが作成される。また、部分桁の指定は、中位桁だけに限らず、当該中位桁より下位部分(例えば下位4桁目〜3桁目の2桁や、更に下位桁)も指定される。   A partial digit is designated in the decoder 722 from the control unit 28. For example, the specified partial digits of the differential movement vector (dx, dz) (for example, the middle 3 digits of the upper 4th to 6th digits of dx and the middle 3 digits of the upper 4th to 6th digits of dz) ). As a result, the counter C0 to C255 creates a histogram relating to the middle three digits of the difference movement vector. In addition, the designation of the partial digit is not limited to the middle digit, and a lower part (for example, the lower fourth digit to the third digit and the lower digits) is designated.

比較器ツリー24から出力される差分移動ベクトルは、レジスタB(1時刻前を推定した座標)までの移動ベクトルであるため、推定に用いたベース移動ベクトルに差分移動ベクトルを加えたものが自車運動の推定値となる。したがって、演算器524は、比較器ツリー24から出力される差分移動ベクトルの部分桁に、1時刻前の自車運動の推定値である移動ベクトルを加算して、求められた移動ベクトルを出力する。これによって、差分移動ベクトルの部分桁の値に、部分桁より上位桁の値が結合された移動ベクトルが求められる。   Since the difference movement vector output from the comparator tree 24 is a movement vector up to the register B (coordinate estimated one time before), the vehicle obtained by adding the difference movement vector to the base movement vector used for estimation This is an estimate of the movement. Accordingly, the arithmetic unit 524 adds the movement vector that is the estimated value of the own vehicle motion one hour before to the partial digits of the difference movement vector output from the comparator tree 24, and outputs the obtained movement vector. . As a result, a movement vector obtained by combining the value of the partial digit of the difference movement vector with the value of the higher digit than the partial digit is obtained.

また、制御部28は、指定する差分移動ベクトルの部分桁、及びカウンタCと部分桁の値との対応関係を適応的に設定する。   Further, the control unit 28 adaptively sets the partial digit of the designated differential movement vector and the correspondence between the counter C and the value of the partial digit.

例えば、制御部28は、差分移動ベクトルの部分桁として、中位桁(上位4桁目〜6桁の3桁)を指定すると共に、カウンタCと部分桁の値との対応関係を設定する。数回のスキャンにわたって、毎回同じ自車運動推定結果が得られる場合に、制御部28は、差分移動ベクトルの部分桁として、上記中位桁より下位の部分桁(上位7桁目〜9桁の3桁)を指定すると共に、カウンタCと部分桁の値との対応関係を設定する。   For example, the control unit 28 designates the middle digit (three digits from the upper fourth digit to the sixth digit) as the partial digit of the differential movement vector, and sets the correspondence between the counter C and the value of the partial digit. When the same own vehicle motion estimation result is obtained every time over several scans, the control unit 28, as a partial digit of the difference movement vector, is a partial digit lower than the middle digit (upper seventh digit to ninth digit). (3 digits) is specified, and the correspondence between the counter C and the value of the partial digit is set.

また、数回のスキャンにわたって、毎回同じ自車運動推定結果が得られる場合に、制御部28は、差分移動ベクトルの部分桁として、さらに下位の部分桁(上位10桁目〜12桁の3桁)を指定すると共に、カウンタCと部分桁の値との対応関係を設定する。   In addition, when the same own vehicle motion estimation result is obtained every time over several scans, the control unit 28 uses the lower partial digits (the upper 10th digit to the 12th digit 3 digits) as the partial digits of the differential movement vector. ) Is specified, and the correspondence between the counter C and the partial digit value is set.

次に、第8の実施の形態の自車運動推定装置810の動作について説明する。   Next, the operation of the own vehicle motion estimation device 810 according to the eighth embodiment will be described.

まず、レジスタA0〜A63レジスタには、レジスタ制御部によって、1時刻前に得られた1走査分の反射点座標が格納される。また、レジスタ制御部によって、1時刻前に推定された自車運動の推定値である移動ベクトルが、レジスタBVに格納される。レジスタBBには、次の走査(現時刻の走査)の反射点座標が1点ずつ、レジスタ制御部によって格納される。そして、演算器512によって、レジスタBBの反射点座標から、レジスタBVの移動ベクトルを差し引いた座標を求め、レジスタBに格納する。   First, in the registers A0 to A63, the reflection point coordinates for one scan obtained one time before by the register control unit are stored. In addition, a movement vector that is an estimated value of the own vehicle motion estimated one time before by the register control unit is stored in the register BV. In the register BB, the reflection point coordinates of the next scan (scan at the current time) are stored one by one by the register control unit. Then, the arithmetic unit 512 obtains coordinates obtained by subtracting the movement vector of the register BV from the reflection point coordinates of the register BB, and stores them in the register B.

そして、複数の演算器20によって、レジスタBの座標と、レジスタA0〜A63が保持している座標との間で差分移動ベクトルが求められる。64個の差分移動ベクトルが同時並列に求められる。複数の比較器820によって、複数の演算器20から求められた64個の差分移動ベクトルの指定された上位桁が、0であるか否かを判定し、肯定判定された場合に、動作許可信号を対応するデコーダ722に出力する。   Then, a difference movement vector is obtained between the coordinates of the register B and the coordinates held by the registers A0 to A63 by the plurality of arithmetic units 20. 64 differential motion vectors are obtained in parallel. The plurality of comparators 820 determine whether or not the designated high-order digits of the 64 differential movement vectors obtained from the plurality of computing units 20 are 0, and when an affirmative determination is made, the operation permission signal Are output to the corresponding decoder 722.

動作許可信号が入力された複数のデコーダ722によって、対応する複数の演算器20から求められた複数の差分移動ベクトルの、制御部28より指定された部分桁の値から、それぞれの投票先(ヒストグラムの対応するビン)のカウンタCを特定し、投票先の指示があったカウンタCのカウント値を+1する。   From the partial digit values designated by the control unit 28 of the plurality of differential movement vectors obtained from the plurality of corresponding calculators 20 by the plurality of decoders 722 to which the operation permission signals are input, The counter C of the corresponding bin) is specified, and the count value of the counter C instructed by the vote destination is incremented by one.

制御部28によって、現時刻のレーザ走査による1ライン分の反射点座標を用いて求められる差分移動ベクトルについて、カウント値のカウントアップが終了したと判断されると、制御部28は、比較器ツリー24に対して、最大のカウント値を有するカウンタCを特定して、特定されたカウンタCに対応するカウント値及び差分移動ベクトルの部分桁の値を出力するように指示する。   When the control unit 28 determines that the count value has been counted up for the differential movement vector obtained using the reflection point coordinates for one line by the laser scanning at the current time, the control unit 28 24, the counter C having the maximum count value is specified, and the count value corresponding to the specified counter C and the value of the partial digit of the differential movement vector are output.

そして、演算器524により、比較器ツリー24から出力された差分移動ベクトルの部分桁と、現時点のベース移動ベクトルとして用いられている1時刻前の移動ベクトルとを加算して求めた移動ベクトルを出力する。   Then, the arithmetic unit 524 outputs the movement vector obtained by adding the partial digit of the difference movement vector output from the comparator tree 24 and the movement vector one hour before used as the current base movement vector. To do.

出力部26により、演算器524から出力された移動ベクトルを、自車運動の推定値として出力する。   The output unit 26 outputs the movement vector output from the calculator 524 as an estimated value of the own vehicle motion.

このとき、制御部28によって、上記第7の実施の形態と同様に部分桁指定処理ルーチンが実行され、複数のデコーダ722に対して、中位の部分桁から、順次、下位の部分桁が指定される。   At this time, the control unit 28 executes the partial digit designation processing routine in the same manner as in the seventh embodiment, and sequentially designates the lower partial digits from the middle partial digits to the plurality of decoders 722. Is done.

以上説明したように、第8の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標から、ベース移動ベクトルを差し引いた反射点座標と、推定対象時点より1時刻前に得られたレーザの1走査分の複数の反射点座標とについて、それぞれの差分を演算して差分移動ベクトルを求め、所定範囲内の差分移動ベクトルの部分桁の値の度数を計数し、最大度数を計数したカウンタに基づいて、自車運動を推定することにより、簡易なハードウエア構成で、自車運動推定の実時間処理を実現することができる。   As described above, according to the host vehicle motion estimation device according to the eighth embodiment, the reflection point obtained by subtracting the base movement vector from the reflection point coordinates for the laser scanning at the estimation target time points sequentially obtained from the laser radar. A difference movement vector is obtained by calculating a difference between coordinates and a plurality of reflection point coordinates for one scan of the laser obtained one time before the estimation target time, and a portion of the difference movement vector within a predetermined range By counting the frequency of the digit value and estimating the vehicle movement based on the counter that has counted the maximum frequency, real-time processing of the vehicle movement estimation can be realized with a simple hardware configuration.

なお、上記の第5の実施の形態及び第8の実施の形態では、ベース移動ベクトルとして、直前に推定された自車運動量を用いる場合を例に説明したが、これに限定されるものではない。例えば、予め定められた基礎運動量を、ベース移動ベクトルとして用いてもよい。この場合には、演算器524により、比較器ツリー24から出力された差分移動ベクトルと、ベース移動ベクトルとして用いられる基礎運動量を加算して求めた移動ベクトルを出力すればよい。   In the fifth and eighth embodiments described above, the case where the own vehicle momentum estimated immediately before is used as the base movement vector is described as an example. However, the present invention is not limited to this. . For example, a predetermined basic momentum may be used as the base movement vector. In this case, the calculator 524 may output a movement vector obtained by adding the difference movement vector output from the comparator tree 24 and the basic momentum used as the base movement vector.

次に、本発明の第9の実施の形態について説明する。なお、第1の実施の形態、第6の実施の形態、第7の実施の形態、及び第8の実施の形態と同様の構成となる部分については、上記の実施の形態と同様の符号を付して説明を省略する。   Next, a ninth embodiment of the present invention will be described. In addition, about the part which becomes the structure similar to 1st Embodiment, 6th Embodiment, 7th Embodiment, and 8th Embodiment, the code | symbol similar to said embodiment is attached | subjected. A description thereof will be omitted.

第9の実施の形態では、並進運動だけでなく、回転運動も推定している点と、レジスタA0〜A63にデータをセットする方法とが、第8の実施の形態と異なっている。   The ninth embodiment is different from the eighth embodiment in that not only translational motion but also rotational motion is estimated and a method of setting data in the registers A0 to A63.

図16に示すように、第9の実施の形態に係る自車運動推定装置910は、レジスタA0〜A63と、レジスタBBと、複数のレジスタB0〜B63と、レジスタBVAと、レジスタBと、座標変換部612と、複数の演算器20と、複数の比較器820と、カウンタC0〜C255と、複数のデコーダ722と、比較器ツリー24と、演算器624と、出力部26と、制御部28と、を備えている。演算器624が、結合部の一例である。   As shown in FIG. 16, the host vehicle motion estimation apparatus 910 according to the ninth embodiment includes a register A0 to A63, a register BB, a plurality of registers B0 to B63, a register BVA, a register B, and coordinates. The conversion unit 612, the plurality of arithmetic units 20, the plurality of comparators 820, the counters C0 to C255, the plurality of decoders 722, the comparator tree 24, the arithmetic unit 624, the output unit 26, and the control unit 28 And. The calculator 624 is an example of a coupling unit.

座標変換部612は、レジスタBBに保持された反射点座標から、ベース移動ベクトルの並進・回転成分(dx、dz、θ)を差し引いて、1時刻前の反射点座標を推定すると共に、制御部28の制御により、推定された1時刻前の反射点座標を、仮定した回転運動量θの複数候補の各々について、当該候補の分だけ差し引いた座標に変換し、レジスタBに保持する。   The coordinate conversion unit 612 subtracts the translation / rotation components (dx, dz, θ) of the base movement vector from the reflection point coordinates held in the register BB to estimate the reflection point coordinates one time ago, and the control unit Under the control of 28, the estimated reflection point coordinates one hour before are converted into coordinates obtained by subtracting the estimated number of candidates of the rotational momentum θ by the amount of the candidates, and are stored in the register B.

各演算器20は、回転運動量θの複数候補の各々について、対応するレジスタAが保持する座標と、レジスタB(座標変換部320によって変換された当該候補の分だけ差し引いた座標)との間で引き算をし、2点間の差分移動ベクトル(dx、dz)を求めて出力する。   For each of a plurality of candidates for rotational momentum θ, each computing unit 20 is between a coordinate held by the corresponding register A and a register B (coordinates subtracted by the candidate converted by the coordinate conversion unit 320). Subtraction is performed, and a difference movement vector (dx, dz) between two points is obtained and output.

各比較器820は、対応する演算器20で求めた差分移動ベクトルの上位桁(例えば、上位3桁)が、0であるかを判定し、差分移動ベクトルの上位桁が、0である場合に、対応するデコーダ722が動作するように動作許可信号を出力する。   Each comparator 820 determines whether or not the upper digit (for example, the upper three digits) of the difference movement vector obtained by the corresponding computing unit 20 is 0, and when the upper digit of the difference movement vector is 0. The operation permission signal is output so that the corresponding decoder 722 operates.

また、カウンタC0〜C255は、差分移動ベクトル(dx、dz)の部分桁の値及び回転運動量θの値の組み合わせ毎に設けられている。差分移動ベクトルの部分桁及び回転運動量の各組み合わせとカウンタC0〜C255との対応関係は、固定的に決めておくのではなく、指定できるようにしておく。   The counters C0 to C255 are provided for each combination of the partial digit value of the differential movement vector (dx, dz) and the value of the rotational momentum θ. The correspondence between each combination of the partial digit and the rotational momentum of the differential movement vector and the counters C0 to C255 is not fixedly determined but can be specified.

各デコーダ722は、対応する比較器720から動作許可信号が入力された場合に、対応する演算器20で求められた差分移動ベクトル(dx、dz)の指定された部分桁の値と、その差分移動ベクトルを求めるのに用いられた反射点座標の変換に用いられた回転運動量θの候補との組み合わせに対応するカウンタCを特定し、特定したカウンタCの値をカウントアップする。デコーダ722は、対応する比較器820から動作許可信号が入力されない場合には、求めた差分移動ベクトルの上位桁が0でないため、カウントアップ動作を行わない。   Each decoder 722, when an operation permission signal is input from the corresponding comparator 720, the value of the specified partial digit of the difference movement vector (dx, dz) obtained by the corresponding arithmetic unit 20 and its difference. The counter C corresponding to the combination with the candidate for the rotational momentum θ used for the conversion of the reflection point coordinates used for obtaining the movement vector is specified, and the value of the specified counter C is counted up. When the operation permission signal is not input from the corresponding comparator 820, the decoder 722 does not perform the count-up operation because the upper digit of the obtained difference movement vector is not 0.

デコーダ722には、制御部28から部分桁、及びカウンタCと部分桁の値との対応関係が指定される。   In the decoder 722, the correspondence between the partial digits and the value of the counter C and the partial digits is designated by the control unit 28.

制御部28は、レーザレーダ12から反射点座標が入力されて、1時刻前の反射点座標が推定され、更に回転運動量の候補を差し引いた反射点座標に変換される毎に、複数のデコーダ722による並列処理が行われるようにタイミング制御する。これにより、レーザレーダ12から反射点座標が入力されて1時刻前の反射点座標が推定され、更に回転運動量の候補を差し引いた反射点座標に変換される毎に、各演算器20による演算、複数の比較器820による上位桁の判定、複数のデコーダ722による並列処理、及びカウンタC0〜C255のカウントアップが繰り返し行われる。これを1スキャン分の反射点座標(例えば64点)について繰り返すことにより、1時刻前t0の各反射点から、1時刻前の反射点座標の推定値から回転運動量の各候補を差し引いて変換された現時刻t1の各反射点への全組み合せ分の差分移動ベクトルを求めると共に、指定範囲外の差分移動ベクトルを排除し、カウンタC0〜C255のカウント値により、指定範囲内の差分移動ベクトル及び回転運動量の組み合わせのヒストグラムが作成される。   The control unit 28 receives the reflection point coordinates from the laser radar 12, estimates the reflection point coordinates one time before, and further converts to the reflection point coordinates obtained by subtracting the rotational momentum candidates, a plurality of decoders 722. The timing is controlled so that parallel processing is performed. Thus, each time the reflection point coordinates are input from the laser radar 12 and the reflection point coordinates one time before are estimated and converted to the reflection point coordinates obtained by subtracting the rotational momentum candidate, The determination of the upper digit by the plurality of comparators 820, the parallel processing by the plurality of decoders 722, and the count-up of the counters C0 to C255 are repeatedly performed. By repeating this for the reflection point coordinates (for example, 64 points) for one scan, conversion is performed by subtracting each rotational momentum candidate from the reflection point coordinate estimation value one time ago from each reflection point t0 one time ago. In addition, the difference movement vector for all combinations to each reflection point at the current time t1 is obtained, the difference movement vector outside the specified range is excluded, and the difference movement vector and rotation within the specified range are determined by the count values of the counters C0 to C255. A histogram of momentum combinations is created.

比較器ツリー24における基本ユニットの出力は、カウント値と、差分移動ベクトル(dx、dz)の部分桁の値および回転運動量θの組み合わせとのセットである。   The output of the basic unit in the comparator tree 24 is a set of a count value, a combination of the partial digit value of the difference movement vector (dx, dz) and the rotational momentum θ.

制御部28は、1スキャン分の反射点座標についての回転運動量の各候補を用いて求められた差分移動ベクトルに対するカウント処理が終わったところで、最大のカウント値をもつカウンタCを探索するため、比較器ツリー24を動作させる。これによって、1スキャン分の反射点座標についての回転運動量の各候補を用いて求められた差分移動ベクトルに対するカウント処理の結果から、最大カウント値を有するカウンタCに対応する差分移動ベクトル(dx、dz)の部分桁の値および回転運動量θが出力される。   The control unit 28 searches for the counter C having the maximum count value after the count process for the differential movement vector obtained by using each rotational momentum candidate for the reflection point coordinates for one scan is completed. The instrument tree 24 is operated. As a result, the difference movement vector (dx, dz) corresponding to the counter C having the maximum count value is obtained from the result of the count process for the difference movement vector obtained by using each rotation momentum candidate for the reflection point coordinates for one scan. ) And the rotational momentum θ are output.

演算器624は、比較器ツリー24から出力される差分移動ベクトル(dx、dz、θ)の部分桁の値に、1時刻前の自車運動の推定値である移動ベクトルの並進成分(dx、dz、θ)を加算して、求められた並進成分(dx、dz、θ)と、比較器ツリー24から出力された回転運動量θとからなる移動ベクトル(dx、dz、θ)を出力する。これによって、差分移動ベクトルの部分桁の値に、部分桁より上位桁の値が結合された移動ベクトルが求められる。   The computing unit 624 converts the translation vector component (dx, dx, dz, θ), which is the estimated value of the vehicle motion one time before, into the partial digit value of the differential movement vector (dx, dz, θ) output from the comparator tree 24. dz, θ) are added, and a movement vector (dx, dz, θ) composed of the obtained translational component (dx, dz, θ) and the rotational momentum θ output from the comparator tree 24 is output. As a result, a movement vector obtained by combining the value of the partial digit of the difference movement vector with the value of the higher digit than the partial digit is obtained.

出力部26は、演算器624から出力される移動ベクトル(dx、dz、θ)を、自車運動の推定値として出力する。   The output unit 26 outputs the movement vector (dx, dz, θ) output from the calculator 624 as an estimated value of the own vehicle motion.

反射点座標は、レーザレーダ12から時系列に入力されて来るので、新しい反射点座標が入力されるつど、制御部28によって、座標データをレジスタBB、B63〜B0内でシフトさせ(BB→B63→B62…B1→B0)、レジスタB0〜B63に1スキャン分の座標をデータセットする。制御部28は、1スキャンの終了または開始のタイミングでレジスタB0〜B63の内容をレジスタA0〜A63にコピーし、レジスタA0〜A63に必要な反射点座標のデータをセットする。   Since the reflection point coordinates are input from the laser radar 12 in time series, each time a new reflection point coordinate is input, the control unit 28 shifts the coordinate data in the registers BB and B63 to B0 (BB → B63). → B62... B1 → B0), and coordinates for one scan are set in the registers B0 to B63. The control unit 28 copies the contents of the registers B0 to B63 to the registers A0 to A63 at the end or start timing of one scan, and sets the necessary reflection point coordinate data in the registers A0 to A63.

また、制御部28は、上記第8の実施の形態と同様に、指定する差分移動ベクトルの部分桁、及びカウンタCと部分桁の値との対応関係を適応的に設定する。   In addition, as in the eighth embodiment, the control unit 28 adaptively sets the correspondence between the partial digit of the designated differential movement vector and the value of the counter C and the partial digit.

次に、第9の実施の形態の自車運動推定装置910の動作について説明する。   Next, the operation of the host vehicle motion estimation device 910 of the ninth embodiment will be described.

まず、制御部28によって、レジスタA0〜A63には、レジスタB0〜B63の内容がコピーされ、1時刻前に得られた1走査分の反射点座標が格納される。また、レジスタ制御部によって、1時刻前に推定された自車運動の推定値である移動ベクトルの並進・回転成分が、レジスタBVAに格納される。レジスタBBには、次の走査(現時刻の走査)の反射点座標が1点ずつ、制御部28によって格納される。   First, the control unit 28 copies the contents of the registers B0 to B63 to the registers A0 to A63, and stores the reflection point coordinates for one scan obtained one time before. In addition, the translation / rotation component of the movement vector, which is the estimated value of the vehicle motion estimated one time before by the register control unit, is stored in the register BVA. The register BB stores the reflection point coordinates of the next scan (scan at the current time) one by one by the control unit 28.

そして、座標変換部612によって、レジスタBBの反射点座標から、レジスタBVAの移動ベクトルを差し引くと共に、回転運動量θの候補の各々について、回転運動量θの候補を差し引いた座標に各々変換する。回転運動量θの各候補は制御部28から指定される。   Then, the coordinate conversion unit 612 subtracts the movement vector of the register BVA from the reflection point coordinates of the register BB and converts each of the rotation momentum θ candidates into coordinates obtained by subtracting the rotation momentum θ candidate. Each candidate of the rotational momentum θ is designated by the control unit 28.

そして、複数の演算器20によって、レジスタBBの座標が変換される毎に、変換された座標と、レジスタA0〜A63が保持している座標との間で差分移動ベクトルが求められる。64個の差分移動ベクトルが同時並列に求められる。複数の比較器820によって、複数の演算器20から求められた64個の差分移動ベクトルの指定された上位桁が、0であるか否かを判定し、肯定判定された場合に、動作許可信号をデコーダ722に出力する。   Each time the coordinates of the register BB are converted by the plurality of arithmetic units 20, a difference movement vector is obtained between the converted coordinates and the coordinates held in the registers A0 to A63. 64 differential motion vectors are obtained in parallel. The plurality of comparators 820 determine whether or not the designated high-order digits of the 64 differential movement vectors obtained from the plurality of computing units 20 are 0, and when an affirmative determination is made, the operation permission signal Is output to the decoder 722.

動作許可信号が入力された複数のデコーダ722によって、対応する複数の演算器20から求められた複数の差分移動ベクトルの部分桁の値及び回転運動量の組み合わせから、それぞれの投票先(ヒストグラムの対応するビン)のカウンタCを決定し、投票先の指示があったカウンタCのカウント値を+1する。   From the combinations of partial digit values and rotational momentum of the plurality of differential movement vectors obtained from the plurality of corresponding computing units 20 by the plurality of decoders 722 to which the operation permission signals are input, the respective vote destinations (corresponding to the histograms). Bin) counter C is determined, and the count value of counter C at the instruction of the vote destination is incremented by one.

制御部28は、現時刻のレーザ走査による1ライン分の反射点座標の各々に対する変換後の座標の各々を用いて求められた差分移動ベクトルについて、カウント値のカウントアップが終了したと判断すると、制御部28は、比較器ツリー24に対して、最大のカウント値を有するカウンタCを特定して、特定されたカウンタCに対応するカウント値、差分移動ベクトル(dx、dz)の部分桁の値、及び回転運動量θを出力するように指示する。   When the control unit 28 determines that the count-up of the count value has been completed for the differential movement vector obtained using each of the coordinates after conversion with respect to each of the reflection point coordinates for one line by the laser scanning at the current time, The control unit 28 specifies the counter C having the maximum count value with respect to the comparator tree 24, the count value corresponding to the specified counter C, and the value of the partial digit of the difference movement vector (dx, dz). , And output a rotational momentum θ.

そして、演算器624により、比較器ツリー24から出力された差分移動ベクトル(dx、dz)と、現時点のベース移動ベクトルとして用いられている1時刻前の移動ベクトルの並進成分(dx、dz)とを加算して求めた並進成分(dx、dz)と、同じく、比較器ツリー24から出力された回転運動量θと1時刻前の移動ベクトルの回転成分θとを加算して求めた回転成分とからなる移動ベクトル(dx、dz、θ)を出力する。   Then, the difference movement vector (dx, dz) output from the comparator tree 24 by the computing unit 624, and the translation component (dx, dz) of the movement vector one hour before used as the current base movement vector, Similarly, the translational component (dx, dz) obtained by adding together the rotational momentum θ output from the comparator tree 24 and the rotational component obtained by adding the rotational component θ of the movement vector one time ago. The following movement vector (dx, dz, θ) is output.

出力部26により、演算器624から出力された移動ベクトルを、自車運動の推定値として出力する。   The output unit 26 outputs the movement vector output from the calculator 624 as an estimated value of the own vehicle motion.

このとき、制御部28によって、上記第7の実施の形態と同様に部分桁指定処理ルーチンが実行され、複数のデコーダ722に対して、中位の部分桁から、順次、下位の部分桁が指定される。   At this time, the control unit 28 executes the partial digit designation processing routine in the same manner as in the seventh embodiment, and sequentially designates the lower partial digits from the middle partial digits to the plurality of decoders 722. Is done.

以上説明したように、第9の実施の形態に係る自車運動推定装置によれば、レーザレーダから順次得られる推定対象時点のレーザの走査に対する反射点座標から、ベース移動ベクトルを差し引くと共に、仮定した回転運動量の候補を差し引いた反射点座標と、推定対象時点より1時刻前に得られたレーザの1走査分の複数の反射点座標とについて、それぞれの差分を演算して差分移動ベクトルを求め、所定範囲内の差分移動ベクトルの部分桁の値及び回転運動量の候補の組み合わせの度数を計数し、最大度数を計数したカウンタに基づいて、回転運動量を含む自車運動を推定することにより、簡易なハードウエア構成で、回転運動量を含む自車運動推定の実時間処理を実現することができる。   As described above, according to the own vehicle motion estimation device according to the ninth embodiment, the base movement vector is subtracted from the reflection point coordinates for the laser scanning at the estimation target time points sequentially obtained from the laser radar, and the assumption is made. The difference movement vector is obtained by calculating the difference between the reflection point coordinates obtained by subtracting the rotational momentum candidates obtained and the plurality of reflection point coordinates for one scan of the laser obtained one time before the estimation target time point. Simple, by counting the frequency of the combination of partial digits of the differential movement vector within the predetermined range and the candidate of the rotational momentum, and estimating the own vehicle motion including the rotational momentum based on the counter counting the maximum frequency With a simple hardware configuration, real-time processing for estimating the vehicle motion including the rotational momentum can be realized.

なお、上記の第6の実施の形態及び第9の実施の形態では、ベース移動ベクトルとして、直前に推定された自車運動量を用いる場合を例に説明したが、これに限定されるものではない。例えば、予め定められた基礎運動量を、ベース移動ベクトルとして用いてもよい。この場合には、演算器624により、比較器ツリー24から出力された差分移動ベクトルと、ベース移動ベクトルとして用いられる基礎運動量を加算して、移動ベクトルの並進成分を求めればよい。   In the sixth embodiment and the ninth embodiment described above, the case where the own vehicle momentum estimated immediately before is used as the base movement vector is described as an example. However, the present invention is not limited to this. . For example, a predetermined basic momentum may be used as the base movement vector. In this case, the translation unit of the movement vector may be obtained by adding the difference movement vector output from the comparator tree 24 and the basic momentum used as the base movement vector by the calculator 624.

なお、上記の第1の実施の形態〜第9の実施の形態において、1スキャン分の反射点座標の数だけ演算器を設ける場合を例に説明したが、これに限定されるものではなく、演算器を少なくとも1つ設けるようにすればよい。この場合には、少なくとも1つの演算器により、現時点の反射点座標と、レジスタA0〜A63の反射点座標との各組み合わせについて移動ベクトル又は差分移動ベクトルを順次求めるようにすればよい。更に、デコーダを少なくとも1つ設けるようにしてもよい。この場合には、少なくとも1つのデコーダにより、演算器により順次演算される移動ベクトル又は差分移動ベクトルに対応するカウンタを順次特定して、カウンタCの値をカウントアップするようにすればよい。
また、1スキャン分の全ての反射点座標を用いて演算する場合を例に説明したが、これに限定されるものではなく、1スキャン分の一部の反射点座標を用いて演算するようにしてもよい。例えば、レーザレーダが1走査あたり128点の反射点座標を出力する場合に、そのうちの100点の反射点座標のみを演算に用いるようにしてもよい。
In the first to ninth embodiments described above, the case where the calculators are provided as many as the number of reflection point coordinates for one scan has been described as an example. However, the present invention is not limited to this. At least one arithmetic unit may be provided. In this case, the movement vector or the difference movement vector may be sequentially obtained for each combination of the current reflection point coordinates and the reflection point coordinates of the registers A0 to A63 by at least one calculator. Furthermore, at least one decoder may be provided. In this case, the counter corresponding to the movement vector or the differential movement vector sequentially calculated by the calculator may be sequentially specified by at least one decoder, and the value of the counter C may be counted up.
Further, the case where the calculation is performed using all the reflection point coordinates for one scan has been described as an example, but the present invention is not limited to this, and the calculation is performed using a part of the reflection point coordinates for one scan. May be. For example, when the laser radar outputs 128 reflection point coordinates per scan, only 100 reflection point coordinates may be used for the calculation.

10、210、310、410、510、610、710、810、910 自車運動推定装置
12 レーザレーダ
20、512、524、624演算器
22、422、722 デコーダ
24 比較器ツリー
26 出力部
28 制御部
320、612座標変換部
418、718範囲指定レジスタ
420、720、820 比較器
724 結合部
A0〜A63 レジスタ
B、BB レジスタ
B0〜B63 レジスタ
BV レジスタ
C0〜C255カウンタ
10, 210, 310, 410, 510, 610, 710, 810, 910 Own vehicle motion estimation device 12 Laser radar 20, 512, 524, 624 calculator 22, 422, 722 Decoder 24 Comparator tree 26 Output unit 28 Control unit 320, 612 coordinate conversion unit 418, 718 range designation register 420, 720, 820 comparator 724 coupling unit A0-A63 register B, BB register B0-B63 register BV register C0-C255 counter

Claims (7)

移動体に搭載され、かつ、レーザを走査して前記レーザの各反射点座標を計測するレーザ装置から順次得られる推定対象時点のレーザの走査に対する前記反射点座標を保持する第1レジスタと、
前記推定対象時点より前に得られた前記レーザの1走査分の複数の前記反射点座標を保持する複数の第2レジスタと、
前記第1レジスタに順次保持される前記レーザの1走査分の反射点座標の各々について、前記反射点座標と、前記複数の第2レジスタに保持された前記複数の反射点座標の各々との差分を各々演算する演算器と、
前記演算器の出力値との対応関係が予め定められた複数のカウンタであって、前記カウンタ毎に、前記レーザの1走査分に対する、前記カウンタに対応する前記演算器の出力値の度数を計数するための複数のカウンタと、
前記演算器の各出力値について、前記演算器の出力値に対応する度数を計数する、前記複数のカウンタのうちの何れか1つのカウンタを指定するデコーダと、
前記複数のカウンタのうち、最大度数を計数した前記カウンタを特定する比較器と、
前記比較器によって特定された前記カウンタに対応する前記演算器の出力値を、前記移動体の運動の推定結果とする運動推定部と、
を含む移動体運動推定装置。
A first register that is mounted on a moving body and holds the reflection point coordinates for the laser scanning at the estimation target time point obtained sequentially from a laser device that scans the laser and measures the respective reflection point coordinates of the laser;
A plurality of second registers holding a plurality of reflection point coordinates for one scan of the laser obtained before the estimation target time point;
For each of the reflection point coordinates for one scan of the laser sequentially held in the first register, the difference between the reflection point coordinates and each of the plurality of reflection point coordinates held in the plurality of second registers. A computing unit for computing each of
A plurality of counters having a predetermined correspondence relationship with the output value of the arithmetic unit, and for each counter, the frequency of the output value of the arithmetic unit corresponding to the counter for one scan of the laser is counted. With multiple counters to
For each output value of the computing unit, a decoder that specifies any one of the plurality of counters that counts the frequency corresponding to the output value of the computing unit;
A comparator that identifies the counter that has counted the maximum frequency among the plurality of counters;
A motion estimator that takes an output value of the computing unit corresponding to the counter specified by the comparator as an estimation result of the motion of the moving body;
A moving body motion estimation apparatus including:
前記演算器は、複数の演算器であって、
前記複数の演算器は、前記第1レジスタに順次保持される前記レーザの1走査分の反射点座標の各々について、前記反射点座標と、前記複数の第2レジスタに保持された前記複数の反射点座標との差分を並列に演算する請求項1記載の移動体運動推定装置。
The computing unit is a plurality of computing units,
For each of the reflection point coordinates for one scan of the laser sequentially held in the first register, the plurality of computing units are configured to reflect the reflection point coordinates and the plurality of reflections held in the plurality of second registers. The mobile body motion estimation apparatus according to claim 1, wherein the difference from the point coordinates is calculated in parallel.
前記デコーダは、複数のデコーダであって、
前記複数のデコーダは、前記複数の演算器の出力値に対応する度数を計数する前記カウンタを並列に指定する請求項2記載の移動体運動推定装置。
The decoder is a plurality of decoders,
The mobile body motion estimation apparatus according to claim 2, wherein the plurality of decoders specify in parallel the counter that counts frequencies corresponding to output values of the plurality of arithmetic units.
仮定する前記移動体の複数の回転量の候補の各々について、前記レーザ装置から順次得られる前記推定対象時点のレーザの走査に対する反射点座標から、前記回転量の候補を差し引いた座標に順次変換する座標変換部を更に含み、
前記第1レジスタは、前記座標変換部によって順次変換された座標を保持し、
前記複数のカウンタは、前記演算器の出力値及び前記回転量の候補の組み合わせとの対応関係が予め定められ、前記カウンタは、前記カウンタに対応する前記演算器の出力値及び前記回転量の候補の組み合わせの度数を計数するためのものであって、
前記デコーダは、前記演算器の出力値及び前記出力値の演算に用いられた前記反射点座標の変換に用いられた前記回転量の候補の組み合わせに対応する度数を計数する前記カウンタを指定し、
前記運動推定部は、前記比較器によって特定された前記カウンタに対応する前記演算器の出力値及び前記回転量の候補を、前記移動体の運動の推定結果とする請求項1〜請求項3の何れか1項記載の移動体運動推定装置。
Each of a plurality of rotation amount candidates of the moving body to be assumed is sequentially converted into a coordinate obtained by subtracting the rotation amount candidates from the reflection point coordinates for the laser scanning at the estimation target time points sequentially obtained from the laser device. A coordinate conversion unit;
The first register holds coordinates sequentially converted by the coordinate conversion unit,
Wherein the plurality of counters, the correspondence relation between the combination of the arithmetic unit output Ne及 beauty the rotation amount candidates predetermined said counter, the output value and the rotation of the operation unit corresponding to the counter For counting the frequency of candidate combinations of quantities ,
The decoder designates the counter that counts the frequency corresponding to the combination of the rotation amount candidates used for the conversion of the reflection point coordinates used in the calculation of the output value of the calculator and the output value;
The said movement estimation part makes the output value of the said arithmetic unit corresponding to the said counter specified by the said comparator, and the candidate of the said rotation amount as the estimation result of the movement of the said mobile body. The mobile body motion estimation apparatus according to any one of the preceding claims.
前記演算器の出力値が所定範囲内であるか否かを判定する判定器を更に含み、
前記デコーダは、前記判定器によって肯定判定された前記演算器の出力値に対応する度数を計数する前記カウンタを指定する請求項1〜請求項4の何れか1項記載の移動体運動推定装置。
A determinator for determining whether or not the output value of the computing unit is within a predetermined range;
5. The mobile body motion estimation apparatus according to claim 1, wherein the decoder designates the counter that counts the frequency corresponding to the output value of the arithmetic unit that has been affirmed by the determiner.
前記演算器は、複数の演算器であって、
前記複数の演算器は、前記第1レジスタに順次保持される前記レーザの1走査分の反射点座標の各々について、前記反射点座標と、前記複数の第2レジスタに保持された前記複数の反射点座標との差分を並列に演算し、
前記デコーダは、複数のデコーダであって、
前記判定器は、複数の判定器であって、
前記複数の判定器は、前記複数の演算器の出力値が所定範囲内であるか否かを並列に判定し、
前記複数のデコーダは、前記複数の演算器のうち、前記判定器によって肯定判定された前記演算器の出力値に対応する度数を計数する前記カウンタを並列に指定する請求項5記載の移動体運動推定装置。
The computing unit is a plurality of computing units,
For each of the reflection point coordinates for one scan of the laser sequentially held in the first register, the plurality of computing units are configured to reflect the reflection point coordinates and the plurality of reflections held in the plurality of second registers. Calculate the difference from the point coordinates in parallel,
The decoder is a plurality of decoders,
The determiner is a plurality of determiners,
The plurality of determiners determine in parallel whether the output values of the plurality of arithmetic units are within a predetermined range,
6. The moving body motion according to claim 5, wherein the plurality of decoders specify, in parallel, the counter that counts the frequency corresponding to the output value of the arithmetic unit that is affirmatively determined by the determination unit among the plurality of arithmetic units. Estimating device.
前記複数のカウンタは、前記演算器の出力値の指定された部分桁の値との対応関係が予め定められ、前記カウンタは、前記レーザの1走査分に対する、前記カウンタに対応する前記演算器の出力値の指定された部分桁の値の度数を計数するためのものであって、
前記デコーダは、前記演算器の出力値の前記指定された部分桁の値に対応する度数を計数する前記カウンタを指定し、
前記運動推定部は、前記比較器によって特定されたカウンタに対応する前記演算器の出力値の部分桁の値に、予め求められた前記部分桁より上位桁の値を結合して、前記移動体の運動の推定結果とする請求項1〜請求項6記載の移動体運動推定装置。
The correspondence between the plurality of counters and the specified partial digit value of the output value of the arithmetic unit is determined in advance , and the counter corresponds to the counter corresponding to the counter for one scan of the laser. For counting the frequency of the specified partial digits of the output value,
The decoder designates the counter that counts the frequency corresponding to the value of the designated partial digit of the output value of the computing unit;
The motion estimation unit combines a value of a higher digit than the partial digit obtained in advance with a partial digit value of the output value of the computing unit corresponding to the counter specified by the comparator, The mobile body motion estimation apparatus according to claim 1, wherein the motion is estimated.
JP2012034511A 2012-02-20 2012-02-20 Mobile body motion estimation device Expired - Fee Related JP5935377B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2012034511A JP5935377B2 (en) 2012-02-20 2012-02-20 Mobile body motion estimation device

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2012034511A JP5935377B2 (en) 2012-02-20 2012-02-20 Mobile body motion estimation device

Publications (2)

Publication Number Publication Date
JP2013170906A JP2013170906A (en) 2013-09-02
JP5935377B2 true JP5935377B2 (en) 2016-06-15

Family

ID=49264942

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2012034511A Expired - Fee Related JP5935377B2 (en) 2012-02-20 2012-02-20 Mobile body motion estimation device

Country Status (1)

Country Link
JP (1) JP5935377B2 (en)

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3613275B2 (en) * 1994-12-28 2005-01-26 オムロン株式会社 Traffic information system
JP3266827B2 (en) * 1997-06-25 2002-03-18 本田技研工業株式会社 Vehicle object detection device
DE10036042B4 (en) * 2000-07-25 2004-12-16 Daimlerchrysler Ag Multisensorial lane assignment
JP2011123551A (en) * 2009-12-08 2011-06-23 Toyota Central R&D Labs Inc Dead corner area estimating device and program

Also Published As

Publication number Publication date
JP2013170906A (en) 2013-09-02

Similar Documents

Publication Publication Date Title
EP3951721A1 (en) Method and apparatus for determining occluded area of virtual object, and terminal device
CN113436238B (en) Point cloud registration accuracy evaluation method and device and electronic equipment
CN110163903B (en) Three-dimensional image acquisition and image positioning method, device, equipment and storage medium
US20100283739A1 (en) Contact grouping and gesture recognition for surface computing
Ttofis et al. Towards accurate hardware stereo correspondence: A real-time fpga implementation of a segmentation-based adaptive support weight algorithm
CN110471409B (en) Robot inspection method and device, computer readable storage medium and robot
WO2015002113A1 (en) Method for fitting primitive shapes to a set of 3d points
CN107808388B (en) Image processing method and device containing moving object and electronic equipment
US20180182166A1 (en) Tracking rigged polygon-mesh models of articulated objects
Ambrosch et al. SAD-based stereo matching using FPGAs
CN112154429A (en) High-precision map positioning method, system, platform and computer readable storage medium
CN112825199B (en) Collision detection method, device, equipment and storage medium
CN115273071A (en) Object identification method and device, electronic equipment and storage medium
CN111462179A (en) Three-dimensional object tracking method and device and electronic equipment
CN110297677B (en) Drawing method, drawing device, drawing equipment and storage medium
JPH08223578A (en) Method for searching motion vector and device therefor
JP5935377B2 (en) Mobile body motion estimation device
CN104866143B (en) The determination method, apparatus and touch-screen equipment of a kind of touch point
CN116823942A (en) Target labeling method of three-dimensional point cloud and target tracking method in continuous frames of point cloud
CN105204693B (en) Touch point identification method and device and touch screen equipment
JP2013167938A (en) Information input device
CN106792510B (en) A kind of prediction type fingerprint image searching method in fingerprint location
JP2005321966A (en) Interface method, device and program
CN114926325B (en) Point cloud registration method, system and storage medium
Chun et al. 3D star skeleton for fast human posture representation

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20141112

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20150831

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20150915

A521 Written amendment

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20151116

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20160412

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20160425

R150 Certificate of patent or registration of utility model

Ref document number: 5935377

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

LAPS Cancellation because of no payment of annual fees