Position estimation device, position estimation method, and program

Abstract

This technology provides the ability to accurately estimate the position of an object even when the data points are sparse. [Solution] The position estimation device (10) comprises a first acquisition unit (120), a second acquisition unit (140), an overlay unit (160), and a first estimation unit (180). The first acquisition unit (120) acquires point cloud data at multiple timings obtained from a sensor mounted on a moving object. The second acquisition unit (140) acquires movement information of the moving object. The overlay unit (160) generates superimposed point cloud data by superimposing point cloud data at multiple timings based on movement information. The first estimation unit (180) estimates the position of the object using the superimposed point cloud data.

Description

[Technical Field]

[0001] The present invention relates to a position estimation apparatus, a position estimation method, and a program. [Background Art]

[0002] A technology is known that detects features installed around a vehicle using radar, a camera, or the like, and estimates the position of the host vehicle based on the detection results.

[0003] Patent Documents 1 and 2 disclose a method for estimating the position of the host vehicle using detection results of lane markings such as white lines provided on roads. [Prior Art Document] [Patent Document]

[0004] [Patent Document 1] International Publication No. WO 2018 / 212302 [Patent Document 2] International Publication No. WO 2019 / 189098 [Summary of the Invention] [Problem to be Solved by the Invention]

[0005] However, in the technologies of Patent Documents 1 and 2, the estimation accuracy strongly depends on the density of data points from which lane markings are detected. That is, when the data points are sparse, position estimation accuracy is degraded.

[0006] An example of the problem to be solved by the present invention is to provide a technology capable of accurately estimating the position of an object even when data points are sparse. [Means for Solving the Problem]

[0007] The first invention includes: a first acquisition unit that acquires point cloud data at a plurality of timings obtained by a sensor mounted on a moving body; A second acquisition unit that acquires movement information of the aforementioned moving object, A superposition unit generates superimposed point cloud data by superimposing the point cloud data from the multiple timings based on the movement information, The system includes a first estimation unit that estimates the position of an object using the superimposed point cloud data. It is a position estimation device.

[0008] The second invention is, A computer-based method for estimating position, The first acquisition step involves acquiring point cloud data at multiple timings obtained from sensors mounted on a moving object, A second acquisition step involves acquiring the movement information of the aforementioned moving object, A superposition step that generates superimposed point cloud data by superimposing the point cloud data from multiple timings based on the movement information, This includes a first estimation step of estimating the position of an object using the superimposed point cloud data, This is a method for estimating location.

[0009] The third invention is, This is a program that causes a computer to execute the position estimation method according to the second invention. [Brief explanation of the drawing]

[0010] [Figure 1] This figure illustrates the configuration of a position estimation device according to an embodiment. [Figure 2] This diagram illustrates how a moving object detects white lines on a road. [Figure 3] (a) and (b) are diagrams showing the data points in the first region when the sensor resolution is high and low, respectively. [Figure 4] This is a flowchart illustrating the processing flow performed by the position estimation device according to the embodiment. [Figure 5] This is a block diagram illustrating the configuration of the position estimation device according to the embodiment. [Figure 6]This flowchart illustrates the flow of the self-position estimation process performed by the position estimation device according to the embodiment. [Figure 7] This diagram illustrates a computer used to implement a position estimation device. [Figure 8] This figure shows the relationship between reflection intensity and distance before and after correction. [Figure 9] This is a diagram illustrating the superposition of point cloud data. [Figure 10] This is a diagram illustrating the superposition of point cloud data. [Figure 11] This figure illustrates how the coordinate transformation unit performs coordinate transformation on point cloud data using velocity v and yaw angular velocity ψ'. [Figure 12] This figure illustrates how the coordinate transformation unit performs coordinate transformation on point cloud data using velocity v, yaw angular velocity ψ', roll angular velocity φ', and pitch angular velocity θ'. [Figure 13] This is a diagram illustrating the control of the number of superpositions. [Figure 14] This is a diagram illustrating the extraction of high reflectivity points. [Figure 15] This is a diagram to explain the meaning of the second domain. [Figure 16] This is a diagram illustrating the principal component analysis of point clouds. [Figure 17] This diagram explains the detection values ​​and confidence information for the white lines. [Figure 18] This diagram explains the detection values ​​and confidence information for the white lines. [Figure 19] This diagram illustrates the positional relationship between a moving object and a white line. [Figure 20] Figure 6 is a flowchart illustrating the processing flow of S70. [Modes for carrying out the invention]

[0011] Embodiments of the present invention will be described below with reference to the drawings. In all drawings, similar components are denoted by the same reference numerals, and their descriptions are omitted as appropriate. Furthermore, any symbol preceded by a "·" or "-" will be represented as "A'" or "A-" (where "A" is any character) for convenience.

[0012] (Embodiment) Figure 1 is a diagram illustrating the configuration of a position estimation device 10 according to this embodiment. The position estimation device 10 according to this embodiment comprises a first acquisition unit 120, a second acquisition unit 140, a superposition unit 160, and a first estimation unit 180. The first acquisition unit 120 acquires point cloud data at multiple timings obtained from a sensor mounted on a moving object. The second acquisition unit 140 acquires movement information of the moving object. The superposition unit 160 generates superimposed point cloud data by superimposing the point cloud data at multiple timings based on the movement information. The first estimation unit 180 estimates the position of the object using the superimposed point cloud data. Note that point cloud data at multiple timings refers to, for example, point cloud data at multiple time points.

[0013] For moving objects such as vehicles, accurately determining their position is crucial for accurate navigation, autonomous driving, and driver assistance systems to function effectively. When a moving object travels on a road, its position at a given point in time, along with its speed and direction of travel up to that point in time, is used to estimate its position at the next point in time. Here, by detecting objects (targets) around the moving object using sensors and comparing them with the location of those objects in map information, it is possible to further improve the accuracy of the estimated position of the moving object. However, in order to improve the accuracy of the estimated position of the moving object, it is necessary to estimate the position of the target object with high accuracy using the detection results from the sensors.

[0014] In this embodiment, the object whose position is estimated is a line on the road surface. Specifically, the object is a lane marking on the road surface, and is a dashed line. The color of the line is not particularly limited, but for example, it may be yellow or white. Preferably, the surface of the object is formed of a retroreflective material.

[0015] Figure 2 illustrates how the mobile device 20 detects the white lines 30 on the road. The sensors mounted on the mobile device 20 measure the area inside the measurement area 40. In particular, the position estimation device 10 extracts measurement data from the first area 41, as will be described in detail later, and uses it to detect the white lines 30.

[0016] In this embodiment, the sensor measures the distance to an object by emitting light and receiving the reflected light reflected from the object. The sensor is not particularly limited, but could be a radar or LiDAR (Laser Imaging Detection and Ranging, Laser Illuminated Detection and Ranging, or LiDAR: Light Detection and Ranging). The light emitted from the sensor is not particularly limited, but could be infrared light, for example. Alternatively, the light emitted from the sensor could be a laser pulse. The sensor calculates the distance from the sensor to the object using, for example, the time from the emission of pulsed light to the reception of its reflected light, and the propagation speed of the pulsed light. The direction of light emission from the sensor is variable, and the measurement area 40 is scanned by sequentially measuring in multiple emission directions.

[0017] The sensor outputs point cloud data that associates the three-dimensional position of the light reflection point with the reflection intensity (i.e., the light received intensity at the sensor). The first acquisition unit 120 of the position estimation device 10 acquires this point cloud data. The point cloud data output from the sensor is composed of frames. One frame consists of data obtained by scanning the measurement area 40 once. The sensor generates multiple consecutive frames by repeatedly scanning within the measurement area 40. For example, by moving the emitted light back and forth horizontally while moving it vertically, the distance to objects in each angular direction can be measured, thus obtaining three-dimensional information data within the horizontal and vertical scan ranges.

[0018] Figures 3(a) and 3(b) show data points within the first region 41 when the sensor resolution is high and low, respectively. In these figures, both the white and black circles are data points, and the black circles indicate that the light reflection intensity is higher than that of the white circles. The estimated direction of the white line 30 is shown by a dashed arrow. For example, since the white line 30 is drawn with retroreflective material, it has a higher light reflectivity and therefore a higher reflection intensity than the surrounding road surface. Therefore, within the measurement region 40, the first region 41 is set in a location where the white line 30 is easily detected, and the white line 30 is recognized by detecting the part with high reflection intensity within that region.

[0019] When the sensor resolution is high, the position of the ends of the white line 30 and the direction of the white line 30 are clear, as shown in Figure 3(a). If the longitudinal direction of the white line 30 is defined as the vertical direction and the width direction as the horizontal direction, multiple points on the white line 30 can be obtained for each horizontal line. Therefore, by determining whether the spacing between the points at both ends of the detected white line 30 is equivalent to the width of the white line 30, the center point of each line can be found, and by connecting them, the center line of the white line 30 can be determined. In addition, it can be determined that there is a dashed endpoint between the line on which a point on the white line 30 was detected and the line on which no point was detected.

[0020] On the other hand, as shown in Figure 3(b), if the sensor resolution is low, it is not possible to clearly determine the position of the ends of the white line 30 or the direction of the white line 30 from data from only one frame. First, because the acquired point cloud is sparse, it is difficult to determine whether a point is on the white line 30 by comparing it with the width of the white line 30. For example, even if it can be determined that a point is on the white line 30, it is not possible to accurately determine the average position for each line, so a center line cannot be generated. Also, because the contrast between lines that detected the white line 30 and lines that did not is low, it is not possible to determine where the endpoints of the dashed lines are.

[0021] According to the position estimation device 10 of this embodiment, the superposition unit 160 generates superimposed point cloud data by superimposing point cloud data from multiple timings based on movement information. The first estimation unit 180 then estimates the position of the object using the superimposed point cloud data. Therefore, even if the data points obtained in each frame are sparse, it is possible to generate superimposed point cloud data with dense data points, and to accurately estimate the position of objects such as lines on a road.

[0022] Figure 4 is a flowchart illustrating the processing flow performed by the position estimation device 10 according to this embodiment. The position estimation method according to this embodiment is executed by a computer. The position estimation method according to this embodiment includes a first acquisition step S101, a second acquisition step S102, an overlay step S103, and a first estimation step S104. In the first acquisition step S101, point cloud data at multiple timings obtained from sensors mounted on the moving object is acquired. In the second acquisition step S102, movement information of the moving object is acquired. In the overlay step S103, overlaid point cloud data is generated by overlaying the point cloud data at multiple timings based on the movement information. In the first estimation step S104, the position of the object is estimated using the overlaid point cloud data.

[0023] (Examples) Figure 5 is a block diagram illustrating the configuration of a position estimation device 10 according to an embodiment. The position estimation device 10 according to this embodiment has the configuration of the position estimation device 10 according to the embodiment. The position estimation device 10 according to this embodiment further comprises a first region data extraction unit 110, an intensity correction unit 150, and a second estimation unit 190. The superposition unit 160 according to this embodiment comprises a superposition point cloud data generation unit 161, a coordinate transformation unit 163, and a storage unit 165. The first estimation unit 180 according to this embodiment comprises a high-intensity point extraction unit 181, a principal component analysis unit 183, a first identification unit 185, a reliability information generation unit 187, and a second identification unit 189. These will be described in detail below.

[0024] Figure 6 is a flowchart illustrating the flow of the self-position estimation process performed by the position estimation device 10 according to the embodiment. The second estimation unit 190 estimates the position of the moving body 20 using the position estimation result of the first estimation unit 180. Referring to this figure, the overall flow of the process for estimating the position of the moving body 20 performed by the position estimation device 10 will be explained. Hereinafter, the object will be described as a white line, but the object may be something other than a white line. Also, the sensor will be described as a lidar, but the sensor may be of a different type.

[0025] When the position estimation device 10 starts operating, it first determines the initial estimated self-position (S10). Specifically, the second estimation unit 190 uses the positioning result from the GNSS (Global Navigation Satellite System) installed on the moving body 20 or the position estimation device 10 as the initial estimated self-position. Next, the second estimation unit 190 calculates the latest predicted self-position using the previous estimated self-position (S20). Specifically, the second estimation unit 190 obtains the velocity and yaw angular velocity of the moving body 20 from the velocity sensor and gyro sensor installed on the moving body 20, and determines the direction and amount of movement of the moving body 20 from the previous estimated self-position. Then, the second estimation unit 190 calculates the position after movement in the determined direction and amount of movement relative to the previous estimated self-position as the predicted self-position.

[0026] Next, the second estimation unit 190 determines whether a map of the area around the predicted self-position has already been acquired (S30). If the map has already been acquired (Yes in S30), the second estimation unit 190 proceeds to the process in S50 without performing the process in S40. If the map has not yet been acquired (No in S30), the second estimation unit 190 acquires a landmark map of the area around the predicted self-position (S40). The second estimation unit 190 may acquire the landmark map by reading it from a storage unit accessible from the second estimation unit 190, or it may acquire it from an external source via a network. The storage unit accessible from the second estimation unit 190 may be located inside the position estimation device 10, or it may be located outside the position estimation device 10.

[0027] Next, the second estimation unit 190 acquires information about white lines within a detectable distance from the moving object 20 from the map and calculates a predicted measurement value for the white line (S50). The predicted measurement value is a predicted value of how the white line will be measured from the perspective of the moving object 20. Specifically, the second estimation unit 190 identifies information about white lines within a detectable distance from the moving object 20 from the landmark map. Then, based on the position and orientation of the white line shown on the landmark map and the predicted self-position, it calculates a predicted measurement value for the white line.

[0028] Next, the first acquisition unit 120 determines whether or not point cloud data has been acquired (S60). If point cloud data cannot be acquired, such as when occlusion occurs due to other vehicles (No in S60), the process returns to S20. If point cloud data has been acquired by the first acquisition unit 120 (Yes in S60), in S70, white line detection by point cloud superposition and confidence information generation are performed. S70 will be described in detail later with reference to Figure 20.

[0029] In S70, when information indicating the position and direction of the white line and confidence information are generated, the second estimation unit 190 corrects the predicted self-position calculated in S20 using this information (S80). The method used to correct the predicted self-position is not particularly limited, but for example, extended Kalman filtering can be used. The second estimation unit 190 then makes the corrected predicted self-position the latest estimated self-position. The estimated self-position may be output to devices other than the position estimation device 10, or it may be stored in a storage device accessible from the second estimation unit 190. The estimated self-position can be used for functions such as navigation of routes, autonomous driving, or driving assistance.

[0030] Next, the estimation unit 190 determines whether the termination condition is met (S90). The termination condition is met, for example, when the movement of the moving body 20 stops or when an operation is performed to stop the estimation process by the position estimation device 10. If the termination condition is met (Yes in S90), the position estimation device 10 terminates the process. If the termination condition is not met (No in S90), the process returns to S20.

[0031] The hardware configuration of the position estimation device 10 is described below. Each functional component of the position estimation device 10 may be implemented by hardware that realizes each functional component (e.g., hardwired electronic circuits), or by a combination of hardware and software (e.g., a combination of electronic circuits and a program that controls them). The case in which each functional component of the position estimation device 10 is implemented by a combination of hardware and software will be further explained below.

[0032] Figure 7 illustrates a computer 1000 for implementing the position estimation device 10. Computer 1000 is any computer. For example, computer 1000 may be an SoC (System on Chip), a Personal Computer (PC), a server machine, a tablet terminal, or a smartphone. Computer 1000 may be a dedicated computer designed to implement the position estimation device 10, or it may be a general-purpose computer.

[0033] Computer 1000 includes a bus 1020, a processor 1040, memory 1060, a storage device 1080, an input / output interface 1100, and a network interface 1120. Bus 1020 is a data transmission path for the processor 1040, memory 1060, storage device 1080, input / output interface 1100, and network interface 1120 to send and receive data to and from each other. However, the method of connecting the processor 1040 and the other components is not limited to bus connection. The processor 1040 is a variety of processor such as a CPU (Central Processing Unit), GPU (Graphics Processing Unit), or FPGA (Field-Programmable Gate Array). Memory 1060 is a main memory device implemented using RAM (Random Access Memory), etc. Storage device 1080 is an auxiliary storage device implemented using a hard disk, SSD (Solid State Drive), memory card, or ROM (Read Only Memory), etc.

[0034] The input / output interface 1100 is an interface for connecting the computer 1000 with input / output devices. For example, input devices such as keyboards and output devices such as displays are connected to the input / output interface 1100.

[0035] The network interface 1120 is an interface for connecting the computer 1000 to a network. This network is, for example, a LAN (Local Area Network) or a WAN (Wide Area Network). The method by which the network interface 1120 connects to the network may be wireless or wired.

[0036] The storage device 1080 stores program modules that realize each functional component of the position estimation device 10. The processor 1040 reads these program modules into the memory 1060 and executes them to realize the functions corresponding to each program module.

[0037] Each component of the position estimation device 10 is described in detail below. The first acquisition unit 120 acquires point cloud data generated by the lidar. The first acquisition unit 120 may acquire point cloud data generated by the lidar and temporarily stored in the memory unit, or it may acquire point cloud data directly from the lidar. The first acquisition unit 120 acquires point cloud data for multiple frames in the order in which they were generated.

[0038] <Extraction of data points within the first region> The first region data extraction unit 110 extracts data points within the first region 41 from the point cloud data acquired by the first acquisition unit 120. Subsequent processing is performed on the extracted data points within the first region 41. The first region 41 is defined in advance based on the moving body 20, with a predetermined position and size, as it is an area where the white line 30 is likely to exist. The first region 41 is provided on both the left and right sides in front of and behind the moving body 20, i.e., a total of four locations. Processing is performed for each first region 41.

[0039] <Strength correction> As described above, each data point included in the point cloud data acquired by the first acquisition unit 120 is associated with the intensity of reflected light received by the lidar. Then, as will be described later, the first estimation unit 180 estimates the position of the white line using the intensities of multiple data points in the superimposed point cloud data. Here, the intensity correction unit 150 corrects the intensity of each data point included in the point cloud data or superimposed point cloud data using the distance between the data points. The first estimation unit 180 estimates the position of the white line using the corrected intensity.

[0040] Figure 8 shows the relationship between reflection intensity and distance before and after correction. The upper graph in this figure shows the relationship between reflection intensity and distance before correction, and the lower graph shows the relationship between reflection intensity and distance after correction. The white lines drawn with retroreflective material have high reflection intensity. Therefore, by selecting data with high reflection intensity within the first region 41, points on the white lines can be extracted. However, the intensity of the light used for measurement by the lidar decreases as the distance increases. Specifically, the reflected light intensity is inversely proportional to the square of the distance. Therefore, as shown in the upper graph in this figure, it is difficult to extract all point cloud data within the first region 41 using a constant reflection intensity threshold. Even if extraction were possible, setting the threshold would not be easy. Therefore, the intensity correction unit 150 corrects the reflection intensity I using the square of the distance r to the detection point. Specifically, the intensity correction unit 150 calculates I' = A × r 2 The corrected reflectance value I' (corrected reflectance) is calculated based on the relationship ×I. A is a predetermined coefficient. This correction eliminates the effect of distance on the detected intensity, as shown in the graph at the bottom of this figure. Therefore, it becomes possible to extract high reflectance points (also called "high-intensity points") based on a certain threshold for the corrected intensity.

[0041] The intensity correction unit 150 acquires information on the data points within the extracted first region 41 from the first region data extraction unit 110 and corrects the reflection intensity as described above.

[0042] <Point cluster superimposition> Figures 9 and 10 are diagrams illustrating the superposition of point cloud data. In these figures, both white and black circles represent data points, and the black circles indicate that the light reflection intensity is higher than that of the white circles. Figure 9 shows the case where the endpoint of the white line 30 enters the first region 41. Figure 10 shows the case where the endpoint of the white line 30 moves out of the first region 41. The position estimation device 10 includes a shift amount determination unit that determines the amount of shift using movement information. In the example in Figure 5, the coordinate transformation unit 163 functions as the shift amount determination unit. The superposition unit 160 generates superimposed point cloud data using the shift amount. Here, the movement information is information about the movement of a moving object between the timings in which the multiple point cloud data superimposed in the superimposed point cloud data were acquired.

[0043] The superposition unit 160 generates superimposed point cloud data by superimposing point cloud data from multiple time points. During superposition, coordinate transformation of the point cloud is performed according to the amount of vehicle movement and changes in attitude. Specifically, this is done as follows: The superimposed point cloud data generated by the superimposed point cloud data generation unit 161 is temporarily stored in the storage unit 165. The storage unit 165 is a memory and is implemented by the memory 1060 of the computer 1000. The coordinate transformation unit 163 reads the previous superimposed point cloud data from the storage unit 165 and performs coordinate transformation on that data. The superimposed point cloud data generation unit 161 obtains point cloud data from the intensity correction unit 150, to which the corrected reflection intensity is associated with each point. The superimposed point cloud data generation unit 161 superimposes the latest point cloud data onto the coordinate-transformed superimposed point cloud data obtained from the coordinate transformation unit 163. The latest superimposed point cloud data thus obtained is output to the first estimation unit 180 and also stored in the storage unit 165.

[0044] <<Coordinate Transformation>> The coordinate transformation unit 163 performs coordinate transformation using the movement information acquired by the second acquisition unit 140. The movement information includes at least the velocity v and yaw angular velocity ψ' of the moving body 20. The movement information may also include the roll angular velocity φ' and pitch angular velocity θ'. The second acquisition unit 140 can acquire movement information from velocity sensors and angular velocity sensors provided on the moving body 20. Alternatively, the second acquisition unit 140 may calculate the velocity v from the time change of its estimated self-position. Furthermore, the yaw angular velocity ψ' may be the yaw rate output of the IMU (Inertial Measurement Unit) mounted on the moving body 20, or it may be derived from the time change of the yaw angle of the self-position estimation result.

[0045] Figure 11 illustrates how the coordinate transformation unit 163 performs coordinate transformation of point cloud data using velocity v and yaw angular velocity ψ'. As shown in this figure, when the moving body 20 moves, the point cloud data from the previous time point is transformed according to the amount of movement and change in attitude. In this way, the point cloud data can be represented as data in the coordinate system of the moving body after movement. When point p is the target, the coordinate transformation can be performed according to the following equations (1) and (2). Here, if the time interval is Δt, the amount of movement Δx in the direction of travel of the moving body 20 and the change in attitude Δψ can be expressed as Δx=vΔt and Δψ=ψ'Δt, respectively. Note that since the changes in roll and pitch are small, they do not have a significant impact even if they are not included as parameters for coordinate transformation for point cloud superposition.

[0046]

number

number

[0047] By substituting the result of equation (1) into equation (2), the point cloud data from the time before last can also be represented in the current moving object coordinate system. At this point, it can be seen that the information for Δψ1 and Δx1 is no longer needed once the result of equation (1) is obtained. In other words, the coordinate transformation unit 163 can superimpose the necessary amount of past point cloud data by performing a coordinate transformation using the amount of movement and attitude change since the previous time each time.

[0048] Figure 12 illustrates how the coordinate transformation unit 163 performs coordinate transformation of point cloud data using velocity v, yaw angular velocity ψ', roll angular velocity φ', and pitch angular velocity θ'. If the roll angular velocity φ' and pitch angular velocity θ' are known, taking them into consideration during coordinate transformation will enable more accurate point cloud superposition. As shown in this figure, if the roll angle change around the x-axis is Δφ, the pitch angle change around the y-axis is Δθ, and the yaw angle change around the z-axis is Δψ, then the coordinate transformation can be performed using three rotation matrices according to equations (3) and (4) below. The x-axis is the direction of travel of the moving body 20, the z-axis is the vertically upward direction, and the y-axis is perpendicular to the x-axis and z-axis. Here, using the velocity v of the moving body 20, the roll angular velocity φ', the pitch angular velocity θ', the yaw angular velocity ψ', and the time interval Δt, the amount of movement and each attitude change are expressed as Δx=vΔt, Δφ=φ'Δt, Δθ=θ'Δt, and Δψ=ψ'Δt. The second acquisition unit 140 can acquire the roll angular velocity φ' and the pitch angular velocity θ' from the roll rate output and pitch rate output of the IMU mounted on the moving body 20, respectively.

[0049]

number

number

[0050] <<Control of the number of superpositions>> Figure 13 is a diagram illustrating the control of the number of superpositions. In this embodiment, the superposition unit 160 generates superimposed point cloud data by superimposing point cloud data a number of times corresponding to the speed of the moving object 20. That is, point cloud data a number of times (number of frames) corresponding to the speed of the moving object 20 is superimposed. As the number of superpositions increases, the number of point cloud data increases, so the number of points on the white line 30 also increases, making detection easier. On the other hand, if the number of superpositions is too high, the dashed line endpoints become indistinguishable. In order to determine the dashed line endpoints of the white line 30, it is necessary that the boundaries of high reflectivity points can be identified within the superimposed point cloud data. Therefore, for accurate endpoint detection, it is preferable to control the number of superpositions so that the boundaries of the dashed line endpoints can be recognized.

[0051] For example, if the length of the first region 41 in the direction of travel is L, then if multiple point cloud superpositions are within the range of movement up to L / 2, then among the multiple superimposed point cloud data, there will always be point cloud data where the dashed portion of the white line 30 is located within the first region 41. Therefore, the coordinate transformation unit 163 uses the length L of the first region 41, the velocity v of the moving object 20, and the frame period T of the lidar to calculate m based on the relationship m=(L / 2) / v / T=L / (2vT). Then, the coordinate transformation unit 163 identifies the largest integer not exceeding m as the number of superpositions C. For example, if L=10[m], v=27.8[m / s](=100[km / h]), and T=42[ms], then m=10 / (2×27.8×0.042)=4.28. Therefore, the number of superpositions C is determined to be 4.

[0052] <High reflectivity point extraction and endpoint detection> Figure 14 is a diagram illustrating the extraction of high-reflectance points. In this figure, both the white and black circles are data points, and the black circles indicate that the light reflectance is higher than that of the white circles. When the first estimation unit 180 acquires superimposed point cloud data from the superimposed point cloud data generation unit 161, the high-intensity point extraction unit 181 extracts high-intensity points from the superimposed point cloud data. Specifically, the high-intensity point extraction unit 181 extracts data points from the superimposed point cloud data that have an intensity exceeding a predetermined threshold.

[0053] <<Whether endpoint determination is possible or not>> The second identification unit 189 of the first estimation unit 180 estimates the endpoint position of the line on the road surface. The endpoint position is the position in the x-axis direction (x-coordinate), and is the position of the end of the white line 30 projected onto the x-axis. The second identification unit 189 identifies the position of the minimum or maximum x-coordinate among the x-coordinates of the high-intensity points extracted by the high-intensity point extraction unit 181 as the endpoint position. Here, the second identification unit 189 determines whether or not the identified endpoint position is within a predetermined second region 42. If the identified endpoint position is within the second region 42, the second identification unit 189 uses it as the estimated endpoint position of the dashed line of the white line 30. This endpoint position estimation result is used in the second estimation unit 190 to estimate the position of the moving body 20. On the other hand, if the identified endpoint position is not within the second region 42, the second identification unit 189 does not use it as the estimated endpoint position of the dashed line of the white line 30. In other words, the second identification unit 189 estimates the endpoint position using the superimposed point cloud data if the endpoint position is within a predetermined second region 42 in the superimposed point cloud data. The second region 42 is the region containing the center in the x-axis direction of the superimposed point cloud data, and is the region where all the superimposed point cloud data overlap. The width of the second region 42 in the x-axis direction can be obtained by transforming the coordinates of the position of the first region 41 used to generate the superimposed point cloud data. In other words, in the cases of Figures 9 and 10, in the right-hand figure after the coordinate transformation, it is calculated as the distance interval between the x-coordinate of the front end position of the first region 41 at time t(k-3) and the x-coordinate of the rear end position of the first region 41 at time t(k). The coordinate transformation of each position in the first region 41 is performed using equations (1) and (2), or equations (3) and (4), on the coordinates of the region, similar to the point cloud data.

[0054] Figure 15 is a diagram illustrating the meaning of the second region 42. In this figure, both the white and black circles are data points, and the black circles indicate that the light reflection intensity is higher than that of the white circles. If the data in the first region 41 does not capture the dashed line endpoints, the number of data points at the endpoint locations will be small even when superimposed. In the example in Figure 15, only the top two data points correctly have data at the endpoint locations. That is, if the minimum or maximum x-coordinate of a high-reflection intensity point is far from the center of the superimposed point cloud data, it is preferable to judge that it is not appropriate as endpoint information and not use it. Doing so enables highly accurate endpoint detection. Therefore, only if the endpoint of a data point in the superimposed point cloud data that has an intensity exceeding a predetermined threshold is located within the second region 42, the endpoint position of that data point is used as the estimated result of the dashed line of the white line 30. However, even such data can be used to calculate the horizontal position and orientation of the white line 30, which will be described later.

[0055] <Identifying the direction of the white line> Meanwhile, the principal component analysis unit 183 of the first estimation unit 180 performs principal component analysis on the high-intensity point group extracted by the high-intensity point extraction unit 181 to obtain the eigenvalues ​​and eigenvectors of each principal component axis. The first identification unit 185 then identifies the eigenvector of the principal component axis with the largest eigenvalue as the direction vector of the straight line indicating the direction of the white line 30 (longitudinal direction).

[0056] Figure 16 is a diagram illustrating the principal component analysis of a point cloud. First, the principal component analysis unit 183 analyzes the N points [x i y i z i ] T Using this, the covariance matrix C is calculated as shown in equations (5) and (6) below.

[0057]

number

number

[0058] When the eigenvalue λ and eigenvector v of this covariance matrix C are used, the following formula (7) holds.

[0059]

Math

[0060] Therefore, eigenvalues can be obtained by solving the following characteristic equation (8), and eigenvectors can be calculated from the eigenvalues.

[0061]

Math

[0062] Let λ1, λ2, and λ3 be the eigenvalues, which are the three solutions of this equation, arranged in descending order. These represent the variance of the first to third principal component axes. Further, v1, v2, and v3, obtained by substituting each of these into formula (7), are the vectors of the first to third principal component axes. Then, the first specifying unit 185 specifies v1, which is the vector of the first principal component axis, as the vector in the longitudinal direction of the white line 30. Further, the first specifying unit 185 uses the x-component value v 1x and the y-component value v 1y to obtain the arctangent, through the atan2(v 1y ,v 1x ) operation, thereby specifying the orientation of the white line 30. Note that v2, which is the vector of the second principal component axis, corresponds to the vector in the width direction of the white line 30, and v3, which is the vector of the third principal component axis, corresponds to the normal vector to the surface of the white line 30. Here, λ1, λ2, and λ3 respectively correspond to the variance of the point cloud in the longitudinal direction of the white line 30, the variance of the point cloud in the width direction of the white line 30, and the variance of the point cloud on the surface of the white line 30. Further, the first specifying unit 185 specifies the position in the y-axis direction of the centroid of the point cloud extracted by the high-intensity point extraction unit 181 as the lateral position of the white line 30.

[0063] <Generation of Reliability Information> Figures 17 and 18 are diagrams illustrating the detected values ​​and confidence information of the white line 30. The confidence information generation unit 187 uses the results of principal component analysis to calculate each detection confidence level as follows.

[0064] The reliability information generation unit 187 generates the endpoint detection reliability r Lx The larger the number of high-reflectance points n in the second region 42, the greater r Lx It is determined so that it becomes larger. Specifically, the reliability information generation unit 187 determines r Lx The endpoint detection confidence r is based on the relationship = 1 - exp(-a × n). Lx This formula calculates r Lx It can be a value between 0 and 1, inclusive. Note that a is r for n. Lx This is a predetermined coefficient that determines the sensitivity to the sensor, and may be adjusted according to the sensor's resolution.

[0065] Furthermore, the reliability information generation unit 187 generates a horizontal position detection reliability r Ly This is defined as 2 × 3√λ² (= a range of ±3 sigma) and the width W of the white line obtained from the landmark map. M The smaller the difference, the better. Ly It is determined so that it becomes larger. Specifically, the reliability information generation unit 187 determines r Ly =exp(-b × |2 × 3√λ² - W) M Based on the relationship |), the horizontal position detection confidence r Ly This formula calculates r Ly b can be a value between 0 and 1, inclusive. Note that b is |2×3√λ²-W M | for r Ly This is a predetermined coefficient that determines the sensitivity to the sensor, and may be adjusted according to the sensor's resolution.

[0066] The reliability information generation unit 187 then generates the orientation detection reliability r Lψ The larger √λ1 is, the greater r Lψ It is determined so that it becomes larger. Note that the direction is the longitudinal direction of the white line 30. Specifically, the reliability information generation unit 187 determines r Lψ The confidence level for direction detection is based on the relationship = 1 - exp(-c × √λ1). LψThis formula calculates r Lψ c can be a value between 0 and 1, inclusive. Note that c is r for √λ1. Lψ This is a predetermined coefficient that determines the sensitivity to the sensor, and may be adjusted according to the sensor's resolution.

[0067] Thus, by utilizing principal component analysis, it becomes possible to apply extended Kalman filtering to point cloud data regardless of the arrangement of the data points; that is, even if the point cloud is not arranged in a line as in Figure 3(a) and is sparse as in Figure 3(b).

[0068] <Correction of moving object position> The second estimation unit 190 obtains information indicating the endpoint position of the white line 30 from the second identification unit 189, obtains information indicating the orientation of the white line 30 and information indicating the lateral position of the white line 30 from the first identification unit 185, and obtains the endpoint detection confidence level r from the confidence level information generation unit 187. Lx Horizontal position detection confidence r Ly , and orientation detection confidence r Lψ Obtain the endpoint detection confidence r. Lx Horizontal position detection confidence r Ly , and orientation detection confidence r Lψ This information is collectively referred to as confidence information. The second estimation unit 190 then uses this acquired information to perform extended Kalman filtering and correct the position of the moving object 20.

[0069] <<Generation of Observation Noise Matrix>> Specifically, the second estimation unit 190 generates an observation noise matrix R(t) shown in the following equation (9) using the measurement accuracy and reliability information of the white line 30.

[0070]

number

[0071] Here, σ Lx (k) is the measurement accuracy of the distance to the endpoint of the white line 30, σ Ly (k) is the measurement accuracy of the lateral distance of the white line 30, σ Lψ(k) is the measurement accuracy of the direction of the white line 30. Since the accuracy of the lidar measurement deteriorates in proportion to the square of the distance, σ Lx (k), σ Ly (k), and σ Lψ (k) may be set to a value proportional to the square of the distance, but it can be set to a predetermined value if the first region 41 is not very large. As can be seen from equation (9), since the reciprocal of each confidence information is multiplied, the larger each confidence value is (closer to 1), the smaller each element of the noise matrix becomes, and the smaller each confidence value is (closer to 0), the larger each element of the noise matrix becomes. Note that when the confidence value is 0, a value such as 0.0001 is used in place of that confidence value.

[0072] <<Correction of moving object position>> Figure 19 illustrates the positional relationship between the moving object 20 and the white line 30. In this figure, x w y w This is the world coordinate system, and x b y b This is a coordinate system for the moving object 20. b The axial direction is the direction of travel of the moving body 20. The endpoint position L of the white line 30 acquired by the second estimation unit 190. x (k), horizontal position L y (k), and direction L ψ Using (k), the measured values ​​of the white line 30 are represented by the matrix L(k) shown in equation (10) below.

[0073]

number

[0074] Furthermore, the second estimation unit 190 calculates the measured predicted value of the white line 30 as described above for S10 to S50 in Figure 6. The measured predicted value is represented by the matrix L-(k) shown in the following equation (11). As shown in Figure 19, x-(k) and y-(k) are the x and y coordinates of the moving object 20 (predicted self-position) in the world coordinate system, respectively, and ψ-(k) is the x in the direction of travel of the moving object 20 (predicted self-position). w It is the angle with respect to the axis. Also, M x My M ψ This value represents the information of the white lines 30 obtained from the landmark map, expressed in the world coordinate system. Note that if the white line information stored in the landmark map is represented by a discrete sequence of points consisting of latitude, longitude, and altitude information, the white line information is converted to a world coordinate system such as the UTM (Universal Transverse Mercator) coordinate system or the plane rectangular coordinate system. x The x of the endpoint of the sequence of points on the white line 30 w Coordinates (endpoint positions), M y The y of the sequence of points on the white line 30 located within the first region 41 is w It is the average value of the coordinates, M ψ This is the angle of the white line 30, calculated as its direction based on the coordinates of each point in the sequence of white lines 30.

[0075]

number

[0076] The second estimation unit 190 then corrects the predicted self-position using the following equation (12), and the corrected predicted self-position is used as the estimated self-position. Here, the Kalman gain K(k) is expressed by the following equation (13), using the observation noise matrix R(k) described above, a 3x3 Jacobian matrix H(k) for the measured predicted value L-(k), and a 3x3 covariance matrix P-(k). When equation (13) is expressed as a fraction, the observation noise matrix R(k) is located in the denominator, so it can be seen that if the observation noise matrix R(k) is large, the Kalman gain K(k) will be small. In other words, if the confidence information is low, the observation noise matrix R(k) will be large, the Kalman gain K(k) will be small, and as a result, the amount of correction for the predicted self-position in equation (12) will be small. Conversely, if the reliability information is high, the observation noise matrix R(k) does not become large, and the Kalman gain K(k) does not become small. As a result, the correction amount for the predicted self-position in equation (12) becomes appropriate. This suppresses inappropriate corrections when inaccurate measurements are obtained, and applies appropriate corrections when accurate measurements are obtained, thus making it possible to constantly improve the accuracy of self-position estimation.

[0077]

number

number

[0078] Figure 20 is a flowchart illustrating the processing flow of S70 shown in Figure 6. When the first acquisition unit 120 acquires point cloud data, in S701 the first region data extraction unit 110 extracts point cloud data within the first region 41. Next, in S702 the intensity correction unit 150 corrects the reflection intensity of each data point using the distance measurement value of each data point. Next, in S703 the superposition unit 160 generates superimposed point cloud data. Specifically, the coordinate transformation unit 163 calculates the number of superpositions C using the length L of the first region 41, the velocity v of the moving object 20, and the frame period T of the lidar. The coordinate transformation unit 163 then reads the superimposed point cloud data held in the storage unit 165 and excludes data from before the previous C times. The coordinate transformation unit 163 then performs a coordinate transformation on the remaining superimposed data (however, if C=2, it is one set of point cloud data; the same applies hereafter) using, for example, the velocity v of the moving object 20 and the yaw angular velocity ψ'. The superimposed point cloud data generation unit 161 superimposes the latest point cloud data onto the coordinate-transformed superimposed data to generate superimposed point cloud data. The generated superimposed point cloud data is passed to the first estimation unit 180 and also stored in the storage unit 165.

[0079] Next, in S704, the high-intensity point extraction unit 181 extracts data points with high reflectance from the superimposed point cloud data using a threshold value.

[0080] In S705, the second identification unit 189 determines that if the minimum or maximum x-coordinate among the high reflectance points is within the second region 42, it uses that x-coordinate as the estimated endpoint position of the white line 30. The reliability information generation unit 187 also calculates the endpoint detection reliability.

[0081] In S706, the principal component analysis unit 183 performs principal component analysis on the high reflectance point cloud and calculates the eigenvalues ​​and eigenvectors of each principal component axis. The first identification unit 185 then calculates the lateral position of the white line 30 based on the centroid of the high reflectance point cloud and calculates the orientation of the white line 30 from the eigenvector of the first principal component. The reliability information generation unit 187 calculates the lateral position detection reliability using the eigenvalue of the second principal component and the width of the white line 30 read from the landmark map, and calculates the orientation detection reliability from the eigenvalue of the first principal component.

[0082] As described above, according to this embodiment, the same actions and effects as in the first embodiment can be obtained.

[0083] The embodiments and examples described above with reference to the drawings are illustrative examples of the present invention, and various other configurations can also be adopted. Examples of reference formats are provided below. 1. A first acquisition unit that acquires point cloud data at multiple timings obtained from sensors mounted on a moving object, A second acquisition unit that acquires movement information of the aforementioned moving object, A superposition unit generates superimposed point cloud data by superimposing the point cloud data from the multiple timings based on the movement information, The system includes a first estimation unit that estimates the position of an object using the superimposed point cloud data. Location estimation device. 2. In the position estimation device described in 1., The system includes a displacement amount determination unit that determines the displacement amount using the aforementioned movement information, The superimposing section generates the superimposed point cloud data using the shift amount. Location estimation device. 3. In the position estimation device described in 1. or 2., The aforementioned movement information is information relating to the movement of the moving object between the timings in which multiple point cloud data superimposed in the superimposed point cloud data were acquired. Location estimation device. 4. In a position estimation device described in any one of 1 to 3, The sensor emits light and measures the distance to the object by receiving the reflected light reflected by the object. Location estimation device. In the position estimation device described in 5.4, Each data point included in the point cloud data acquired by the first acquisition unit is associated with the intensity of the reflected light received by the sensor. The first estimation unit estimates the position of the object using the intensities of multiple data points in the superimposed point cloud data. Location estimation device. In the position estimation device described in 6.5, The system further includes an intensity correction unit that corrects the intensity of each data point included in the point cloud data or the superimposed point cloud data using the distance between the data points, The first estimation unit estimates the position of the object using the corrected intensity. Location estimation device. 7. In a position estimation device described in any one of items 1 to 6, The aforementioned object is a line on the road surface. Location estimation device. 8. In the position estimation device described in 7., The first estimation unit estimates the endpoint positions of the line on the road surface. Location estimation device. In the position estimation device described in 9.8, The first estimation unit estimates the endpoint position using the superimposed point cloud data when the endpoint position is within a predetermined area in the superimposed point cloud data. Location estimation device. 10. In a position estimation device described in any one of items 1 to 9, The superimposing section generates superimposed point cloud data, which is formed by superimposing point cloud data a number of times corresponding to the speed of the moving object. Location estimation device. 11. In a position estimation device described in any one of items 1 to 10, The system further includes a second estimation unit that estimates the position of the moving body using the position estimation result of the first estimation unit. Location estimation device. 12. A computer-based method for estimating position, The first acquisition step involves acquiring point cloud data at multiple timings obtained from sensors mounted on a moving object, A second acquisition step involves acquiring the movement information of the aforementioned moving object, A superposition step that generates superimposed point cloud data by superimposing the point cloud data from multiple timings based on the movement information, This includes a first estimation step of estimating the position of an object using the superimposed point cloud data, Location estimation method. 13. A program that causes a computer to execute the position estimation method described in 12.

[0084] This application claims priority based on Japanese Patent Application No. 2022-039388, filed on 14 March 2022, and incorporates all of its disclosures herein. [Explanation of Symbols]

[0085] 10 Position estimation device 20 Mobile Units 30 White Line 40 measurement area 41 First area 42 Second area 110 Data extraction unit within the first region 120 First acquisition part 140 Second acquisition part 150 Strength correction section 160 Overlay section 161 Superimposed point cloud data generation unit 163 Coordinate Transformation Unit 165 Storage section 180 1st estimation part 181 High intensity point extraction part 183 Principal component analysis section 185 1st Specific Part 187 Reliability Information Generation Unit 189 Second Specific Part 190 Second estimation part 1000 calculator

Claims

1. A first acquisition unit acquires point cloud data at multiple timings obtained from sensors mounted on a moving object, A second acquisition unit acquires movement information of the aforementioned moving object, The superposition unit repeatedly generates superimposed point cloud data by superimposing the point cloud data at multiple timings based on the movement information, The system includes a first estimation unit that estimates the position of an object using the superimposed point cloud data, After generating the superimposed point cloud data, the position of the object is estimated. The aforementioned superimposed portion is, The number of superpositions C is calculated using the velocity of the moving object. The superimposed point cloud data stored in the memory unit is read out. The latest superimposed point cloud data is generated by excluding the point cloud data that was superimposed C times or earlier from the read-out superimposed point cloud data and superimposing the latest point cloud data acquired by the first acquisition unit. The latest superimposed point cloud data is output to the first estimation unit and stored in the storage unit. Location estimation device.

2. In the position estimation device according to claim 1, The system further includes a first region data extraction unit that extracts data points within a predetermined first region from the point cloud data acquired by the first acquisition unit, The superimposing unit generates the superimposed point cloud data using the data points within the first region extracted by the first region data extraction unit. The number of superpositions C is the largest integer not exceeding m, calculated based on the relationship m = L / (2vT), where L is the length of the first region, v is the velocity of the moving object, and T is the point cloud data generation period of the sensor. Location estimation device.

3. In the position estimation device according to claim 1 or 2, The superimposed point cloud data is denser than the individual point cloud data from the multiple timings. Location estimation device.

4. In the position estimation device according to claim 1 or 2, The system includes a displacement amount determination unit that determines the displacement amount using the aforementioned movement information, The superimposing section generates the superimposed point cloud data using the shift amount. Location estimation device.

5. In the position estimation device according to claim 1 or 2, The aforementioned movement information is information relating to the movement of the moving object between the timings in which multiple point cloud data superimposed in the superimposed point cloud data were acquired. Location estimation device.

6. In the position estimation device according to claim 1 or 2, The sensor emits light and measures the distance to the object by receiving the reflected light reflected by the object. Location estimation device.

7. In the position estimation device according to claim 6, Each data point included in the point cloud data acquired by the first acquisition unit is associated with the intensity of the reflected light received by the sensor. The first estimation unit estimates the position of the object using the intensities of multiple data points in the superimposed point cloud data. Location estimation device.

8. In the position estimation device according to claim 7, The system further includes an intensity correction unit that corrects the intensity of each data point included in the point cloud data or the superimposed point cloud data using the distance between the data points, The first estimation unit estimates the position of the object using the corrected intensity. Location estimation device.

9. In the position estimation device according to claim 1 or 2, The aforementioned object is a line on the road surface. Location estimation device.

10. In the position estimation device according to claim 9, The first estimation unit estimates the endpoint positions of the line on the road surface. Location estimation device.

11. In the position estimation device according to claim 1 or 2, The system further includes a second estimation unit that estimates the position of the moving body using the position estimation result of the first estimation unit. Location estimation device.

12. A computer-based method for estimating position, The first acquisition step involves acquiring point cloud data at multiple timings obtained from sensors mounted on a moving object, A second acquisition step involves acquiring movement information of the aforementioned moving object, The superposition step repeatedly generates superimposed point cloud data in which the point cloud data from multiple timings is superimposed based on the movement information, This includes a first estimation step of estimating the position of an object using the superimposed point cloud data, After generating the superimposed point cloud data, the position of the object is estimated. In the aforementioned superposition step, The number of superpositions C is calculated using the velocity of the moving object. The superimposed point cloud data stored in the memory unit is read out. The latest superimposed point cloud data is generated by excluding the point cloud data that was superimposed C times or earlier from the read-out superimposed point cloud data, and superimposing the latest point cloud data acquired in the first acquisition step. The latest superimposed point cloud data is output for the first estimation step and stored in the storage unit. Location estimation method.

13. A program that causes a computer to execute the position estimation method described in claim 12.

Images