JP5935377B2 - Mobile body motion estimation device - Google Patents

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.

  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.

  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.

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.

  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.

  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.

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.

  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.

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.

  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.

  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.

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.

  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.

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.

  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.

It is a figure which shows arrangement | positioning of the own vehicle motion estimation apparatus and laser radar of 1st Embodiment. 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. It is a timing chart for demonstrating operation | movement of the own vehicle motion estimation apparatus of 1st Embodiment. It is a figure which shows the structure of the own vehicle motion estimation apparatus of 2nd Embodiment. It is a timing chart for demonstrating operation | movement of the own vehicle motion estimation apparatus of 2nd Embodiment. 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. It is a timing chart for demonstrating operation | movement of the own vehicle motion estimation apparatus of 3rd Embodiment. It is a figure which shows the structure of the own vehicle motion estimation apparatus of 4th Embodiment. It is a figure which shows the structure of the own vehicle motion estimation apparatus of 5th Embodiment. It is a figure which shows the structure of the own vehicle motion estimation apparatus of 6th Embodiment. It is a figure which shows the structure of the own vehicle motion estimation apparatus of 7th Embodiment. 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. It is a figure which shows the structure of the own vehicle motion estimation apparatus of 8th Embodiment. 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.

  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.

  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.

  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.

  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.

  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).

  In the register B, one reflection point coordinate (x, z) sequentially obtained by the current laser scanning is stored by the register control unit.

  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.

  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.

  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.

  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.

  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.

  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).

  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.

  In addition, when the operation of the comparator tree 24 is completed, the control unit 28 initializes the counters C0 to C255.

  The output unit 26 outputs the movement vector output from the comparator tree 24 as an estimated value of the own vehicle motion.

  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.

  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.

  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.

  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.

  Then, the output unit 26 outputs the movement vector output from the comparator tree 24 as an estimated value of the own vehicle motion.

  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.

  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.

  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.

  In the second embodiment, the method for setting data in the registers A0 to A63 is different from that in the first embodiment.

  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.

  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).

  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.

  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.

  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.

  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.

  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.

  The reflection point coordinates input from the laser radar 12 are taken into the register BB when the reflection point timing signal is “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.

  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.

  Then, the output unit 26 outputs the movement vector output from the comparator tree 24 as an estimated value of the own vehicle motion.

  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.

  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).

  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.

  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.

  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.

  The third embodiment is different from the first embodiment in that not only translational motion but also rotational motion is estimated.

  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.

  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.

  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.

  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.

  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.

  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.

  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).

  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.

  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.

  The reflection point coordinates input from the laser radar 12 are taken into the register BB when the reflection point timing signal is “1”.

  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.

  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.

  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, θ).

  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.

  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.

  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.

  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. .

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  Next, operation | movement of the own vehicle motion estimation apparatus 410 of 4th Embodiment is demonstrated.

  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.

  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.

  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

  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.

  Then, the output unit 26 outputs the movement vector output from the comparator tree 24 as an estimated value of the own vehicle motion.

  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.

  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.

  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.

  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.

  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.

  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.

  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. .

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  The output unit 26 outputs the movement vector output from the calculator 524 as an estimated value of the own vehicle motion.

  Next, the operation of the own vehicle motion estimation device 510 according to the fifth embodiment will be described.

  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.

  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.

  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.

  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.

  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.

  The output unit 26 outputs the movement vector output from the calculator 524 as an estimated value of the own vehicle motion.

  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.

  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. .

  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.

  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.

  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.

  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.

  Further, the counters C0 to C255 are provided for each combination of the differential movement vector (dx, dz) and the rotational momentum θ.

  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.

  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.

  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).

  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.

  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.

  The output unit 26 outputs the movement vector (dx, dz, θ) output from the calculator 524 as an estimated value of the own vehicle motion.

  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.

  Next, the operation of the own vehicle motion estimation device 610 according to the sixth embodiment will be described.

  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.

  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.

  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.

  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.

  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.

  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

  The output unit 26 outputs the movement vector output from the calculator 624 as an estimated value of the own vehicle motion.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  The output unit 26 outputs the movement vector output from the combining unit 724 as an estimated value of the own vehicle motion.

  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.

  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.

  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.

  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).

  At this stage, the true value is considered to be in the range of 3.0 to 3.9 m / s.

  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.

  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.

  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.

  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.

  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).

  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.

  Next, operation | movement of the own vehicle motion estimation apparatus 710 of 7th Embodiment is demonstrated.

  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.

  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.

  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.

  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.

  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.

  Then, the output unit 26 outputs the movement vector output from the combining unit 724 as an estimated value of the own vehicle motion.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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. .

  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.

  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.

  The register BB holds the current reflection point coordinates (x, z). The assumed base movement vector is held in the register BV.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  Next, the operation of the own vehicle motion estimation device 810 according to the eighth embodiment will be described.

  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.

  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.

  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.

  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.

  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.

  The output unit 26 outputs the movement vector output from the calculator 524 as an estimated value of the own vehicle motion.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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.

  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 θ.

  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.

  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.

  The output unit 26 outputs the movement vector (dx, dz, θ) output from the calculator 624 as an estimated value of the own vehicle motion.

  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.

  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.

  Next, the operation of the host vehicle motion estimation device 910 of the ninth embodiment will be described.

  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.

  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.

  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.

  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.

  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 θ.

  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.

  The output unit 26 outputs the movement vector output from the calculator 624 as an estimated value of the own vehicle motion.

  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.

  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.

  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.

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 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)

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: 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. 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. 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. 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. 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. 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.