Position estimation device, position estimation method, and position estimation program

Abstract

A position estimation device (11) estimates the position of a moving body (10) having a distance measurement sensor (12) for detecting the surroundings, and is provided with: a feature extraction unit (101) for extracting a plurality of feature points (F) from distance measurement data output from the distance measurement sensor (12); a posture information calculation unit (102) that calculates posture information indicating the posture of the distance measurement sensor (12) on the basis of the plurality of feature points (F); a feature amount weight determination unit (103) that determines, on the basis of the distribution of the plurality of feature points (F), a plurality of weights (Wfeatures) to be given to each of the plurality of feature points; and a position estimation unit (107) that estimates the position of the moving body (10) on the basis of a plurality of feature points (F), a plurality of corresponding points corresponding to each of the plurality of feature points in the stored map information, the attitude information, and the plurality of weights (Wfeature).

Description

Technical Field

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

[0002] A positioning system has been proposed, which includes: a reference data DB that stores reference data; a feature point extraction unit that extracts feature points from a captured image of a scenery seen from a vehicle; a captured image processing unit that generates feature point data for each captured image based on the feature points and outputs the same as matching data; and a scenery matching unit that performs matching between the reference data extracted from the reference data DB and the matching data, and determines the position of the own vehicle based on the captured position associated with the reference data for which the matching is successful (for example, refer to Patent Document 1). In this positioning system, when constructing the reference data DB, weighting of the feature points is performed according to the distribution of the feature points and the like.

[0003] Prior Art Documents

[0004] Patent Documents

[0005] Patent Document 1: Japanese Unexamined Patent Application Publication No. 2011-215972 (for example, refer to paragraphs 0016 - 0019) Summary of the Invention

[0006] Problems to be Solved by the Invention

[0007] However, in the above-described conventional positioning system, when estimating the position based on sensor data of a distance measuring sensor (such as LiDAR, camera, etc.), the position is estimated without considering the most recent attitude of the distance measuring sensor. Therefore, in the conventional positioning system, high-precision position information cannot be obtained.

[0008] The present disclosure has been made to solve the above-described problems, and an object thereof is to provide a position estimation device, a position estimation method, and a position estimation program capable of improving the accuracy of position estimation.

[0009] Means for Solving the Problems

[0010] The position estimation device of the present disclosure estimates the position of a moving body having a distance measurement sensor for detecting the surrounding environment, characterized in that the position estimation device includes: a feature extraction unit that extracts a plurality of feature points from the distance measurement data output from the distance measurement sensor; an attitude information calculation unit that calculates attitude information representing the attitude of the distance measurement sensor based on the plurality of feature points; a feature quantity weight determination unit that determines a plurality of weights respectively assigned to the plurality of feature points according to the distribution of the plurality of feature points; and a position estimation unit that estimates the position of the moving body based on the plurality of feature points of the distance measurement data, a plurality of corresponding points respectively corresponding to the plurality of feature points in the stored map information, the attitude information, and the plurality of weights.

[0011] The position estimation method of the present disclosure is executed by a position estimation device that estimates the position of a moving body having a distance measurement sensor for detecting the surrounding environment, characterized in that the position estimation method includes the following steps: extracting a plurality of feature points from the distance measurement data output from the distance measurement sensor; calculating attitude information representing the attitude of the distance measurement sensor based on the plurality of feature points; determining a plurality of weights respectively assigned to the plurality of feature points according to the distribution of the plurality of feature points; and estimating the position of the moving body based on the plurality of feature points of the distance measurement data, a plurality of corresponding points respectively corresponding to the plurality of feature points in the stored map information, the attitude information, and the plurality of weights.

[0012] Advantages of the Invention

[0013] According to the present disclosure, the accuracy of position estimation can be improved. Description of the Drawings

[0014] Figure 1 FIG. is an example showing the case where the position estimation device of Embodiment 1 is mounted on a vehicle as a moving body.

[0015] Figure 2 FIG. is a block diagram schematically showing the structure of the position estimation device of Embodiment 1.

[0016] Figure 3 FIG. is an example showing the hardware structure of the position estimation device of Embodiment 1.

[0017] Figure 4 FIG. shows by Figure 2 FIG. is an example of a plurality of feature points obtained by the feature extraction unit.

[0018] Figure 5 (A) and (B) of FIG. are schematic diagrams showing the estimated axial directions of the attitude in each feature.

[0019] Figure 6(A) and (B) are schematic diagrams showing a histogram of normal vectors of feature points within each region and frequency information of each constraint possessed by the feature points within each region.

[0020] Figure 7 It is a flowchart showing the operation of the position estimation device according to Embodiment 1.

[0021] Figure 8 It is a diagram showing an example in which the position estimation device according to Embodiment 2 is mounted on a three-dimensional measurement device as a moving body. Detailed Embodiments

[0022] Hereinafter, the position estimation device, position estimation method, and position estimation program according to the embodiments will be described with reference to the drawings. The following embodiments are merely examples, and the embodiments can be appropriately combined and each embodiment can be appropriately modified.

[0023] 《1》 Embodiment 1

[0024] 《1-1》 Structure

[0025] (Outline of the position estimation device 11)

[0026] Figure 1 It is a diagram showing an example in which the position estimation device 11 according to Embodiment 1 is mounted on a vehicle as a moving body 10. As Figure 1 shown, the moving body 10 has a position estimation system for estimating its own position. The position estimation system has: a distance measurement sensor 12 that measures the surrounding environment; a state sensor 13 that acquires the steering angle, speed, acceleration, etc. of the moving body; a GNSS receiver 15 as a position information receiver that acquires the absolute position of the moving body 10 from GNSS (Global Navigation Satellite System); and a position estimation device 11 that performs position estimation and other processes based on the sensor data acquired from the distance measurement sensor 12, the state sensor 13, and the GNSS receiver 15. The distance measurement sensor 12 includes one or more of LiDAR (Light Detection and Ranging) 121, a camera 122, and a millimeter-wave radar 123. The state sensor 13 may also include an inertial sensor that acquires the inertial information of the moving body. The moving body 10 is, for example, an autonomous driving vehicle, a PMV (Personal Mobility Vehicle), an AMR (Autonomous Mobile Robot), an unmanned aerial vehicle, i.e., a drone, etc.

[0027] Figure 2It is a block diagram schematically showing the structure of the position estimation device 11 of Embodiment 1. The position estimation device 11 is a device that implements the position estimation method of Embodiment 1, for example, a computer that executes the position estimation program of Embodiment 1. As Figure 2 shown, the position estimation device 11 has a ranging data acquisition unit 100 that acquires ranging data as sensor data from the ranging sensor 12, a feature extraction unit 101 that extracts feature points (and their feature amounts) from the ranging data, an attitude information calculation unit 102 that calculates attitude (i.e., the attitude of the ranging sensor 12, the attitude of the moving body 10.) information based on the distribution of the extracted feature points, a feature amount weight determination unit 103, a position estimation unit 107, a local map creation unit 108, a map creation unit 109, and a map database (map DB) 110. In addition, the position estimation device 11 has an attitude integration unit 104 that acquires state information from the state sensor 13, a coordinate axis conversion unit 105 that acquires position information (absolute position information) from the GNSS receiver 15, and a pre-attitude synthesis unit 106. The map database 110 may also be provided in an external storage device.

[0028] The position estimation device 11 estimates the position of the moving body 10 having at least the ranging sensor 12 that detects the surrounding environment. The position estimation device 11 has: a feature extraction unit 101 that extracts a plurality of feature points from the ranging data output from the ranging sensor 12; an attitude information calculation unit 102 that calculates attitude information representing the attitude of the ranging sensor 12 (i.e., the attitude of the moving body 10) based on the plurality of feature points F; a feature amount weight determination unit 103 that determines a plurality of weights W respectively assigned to the plurality of feature points F according to the distribution of the plurality of feature points F feature ; and a position estimation unit 107 that estimates the position of the moving body 10 based on the plurality of feature points F of the ranging data, the plurality of corresponding points corresponding to the plurality of feature points F in the stored map information, the calculated attitude information, and the plurality of weights W feature The attitude information, the plurality of feature points F, and the plurality of weights W feature will be described later.

[0029] In addition, the position estimation unit 107 estimates the position of the moving body 10 having the ranging sensor 12 based on the state data output from the state sensor 13 provided on the moving body 10 and the position data output from the GNSS receiver 15 provided on the moving body 10. The ranging sensor 12 detects position and attitude information representing the position and attitude of the ranging sensor 12. The position estimation unit 107 uses the position and attitude information obtained by synthesizing the state data output from the state sensor 13 provided on the moving body 10 and the position data output from the GNSS receiver 15 provided on the moving body 10 as an initial value for the matching between the plurality of feature points F and the plurality of corresponding points.

[0030] Figure 3 This is a diagram showing an example of the hardware configuration of the position estimation device 11 according to Embodiment 1. The position estimation device 11 includes, for example, a processor 31 such as a CPU (Central Processing Unit), a memory 32, a non-volatile storage device 33, and an input / output (I / O) interface 34. Each part constituting the position estimation device 11 is formed of a processing circuit, for example. The processing circuit may be dedicated hardware, or may include a CPU that executes a program (such as a position estimation program) stored in the memory 32. The processor 31 implements Figure 2 each functional block shown.

[0031] The memory 32 is a semiconductor memory such as a RAM (Random Access Memory), for example, and the storage device 33 is an HDD (hard disk drive), an SSD (solid state drive), or the like. In addition, the position estimation device 11 may be a device in which a structural part formed of a processing circuit and a structural part formed of a processor coexist. In addition, part or all of the position estimation device 11 may be a server computer on a network. Further, the position estimation program is provided by downloading via a network, or by a storage medium such as a USB memory that stores information. Further, Figure 3 the hardware configuration is an example and can be changed.

[0032] (State sensor 13)

[0033] The state sensor 13 includes an IMU (Inertial Measurement Unit) mounted on the moving body 10, a sensor that measures the motion state of the moving body 10, and the like. The sensor data, that is, the state data obtained by the state sensor 13 is temporarily stored in the memory 32 via the input / output interface 34. The state data stored in the memory 32 is retrieved by the attitude integration unit 104 and used for calculating the attitude of the moving body 10. Specific examples of the sensor that measures the motion state are a speedometer such as a speed meter, a sensor that detects input information for the moving body 10 such as a rotary encoder that detects the steering angle. However, other sensors can also be included. The driving state data stored in the memory 32 is retrieved by the attitude integration unit 104 and used for calculating the attitude of the moving body 10.

[0034] (Distance measurement sensor 12)

[0035] The ranging sensor 12, such as a LiDAR (Light Detection And Ranging) mounted on the mobile body 10, a camera as an imaging device, a millimeter-wave radar, a sonar (Sound Navigation and Ranging), etc., detects the surrounding environment of the mobile body 10 and obtains information on the surrounding environment. The ranging data obtained by the ranging sensor 12 is temporarily stored in the memory 32 via the input / output interface 34. The ranging data stored in the memory 32 is retrieved by the feature extraction unit 101 and used for the calculation of the attitude information of the mobile body 10.

[0036] (Ranging data acquisition unit 100)

[0037] The ranging data acquisition unit 100 receives the sensor data, i.e., the ranging data, transmitted from the ranging sensor 12 and converts the ranging data into data in a specified data format. The communication of the ranging data can use general communication standards such as serial communication and Ethernet, but a unique communication standard can also be used. After the ranging data received by the ranging data acquisition unit 100 is temporarily stored in the buffer, it is converted into a specified data format and output to the feature extraction unit 101.

[0038] (Feature extraction unit 101)

[0039] The feature extraction unit 101 extracts various feature quantities from the ranging data input from the ranging data acquisition unit 100. When the ranging sensor 12 is a LiDAR, three-dimensional features (edges, corners, planes) obtained from point cloud data including a plurality of feature points are used, but other feature quantities can also be used. The feature extraction unit 101 outputs the extracted various feature quantities to the attitude information calculation unit 102.

[0040] Figure 4 It shows an example of a plurality of feature points F (a plurality of black dots within the shooting range 72) obtained by Figure 2 the feature extraction unit 101. This example shows the following environment: When observed from the mobile body 10 traveling towards the intersection near an intersection where there are things such as a traffic light 71, there are deviations in the distribution of the feature points F in each area. Thus, when the surrounding environment of the mobile body 10 has uneven multiple feature points or in a monotonous feature environment (e.g., the ground, wall surface of a corridor with a monotonous shape, the ground in an open area, etc.), the position estimation device 11 is likely to fall into a situation of local solution during position estimation. Therefore, in Embodiment 1, there is a position estimation unit 107 that estimates the position of the mobile body 10 based on the multiple feature points F of the ranging data, the multiple corresponding points corresponding to the multiple feature points F in the stored map information, the calculated attitude information, and the multiple weights W feature to estimate the position of the mobile body 10.

[0041] (Attitude information calculation unit 102)

[0042] The attitude information calculation unit 102 calculates the weights (i.e., weight coefficients) of each feature point according to the constraint information, and the constraint information represents the direction in which the attitude of the feature quantity of each feature point F extracted by the feature extraction unit 101 is constrained. As a specific example, when using a plane as a feature point, the feature quantity is represented by the normal vector of each point. In most cases, the evaluation function at the time of matching when using a plane as a feature point is represented by the length of the plane and the perpendicular line dropped to the plane.

[0043] Figure 5 (A) and (B) are schematic diagrams showing the estimated axis directions of the attitudes at each feature point. As Figure 5 shown in (A), at the time of attitude estimation, when using a plane as a feature point, the attitude information estimated by each feature point becomes the amount of movement (distance D1) with respect to the normal directions (the directions of the normal lines 41a and 42a) of the planes (plane 41 and plane 42), the rotation around the normal line 41a (arrow 46), and the amount of rotation of the rotation with the two-axis directions (the directions of the axes 43 and 44) orthogonal to the normal direction as the rotation axes).

[0044] Similarly, as Figure 5 shown in (B), when using the feature points of the edge as feature points at the time of attitude estimation, regarding the evaluation function at the time of matching, it is considered by the perpendicular line (normal line 53) dropped from the edge (for example, edge 51) to another edge (for example, edge 52). In this case, the amount of movement in the plane orthogonal to the edge (for example, edge 51) and the rotation with the edge direction (the direction of the axis 51a) and the two orthogonal axis directions (the directions of the axes 51b and 51c) as the rotation axes (the rotations in the directions of the arrows 56a, 56b, and 56c) can be obtained by calculation.

[0045] Thus, when the attitude information calculation unit 102 estimates the attitude information of the moving body 10 in the current frame based on the data of the previous frame, it can estimate in which axis direction there is a constraint (i.e., which axis direction is the constraint direction) according to the feature points shown in Figure 5 (A) and (B). The attitude information calculation unit 102 observes the distribution of these feature points with constraints, and thus performs weighting.

[0046] Figure 6(A) and (B) are schematic diagrams showing an example of a histogram of normal vectors for feature points in each region and frequency information for each constraint possessed by each feature point. For processing the distribution of feature points, methods such as SHOT (Signatures of Histograms of Orientations) can also be used, for example. SHOT defines a sphere 61 centered on a center point 60 as a representative point, and uses, as a feature, a histogram of the inner product between the set of nearby points contained in each of the 8 regions obtained by dividing the sphere 61 (8-divided using the xy plane, yz plane, and xz plane) and the normal vector of the center point 60. Imitating this method, as Figure 6 shown in (A) and (B), 8 regions are generated by dividing the sphere 61 into 2 parts around the xyz axes in a certain local coordinate system, and a histogram of the normal vectors of the feature points in each region is generated, whereby frequency information for each constraint possessed by each feature point can be obtained. Here, in Figure 6 (B), the vertical axis is the histogram and the horizontal axis is the feature quantity (the orientation constrained by the feature point).

[0047] In addition, as a method other than the method using a histogram such as SHOT, for example, a three-dimensional normal distribution can be generated for each feature point based on the normal vector of the plane feature, and the frequency of each feature point can be used. The obtained histogram is output to the feature quantity weight determination unit 103.

[0048] (Feature quantity weight determination unit 103)

[0049] The feature quantity weight determination unit 103 weights each feature point according to the histogram calculated by the pose information calculation unit 102. For example, as the method for calculating the weight W feature based on the histogram, weighting can be performed according to the frequency of the histogram as shown in the following formula (1).

[0050]

Mathematical formula 1

[0051]

[0052] In formula (1), W feature represents the weight, and f represents the frequency in the histogram. The frequency f is represented by a value in the range of 0 to 1, that is, a ratio. The feature quantity weight determination unit 103 outputs the weight W feature of each feature point calculated by formula (1) to the position estimation unit 107.

[0053] (Pose accumulation unit 104)

[0054] The attitude integration unit 104 calculates the attitude information (such as speed, angular velocity, roll / pitch / yaw) in the latest data frame using the sensor data received from the state sensor 13, i.e., the state data. The state sensor 13 can use inertial sensors such as an acceleration sensor and a gyro sensor. In the case of using inertial sensors, generally, sensor data such as acceleration and angular velocity are output as state data. These values do not directly represent the movement amount of the device, but by integrating the state data from the start of measurement and successively updating the attitude information, the latest attitude information can be obtained. The attitude integration unit 104 outputs the obtained attitude information to the pre-attitude synthesis unit 106.

[0055] (Coordinate axis conversion unit 105)

[0056] The coordinate axis conversion unit 105 converts the latitude and longitude information measured by the GNSS receiver 15 into the coordinate system used by the moving body 10 for its own position estimation. Generally, these coordinate systems have reference points as benchmarks set around the world, and in most cases, a plane rectangular coordinate system with these reference points as the origin is used. For example, in Japan, 19 reference points are prepared, and the coordinate values in the xy coordinate system based on each point are calculated by a predetermined calculation method (shown in Non-Patent Document 1). The coordinate axis conversion unit 105 outputs the calculated position information to the pre-attitude synthesis unit 106.

[0057] Non-Patent Document 1: Kazushige Kawase, "A Simplified Calculation Method for Coordinate Conversion between Latitude-Longitude Coordinates and Plane Rectangular Coordinates in Gauss-Kruger Projection", Bulletin of the Geospatial Information Authority of Japan, 2011, No. 121, pp. 109-124, URL: https: / / www.gsi.go.jp / common / 000061216.pdf

[0058] (Pre-attitude synthesis unit 106)

[0059] The pre-attitude synthesis unit 106 calculates the integrated position and attitude information based on the estimated attitude information based on the state data obtained by the attitude integration unit 104 and the position information obtained by the coordinate axis conversion unit 105. As a method for synthesizing each sensor information, generally, a Kalman filter or the like is used, but methods such as weighted average based on the variance value of each estimated attitude information or determination of a representative value based on a voting system can also be adopted.

[0060] (Position estimation unit 107)

[0061] The position estimation unit 107 retrieves the feature quantities extracted from the ranging data by the feature extraction unit 101 and the local map information generated by the local map creation unit 108 described later, searches for corresponding points between the map and the newly measured ranging data, and estimates the pose that minimizes (or maximizes) the cost function representing the difference between these two point clouds.

[0062] As representative pose estimation methods, there are the method of using a 3D LiDAR as a ranging sensor, namely LOAM (Lidar Odometry and Mapping), and the method of using a 2D LiDAR as a ranging sensor, namely Cartographer. In these methods, generally, pairs are made using the points measured previously or the similar points between the immediately preceding measured ranging data and the latest ranging data, and the rotation / translation with the minimum distance between the similar points in each pair is obtained, thereby enabling the calculation of the movement amount. The relationship between the similar points (feature points) can be expressed as in the following equation (2).

[0063]

Mathematical formula 2

[0064] y = Rx + t (2)

[0065] In equation (2), y represents the position of the feature point in the immediately preceding ranging data, R represents the rotation matrix, x represents the position of the feature point in the latest ranging data, and t represents the translation vector.

[0066] This is equivalent to finding the change amount of the pose between the two ranging data by finding the translation amount / rotation amount that is the same at the same location between the two ranging data. However, in reality, due to the influence of noise, etc., the number of equations becomes larger than the number of parameters to be found, so it is difficult to uniquely find the solution. Therefore, an evaluation function represented by the following equation (3) is used to approximately find the solution that minimizes the evaluation value obtained from this evaluation function.

[0067]

Mathematical formula 3

[0068]

[0069] As a method for solving these problems, generally, based on a shape that can be calculated using SVD (Singular Value Decomposition) etc., an optimal solution can be obtained by an optimization method such as the Newton method or the Levenberg-Marquardt method. However, in the evaluation function of equation (3), when the feature points are concentrated on a specific plane or a specific edge, even if it is assumed that the rotation amount / translation amount other than the axis direction estimated by these feature points does not converge, the evaluation value obtained from equation (3) may be below the threshold.

[0070] Therefore, as in the following equation (4), by using the weight w calculated in advance for each feature pointi The expression obtained by adding it as a coefficient of the squared error to Equation (3) can suppress the incorrect convergence of the calculation due to a part of the feature points (that is, the evaluation value incorrectly becomes below the threshold).

[0071]

Mathematical formula 4

[0072]

[0073] (Local map creation unit 108)

[0074] In the case where the position has been estimated by the position estimation unit 107 and there is previously generated map information in the map DB 110, the local map creation unit 108 generates local map data around the own position of the moving body 10 based on this map information.

[0075] (Map creation unit 109)

[0076] The map creation unit 109 performs coordinate conversion on the ranging data acquired by the ranging data acquisition unit 100 based on the attitude information obtained when the calculation result (evaluation value) in the position estimation unit 107 is lower than the threshold or exceeds the predetermined number of trial times. Then, the map creation unit 109 adds the coordinate-converted ranging data to the map DB 110 to expand the map information.

[0077] The position estimation device 11 estimates the own position of the moving body 10 using the data obtained from the ranging sensor 12, the state sensor 13, and the GNSS receiver 15 through the input / output interface 34. The position estimation device 11 performs correspondence between data frames using the feature amounts extracted from the obtained ranging data, thereby estimating the change amount of the attitude (= (movement amount) + (change amount of roll / pitch / yaw)), and moreover, comprehensively processes the attitude information obtained from the state sensor 13 and the GNSS receiver 15, thereby estimating the highly accurate own position.

[0078] 《1-2》Operation

[0079] Figure 7 It is a flowchart showing the operation of the position estimation device 11 of the first embodiment.

[0080] (Step S10)

[0081] In step S10, the attitude integration unit 104 calculates the mileage based on the sensor data (such as information on acceleration, angular velocity, and steering angle) obtained from the state sensor 13. In parallel therewith, the coordinate axis conversion unit 105 performs coordinate conversion on the position information obtained by the GNSS receiver 15 so that the coordinates after conversion are consistent with the coordinates of the position information calculated by the attitude integration unit 104.

[0082] (Step S11)

[0083] In step S11, the pre - attitude synthesizing unit 106 synthesizes the information obtained by the state sensor 13 and calculated by the attitude integrating unit 104 and the position information obtained by the GNSS receiver 15 and subjected to coordinate conversion, both of which are calculated in step S10. As a synthesizing method, a method using a Kalman filter or the like can be used, and the estimation result is output as the pre - predicted own position to the position estimating unit 107.

[0084] (Step S12)

[0085] In step S12, the ranging data acquisition unit 100 receives ranging data as sensor data from the ranging sensor 12, and converts the received ranging data into ranging data in a predetermined format.

[0086] (Step S13)

[0087] In step S13, the feature extraction unit 101 calculates feature quantities used in the matching of data between frames based on the ranging data converted into a predetermined format in step S12. As an example, the feature extraction unit 101 calculates information quantities such as the change amount of brightness and the three - dimensional shape (plane, edge) of the surrounding environment based on the ranging data.

[0088] (Step S14)

[0089] In step S14, the attitude information calculation unit 102 calculates whether each of the feature quantities detected by the feature extraction unit 101 has a constraint on which attitude. Furthermore, the feature quantity weight determination unit 103 sets the weights of the respective feature quantities used in the subsequent attitude estimation to values within the range of 0 to 1 based on the information calculated by the attitude information calculation unit 102.

[0090] (Step S15)

[0091] In step S15, when estimating the attitude using the feature quantities calculated in step S13, the position estimating unit 107 applies the attitude information estimated by the pre - attitude synthesizing unit 106 as the initial value of the matching.

[0092] (Step S16)

[0093] In step S16, when position estimation has been performed in the previous steps and there is map information generated previously in the map DB 110, the local map creation unit 108 generates local map data around the own position of the moving body 10 based on this map information.

[0094] (Step S17)

[0095] In step S17, the position estimation unit 107 searches for corresponding points within the feature amounts extracted in step S13 and the local map data generated in step S16, and makes the correspondence.

[0096] (Steps S18, S19)

[0097] In step S18, the position estimation unit 107 calculates, by the iterative method, the pose with the minimum evaluation value obtained from the evaluation function between the feature amounts made to correspond in step S17. When the calculation result is not below a preset threshold (step S19: No), the process of step S17 is performed again, the correspondence of the feature amounts is made based on the finally calculated estimated pose, and the process of step S18 is executed. When the calculation result is below the preset threshold (step S19: Yes), the process proceeds to step S20.

[0098] (Step S20)

[0099] In step S20, the map creation unit 109 performs coordinate conversion on the distance measurement data acquired by the distance measurement data acquisition unit 100 based on the pose obtained when the calculation result in step S18 is below the preset threshold or exceeds a predetermined number of trial times.

[0100] (Step S21)

[0101] In step S21, the map creation unit 109 adds the distance measurement data subjected to coordinate conversion in step S20 to the map DB 110, and expands the map information.

[0102] (Step S22)

[0103] In step S22, the position estimation unit 107 updates the own position of the moving body 10 from the previous step based on the pose obtained in step S18.

[0104] 《1-3》Effect

[0105] As described above, the position estimation device 11 of Embodiment 1 assigns weights to the feature amounts, thereby weighting the evaluation value in position estimation. Thus, even in an environment where the distribution of the feature amounts in each area is biased or in a monotonous feature environment, it is not likely to fall into a situation of getting into a local solution in position estimation due to a part of the feature amounts.

[0106] In addition, the position estimation device 11 of Embodiment 1 uses the pose information estimated from the feature amounts for weighting, thereby enabling appropriate data correspondence between frames and further suppressing the situation where the pose estimation process gets into a local solution.

[0107] 《2》Embodiment 2

[0108] In Embodiment 1, the position estimation device is mounted on a vehicle as a moving body. However, in Embodiment 2, the position estimation device is mounted on a three-dimensional measurement device as a moving body. Figure 8 It is a diagram showing an example in the case where the position estimation device 21 of Embodiment 2 is mounted on the three-dimensional measurement device 20. In Figure 8 this, the three-dimensional measurement device 20 includes a distance measurement sensor 22, a state sensor 23 including an inertial sensor, a position estimation device 21, and a holding unit 24. The three-dimensional measurement device 20 may also include a GNSS receiver. The position estimation device 21 has the same structure as the position estimation device 11 of Embodiment 1.

[0109] For example, the three-dimensional measurement device 20 is a measurement device for measuring the structural shape of land, roads, tunnels, buildings, etc., or a detection device for detecting shape changes of cliffs or mountains. In addition, in three-dimensional measurement, measurement is performed while moving. Therefore, the three-dimensional measurement device 20 is mounted on a work vehicle or operated by an operator holding it. The distance measurement sensor 22 can use the distance measurement sensor 12 described in Embodiment 1. In addition, the state sensor 23 can similarly use the state sensor 13 described in Embodiment 1. The position estimation device 21 is used to estimate the attitude of the three-dimensional measurement device 20, and the parameters can be adjusted according to the structure described in Embodiment 1 to suit the measurement characteristics of the three-dimensional measurement device. The holding unit 24 is a structure for holding the three-dimensional measurement device 20. For example, it is a frame for fixing the three-dimensional measurement device 20 to a work vehicle or a handle for holding the three-dimensional measurement device 20 by hand.

[0110] According to the structure of Embodiment 2, in an environment where there are deviations in the distribution of feature amounts in each area such as flat land, cliffs, and tunnels, or in a monotonous feature environment, it is not easy to fall into a situation of local solutions in position estimation due to a part of the feature amounts. As a result, the measurement accuracy of the three-dimensional measurement device can be improved.

[0111] Reference Numeral Explanation

[0112] 10: Mobile body; 11: Position estimation device; 12: Distance measurement sensor; 121: LiDAR; 122: Camera; 123: Millimeter wave radar; 13: State sensor; 15: GNSS receiver (position information receiver); 20: Three-dimensional measurement device (mobile body); 21: Position estimation device; 22: Distance measurement sensor; 23: State sensor; 24: Holding unit; 31: Processor; 32: Memory; 33: Storage device; 34: Input / output interface; 100: Distance measurement data acquisition unit; 101: Feature extraction unit; 102: Attitude information calculation unit; 103: Feature quantity weight determination unit; 104: Attitude accumulation unit; 105: Coordinate axis conversion unit; 106: Pre-attitude synthesis unit; 107: Position estimation unit; 108: Local map creation unit; 109: Map generation unit; 110: Map DB.

Claims

1. A position estimation device that estimates the position of a moving body having a distance measuring sensor for detecting the surrounding environment, characterized in that The position estimation device has: A feature extraction unit that extracts a plurality of feature points from the distance measurement data output from the distance measurement sensor; An attitude information calculation unit that calculates attitude information representing the attitude of the distance measurement sensor based on the plurality of feature points; A feature quantity weight determination unit that determines a plurality of weights respectively assigned to the plurality of feature points based on the distribution of the plurality of feature points; And A position estimation unit that estimates the position of the moving body based on the plurality of feature points of the distance measurement data, the plurality of corresponding points respectively corresponding to the plurality of feature points in the stored map information, the attitude information, and the plurality of weights.

2. The position estimation device according to claim 1, wherein The position estimation unit uses the following position and attitude information as the initial value of the matching between the plurality of feature points and the plurality of corresponding points, and this position and attitude information is obtained by synthesizing the state data output from the state sensor provided on the moving body and the position data output from the position information receiver provided on the moving body.

3. The position estimation device according to claim 1 or 2, wherein The attitude information calculation unit calculates the plurality of weights according to constraint information, and this constraint information represents the direction in which the attitude of the feature quantity of each feature point extracted by the feature extraction unit is constrained.

4. The position estimation device according to any one of claims 1 to 3, wherein The moving body is a vehicle.

5. The position estimation device according to any one of claims 1 to 3, wherein The moving body is a three-dimensional measurement device.

6. A position estimation method, which is executed by a position estimation device that estimates the position of a moving body having a distance measurement sensor for detecting the surrounding environment, characterized in that The position estimation method has the following steps: Extract a plurality of feature points from the distance measurement data output from the distance measurement sensor; Calculate attitude information representing the attitude of the distance measurement sensor based on the plurality of feature points; Determine a plurality of weights respectively assigned to the plurality of feature points according to the distribution of the plurality of feature points; And Estimate the position of the moving body based on the plurality of feature points of the distance measurement data, the plurality of corresponding points respectively corresponding to the plurality of feature points in the stored map information, the attitude information, and the plurality of weights.

7. A position estimation program, characterized in that, The position estimation program causes a computer that estimates the position of a moving body, which has a distance measurement sensor for detecting the surrounding environment, to execute the following steps: Extract a plurality of feature points from the distance measurement data output from the distance measurement sensor; Calculate attitude information representing the attitude of the distance measurement sensor based on the plurality of feature points; Determine a plurality of weights respectively assigned to the plurality of feature points according to the distribution of the plurality of feature points; And Estimate the position of the moving body based on the plurality of feature points of the distance measurement data, the plurality of corresponding points respectively corresponding to the plurality of feature points in the stored map information, the attitude information, and the plurality of weights.

Images