Movement amount estimation device and movement amount estimation program

Abstract

To provide a movement amount estimation device and a movement amount estimation program for suppressing deterioration in estimation accuracy of a movement amount.SOLUTION: The movement amount estimation device 30 executes the movement amount estimation program to acquire distance measurement data that is data regarding the position and the Doppler velocity of a distance measurement point detected by the distance measurement sensor 12. An extraction point which is a distance measurement point located outside a range from a position where a distance in a vertical direction from a road surface point which is a distance measurement point on a road surface on which the moving object 10 moves is a first predetermined distance to a position where the distance in the vertical direction from the road surface point is a second predetermined distance is extracted from distance measurement data, and a movement amount Δ L of the moving object 10 moving on the road surface is estimated based on a value related to a Doppler velocity of the extraction point.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present disclosure relates to a movement amount estimation device and a movement amount estimation program. [Background technology]

[0002] A conventional technique for Doppler point cloud registration is known, as described in Patent Document 1. In the technique described in Patent Document 1, a first predicted point cloud of Doppler velocity information is compared with a second point cloud of Doppler velocity information, thereby aligning the first point cloud with the second point cloud. Furthermore, if the point cloud contains points representing moving objects such as pedestrians, vehicles, raindrops, or falling snow, these points are determined to be outliers, and the determined outliers are removed. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] U.S. Patent No. 1,1287,529 Summary of the Invention [Problem to be solved by the invention]

[0004] When the relative velocity of a stationary object with respect to a moving object is small, for example because the moving object is moving at a low speed, the difference between the relative velocity of the moving object with respect to the object to be estimated for the amount of movement and the relative velocity of the stationary object required to estimate the amount of movement with respect to the object to be estimated for the amount of movement becomes small. In this case, when the technology described in Patent Document 1 is used, points indicating moving objects may be treated as points indicating stationary objects. If points indicating moving objects are treated as points indicating stationary objects, the accuracy of estimating the amount of movement decreases.

[0005] The present disclosure aims to provide a movement amount estimation device and a movement amount estimation program that suppress a decrease in the estimation accuracy of a movement amount. [Means for solving the problem]

[0006] The invention described in claim 1 is a movement amount estimation device that estimates the movement amount (ΔL) of a moving body (10) moving on a road surface (G), and is equipped with an acquisition unit (S100) that acquires distance measurement data, which is data regarding the position and Doppler velocity (Vd) of a distance measurement point (Pm) detected by a distance measurement sensor (12), an extraction unit (S106) that extracts from the distance measurement data an extraction point (Pe), which is a distance measurement point on the road surface, located outside a range (Rm) from a position where the distance in the vertical direction from a road surface point (Pr) that is a distance measurement point on the road surface is a first predetermined distance (Hm_th1) to a position where the distance in the vertical direction from the road surface point is a second predetermined distance (Hm_th2) that is greater than the first predetermined distance, and an estimation unit (S108) that estimates the movement amount based on a value regarding the Doppler velocity of the extraction point.

[0007] The invention described in claim 14 is a movement amount estimation program for estimating the movement amount (ΔL) of a moving body (10) moving on a road surface (G), which causes a movement amount estimation device to function as an acquisition unit (S100) that acquires distance measurement data, which is data regarding the position and Doppler velocity (Vd) of a distance measurement point (Pm) detected by a distance measurement sensor (12), an extraction unit (S106) that extracts from the distance measurement data an extraction point (Pe), which is a distance measurement point located outside a range (Rm) from a position where the distance in the vertical direction from a road surface point (Pr), which is a distance measurement point on the road surface, is a first predetermined distance (Hm_th1) to a position where the distance in the vertical direction from the road surface point is a second predetermined distance (Hm_th2) that is greater than the first predetermined distance, and an estimation unit (S108) that estimates the movement amount based on a value regarding the Doppler velocity of the extraction point.

[0008] This eliminates the ranging points of moving objects and extracts the ranging points of stationary objects, regardless of the speed of the moving objects. This prevents the ranging points of moving objects from being used as ranging points of stationary objects to estimate the amount of movement. This prevents a decrease in the accuracy of estimating the amount of movement.

[0009] The reference symbols in parentheses attached to each component indicate an example of the correspondence between the component and the specific components described in the embodiments described below. [Brief explanation of the drawings]

[0010] [Figure 1] 1 is a configuration diagram of a moving body in which a movement amount estimation device according to a first embodiment is used. [Figure 2] FIG. 2 is a diagram showing a moving object and a ranging point. [Figure 3] 4 is a flowchart showing the processing of the movement amount estimation device. [Figure 4] 5A and 5B are diagrams for explaining road surface height in the processing of the movement amount estimation device; [Figure 5] FIG. 4 is a diagram for explaining extraction points in the processing of the movement amount estimation device. [Figure 6] FIG. 10 is a diagram showing the relationship between the azimuth angle and Doppler velocity of the sampling point. [Figure 7] 10A and 10B are diagrams for explaining prediction and association of distance measurement points in the processing of the movement amount estimation device of the second embodiment. [Figure 8] FIG. 11 is a configuration diagram of a moving body in which a movement amount estimation device according to a third embodiment is used. [Figure 9] 10 is a flowchart showing the processing of the movement amount estimation device of the fourth embodiment. [Figure 10] FIG. 11 is a configuration diagram of a moving body in which a movement amount estimation device according to a fifth embodiment is used. [Figure 11] 4 is a flowchart showing the processing of the movement amount estimation device. [Figure 12] 13 is a flowchart showing the processing of the movement amount estimation device of the sixth embodiment. [Figure 13] 4A and 4B are diagrams for explaining a first velocity vector, a second velocity vector, and a vector angle in processing by the movement amount estimation device. DETAILED DESCRIPTION OF THE INVENTION

[0011] Hereinafter, embodiments will be described with reference to the drawings. In the following embodiments, identical or equivalent parts will be denoted by the same reference numerals, and description thereof will be omitted.

[0012] (First embodiment) The movement amount estimation device that executes the movement amount estimation program of this embodiment suppresses a decrease in the estimation accuracy of the movement amount. For example, the movement amount estimation device is used for a moving body such as a vehicle. First, this moving body will be described.

[0013] As shown in FIG. 1, a moving object 10 is an object for estimating a movement amount ΔL, and includes a distance measurement sensor 12, a movement amount estimation device 30, and a movement control unit .

[0014] The ranging sensor 12 includes a radar, LiDAR, sonar, etc., that uses radio waves such as millimeter waves. As shown in FIG. 2, the ranging sensor 12 detects a point cloud consisting of at least one ranging point Pm, which is a point at each position on an object ahead of the moving body 10, using probe waves such as electromagnetic waves and ultrasonic waves. The ranging sensor 12 also detects the position, azimuth angle θd, and Doppler velocity Vd of the ranging point Pm using the reflected waves and frequency of the probe waves. Furthermore, the ranging sensor 12 outputs data related to the position, azimuth angle θd, and Doppler velocity Vd of the ranging point Pm as ranging data to the movement distance estimation device 30 (described later) via a LAN 14 such as a CAN, as shown in FIG. 1. The Doppler velocity Vd here is the relative velocity of the ranging point Pm with respect to the ranging sensor 12.

[0015] The movement amount estimation device 30 is mainly composed of a microcomputer and includes a CPU, ROM, flash memory, RAM, I / O, a communication interface, and a bus line connecting these components. The movement amount estimation device 30 estimates the movement amount ΔL of the moving object 10 based on the distance measurement data from the distance measurement sensor 12 by executing a program stored in the ROM of the movement amount estimation device 30. The movement amount estimation device 30 outputs the estimated movement amount ΔL to a movement control unit 40 (described later) via the LAN 14. Details of the estimation of the movement amount ΔL by the movement amount estimation device 30 will be described later.

[0016] The movement control unit 40 has an engine, a motor, and a drive circuit for driving these. The movement control unit 40 also controls the engine, the motor, and the drive circuit based on the movement amount ΔL from the movement amount estimation device 30. In this way, the movement control unit 40 controls the movement of the moving object 10.

[0017] The moving body 10 is configured as described above. Next, details of the estimation of the movement amount ΔL by the program execution of the movement amount estimation device 30 will be described with reference to the flowchart of Fig. 3 and Figs. 4 to 6. The program of the movement amount estimation device 30 is executed, for example, when the power of the moving body 10 is turned on.

[0018] As shown in the flowchart of FIG. 3, the movement amount estimation device 30 acquires distance measurement data from the distance measurement sensor 12 in step S100.

[0019] Next, in step S102, the movement amount estimation device 30 extracts road surface points Pr, which are distance measurement points Pm on the road surface G on which the moving object 10 is moving, from the distance measurement data acquired in step S100, as shown in Fig. 4. The movement amount estimation device 30 extracts the road surface points Pr using a method such as plane fitting or curved surface fitting, for example.

[0020] Next, in step S104, the movement amount estimation device 30 calculates the road surface height Hm for each distance measurement point Pm from the extracted road surface point Pr. The road surface height Hm is the distance in the vertical direction from the road surface point Pr to the distance measurement point Pm.

[0021] Here, the ranging point Pm of the moving object becomes noise in estimating the movement amount ΔL using the Doppler velocity Vd. Furthermore, the ranging point Pm whose road surface height Hm is the same as the height of the moving object 10 is likely to be the ranging point Pm of a moving object that is moving on the road surface G together with the moving object 10. Therefore, the ranging point Pm whose road surface height Hm is different from the height of the moving object 10 is likely to be the ranging point Pm of a stationary object.

[0022] Therefore, in step S106 following step S104, the movement amount estimation device 30 uses the road surface height Hm calculated in step S104 to extract distance measurement points Pm located outside the predetermined distance range Rm from the road surface point Pr, as shown in FIG. 5. As a result, the movement amount estimation device 30 removes distance measurement points Pm of moving objects from the distance measurement data acquired in step S100 and extracts distance measurement points Pm of stationary objects. The movement amount estimation device 30 then designates the extracted distance measurement points Pm as extraction points Pe. Note that the extraction points Pe also include the road surface point Pr. The predetermined distance range Rm is the range from a position where the distance from the road surface point Pr in the vertical direction is a first predetermined distance Hm_th1 to a position where the distance from the road surface point Pr in the vertical direction is a second predetermined distance Hm_th2. The second predetermined distance Hm_th2 is greater than the first predetermined distance Hm_th1. Furthermore, the first predetermined distance Hm_th1 and the second predetermined distance Hm_th2 are set by experiment, simulation, or the like so that the distance measurement point Pm of the moving object is removed from the distance measurement data.

[0023] Returning to the flowchart in Fig. 3, in step S108 following step S106, the movement amount estimation device 30 extracts the azimuth angle θd and Doppler velocity Vd of the extraction point Pe extracted in step S106 from the distance measurement data acquired in step S100. In addition, as shown in Fig. 6, the movement amount estimation device 30 calculates a function f(θd) of the Doppler velocity Vd of the extraction point Pe with respect to the azimuth angle θd of the extraction point Pe. Note that the function f(θd) is expressed, for example, by a trigonometric function.

[0024] Here, as shown in Figure 6, assume that the extracted azimuth angle θd and Doppler velocity Vd are at two points. The azimuth angle θd at one of the two points is assumed to be θd1. The Doppler velocity Vd corresponding to θd1 is assumed to be Vd1. The azimuth angle θd at the other of the two points is assumed to be θd2. The Doppler velocity Vd corresponding to θd2 is assumed to be Vd2. The speed of the distance measuring sensor 12 in the forward / backward direction of the moving body 10 is assumed to be Vx. The speed of the distance measuring sensor 12 in the left / right direction of the moving body 10 is assumed to be Vy.

[0025] Then, θd1, Vd1, θd2, Vd2, Vx, and Vy are expressed as in the following relational expression (1). Furthermore, the movement amount estimation device 30 substitutes the extracted θd1, Vd1, θd2, and Vd2 into the following relational expression (1). As a result, the movement amount estimation device 30 calculates Vx and Vy.

[0026]

number

[0027] Furthermore, let Vs be the magnitude of the velocity of the distance measurement sensor 12. Let α be the angle related to the direction of the velocity of the distance measurement sensor 12.

[0028] Then, Vs is expressed as the following relational expression (2-1) using Vx and Vy. Also, α is expressed as the following relational expression (2-2) using Vx and Vy. Furthermore, the movement amount estimation device 30 substitutes the calculated Vx and Vy into the following relational expressions (2-1) and (2-2). In this way, the movement amount estimation device 30 calculates Vs and α.

[0029]

number

[0030] Also, let B be the distance from the reference position of the moving body 10 to the distance measuring sensor 12 in the forward / backward direction of the moving body 10. Let I be the distance from the reference position of the moving body 10 to the distance measuring sensor 12 in the left / right direction of the moving body 10. Let β be the angle related to the orientation of the distance measuring sensor 12 with respect to the forward / backward direction of the moving body 10. Let V be the speed of the moving body 10. Let ω be the angular velocity of the moving body 10. Note that the reference position of the moving body 10 is, for example, the center of gravity of the moving body 10. B, I, and β are set in advance in the movement amount estimation device 30.

[0031] Then, V is expressed as in the following relational expression (3-1) using Vs, α, β, B, and I. ω is expressed as in the following relational expression (3-2) using Vs, α, β, and I. Furthermore, the movement amount estimation device 30 substitutes the calculated Vs and α and preset β, B, and I into the following relational expression (3-1). In this way, the movement amount estimation device 30 calculates V. Furthermore, the movement amount estimation device 30 substitutes the calculated Vs and α and preset β and I into the following relational expression (3-2). In this way, the movement amount estimation device 30 calculates ω.

[0032]

number

[0033] Furthermore, the amount of movement ΔL in the forward / backward direction of the moving body 10 is defined as ΔX. The amount of movement ΔL in the left / right direction of the moving body 10 is defined as ΔY. The amount of change in the yaw angle of the moving body 10 is defined as Δθ. The period of a series of operations from when the processing of step S100 of the movement amount estimation device 30 starts to when the processing of step S100 returns is defined as the control cycle of the movement amount estimation device 30, which is defined as ΔT.

[0034] Then, ΔX, ΔY, and Δθ are expressed as in the following relational expression (4) using V, ω, and ΔT. Furthermore, the movement amount estimation device 30 substitutes the calculated V and ω and a preset ΔT into the following relational expression (4). In this way, the movement amount estimation device 30 estimates the movement amount ΔL.

[0035]

number

[0036] Returning to the flowchart in FIG. 3, in step S110 following step S108, the movement amount estimation device 30 outputs the movement amount ΔL estimated in step S108 to the movement control unit 40 via the LAN 14. The movement control unit 40 controls the engine, motor, and drive circuit based on the movement amount ΔL from the movement amount estimation device 30. In this way, the movement control unit 40 controls the movement of the mobile object 10. Thereafter, the processing of the movement amount estimation device 30 returns to step S100.

[0037] As described above, the movement amount estimation device 30 estimates the movement amount ΔL. Next, it will be explained how the movement amount estimation device 30 suppresses a decrease in the estimation accuracy of the movement amount ΔL.

[0038] Here, when the relative velocity of a stationary object with respect to the moving object is small, for example because the moving object is moving at a low speed, the difference between the relative velocity of the moving object with respect to the moving object 10 and the relative velocity of the stationary object required to estimate the movement amount ΔL with respect to the moving object 10 becomes small. In this case, when the technology described in Patent Document 1 is used, points indicating moving objects may be treated as points indicating stationary objects. If points indicating moving objects are treated as points indicating stationary objects, the accuracy of estimating the movement amount ΔL decreases.

[0039] In the technology described in Patent Document 1, a first predicted point cloud of Doppler velocity information is compared with a second point cloud of Doppler velocity information. Furthermore, suppose that the moving object 10 undergoes a sudden change, such as sudden acceleration / deceleration, a sudden turn, or a sudden pitching, when estimating the movement amount ΔL using the azimuth angle θd and Doppler velocity Vd of the ranging point Pm. In this case, the sudden change in velocity and angular velocity increases the error in the first predicted point cloud of Doppler velocity information. Therefore, using the technology described in Patent Document 1 makes it difficult to distinguish between moving and stationary objects. Therefore, when the moving object 10 undergoes a sudden change, such as sudden acceleration / deceleration, a sudden turn, or a sudden pitching, the technology described in Patent Document 1 reduces the accuracy of estimating the movement amount ΔL.

[0040] In contrast, the movement amount estimation device 30 of this embodiment serves as an acquisition unit that acquires distance measurement data. Furthermore, the movement amount estimation device 30 serves as an extraction unit that extracts, from the distance measurement data, an extraction point Pe, which is a distance measurement point Pm located outside the predetermined distance range Rm. As described above, the predetermined distance range Rm corresponds to the range from a position where the distance from the road surface point Pr in the vertical direction is the first predetermined distance Hm_th1 to a position where the distance from the road surface point Pr in the vertical direction is the second predetermined distance Hm_th2. Furthermore, the movement amount estimation device 30 serves as an estimation unit that estimates the movement amount ΔL based on a value related to the Doppler velocity Vd of the extraction point Pe. Furthermore, here, the movement amount estimation device 30 estimates the movement amount ΔL based on a value related to the azimuth angle θd of the extraction point Pe in addition to the value related to the Doppler velocity Vd of the extraction point Pe.

[0041] As a result, regardless of the speed of the moving object, the ranging points Pm of the moving object are removed and the ranging points Pm of the stationary object are extracted. This prevents the ranging points Pm of the moving object from being used as the ranging points Pm of the stationary object to be used in estimating the movement amount ΔL. Therefore, a decrease in the estimation accuracy of the movement amount ΔL is suppressed.

[0042] Furthermore, even if the moving body 10 undergoes sudden changes such as sudden acceleration / deceleration, sudden turns, and sudden pitching, the ranging points Pm of the moving object are removed and the ranging points Pm of the stationary object are extracted. Therefore, similar to the above, the ranging points Pm of the moving object are prevented from being treated as the ranging points Pm of the stationary object used for estimating the movement amount ΔL. Therefore, even if the moving body 10 undergoes sudden changes such as sudden acceleration / deceleration, sudden turns, and sudden pitching, a decrease in the estimation accuracy of the movement amount ΔL is prevented.

[0043] (Second embodiment) The second embodiment differs from the first embodiment in the method of estimating the amount of movement ΔL in step S108. The rest is the same as the first embodiment.

[0044] Here, the period of a series of operations from when the processing of step S100 of the movement amount estimation device 30 starts until when the processing of step S100 returns is defined as the control period of the movement amount estimation device 30.

[0045] Then, in step S108, the movement amount estimation device 30 predicts the position and Doppler velocity Vd of the extraction point Pe extracted in the current control cycle, as shown in FIG. 7, without using the azimuth angle θd. For example, the movement amount estimation device 30 assumes that the moving object 10 moves at a constant velocity and constant acceleration. Furthermore, the movement amount estimation device 30 predicts the predicted position and predicted Doppler velocity Vd_pr of the extraction point Pe extracted in the current control cycle, based on the distance measurement point Pm acquired in the previous control cycle and the distance ΔL estimated in the previous control cycle. Note that the distance ΔL estimated in the previous control cycle corresponds to the distance ΔL estimated before the current time. Furthermore, the distance measurement point Pm and the distance ΔL used in the prediction for the first control cycle are set by experiment, simulation, or the like so that the predicted position and predicted Doppler velocity Vd_pr of the extraction point Pe can be predicted. Furthermore, in FIG. 7, the extraction point Pe extracted in the previous control cycle is indicated as Pe(k-1). The Doppler velocity Vd in the previous control cycle is shown as Vd(k-1). The sampling point Pe acquired in the current control cycle is shown as Pe(k). The Doppler velocity Vd in the current control cycle is shown as Vd(k). The sampling point Pe predicted in the current control cycle is shown as Pe_pr(k). The predicted Doppler velocity Vd_pr predicted in the current control cycle is shown as Vd_pr(k). k is a natural number greater than or equal to 2.

[0046] Then, the movement amount estimation device 30 estimates the movement amount ΔL using ICP etc. Note that ICP is an abbreviation for Iterative Closest Point.

[0047] Specifically, the movement amount estimation device 30 associates the predicted position and predicted Doppler velocity Vd_pr of the predicted extraction point Pe_pr with the position and Doppler velocity Vd of the extraction point Pe extracted in the current control cycle using a nearest neighbor search or the like. In this way, the movement amount estimation device 30 associates the predicted extraction point Pe_pr with the extraction point Pe extracted in the current control cycle. Note that the predicted extraction point Pe_pr corresponds to the predicted extraction point Pe.

[0048] Furthermore, the movement amount estimation device 30 calculates a rotation matrix and a translation vector for the moving object 10 from the corresponding positions and Doppler velocities Vd. The movement amount estimation device 30 also repeats the above-mentioned correspondence by nearest neighbor search or the like and the calculation of the rotation matrix and translation vector. The movement amount estimation device 30 then calculates a rotation matrix and a translation vector that minimizes the sum of squares of the error between the corresponding positions and Doppler velocities Vd. This rotation matrix corresponds to a change in the attitude of the moving object 10. Furthermore, this translation vector corresponds to a change in the position of the moving object 10. Therefore, the movement amount estimation device 30 estimates the movement amount ΔL from the rotation matrix and translation vector that minimizes the sum of squares of the error between the corresponding positions and Doppler velocities Vd.

[0049] As described above, the movement amount estimation device 30 of the second embodiment estimates the movement amount ΔL in step S108. The second embodiment also provides the same effects as the first embodiment. Furthermore, the second embodiment also provides the following effects.

[0050] Here, when estimating the movement amount ΔL using the predicted position and predicted Doppler velocity Vd_pr of the predicted extraction point Pe_pr, if the moving body 10 undergoes sudden changes such as sudden acceleration / deceleration, sudden turns, or sudden pitching, the prediction error becomes large. As a result, when estimating the movement amount ΔL using the predicted position and predicted Doppler velocity Vd_pr is combined with the technology described in Patent Document 1, the prediction error of the distance measurement point Pm for moving and stationary objects becomes large. For this reason, using the technology described in Patent Document 1 makes it difficult to distinguish between moving and stationary objects. Therefore, when the moving body 10 undergoes sudden changes, if the technology described in Patent Document 1 is used to estimate the movement amount ΔL using the predicted position and predicted Doppler velocity Vd_pr, points representing moving objects may be treated as points representing stationary objects. If points representing moving objects are treated as points representing stationary objects, the estimation accuracy of the movement amount ΔL decreases.

[0051] In contrast, the movement amount estimation device 30 of the second embodiment estimates the movement amount ΔL using the predicted position and predicted Doppler velocity Vd_pr. As described above, even if the moving body 10 undergoes a sudden change, the ranging point Pm of the moving object is removed and the ranging point Pm of the stationary object is extracted. This prevents the ranging point Pm of the moving object from being treated as the ranging point Pm of the stationary object used for estimating the movement amount ΔL. Therefore, even if the moving body 10 undergoes a sudden change such as sudden acceleration / deceleration, a sudden turn, or a sudden pitching, a decrease in the estimation accuracy of the movement amount ΔL using the predicted position and predicted Doppler velocity Vd_pr is prevented.

[0052] (Third embodiment) In the third embodiment, as shown in Fig. 8, the moving body 10 further includes an internal sensor 16. Also, the processing of the movement amount estimation device 30 differs from that of the second embodiment. Other than this, the third embodiment is the same as the second embodiment.

[0053] The internal sensor 16 has a speed sensor, a steering angle sensor, an IMU, etc., and detects the state of the moving body 10, for example, the speed, acceleration, and attitude of the moving body 10. Note that IMU is an abbreviation for Inertial Measurement Unit.

[0054] In step S100, the movement amount estimation device 30 acquires the state of the moving object 10 from the internal sensor 16 in addition to the distance measurement data from the distance measurement sensor 12. Furthermore, the movement amount estimation device 30 performs the processes from step S102 to step S106 in the same manner as in the second embodiment.

[0055] In step S108 following step S106, the movement amount estimation device 30 estimates the predicted position and predicted Doppler velocity Vd_pr of the extraction point Pe extracted in the current control cycle without using the movement amount ΔL estimated in the previous control cycle. Specifically, the movement amount estimation device 30 predicts the predicted position and predicted Doppler velocity Vd_pr of the extraction point Pe from the ranging point Pm acquired in the previous control cycle and the state of the moving body 10 acquired in the previous control cycle, instead of the movement amount ΔL estimated in the previous control cycle.

[0056] Then, similarly to the above, the movement amount estimation device 30 associates the predicted extraction point Pe_pr with the extraction point Pe extracted in the current control cycle using ICP. The movement amount estimation device 30 also calculates the rotation matrix and translation matrix from the associated predicted extraction point Pe_pr and extraction point Pe. The movement amount estimation device 30 then estimates the movement amount ΔL from the calculated rotation matrix and translation matrix. The movement amount estimation device 30 also performs the process of step S110 following step S108 in the same manner as in the second embodiment.

[0057] As described above, the moving body 10 is configured to include the movement amount estimation device 30 of the third embodiment, and performs the processing of the movement amount estimation device 30. In this third embodiment, the same effects as in the second embodiment are achieved.

[0058] (Fourth embodiment) In the fourth embodiment, the processing of the movement amount estimation device 30 differs from that of the first embodiment, as shown in the flowchart of Fig. 9. Other than this, the fourth embodiment is similar to the first embodiment.

[0059] Here, the period of a series of operations from when the processing of step S100 of the movement amount estimation device 30 starts until when the processing of step S100 returns is defined as the control period of the movement amount estimation device 30.

[0060] Then, the movement amount estimation device 30 performs the processes from step S100 to step S108 in the same manner as in the first embodiment.

[0061] Furthermore, when the moving object 10 is moving normally, there is little fluctuation between the current movement amount ΔL(k) estimated in the current control cycle and the previous movement amount ΔL(k-1) estimated in the previous control cycle. Therefore, when the moving object 10 is moving normally, the absolute value of the difference between the current movement amount ΔL(k) estimated in the current control cycle and the previous movement amount ΔL(k-1) estimated in the previous control cycle is small. In contrast, when the absolute value of the difference between the current movement amount ΔL(k) estimated in the current control cycle and the previous movement amount ΔL(k-1) estimated in the previous control cycle is large, there is a possibility that the estimation of the movement amount ΔL has not converged to a correct value. Furthermore, because the estimation of the movement amount ΔL has not converged to a correct value, there is a possibility that the error contained in the estimated movement amount ΔL is large, or the estimation accuracy of the movement amount ΔL is low.

[0062] Therefore, in step S200 following step S108, the movement amount estimation device 30 calculates the absolute value |ΔL(k)-ΔL(k-1)| of the difference between the current movement amount ΔL(k) and the previous movement amount ΔL(k-1). Furthermore, the movement amount estimation device 30 determines whether the calculated absolute value of the difference |ΔL(k)-ΔL(k-1)| is equal to or less than a threshold value ε. Note that the threshold value ε is set by experiment, simulation, or the like so that it can be determined whether the absolute value of the difference |ΔL(k)-ΔL(k-1)| is appropriate.

[0063] When the absolute value of the difference |ΔL(k)-ΔL(k-1)| is equal to or smaller than the threshold ε, the difference between the current movement amount ΔL(k) and the previous movement amount ΔL(k-1) is small. Therefore, at this time, the processing of the movement amount estimation device 30 proceeds to step S110. On the other hand, when the absolute value of the difference |ΔL(k)-ΔL(k-1)| is greater than the threshold ε, the absolute value of the difference between the current movement amount ΔL(k) and the previous movement amount ΔL(k-1) is large. At this time, the estimation of the movement amount ΔL has not converged to a correct value, so there is a possibility that the error contained in the estimated movement amount ΔL is large or the estimation accuracy of the movement amount ΔL is low. Therefore, at this time, the processing of the movement amount estimation device 30 proceeds to step S202.

[0064] In step S202 following step S200, since there is a possibility that the error included in the estimated movement amount ΔL is large or that the estimation accuracy of the movement amount ΔL is low, the movement amount estimation device 30 adds an extraction point Pe. Note that the extraction point Pe extracted in step S106 corresponds to the first extraction point. The extraction point Pe added in step S202 corresponds to the second extraction point.

[0065] Here, a distance measurement point Pm that is close to the road surface point Pr and has a Doppler velocity Vd of the road surface point Pr is likely to be a distance measurement point Pm of a stationary object.

[0066] For this reason, the movement amount estimation device 30 extracts, as the second extraction point, for example, a ranging point Pm whose shortest distance from the road surface point Pr is equal to or less than a distance threshold and whose absolute value of the difference between the Doppler velocity Vd of the road surface point Pr and the ranging point Pm is equal to or less than a speed difference threshold. As a result, the movement amount estimation device 30 re-extracts the ranging point Pm of the stationary object. Furthermore, the movement amount estimation device 30 adds the ranging point Pm of the stationary object re-extracted in step S202 to the point cloud of the ranging points Pm of the stationary object extracted in step S106. Note that the distance threshold and the speed difference threshold are set by experiment, simulation, or the like so that the ranging point Pm of the stationary object is re-extracted.

[0067] Furthermore, when the ranging points Pm are clustered, the size of the cluster of stationary objects is likely to be larger than the size of the cluster of moving objects. Note that the size of the cluster is the size of the aggregate of the point cloud of the classified ranging points Pm.

[0068] For this reason, the movement amount estimation device 30 clusters the ranging points Pm from the ranging data acquired in step S100, for example. Furthermore, the movement amount estimation device 30 extracts, as second extraction points, ranging points Pm whose cluster size for the clustered ranging points Pm is equal to or greater than a cluster threshold. In this way, the movement amount estimation device 30 re-extracts the ranging points Pm of stationary objects. Furthermore, the movement amount estimation device 30 adds the ranging points Pm of stationary objects re-extracted in step S202 to the point cloud of ranging points Pm of stationary objects extracted in step S106. Note that the cluster threshold is set by experiment, simulation, or the like so that the ranging points Pm of stationary objects are re-extracted.

[0069] In step S204 following step S202, the movement amount estimation device 30 calculates the number of added points Na. The number of added points Na is the number of extracted points Pe added in step S202.

[0070] Furthermore, the movement amount estimation device 30 calculates the current number of additions Nk by adding, for example, 1 to the previous number of additions Nk. Note that the number of additions Nk is the number of times the process of step S202 has been performed.

[0071] In step S206 following step S204, the movement amount estimation device 30 determines whether the additional points Na calculated in step S204 is equal to or less than the point threshold Na_th. As a result, the movement amount estimation device 30 determines whether the additional points Na in step S202 is too small. Note that the point threshold Na_th is set by experiment, simulation, or the like so that it can be determined whether the additional points Na in step S202 is too small.

[0072] Furthermore, the movement amount estimation device 30 determines whether the number of additions Nk calculated in step S204 is equal to or greater than a count threshold Nk_th. This prevents the movement amount estimation device 30 from continuing the processes from step S200 to step S206. The count threshold Nk_th is set by experiment, simulation, or the like so that the processes from step S200 to step S206 are not continued.

[0073] When the number of added points Na is equal to or less than the point threshold Na_th, the number of added extraction points Pe in step S202 is too small, and therefore adding extraction points Pe has little effect on the estimation of the movement amount ΔL. Therefore, at this time, the processing of the movement amount estimation device 30 proceeds to step S110. Alternatively, when the number of additions Nk is equal to or greater than the number of additions threshold Nk_th, the processing of the movement amount estimation device 30 proceeds to step S110.

[0074] Furthermore, when the number of added points Na is greater than the point threshold Na_th, the number of added extraction points Pe in step S202 is not too small, and is sufficient here. Therefore, at this time, the processing of the movement amount estimation device 30 returns to step S108. Alternatively, when the number of additions Nk is smaller than the number of additions threshold Nk_th, the processing of the movement amount estimation device 30 returns to step S108. In step S108, the extraction points Pe in step S202 are added to the extraction points Pe in step S106, and the movement amount ΔL is re-estimated.

[0075] In step S110, the movement amount estimation device 30 outputs the movement amount ΔL estimated in step S108 to the movement control unit 40 via the LAN 14. The movement control unit 40 controls the engine, motor, and drive circuit based on the movement amount ΔL from the movement amount estimation device 30. In this way, the movement control unit 40 controls the movement of the mobile object 10. Thereafter, the processing of the movement amount estimation device 30 returns to step S100.

[0076] As described above, the movement amount estimation device 30 of the fourth embodiment performs the process. The fourth embodiment also achieves the same effects as the first embodiment. Furthermore, the fourth embodiment also achieves the following effects.

[0077] [1-1] In step S202, the movement amount estimation device 30 adds an extraction point Pe. For example, the movement amount estimation device 30 adds, as a second extraction point, a ranging point Pm whose shortest distance from the road surface point Pr is equal to or less than a distance threshold and whose absolute value of the difference between the Doppler velocity Vd of the road surface point Pr and the Doppler velocity Vd is equal to or less than a velocity difference threshold. Alternatively, the movement amount estimation device 30 adds, as a second extraction point, a ranging point Pm whose cluster size for the clustered ranging point Pm is equal to or greater than a cluster threshold.

[0078] As a result, the number of extraction points Pe increases, and the number of distance measurement points Pm of stationary objects increases, thereby suppressing a decrease in the estimation accuracy of the amount of movement ΔL.

[0079] [1-2] In the processes from step S108 to step S206, when the additional score Na is greater than the score threshold Na_th, the movement amount estimation device 30 re-estimates the movement amount ΔL.

[0080] This allows the amount of movement ΔL to be estimated when the number of second extraction points is relatively large, thereby preventing a decrease in the accuracy of estimating the amount of movement ΔL.

[0081] [1-3] The movement amount estimation device 30 outputs the movement amount ΔL when the number of additions Nk is equal to or greater than the number threshold Nk_th in steps S202, S204, S206, and S110.

[0082] As a result, when the number of additions Nk reaches the number of times threshold Nk_th, the addition of the extraction point Pe in step S202 is not performed and the movement amount ΔL is output, which prevents the addition of the extraction point Pe in step S202 from continuing forever.

[0083] (Fifth embodiment) In the fifth embodiment, as shown in Fig. 10, the moving object 10 further includes an internal sensor 16 and a GNSS receiver 18. Also, the processing of the movement amount estimation device 30 differs from that of the fourth embodiment. Other than this, the fifth embodiment is similar to the fourth embodiment.

[0084] As described above, the internal sensor 16 detects the state of the moving body 10, for example, the speed, acceleration, and attitude of the moving body 10. As a result, the internal sensor 16 corresponds to a state estimator that estimates the state of the moving body 10.

[0085] The GNSS receiver 18 receives signals from a plurality of positioning satellites (not shown). Furthermore, the GNSS receiver 18 calculates the GNSS absolute position, GNSS absolute orientation, GNSS velocity, etc. of the moving object 10 based on the received signals. As a result, the GNSS receiver 18 corresponds to a state estimator that estimates the state of the moving object 10. The GNSS receiver 18 also outputs the calculated GNSS absolute position, GNSS absolute orientation, GNSS velocity, etc. to the movement amount estimation device 30. Note that positioning satellites used by the GNSS receiver 18 include, for example, GPS satellites, GLONASS satellites, Galileo satellites, and quasi-zenith satellites.

[0086] In step S100, the movement amount estimation device 30 acquires the speed, acceleration, and attitude of the moving object 10 from the internal sensor 16 as the state of the moving object 10, in addition to the distance measurement data from the distance measurement sensor 12. Furthermore, the movement amount estimation device 30 acquires the GNSS absolute position, GNSS absolute orientation, and GNSS speed as the state of the moving object 10 from the GNSS receiver 18. The movement amount estimation device 30 also acquires a point cloud of distance measurement points Pm indicating positions around the moving object 10 from a database or the like external to the moving object 10. After processing in step S100, the movement amount estimation device 30 performs the processes from step S102 to step S106 in the same manner as in the second embodiment.

[0087] 11, in step S108 following step S106, the movement amount estimation device 30 estimates the movement amount ΔL in the same manner as described above. Furthermore, the movement amount estimation device 30 estimates the self-position Ps in addition to the movement amount ΔL. Note that the self-position Ps corresponds to the position of the moving object 10 in the point cloud of the ranging points Pm.

[0088] Specifically, the movement amount estimation device 30 calculates a search range for the point cloud of ranging points Pm acquired from a database or the like in step S100, based on the state of the moving object 10 acquired in step S100. Furthermore, the movement amount estimation device 30 uses ICP to associate ranging points Pm in the calculated search range with extraction points Pe extracted in step S106 and extraction points Pe added in step S202. In this way, the movement amount estimation device 30 estimates its own position Ps. After the process of step S108, the movement amount estimation device 30 performs the processes from step S200 to step S206 in the same manner as in the second embodiment.

[0089] Then, in step S110 following step S206, the movement amount estimation device 30 outputs the self-position Ps estimated in step S108, in addition to the movement amount ΔL estimated in step S108, to the movement control unit 40 via the LAN 14. The movement control unit 40 controls the engine, motor, and drive circuit based on the movement amount ΔL and self-position Ps from the movement amount estimation device 30. In this way, the movement control unit 40 controls the movement of the moving object 10. Thereafter, the processing of the movement amount estimation device 30 returns to step S100.

[0090] As described above, the moving body 10 is configured to include the movement amount estimation device 30 of the fifth embodiment, and the processing of the movement amount estimation device 30 is performed. The fifth embodiment also provides the same effects as the fourth embodiment. Furthermore, the fifth embodiment also provides the following effects.

[0091] [2-1] In step S108, the movement amount estimation device 30 associates the point group of the distance measurement points Pm with the extraction points Pe, thereby estimating the self-position Ps.

[0092] This makes it possible to control the movement of the moving body 10 using the self-position Ps in addition to the movement amount ΔL, which makes it easier to control the movement of the moving body 10.

[0093] [2-2] The movement amount estimation device 30 calculates a search range for the point cloud of the ranging points Pm based on the state of the moving body 10. The movement amount estimation device 30 also estimates its own position Ps by associating the ranging points Pm in the search range with the extraction points Pe. As described above, the state of the moving body 10 is, for example, the speed, acceleration, and attitude of the moving body 10 detected by the internal sensor 16. Alternatively, the state of the moving body 10 is the GNSS absolute position, GNSS absolute orientation, and GNSS velocity calculated by the GNSS receiver 18.

[0094] By calculating the search range, it becomes easier to associate the distance measurement point Pm with the extraction point Pe, which prevents a decrease in the estimation accuracy of the self-position Ps.

[0095] (Sixth embodiment) In the sixth embodiment, the moving object 10 further includes an internal sensor 16 and a GNSS receiver 18. Also, the processing of the movement amount estimation device 30 differs from that of the first embodiment. Other than this, the sixth embodiment is similar to the first embodiment.

[0096] As described above, the internal sensor 16 detects the speed, acceleration, and attitude of the moving body 10. Furthermore, the internal sensor 16 estimates a first speed vector V1, which is the speed vector of the moving body 10, based on the detected speed, acceleration, and attitude of the moving body 10. Therefore, the internal sensor 16 corresponds to a vector estimator that estimates the first speed vector V1.

[0097] As described above, the GNSS receiver 18 calculates the GNSS absolute position, GNSS absolute orientation, GNSS velocity, etc. of the moving object 10 based on signals received from multiple positioning satellites. The GNSS receiver 18 also estimates a first velocity vector V1 based on the calculated GNSS absolute position, GNSS absolute orientation, and GNSS velocity of the moving object 10. Thus, the GNSS receiver 18 corresponds to a vector estimator that estimates the first velocity vector V1.

[0098] In step S100, the movement amount estimation device 30 acquires a first velocity vector V1 from the internal sensor 16 and the GNSS receiver 18, in addition to the distance measurement data from the distance measurement sensor 12. After the processing of step S100, the movement amount estimation device 30 performs the processing from step S102 to step S108 in the same manner as in the second embodiment.

[0099] Then, as shown in the flowchart of FIG. 12, in step S300 following step S108, the movement amount estimation device 30 calculates a second velocity vector V2 from the movement amount ΔL estimated in step S108.

[0100] Next, in step S302, the movement amount estimation device 30 calculates a vector angle θv from the first velocity vector V1 acquired in step S100 and the second velocity vector V2 estimated in step S300, as shown in Fig. 13. The vector angle θv is the angle formed by the first velocity vector V1 and the second velocity vector V2.

[0101] 12, in step S304 following step S302, the movement amount estimation device 30 determines whether the vector angle θv calculated in step S302 is equal to or greater than the angle threshold value θv_th. As a result, the movement amount estimation device 30 determines whether the deviation in the attitude of the moving object 10 is large.

[0102] When the vector angle θv is less than the angle threshold θv_th, the deviation in the attitude of the moving body 10 is small, and the processing of the movement amount estimation device 30 proceeds to step S110. In step S110, the reliability of the estimation of the movement amount ΔL is high, and the movement amount estimation device 30 outputs the movement amount ΔL estimated in step S108 to the movement control unit 40 via the LAN 14. On the other hand, when the vector angle θv is equal to or greater than the angle threshold θv_th, the deviation in the attitude of the moving body 10 is large, and the processing of the movement amount estimation device 30 proceeds to step S306.

[0103] In step S306 following step S304, since the deviation in the attitude of the moving object 10 is large, the movement amount estimation device 30 determines that the moving object 10 is abnormal. Furthermore, the movement amount estimation device 30 outputs a signal indicating that the moving object 10 is abnormal to the movement control unit 40 via the LAN 14. When the movement control unit 40 receives the signal indicating that the moving object 10 is abnormal from the movement amount estimation device 30, it stops the moving object 10, for example. Alternatively, for example, the movement control unit 40 outputs an alarm using text display, sound, and light. Thereafter, the processing of the movement amount estimation device 30 returns to step S100.

[0104] As described above, the moving body 10 is configured to include the movement amount estimation device 30 of the sixth embodiment, and the processing of the movement amount estimation device 30 is performed. The sixth embodiment also provides the same effects as the first embodiment. Furthermore, the sixth embodiment also provides the following effects.

[0105] [3] In step S302, the movement amount estimation device 30 calculates a vector angle θv based on the first velocity vector V1 and the second velocity vector V2. Furthermore, in steps S304 and S306, the movement amount estimation device 30 functions as a determination unit that determines that the moving object 10 is abnormal when the vector angle θv is equal to or greater than an angle threshold θv_th. Note that the vector angle θv corresponds to a value related to a deviation in the posture of the moving object 10. Furthermore, the angle threshold θv_th corresponds to a deviation threshold.

[0106] This allows a determination to be made regarding a deviation in the posture of the moving body 10. Therefore, an abnormality in the moving body 10 can be determined.

[0107] (Other embodiments) The present disclosure is not limited to the above-described embodiments, and appropriate modifications can be made to the above-described embodiments. Furthermore, it goes without saying that the elements constituting the embodiments in the above-described embodiments are not necessarily essential unless they are specifically stated as essential or are considered to be clearly essential in principle.

[0108] The acquiring unit, extracting unit, estimating unit, determining unit, and methods described herein may be implemented by a special-purpose computer configured with a processor and memory programmed to perform one or more functions embodied in a computer program. Alternatively, the acquiring unit, extracting unit, estimating unit, determining unit, and methods described herein may be implemented by a special-purpose computer configured with a processor configured with one or more dedicated hardware logic circuits. Alternatively, the acquiring unit, extracting unit, estimating unit, determining unit, and methods described herein may be implemented by one or more special-purpose computers configured with a processor and memory programmed to perform one or more functions in combination with a processor configured with one or more hardware logic circuits. Furthermore, the computer program may be stored as instructions executed by a computer on a computer-readable non-transitory tangible storage medium.

[0109] In each of the above embodiments, the road surface height Hm is the vertical distance from the road surface point Pr to the distance measurement point Pm. Alternatively, the road surface height Hm may be the vertical distance from a plane or curved surface that passes through the road surface point Pr to the distance measurement point Pm. The plane or curved surface that passes through the road surface point Pr is estimated by, for example, plane fitting or curved surface fitting. As a result, when the distance measurement point Pm on the road surface G is detected by the distance measurement sensor 12 and then is no longer detected, the distance measurement point Pm of a stationary object is extracted.

[0110] In the first embodiment, the movement amount estimation device 30 estimates the movement amount ΔL based on values ​​related to the azimuth angle θd and the Doppler velocity Vd of the extraction point Pe. Alternatively, the movement amount estimation device 30 may estimate the movement amount ΔL based only on values ​​related to the Doppler velocity Vd of the extraction point Pe.

[0111] In the above-described fifth embodiment, the point cloud of the ranging points Pm is a point cloud accumulated in a database external to the moving body 10. In contrast, the point cloud of the ranging points Pm is not limited to a point cloud accumulated in a database external to the moving body 10, and may be, for example, a point cloud of the ranging points Pm accumulated by the ranging sensor 12 of the moving body 10.

[0112] The above embodiments may be combined as appropriate.

[0113] (Aspects of the present disclosure) [Point 1] A movement amount estimation device that estimates a movement amount (ΔL) of a moving object (10) moving on a road surface (G), an acquisition unit (S100) that acquires distance measurement data, which is data relating to the position and Doppler velocity (Vd) of a distance measurement point (Pm) detected by a distance measurement sensor (12); an extraction unit (S106) that extracts, from the distance measurement data, an extraction point (Pe) that is the distance measurement point located outside a range (Rm) from a position where the distance in the vertical direction from a road surface point (Pr) that is the distance measurement point on the road surface is a first predetermined distance (Hm_th1) to a position where the distance in the vertical direction from the road surface point is a second predetermined distance (Hm_th2) that is larger than the first predetermined distance; an estimation unit (S108) that estimates the amount of movement based on a value related to the Doppler velocity of the extraction point; A movement amount estimation device comprising: [Point 2] the acquisition unit acquires data relating to the azimuth angle of the range finding point, The movement amount estimation device according to Aspect 1, wherein the estimation unit estimates the movement amount based on values ​​related to a Doppler velocity and an azimuth angle of the extraction point. [Point 3] The estimation unit predicting values ​​for the location and Doppler velocity of said sampled points; Correlating predicted extraction points (Pe_pr) that are the predicted extraction points with the extraction points extracted by the extraction unit; The movement amount estimation device according to Aspect 1, wherein the movement amount is estimated based on values ​​relating to positions and Doppler velocities of the predicted sampling point and the sampling point that correspond to each other. [Point 4] The movement amount estimation device according to Aspect 3, wherein the estimation unit predicts values ​​related to the position and Doppler velocity of the extraction point based on the movement amount estimated before the current time. [Point 5] the acquisition unit acquires a state of the moving body detected by an internal sensor (16) that detects a state of the moving body; The movement amount estimation device according to Aspect 3, wherein the estimation unit predicts values ​​relating to the positions and Doppler velocities of the ranging points extracted by the extraction unit based on the state of the moving body. [Point 6] The extraction point is a first extraction point, the extraction unit extracts, from the ranging data, a second extraction point that is the ranging point whose distance from the road surface point is equal to or less than a distance threshold and whose absolute value of the difference between the Doppler velocity of the road surface point and the Doppler velocity of the road surface point is equal to or less than a velocity difference threshold; The movement amount estimation device according to any one of Aspects 1 to 5, wherein the estimation unit estimates the movement amount based on values ​​related to Doppler velocities at the first extraction point and the second extraction point. [Point 7] The extraction point is a first extraction point, the extraction unit clusters the distance measurement points, and extracts second extraction points from the distance measurement data, the second extraction points being distance measurement points whose cluster size is equal to or greater than a cluster threshold; The movement amount estimation device according to any one of Aspects 1 to 5, wherein the estimation unit estimates the movement amount based on values ​​related to Doppler velocities at the first extraction point and the second extraction point. [Point 8] The movement amount estimation device according to aspect 6 or 7, wherein the estimation unit estimates the movement amount based on values ​​related to the Doppler velocities of the first extraction points and the second extraction points when the number of points (Na) of the second extraction points is greater than a point number threshold (Na_th). [Point 9] The movement amount estimation device according to any one of Aspects 6 to 8, wherein the estimation unit outputs the movement amount when the number of times (Nk) that the second extraction point has been extracted is equal to or greater than a number threshold (Nk_th). [Point 10] The movement amount estimation device according to any one of aspects 1 to 9, wherein the estimation unit estimates the position (Ps) of the moving body in the point cloud by associating the point cloud of the ranging points with the extracted points. [Point 11] the acquisition unit acquires a state of the moving body estimated by a state estimator (16, 18) that estimates a state of the moving body; The estimation unit calculating a search range for the point cloud based on the state of the moving object; The movement amount estimation device according to aspect 10, wherein the position of the moving body in the point cloud is estimated by associating the ranging points in the search range with the extraction points. [Point 12] the acquisition unit acquires a first velocity vector (V1) that is a velocity vector of the moving object estimated by a vector estimator (16, 18) that estimates a velocity vector of the moving object; The movement amount estimation device according to any one of aspects 1 to 11, wherein the estimation unit estimates a value (θv) relating to a deviation in the attitude of the moving body based on the first velocity vector and a second velocity vector (V2) which is a velocity vector of the moving body based on the movement amount. [Point 13] The movement amount estimation device according to aspect 12, further comprising a determination unit (S306) that determines that the moving body is abnormal when a value relating to a deviation in the posture of the moving body is equal to or greater than a deviation threshold (θv_th). [Point 14] A movement amount estimation program for estimating a movement amount (ΔL) of a moving object (10) moving on a road surface (G), A movement amount estimation device an acquisition unit (S100) for acquiring distance measurement data, which is data relating to the position and Doppler velocity (Vd) of a distance measurement point (Pm) detected by a distance measurement sensor (12); an extraction unit (S106) that extracts, from the distance measurement data, an extraction point (Pe) that is the distance measurement point located outside a range (Rm) from a position where the distance in the vertical direction from a road surface point (Pr) that is the distance measurement point on the road surface is a first predetermined distance (Hm_th1) to a position where the distance in the vertical direction from the road surface point is a second predetermined distance (Hm_th2) that is larger than the first predetermined distance; and a movement amount estimation program that functions as an estimation unit (S108) that estimates the movement amount based on a value related to the Doppler velocity of the extraction point; [Explanation of symbols]

[0114] 10 Mobile 12 Distance measurement sensor 16 Internal Sensors 18 GNSS receivers 30 Travel amount estimation device Vd Doppler velocity Pm AF point Pr road point Pe extraction point

Claims

1. A movement amount estimation device that estimates a movement amount (ΔL) of a moving object (10) moving on a road surface (G), an acquisition unit (S100) that acquires distance measurement data, which is data relating to the position and Doppler velocity (Vd) of a distance measurement point (Pm) detected by a distance measurement sensor (12); an extraction unit (S106) that extracts, from the distance measurement data, an extraction point (Pe) that is the distance measurement point located outside a range (Rm) from a position where the distance in the vertical direction from a road surface point (Pr) that is the distance measurement point on the road surface is a first predetermined distance (Hm_th1) to a position where the distance in the vertical direction from the road surface point is a second predetermined distance (Hm_th2) that is larger than the first predetermined distance; an estimation unit (S108) that estimates the amount of movement based on a value related to the Doppler velocity of the extraction point; A movement amount estimation device comprising:

2. the acquisition unit acquires data relating to the azimuth angle of the range finding point, The movement amount estimation device according to claim 1 , wherein the estimation unit estimates the movement amount based on values ​​related to a Doppler velocity and an azimuth angle of the extraction point.

3. The estimation unit predicting values ​​for the location and Doppler velocity of said sampled points; Correlating predicted extraction points (Pe_pr) that are the predicted extraction points with the extraction points extracted by the extraction unit; 2. The movement amount estimation device according to claim 1, wherein the movement amount is estimated based on values ​​relating to positions and Doppler velocities of the predicted sampling points and the sampling points corresponding to each other.

4. The movement amount estimation device according to claim 3 , wherein the estimation unit predicts values ​​relating to the position and Doppler velocity of the extraction point based on the movement amount estimated before the current time.

5. The acquisition unit acquires a state of the moving body detected by an internal sensor (16) that detects a state of the moving body, The movement amount estimation device according to claim 3 , wherein the estimation unit predicts values ​​relating to the positions and Doppler velocities of the distance measurement points extracted by the extraction unit based on the state of the moving body.

6. The extraction point is a first extraction point, the extraction unit extracts, from the ranging data, a second extraction point, which is the ranging point whose distance from the road surface point is equal to or less than a distance threshold and whose absolute value of the difference between the Doppler velocity of the road surface point and the Doppler velocity of the road surface point is equal to or less than a velocity difference threshold; The movement amount estimation device according to claim 1 , wherein the estimation unit estimates the movement amount based on values ​​related to Doppler velocities at the first extraction point and the second extraction point.

7. The extraction point is a first extraction point, the extraction unit clusters the distance measurement points, and extracts second extraction points from the distance measurement data, the second extraction points being distance measurement points whose cluster size is equal to or greater than a cluster threshold; The movement amount estimation device according to claim 1 , wherein the estimation unit estimates the movement amount based on values ​​related to Doppler velocities at the first extraction point and the second extraction point.

8. 8. The movement amount estimation device according to claim 6, wherein the estimation unit estimates the movement amount based on values ​​related to Doppler velocities of the first extraction points and the second extraction points when the number of points (Na) of the second extraction points is greater than a point number threshold (Na_th).

9. The movement amount estimation device according to claim 6 or 7, wherein the estimation unit outputs the movement amount when the number of times (Nk) that the second extraction point is extracted is equal to or greater than a number threshold (Nk_th).

10. 4. The movement amount estimation device according to claim 1, wherein the estimation unit estimates the position (Ps) of the moving body in the point cloud by associating the point cloud of the ranging points with the extracted points.

11. the acquisition unit acquires a state of the moving body estimated by a state estimator (16, 18) that estimates a state of the moving body; The estimation unit calculating a search range for the point cloud based on the state of the moving object; The movement amount estimation device according to claim 10 , wherein the position of the moving object in the point cloud is estimated by associating the ranging points in the search range with the extracted points.

12. the acquisition unit acquires a first velocity vector (V1) that is a velocity vector of the moving body estimated by a vector estimator (16, 18) that estimates a velocity vector of the moving body; 4. The movement amount estimation device according to claim 1, wherein the estimation unit estimates a value (θv) related to a deviation in the attitude of the moving body based on the first velocity vector and a second velocity vector (V2) that is a velocity vector of the moving body based on the movement amount.

13. The movement amount estimation device according to claim 12, further comprising a determination unit (S306) that determines that the moving body is abnormal when a value relating to a deviation in the posture of the moving body is equal to or greater than a deviation threshold (θv_th).

14. A movement amount estimation program for estimating a movement amount (ΔL) of a moving object (10) moving on a road surface (G), A movement amount estimation device an acquisition unit (S100) that acquires distance measurement data, which is data relating to the position of a distance measurement point (Pm) and a Doppler velocity (Vd) detected by a distance measurement sensor (12); an extraction unit (S106) that extracts, from the distance measurement data, an extraction point (Pe) that is the distance measurement point located outside a range (Rm) from a position where the distance in the vertical direction from a road surface point (Pr) that is the distance measurement point on the road surface is a first predetermined distance (Hm_th1) to a position where the distance in the vertical direction from the road surface point is a second predetermined distance (Hm_th2) that is larger than the first predetermined distance; and a movement amount estimation program that functions as an estimation unit (S108) that estimates the movement amount based on a value related to the Doppler velocity of the extraction point;

Images