Position estimation device, position estimation method and program
By generating and using overlapping point cloud data, the problem of reducing position estimation accuracy caused by sparse data points is solved, and high-precision position estimation under sparse data points is achieved.
Patent Information
- Application Number
- CN202380026147.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Priority Date
- 2022-03-14
- Filing Date
- 2023-03-08
- Publication Date
- 2024-11-05
AI Technical Summary
In the prior art, the position estimation accuracy is strongly dependent on the density of the data points obtained by detecting the dividing line, resulting in a decrease in the position estimation accuracy when the data points are sparse.
By obtaining multiple timing point cloud data obtained by sensors mounted on mobile bodies, and generating overlapping point cloud data based on mobile information, the position of the object is estimated using overlapping point cloud data, thereby improving the accuracy of position estimation.
Even when the data points are sparse, the position of objects such as lines on the road can be accurately estimated, which improves the accuracy and reliability of position estimation.
Smart Images

Figure CN118900984A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to a position estimating device, a position estimating method and a program. Background Art
[0002] There is known a technique for detecting objects installed around a vehicle using radar, a camera, or the like, and estimating the position of the vehicle using the detection results.
[0003] Patent Documents 1 and 2 disclose methods of estimating the position of a vehicle using the results of detecting demarcation lines such as white lines provided on a road.
[0004] Prior art literature
[0005] Patent Literature
[0006] Patent Document 1: International Publication No. 2018 / 212302
[0007] Patent Document 2: International Publication No. 2019 / 189098 Summary of the invention
[0008] Problems to be solved by the invention
[0009] However, in the techniques of Patent Documents 1 and 2, the estimation accuracy strongly depends on the density of data points obtained by detecting the dividing lines. That is, when the data points are sparse, the position estimation accuracy decreases.
[0010] As an example of a problem to be solved by the present invention, it is possible to provide a technology capable of estimating the position of an object with high accuracy even if data points are sparse.
[0011] Means for solving problems
[0012] The invention described in claim 1 is a position estimation device comprising:
[0013] A first acquisition unit acquires a plurality of timed point cloud data obtained by a sensor mounted on the mobile body;
[0014] A second acquiring unit, acquiring movement information of the moving object;
[0015] an overlapping unit for generating overlapping point cloud data by overlapping the plurality of timed point cloud data based on the movement information; and
[0016] The first estimating unit estimates the position of the object using the overlapping point cloud data.
[0017] The invention described in claim 12 is a position estimation method, which is a position estimation method executed by a computer, and the position estimation method comprises:
[0018] A first acquisition step is to acquire a plurality of timed point cloud data obtained by a sensor mounted on the mobile body;
[0019] A second obtaining step of obtaining movement information of the moving object;
[0020] an overlapping step of generating overlapping point cloud data by overlapping the plurality of timed point cloud data based on the movement information; and
[0021] The first estimating step estimates the position of the object using the overlapping point cloud data.
[0022] The invention described in claim 13 is a program for causing a computer to execute the position estimation method described in claim 12 . BRIEF DESCRIPTION OF THE DRAWINGS
[0023] Figure 1 It is a diagram illustrating the configuration of a position estimating device according to an embodiment.
[0024] Figure 2 This is a diagram illustrating a situation where a mobile object detects a white line on a road.
[0025] Figure 3 (a) and (b) are diagrams showing data points in the first region when the resolution of the sensor is high and when the resolution of the sensor is low, respectively.
[0026] Figure 4 This is a flowchart illustrating the flow of processing performed by the position estimating device according to the embodiment.
[0027] Figure 5 This is a block diagram illustrating the structure of a position estimating device according to an embodiment.
[0028] Figure 6 This is a flowchart illustrating the flow of a self-position estimation process performed by the position estimation device according to the embodiment.
[0029] Figure 7 This is a diagram illustrating a computer for realizing a position estimating device.
[0030] Figure 8 It is a graph showing the relationship between the reflection intensity and the distance before and after correction.
[0031] Fig. 9 This is a diagram for explaining the overlapping of point cloud data.
[0032] Fig.10 This is a diagram for explaining the overlapping of point cloud data.
[0033] Fig.11 This is a diagram for explaining a method in which the coordinate conversion unit performs coordinate conversion of point cloud data using the velocity v and the yaw angular velocity ψ′.
[0034] Fig.12 This is a diagram for explaining a method in which a coordinate conversion unit performs coordinate conversion of point cloud data using a velocity v, a yaw angular velocity ψ', a roll angular velocity φ', and a pitch angular velocity θ'.
[0035] Fig.13 This is a diagram for explaining the control of the number of overlaps.
[0036] Fig.14 It is a diagram for explaining the extraction of high reflection intensity points.
[0037] Fig.15 This is a diagram for explaining the meaning of the second area.
[0038] Fig.16 This is a diagram for explaining principal component analysis of a point cloud.
[0039] Fig.17 It is a diagram for explaining the detection value and reliability information of the white line.
[0040] Fig.18 It is a diagram for explaining the detection value and reliability information of the white line.
[0041] Fig.19 This is a diagram illustrating the positional relationship between a moving object and a white line.
[0042] Fig. 20 is an example Figure 6 The flowchart of the process flow of S70 is shown. DETAILED DESCRIPTION
[0043] Hereinafter, the embodiments of the present invention will be described using the accompanying drawings. It should be noted that, in all the accompanying drawings, the same components are marked with the same reference numerals, and the description is appropriately omitted. In addition, for convenience, the characters formed by marking "·" or "-" on any symbol are respectively represented as "A'" or "A-" ("A" is an arbitrary character).
[0044] (Implementation Method)
[0045] Figure 1 1 is a diagram illustrating the structure of a position estimation device 10 according to an exemplary embodiment. The position estimation device 10 according to the present embodiment includes a first acquisition unit 120, a second acquisition unit 140, an overlapping unit 160, and a first estimation unit 180. The first acquisition unit 120 acquires a plurality of timed point cloud data obtained by a sensor mounted on a mobile body. The second acquisition unit 140 acquires movement information of the mobile body. The overlapping unit 160 generates overlapping point cloud data by overlapping a plurality of timed point cloud data based on the movement information. The first estimation unit 180 estimates the position of an object using the overlapping point cloud data. It should be noted that the plurality of timed point cloud data are, for example, point cloud data at a plurality of moments.
[0046] In a mobile body such as a vehicle, it is important to correctly determine the position of the mobile body in order to make the navigation of a path, automatic driving, driving assistance, etc. function with good accuracy. In a mobile body, the following work is performed while traveling on a road: the position of the mobile body at a certain point in time, the moving speed and moving direction of the mobile body until the next point in time are used to estimate the position of the mobile body at the next point in time. Here, by detecting the objects (targets) around the mobile body with sensors and the like, and comparing them with the positions of the objects in the map information, corrections can be made to further improve the estimation accuracy of the position of the mobile body. However, in order to improve the estimation accuracy of the mobile body, it is necessary to use the detection results of the sensors to estimate the position of the target object with high accuracy.
[0047] In this embodiment, the object of the estimated position is a line on the road surface. Specifically, the object is a dividing line on the road surface, which is a dotted line. The color of the line as the object is not particularly limited, for example, yellow or white. The surface of the object is preferably formed by a retro-reflective material.
[0048] Figure 2 4 is a diagram illustrating a situation where a mobile body 20 detects a white line 30 on a road. The sensor mounted on the mobile body 20 measures the area inside the measurement area 40. As described in detail later, the position estimation device 10 extracts the measurement data in the first area 41 and uses it for detecting the white line 30.
[0049] In the present embodiment, the sensor measures the distance to the object by emitting light and receiving reflected light reflected by the object. The sensor is not particularly limited, and may be a radar or a laser radar (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, and may be, for example, infrared light. In addition, the light emitted from the sensor may be, for example, a laser pulse. In the sensor, the distance from the sensor to the object is calculated using, for example, the time from emitting pulse light to receiving its reflected light and the propagation speed of the pulse light. The emission direction of the light from the sensor is variable, and by sequentially measuring a plurality of emission directions, the measurement area 40 is scanned.
[0050] Point cloud data that is formed by associating the three-dimensional position of the reflection point of the light output from the sensor with the reflection intensity (i.e., the light intensity received in the sensor). The first acquisition unit 120 of the position estimation device 10 acquires the point cloud data. The point cloud data output from the sensor is composed of frames. One frame is composed of data obtained by scanning the measurement area 40 once. In the sensor, a plurality of continuous frames are generated by repeatedly scanning within the measurement area 40. For example, by causing the emitted light to reciprocate in the horizontal direction while moving in the vertical direction, the distance to an object existing in each angular direction can be measured, so that data of three-dimensional information within the horizontal and vertical scanning ranges can be obtained.
[0051] Figure 3 (a) and Figure 3 (b) is a diagram showing data points in the first area 41 when the resolution of the sensor is high and low. In each of these diagrams, it is shown that the white circles and the black circles are data points, and the light reflection intensity is higher in the black circles than in the white circles. In addition, the image in the direction of the estimated white line 30 is shown by the dotted arrow. For example, since the white line 30 is drawn with a retro-reflective material, the reflectivity of the light is higher than that of the surrounding road surface, and the reflection intensity becomes higher. Therefore, in the measurement area 40, the first area 41 is set at a position where the white line 30 is easily detected, and the part with higher reflection intensity is detected to identify the white line 30.
[0052] In the case of higher resolution sensors, such as Figure 3 As shown in (a), the positions of the ends of the white line 30 and the direction of the white line 30 are clear. When the length direction of the white line 30 is set as the longitudinal direction and the width direction is set as the transverse direction, since a plurality of points on the white line 30 can be obtained in a row in the transverse direction, it is possible to determine whether the interval between the points detected at both ends of the white line 30 is equal to the width of the white line 30, find the center point in each row, and connect them to find the center line of the white line 30. In addition, it can be determined that there is an endpoint of a dotted line between a row where the points on the white line 30 are detected and a row where the points on the white line 30 are not detected.
[0053] On the other hand, Figure 3 As shown in (b), when the resolution of the sensor is low, the position of the end of the white line 30 and the direction of the white line 30 cannot be clearly determined based on the data of one frame. First, since the point cloud obtained is sparse, it is difficult to determine whether it is a point 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 it is a point on the white line 30, the center line cannot be generated because the average position of each row cannot be correctly calculated. In addition, since the contrast between the row where the white line 30 is detected and the row where it is not detected is low, it is unknown where the end point of the dotted line is.
[0054] According to the position estimation device 10 of this embodiment, the overlapping unit 160 generates overlapping point cloud data by overlapping a plurality of timed point cloud data based on the movement information. Then, the first estimation unit 180 estimates the position of the object using the overlapping point cloud data. Therefore, even if the data points obtained in each frame are sparse, overlapping point cloud data with dense data points is generated, and the position of the object such as the line on the road can be estimated with good accuracy.
[0055] Figure 4 It is a flowchart illustrating the process of processing performed by the position estimation device 10 of the present embodiment. The position estimation method of the present embodiment is executed by a computer. The position estimation method of the present embodiment includes a first acquisition step S101, a second acquisition step S102, an overlapping step S103 and a first estimation step S104. In the first acquisition step S101, a plurality of timed point cloud data obtained by a sensor mounted on a moving body are acquired. In the second acquisition step S102, movement information of the moving body is acquired. In the overlapping step S103, overlapping point cloud data is generated by overlapping a plurality of timed point cloud data based on the movement information. In the first estimation step S104, the position of the object is estimated using the overlapping point cloud data.
[0056] (Example)
[0057] Figure 5 1 is a block diagram illustrating the structure of the position estimation device 10 of the embodiment. The position estimation device 10 of the present embodiment has the structure of the position estimation device 10 of the embodiment. The position estimation device 10 of the present embodiment further includes a first region data extraction unit 110, an intensity correction unit 150, and a second estimation unit 190. The overlapping unit 160 of the present embodiment includes an overlapping point cloud data generation unit 161, a coordinate conversion unit 163, and a storage unit 165. The first estimation unit 180 of the present embodiment includes a high intensity point extraction unit 181, a principal component analysis unit 183, a first determination unit 185, a reliability information generation unit 187, and a second determination unit 189. The following is a detailed description.
[0058] Figure 6 This is a flowchart of the process of the self-position estimation processing performed by the position estimation device 10 of the exemplary embodiment. The second estimation unit 190 estimates the position of the moving body 20 using the estimation result of the position of the object estimated by the first estimation unit 180. With reference to this figure, the overall process of the processing for position estimation of the moving body 20 performed by the position estimation device 10 is described. In the following, the object is set as a white line and described, but the object may be an object other than the white line. In addition, the sensor is set as a laser radar and described, but the sensor may also be a sensor of another type.
[0059] When the operation of the position estimation device 10 is started, the position estimation device 10 first determines the initial estimated self-position (S10). Specifically, the second estimation unit 190 uses the positioning result of the GNSS (Global Navigation Satellite System) installed in the mobile body 20 or the position estimation device 10 as the initial estimated self-position. Then, the second estimation unit 190 uses the previous estimated self-position to calculate the latest predicted self-position (S20). Specifically, the second estimation unit 190 obtains the speed and yaw angular velocity of the mobile body 20 from the speed sensor and gyro sensor installed in the mobile body 20, and determines the moving direction and movement amount of the mobile body 20 from the previous estimated self-position. Then, the second estimation unit 190 calculates the position after the movement in the determined moving direction and movement amount relative to the previous estimated self-position as the predicted self-position.
[0060] Next, the second estimation unit 190 determines whether a map of the predicted own position surroundings has been obtained (S30). If the map has been obtained (S30 is yes), the second estimation unit 190 does not perform the process of S40 but performs the process of S50. If the map has not been obtained (S30 is no), the second estimation unit 190 obtains a landmark map of the predicted own position surroundings (S40). The second estimation unit 190 can read and obtain the landmark map from a storage unit accessible from the second estimation unit 190, or obtain it from the outside through a network. The storage unit accessible from the second estimation unit 190 can be set inside the position estimation device 10, or can be set outside the position estimation device 10.
[0061] Next, the second estimation unit 190 obtains information about a white line located at a distance detectable by the laser radar from the moving body 20 from the map, and calculates a measurement prediction value of the white line (S50). The measurement prediction value is a prediction value of how the white line is measured when viewed from the moving body 20. Specifically, the second estimation unit 190 determines information about a white line located at a distance detectable from the moving body 20 in the landmark map. Then, based on the position and direction of the white line shown on the landmark map and the predicted own position, the measurement prediction value of the white line is calculated.
[0062] Next, the first acquisition unit 120 determines whether the point cloud data can be acquired (S60). If the point cloud data cannot be acquired due to a blockage caused by other vehicles or the like (S60 is No), the process returns to S20. If the point cloud data is acquired by the first acquisition unit 120 (S60 is Yes), white line detection based on point cloud overlap and generation of reliability information are performed in S70. Fig. 20 S70 is described in detail.
[0063] When the information showing the position and direction of the white line and the reliability information are generated in S70, the second estimation unit 190 uses this information to correct the predicted self-position calculated in S20 (S80). The method of using the predicted self-position for correction is not particularly limited, and for example, extended Kalman filter processing can be used. Then, the second estimation unit 190 uses the corrected predicted self-position as the latest estimated self-position. The estimated self-position can be output to a device other than the position estimation device 10, or it can be maintained in a storage device accessible from the second estimation unit 190. The estimated self-position can be used for navigation of a route, automatic driving, or driving assistance, etc.
[0064] Next, the second estimation unit 190 determines whether the termination condition is satisfied (S90). The termination condition is satisfied when, for example, the movement of the moving body 20 stops or an operation is performed to stop the estimation process of the position estimation device 10. If the termination condition is satisfied (Yes in S90), the position estimation device 10 ends the process. If the termination condition is not satisfied (No in S90), the process returns to S20.
[0065] The following describes the hardware structure of the position estimation device 10. Each functional component of the position estimation device 10 may be implemented by hardware that implements each functional component (e.g., a hard-wired electronic circuit, etc.), or may be implemented by a combination of hardware and software (e.g., a combination of an electronic circuit and a program that controls the electronic circuit, etc.). The following further describes the case where each functional component of the position estimation device 10 is implemented by a combination of hardware and software.
[0066] Figure 7 1 is a diagram illustrating a computer 1000 for realizing the position estimation device 10. The computer 1000 is any computer. For example, the computer 1000 is a SoC (System On Chip), a Personal Computer (PC), a server machine, a tablet terminal, or a smartphone. The computer 1000 may be a dedicated computer designed to realize the position estimation device 10, or may be a general-purpose computer.
[0067] The computer 1000 has a bus 1020, a processor 1040, a memory 1060, a storage device 1080, an input / output interface 1100, and a network interface 1120. The bus 1020 is a data transmission path for the processor 1040, the memory 1060, the storage device 1080, the input / output interface 1100, and the network interface 1120 to send and receive data to each other. However, the method of connecting the processor 1040 and the like to each other is not limited to the bus connection. The processor 1040 is a CPU (Central Processing Unit), a GPU (Graphics Processing Unit), or a FPGA (Field-Programmable Gate Array) and other processors. The memory 1060 is a main storage device implemented using a RAM (Random Access Memory) and the like. The storage device 1080 is an auxiliary storage device implemented using a hard disk, an SSD (Solid State Drive), a memory card, or a ROM (Read Only Memory) and the like.
[0068] The input / output interface 1100 is an interface for connecting the computer 1000 to an input / output device. For example, an input device such as a keyboard and an output device such as a display are connected to the input / output interface 1100 .
[0069] The network interface 1120 is an interface for connecting the computer 1000 to a network. The communication network is, for example, a LAN (Local Area Network) or a WAN (Wide Area Network). The network interface 1120 may be connected to the network by wireless connection or by wired connection.
[0070] The storage device 1080 stores program modules for realizing the functional components of the position estimation apparatus 10. The processor 1040 reads the program modules into the memory 1060 and executes the program modules, thereby realizing the functions corresponding to the program modules.
[0071] The following describes in detail the components of the position estimation device 10. The first acquisition unit 120 acquires point cloud data generated by the laser radar. The first acquisition unit 120 may acquire point cloud data generated by the laser radar and temporarily stored in the storage unit, or may directly acquire point cloud data from the laser radar. The first acquisition unit 120 acquires a plurality of frames of point cloud data in the order in which they are generated.
[0072] <Extraction of Data Points in the First Region>
[0073] The first area data extraction unit 110 extracts data points in the first area 41 of the point cloud data acquired by the first acquisition unit 120. The following processing is performed on the extracted data points in the first area 41. The first area 41 is a region with a high probability of the existence of the white line 30, and the position and size are preset based on the moving body 20. The first area 41 is set on the left and right of the front and rear of the moving body 20, that is, it is set in 4 places in total. Each first area 41 is processed.
[0074] <Intensity Correction>
[0075] As described above, the intensity of the reflected light received by the laser radar is associated with each data point included in the point cloud data obtained by the first acquisition unit 120. Then, as described later, the first estimation unit 180 estimates the position of the white line using the intensities of multiple data points in the overlapping point cloud data. Here, the intensity correction unit 150 corrects the intensity of each data point included in the point cloud data or the overlapping point cloud data using the distance of the data point. The first estimation unit 180 estimates the position of the white line using the corrected intensity.
[0076] Figure 8 It is a graph showing the relationship between the reflection intensity and the distance before and after correction. The upper graph of this figure shows the relationship between the reflection intensity and the distance before correction, and the lower graph shows the relationship between the reflection intensity and the distance after correction. The reflection intensity of the white line drawn with the retro-reflective material is high. Therefore, by selecting data with higher reflection intensity in the first area 41, it is possible to extract the points on the white line. However, the farther the distance, the lower the intensity of the light used for measurement in the laser radar. Specifically, the intensity of the reflected light is inversely proportional to the square of the distance. Therefore, as shown in the upper graph of this figure, it is difficult to use a certain reflection intensity threshold for all the point cloud data in the first area 41 for extraction. Even if it is possible to extract, it is not easy to set the threshold. Therefore, the intensity correction unit 150 uses the square value of the distance r to the detection point to correct the reflection intensity I. Specifically, the intensity correction unit 150 is based on I'=A×r 2 ×I, the reflection intensity correction value I' (corrected reflection intensity) is calculated. It should be noted that A is a specified coefficient. Through this correction, as shown in the chart on the lower side of this figure, the influence of distance on the detected intensity is eliminated. Therefore, for the corrected intensity, a certain threshold value can be used to extract high reflection intensity points (also called "high intensity points").
[0077] The intensity correction unit 150 obtains information of the data points in the first region 41 extracted from the first region data extraction unit 110 , and corrects the reflection intensity as described above.
[0078] <Point Cloud Overlap>
[0079] Fig. 9 and Fig.10 2 and 3 are diagrams for explaining the overlapping of point cloud data. In these diagrams, it is shown that both white circles and black circles are data points, and the light reflection intensity is higher in the black circles than in the white circles. Fig. 9 The state in which the end point of the white line 30 gradually enters the first area 41 is shown. Fig.10 The end point of the white line 30 gradually moves away from the first area 41. The position estimation device 10 includes an offset determination unit that determines the offset using the movement information. Figure 5 In the example of , the coordinate conversion unit 163 functions as an offset determination unit. The superposition unit 160 generates superimposed point cloud data using the offset. Here, the movement information is information related to the movement of the moving object between the timings of acquiring a plurality of point cloud data superimposed in the superimposed point cloud data.
[0080] The overlapping unit 160 overlaps the point cloud data at multiple times to generate overlapping point cloud data. Here, when overlapping, the coordinate transformation of the point cloud is performed according to the movement amount or posture change of the vehicle. Specifically, it is as follows. The overlapping point cloud data generated by the overlapping point cloud data generating unit 161 is temporarily stored in the storage unit 165. The storage unit 165 is a memory, which is implemented using the memory 1060 of the computer 1000. In the coordinate transformation unit 163, the previous overlapping point cloud data is read from the storage unit 165 and the coordinate transformation is performed on the data. The overlapping point cloud data generating unit 161 obtains the point cloud data in which the corrected reflection intensity is associated with each point from the intensity correction unit 150. The overlapping point cloud data generating unit 161 overlaps the latest point cloud data on the overlapping point cloud data after coordinate transformation obtained from the coordinate transformation unit 163. The latest overlapping point cloud data obtained in this way is output to the first estimation unit 180 and stored in the storage unit 165.
[0081] <<Coordinate Conversion>>
[0082] The coordinate conversion unit 163 performs coordinate conversion using the movement information obtained by the second acquisition unit 140. The movement information includes at least the speed v and the yaw angular velocity ψ' of the moving body 20. The movement information may also include the roll angular velocity φ' and the pitch angular velocity θ'. The second acquisition unit 140 can obtain the movement information from the speed sensor and the angular velocity sensor provided on the moving body 20. Alternatively, the second acquisition unit 140 can calculate the speed v based on the estimated time change of its own position. In addition, the yaw angular velocity ψ' can be the yaw rate output of the IMU (Inertial Measurement Unit) mounted on the moving body 20, or it can be derived from the time change of the yaw angle of the self-position estimation result.
[0083] Fig.11This is a diagram for explaining a method in which the coordinate conversion unit 163 uses velocity v and yaw angular velocity ψ' to perform coordinate conversion on point cloud data. When the mobile body 20 moves as shown in this figure, the point cloud data at the previous moment is converted into coordinates according to its movement amount and posture change. In this way, the point cloud data can be represented as data in the coordinate system of the mobile body after the movement. When point p is taken as an object, the coordinate conversion can be implemented according to the following equations (1) and (2). Here, when the time interval is set to Δt, the movement amount Δx and posture change Δψ in the direction of travel of the mobile body 20 can be represented by Δx=vΔt and Δψ=ψ'Δt, respectively. It should be noted that since the changes in roll and pitch are relatively small, there is no significant impact even if they are not added as parameters for coordinate conversion for point cloud overlapping.
[0084] [Mathematical formula 1]
[0085]
[0086] [Mathematical formula 2]
[0087]
[0088] If the result of equation (1) is substituted into equation (2), the point cloud data at the previous moment can also be expressed in the current moving body coordinate system. At this time, it can be seen that at the moment when the result of equation (1) is obtained, the information of Δψ1 and Δx1 is no longer required. That is, the coordinate conversion unit 163 can overlap the required amount of past point cloud data by performing coordinate conversion using the movement amount and posture change from the previous time each time.
[0089] Fig.12 This is a diagram for explaining a method in which the coordinate conversion unit 163 uses velocity v, yaw angular velocity ψ', roll angular velocity φ', and pitch angular velocity θ' to perform coordinate conversion on point cloud data. When the roll angular velocity φ' and the pitch angular velocity θ' are known, by also taking them into consideration for coordinate conversion, more accurate point cloud superposition can be achieved. As shown in this figure, when the roll angle change around the x-axis is set to Δφ, the pitch angle change around the y-axis is set to Δθ, and the yaw angle change around the z-axis is set to Δψ, a cubic rotation matrix can be used to implement coordinate conversion using the following equations (3) and (4). The x-axis is the direction of travel of the mobile body 20, the z-axis is vertically upward, and the y-axis is a direction perpendicular to the x-axis and the z-axis. Here, the movement amount and each posture change are represented by Δx=vΔt, Δφ=φ'Δt, Δθ=θ'Δt, and Δψ=ψ'Δt using the velocity v, roll angular velocity φ', pitch angular velocity θ', yaw angular velocity ψ', and time interval Δt of the mobile body 20. The second acquisition unit 140 can acquire the roll angular velocity φ' and the pitch angular velocity θ' from the roll rate output and the pitch rate output of the IMU mounted on the mobile body 20, respectively.
[0090] [Mathematical formula 3]
[0091]
[0092] [Formula 4]
[0093]
[0094] <<Overlap times control>>
[0095] Fig.13 : is a diagram for explaining the control of the number of overlaps. In the present embodiment, the overlapping unit 160 generates overlapping point cloud data in which point cloud data of a number corresponding to the speed of the moving body 20 are overlapped. That is, point cloud data of a number (number of frames) corresponding to the speed of the moving body 20 are overlapped. Since the number of point cloud data increases as the number of overlaps increases, the number of points on the white line 30 also increases, and detection becomes easier. On the other hand, when the number of overlaps is too large, it becomes impossible to determine the endpoint of the dotted line. In order to find the dotted line endpoint of the white line 30, it is necessary to be able to grasp the state of the boundary of the high reflection intensity point in the overlapping point cloud data. Therefore, in order to perform correct endpoint detection, it is preferable to control the number of overlaps in a manner that can identify the boundary of the dotted line endpoint.
[0096] For example, when the length of the first area 41 in the direction of travel is set to L, if multiple point clouds overlap within the range of moving to L / 2, then among the overlapping multiple point cloud data, there must be point cloud data in which the dotted line portion of the white line 30 is located in the first area 41. Therefore, the coordinate conversion unit 163 uses the length L of the first area 41, the speed v of the moving body 20, and the frame period T of the laser radar to calculate m based on the relationship of m=(L / 2) / v / T=L / (2vT). Then, the coordinate conversion unit 163 determines the maximum integer not exceeding m as the number of overlaps C. For example, when L=10[m], v=27.8[m / s](=100[km / h]), and T=42[ms], m=10 / (2×27.8×0.042)=4.28. Therefore, the number of overlaps C is determined to be 4 times.
[0097] <High reflection intensity point extraction and endpoint detection>
[0098] Fig.14 is a diagram for explaining the extraction of high reflection intensity points. In this figure, it is shown that the white circles and the black circles are both data points, and the light reflection intensity at the black circles is higher than that at the white circles. When the first estimating unit 180 obtains the overlapping point cloud data from the overlapping point cloud data generating unit 161, the high intensity point extracting unit 181 extracts the high intensity points in the overlapping point cloud data. Specifically, the high intensity point extracting unit 181 extracts data points having an intensity exceeding a predetermined threshold value in the overlapping point cloud data.
[0099] <<Can endpoint determination be made>>
[0100] The second determination unit 189 of the first estimation unit 180 estimates the endpoint position of the line on the road surface. The endpoint position is a 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 determination unit 189 determines 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 determination unit 189 determines whether the determined endpoint position is located within the prescribed second area 42. When the determined endpoint position is located within the second area 42, the second determination unit 189 uses it as the estimation result of the endpoint position of the dashed line of the white line 30. The estimation result of the endpoint position is used in the second estimation unit 190 for the position estimation of the moving body 20. On the other hand, when the determined endpoint position is not located within the second area 42, the second determination unit 189 does not use it as the estimation result of the endpoint position of the dashed line of the white line 30. That is, when the endpoint position is located within the prescribed second area 42 in the superimposed point cloud data, the second determination unit 189 estimates the endpoint position using the superimposed point cloud data. The second region 42 is a region including the center of the overlapping point cloud data in the x-axis direction, and is a region where all the overlapping point cloud data overlap. It should be noted that the width of the second region 42 in the x-axis direction can be obtained by performing coordinate transformation on the position of the first region 41 used in generating the overlapping point cloud data. Fig. 9 and Fig.10 In the case of, in the figure on the right after the coordinate conversion, the distance interval between the x coordinate of the front end position of the first area 41 at time t(k-3) and the x coordinate of the rear end position of the first area 41 at time t(k) is calculated. It should be noted that the coordinate conversion of each position of the first area 41 is performed in the same way as the point cloud data, using equations (1) and (2) or equations (3) and (4) for the coordinates of the area.
[0101] Fig.15 This is a diagram for explaining the meaning of the second region 42. In this figure, it is shown that the white circles and the black circles are data points, and the light reflection intensity is higher in the black circles than in the white circles. Sometimes the data in the first region 41 does not capture the dotted line endpoints, and even if they overlap, the data at the endpoints is small. Fig.15In the example, only the two data above correctly have data on the endpoint position. That is, when the minimum or maximum x-coordinate of the x-coordinates of the high reflection intensity points is far away from the central part of the overlapping point cloud data, it is judged to be inappropriate as endpoint information and is preferably not adopted. As a result, high-precision endpoint detection can be performed. Therefore, only when the endpoint of the data point with an intensity exceeding a specified threshold in the overlapping point cloud data is located in the second area 42, the endpoint position is used as the estimated result of the dotted line of the white line 30. However, even such data can be used in the calculation of the lateral position and direction of the white line 30 described later.
[0102] <Determination of the direction of the white line>
[0103] On the other hand, the principal component analysis unit 183 of the first estimation unit 180 performs principal component analysis on the high-intensity point cloud extracted by the high-intensity point extraction unit 181, and obtains the eigenvalue and eigenvector of each principal component axis. Then, the first determination unit 185 determines the eigenvector of the principal component axis having the largest eigenvalue as the direction vector of the straight line indicating the direction (length direction) of the white line 30.
[0104] Fig.16 1 is a diagram for explaining the principal component analysis of a point cloud. First, the principal component analysis unit 183 uses the N points [xi yi zi] extracted by the high intensity point extraction unit 181. T , the covariance matrix C is calculated as shown in the following equations (5) and (6).
[0105] [Formula 5]
[0106]
[0107] [Mathematical formula 6]
[0108]
[0109] When the eigenvalue λ and eigenvector v of the covariance matrix C are used, the following equation (7) holds.
[0110] [Formula 7]
[0111] Cv=λv…(7)
[0112] Therefore, the eigenvalue can be obtained by solving the following characteristic equation (8), and the eigenvector can be calculated based on the eigenvalue.
[0113] [Mathematical formula 8]
[0114] |C-λI|=0…(8)
[0115] When the three eigenvalues of the solution to the equation are set to λ1, λ2, and λ3 in descending order, they refer to the variances of the first to third principal component axes. In addition, v1, v2, and v3 obtained by substituting them into equation (7) are vectors of the first to third principal component axes. Then, the first determination unit 185 determines v1, which is the vector of the first principal component axis, as the vector in the length direction of the white line 30. In addition, the first determination unit 185 uses the value v of the x component of the vector in the length direction of the white line 30 that has been determined 1x and the value v of the y component 1y , through atan2(v as a function to find the arc tangent 1y , v 1x ) operation is performed to determine the direction of the white line 30. It should be noted that v2, which is the vector of the second principal component axis, is equivalent to the vector in the width direction of the white line 30, and v3, which is the vector of the third principal component axis, is equivalent to the normal vector of the surface of the white line 30. Here, λ1, λ2, and λ3 are respectively equivalent to the variance of the point cloud in the length 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. In addition, the first determination unit 185 determines the position in the y-axis direction of the center of gravity of the point cloud extracted by the high-intensity point extraction unit 181 as the lateral position of the white line 30.
[0116] <Generation of Reliability Information>
[0117] Fig.17 and Fig.18 3 is a diagram for explaining the detection value and reliability information of the white line 30. The reliability information generating unit 187 calculates each detection reliability as follows using the result of the principal component analysis.
[0118] The reliability information generating unit 187 determines the endpoint detection reliability r as follows: Lx : The larger the number n of high reflection intensity points located in the second region 42, the greater the Lx Specifically, the reliability information generating unit 187 generates the reliability information based on r Lx =1-exp(-a×n) to calculate the endpoint detection reliability r Lx Using this formula, r Lx It can be set to a value of 0 or more and 1 or less. It should be noted that a is r relative to n. Lx The sensitivity of the sensor is adjusted according to its resolution.
[0119] In addition, the reliability information generating unit 187 determines the lateral position detection reliability r as follows: Ly : 2×3√λ2 (=±3σ range) and the width W of the white line obtained from the landmark map M The smaller the difference, theLy Specifically, the reliability information generating unit 187 generates the reliability information based on r Ly =exp(-b×|2×3√λ2-W M |) to calculate the lateral position detection reliability r Ly Using this formula, r Ly It can be set to a value of 0 or more and 1 or less. It should be noted that b is a value relative to |2×3√λ2-W. M |In terms of r Ly The sensitivity of the sensor is adjusted according to its resolution.
[0120] Then, the reliability information generating unit 187 determines the direction detection reliability r as follows: Lψ : The larger the √λ1, the greater the r Lψ It should be noted that the direction refers to the length direction of the white line 30. Specifically, the reliability information generation unit 187 is based on r Lψ =1-exp(-c×√λ1) to calculate the direction detection reliability r Lψ Using this formula, r Lψ It can be set to a value of 0 or more and 1 or less. It should be noted that c is r relative to √λ1. Lψ The sensitivity of the sensor is adjusted according to its resolution.
[0121] Thus, by using principal component analysis, no matter how the data points of the point cloud data are arranged, that is, even if the point cloud is not Figure 3 (a) instead of being arranged in rows Figure 3 (b) Even if the data is sparse, the extended Kalman filter can be applied.
[0122] <Correction of Moving Body Position>
[0123] The second estimating unit 190 obtains information indicating the end point position of the white line 30 from the second determining unit 189, obtains information indicating the direction of the white line 30 and information indicating the lateral position of the white line 30 from the first determining unit 185, and obtains the end point detection reliability r from the reliability information generating unit 187. Lx , lateral position detection reliability r Ly and direction detection reliability r Lψ . The endpoint detection reliability r Lx , lateral position detection reliability r Ly and direction detection reliability r Lψ The second estimating unit 190 then performs an extended Kalman filter process using the acquired information to correct the position of the moving object 20 .
[0124] <<Generation of Observation Noise Matrix>>
[0125] Specifically, the second estimating unit 190 generates an observation noise matrix R(t) represented by the following equation (9) using the measurement accuracy and reliability information of the white line 30 .
[0126] [Mathematical formula 9]
[0127]
[0128] Here, σ Lx (k) is the measurement accuracy of the distance to the end point position 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 measurement value of the laser radar deteriorates in proportion to the square of the distance, σ Lx (k), σ Ly (k) and σ Lψ (k) can be set to a value proportional to the square of the distance, but can be set to a predetermined value when the first region 41 is not very large. As can be seen from formula (9), since the reciprocals of the reliability information are multiplied, the larger the reliability (the closer to 1), the smaller the elements of the noise matrix become, and the smaller the reliability (the closer to 0), the larger the elements of the noise matrix become. It should be noted that when the reliability is 0, a value such as 0.0001 is used instead of the reliability.
[0129] <<Correction of moving object position>>
[0130] Fig.19 is a diagram illustrating the positional relationship between the moving body 20 and the white line 30. In this figure, x w y w is the world coordinate system, x b y b is a moving body coordinate system based on the moving body 20. b The axial direction is the traveling direction of the moving body 20. The end point position L of the white line 30 obtained by the second estimating unit 190 is used. x (k), lateral position L y (k) and direction L ψ (k), the measured value of the white line 30 is expressed by the matrix L(k) shown in the following equation (10).
[0131] [Formula 10]
[0132]
[0133] In addition, about Figure 6As described above, in S10 to S50, the second estimation unit 190 calculates the measurement prediction value of the white line 30. The measurement prediction value is represented by the matrix L-(k) shown in the following equation (11). Fig.19 As shown in the figure, x-(k) and y-(k) are the x-coordinate and y-coordinate of the moving body 20 (predicted own position) in the world coordinate system, respectively, and ψ-(k) is the moving direction of the moving body 20 (predicted own position) relative to x w The angle of the axis. In addition, M x 、M y 、M ψ It is a value obtained by expressing the information of the white line 30 obtained from the landmark map in the world coordinate system. It should be noted that when the white line information stored in the landmark map is represented by a discrete point array consisting of information on latitude, longitude, and altitude, the white line information is converted into a world coordinate system such as the UTM (Universal Transverse Mercator) coordinate system or a plane rectangular coordinate system. x is the x of the endpoint of the point sequence of white line 30 w Coordinates (endpoint position), M y is the y value of the point sequence of the white line 30 existing in the first area 41 w The mean value of the coordinates, M ψ This is the angle of the white line 30 calculated based on the coordinates of the point sequence of the white line 30 as a direction.
[0134] [Mathematical formula 11]
[0135]
[0136] Then, the second estimation unit 190 uses the following formula (12) to correct the predicted self-position, and uses the corrected predicted self-position as the estimated self-position. Here, the above-mentioned observation noise matrix R(k), the 3-row 3-column Jacobian matrix H(k) relative to the measured predicted value L-(k), and the 3-row 3-column covariance matrix P-(k) are used to represent the Kalman gain K(k) using the following formula (13). It can be seen that: since the observation noise matrix R(k) is located in the denominator when formula (13) is expressed as a fraction, the Kalman gain K(k) becomes smaller when the observation noise matrix R(k) is larger. That is, when the reliability information is low, the observation noise matrix R(k) becomes larger and the Kalman gain K(k) becomes smaller. As a result, the correction amount for the predicted self-position in formula (12) becomes smaller. On the contrary, when the reliability information is high, the observation noise matrix R(k) does not increase, and the Kalman gain K(k) does not decrease. As a result, the correction amount for the predicted own position in equation (12) becomes an appropriate amount. Thus, inappropriate correction is suppressed when a measurement value with insufficient accuracy is obtained, and appropriate correction is performed when a measurement value with high accuracy is obtained, so the accuracy of the own position estimation can be always improved.
[0137] [Mathematical formula 12]
[0138]
[0139] [Mathematical formula 13]
[0140]
[0141] Fig. 20 is an example Figure 6 Flowchart of the process flow of S70 shown in FIG. 1 . When the first acquisition unit 120 acquires point cloud data, in S701, the first region data extraction unit 110 extracts point cloud data in 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 superposition point cloud data. Specifically, the coordinate conversion unit 163 calculates the number of superpositions C using the length L of the first region 41, the speed v of the moving body 20, and the frame period T of the laser radar. Then, the coordinate conversion unit 163 reads the superposition point cloud data stored in the storage unit 165, and removes the data before the past C times. Then, the coordinate conversion unit 163 performs coordinate conversion on the remaining superposition data (however, in the case of C=2, it is the point cloud data of one time. The same applies hereinafter.) using, for example, the speed v and the yaw angular velocity ψ' of the moving body 20. The superposition point cloud data generation unit 161 superimposes the latest point cloud data on the superposition data obtained by coordinate conversion, and generates superposition point cloud data. The generated overlapping point cloud data is transmitted to the first estimating unit 180 and is stored in the storage unit 165 .
[0142] Next, in S704 , the high-intensity point extraction unit 181 extracts data points with high reflection intensity in the overlapping point cloud data using a threshold value.
[0143] In S705, when the minimum or maximum x coordinate among the x coordinates of the high reflection intensity points is located in the second region 42, the second specifying unit 189 uses the x coordinate as the estimated result of the endpoint position of the white line 30. In addition, the reliability information generating unit 187 calculates the endpoint detection reliability.
[0144] In S706, the principal component analysis unit 183 performs principal component analysis on the high reflection intensity point cloud and calculates the eigenvalue and eigenvector of each principal component axis. Then, the first determination unit 185 calculates the lateral position of the white line 30 based on the centroid of the high reflection intensity point cloud, and calculates the direction of the white line 30 based on the eigenvector of the first principal component. In addition, 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 direction detection reliability based on the eigenvalue of the first principal component.
[0145] As described above, according to the present embodiment, the same operations and effects as those of the first embodiment can be obtained.
[0146] As mentioned above, although embodiment and Example were described with reference to drawings, these are illustrations of the present invention, and various structures other than the above-mentioned can also be adopted.
[0147] This application claims the benefit of priority based on Japanese patent application No. 2022-039388, filed on March 14, 2022, the entire contents of which are incorporated herein by reference.
[0148] Description of Reference Numerals
[0149] 10 Position estimation device
[0150] 20 Mobile
[0151] 30 White Line
[0152] 40 Measurement area
[0153] 41 First Area
[0154] 42 Second Area
[0155] 110 First Area Data Extraction Unit
[0156] 120 First Acquisition
[0157] 140 Second Acquisition
[0158] 150 Intensity calibration unit
[0159] 160 Overlap
[0160] 161 Overlap point cloud data generation unit
[0161] 163 Coordinate conversion unit
[0162] 165 Storage
[0163] 180 First Presumption Division
[0164] 181 High Strength Point Extraction Department
[0165] 183 Principal Component Analysis Department
[0166] 185 First Determination Department
[0167] 187 Reliability Information Generation Unit
[0168] 189 Second Determination Section
[0169] 190 Second Presumption Section
[0170] 1000 Computer
Claims
1. A position estimation device comprising: A first acquisition unit acquires a plurality of timed point cloud data obtained by a sensor mounted on the mobile body; A second acquiring unit, acquiring movement information of the moving object; an overlapping unit for generating overlapping point cloud data by overlapping the plurality of timed point cloud data based on the movement information; and The first estimating unit estimates the position of the object using the overlapping point cloud data.
2. The position estimation device according to claim 1, wherein: comprising an offset determination unit for determining an offset using the movement information, The overlapping unit generates the overlapping point cloud data using the offset.
3. The position estimation device according to claim 1 or 2, wherein: The movement information is information on the movement of the moving object between timings of acquiring a plurality of point cloud data superimposed in the superimposed point cloud data.
4. The position estimation device according to any one of claims 1 to 3, wherein: The sensor measures the distance to the object by emitting light and receiving reflected light reflected by the object.
5. The position estimation device according to claim 4, wherein: The intensity of the reflected light received by the sensor is associated with each data point included in the point cloud data acquired by the first acquisition unit, The first estimating unit estimates the position of the object using the intensities of a plurality of data points in the overlapping point cloud data.
6. The position estimation device according to claim 5, wherein: The method further comprises an intensity correction unit for correcting the intensity of each data point included in the point cloud data or the overlapped point cloud data using the distance between the data points. The first estimating unit estimates the position of the object using the corrected intensity.
7. The position estimation device according to any one of claims 1 to 6, wherein: The object is a line on a road surface.
8. The position estimation device according to claim 7, wherein: The first estimating unit estimates an endpoint position of a line on a road surface.
9. The position estimation device according to claim 8, wherein: The first estimating 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.
10. The position estimation device according to any one of claims 1 to 9, wherein: The superimposing unit generates the superimposed point cloud data by superimposing point cloud data of a number of times corresponding to the speed of the moving object.
11. The position estimation device according to any one of claims 1 to 10, wherein: The device further includes a second estimating unit that estimates the position of the moving body using the estimation result of the position of the object estimated by the first estimating unit.
12. A method for estimating a position, which is a method for estimating a position executed by a computer, the method comprising: A first acquisition step is to acquire a plurality of timed point cloud data obtained by a sensor mounted on the mobile body; A second obtaining step of obtaining movement information of the moving object; an overlapping step of generating overlapping point cloud data by overlapping the plurality of timed point cloud data based on the movement information; as well as The first estimating step estimates the position of the object using the overlapping point cloud data.
13. A program for causing a computer to execute the position estimating method according to claim 12.
