Own-position estimation device and own-position estimation method

Abstract

To reduce a processing load when updating a map based on an estimated position.SOLUTION: An own-position estimation device 20 comprises: a calculation section 222 which calculates a coordinate set, at a predetermined time, corresponding to an outer boundary of an object based on outer boundary information acquired at a time prior to the predetermined time and obtains average and variance of coordinates, in the coordinate set, included in each of a plurality of divided regions defined by partitioning an area surrounding a vehicle; and an identification section 223 which identifies an amount of movement of the vehicle over a period from a time unit time before the predetermined time to the predetermined time so as to minimize an evaluation result obtained by inputting the average and variance calculated with the calculation section 222 and the coordination set acquired at the predetermined time into an evaluation formula including the amount of movement of the vehicle. The calculation section 222 identifies a weighting factor which decreases as the unit time elapses based on the time acquired with an acquisition section for each piece of the outer boundary information and calculates weighted average and variance using the weighting factor corresponding to the outer boundary information as the average and variance.SELECTED DRAWING: Figure 1

Description

[Technical field]

[0001] The present invention relates to a self-location estimation device and a self-location estimation method. [Background technology]

[0002] The map generating device of Patent Document 1 estimates the vehicle position and generates a map by performing NDT (Normal Distributions Transform) scan matching using multiple voxel data in which point cloud data indicating the positions of structures is represented by a normal distribution and point cloud data output by a sensor. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] JP 2020-34771 A Summary of the Invention [Problem to be solved by the invention]

[0004] In SLAM (Simultaneous Localization And Mapping), due to sensor measurement errors, errors in the direction in which the sensor measures obstacles, and insufficient amount of point cloud data, a different position corresponding to the same position may be estimated and the map updated each time the vehicle travels through the same position. Therefore, it is conceivable to carry out a process (so-called loop closure) to correct the estimated position so that it is the same at a specific position included in the circuit route when the vehicle travels around the circuit route, but since the processing load for detecting the circuit route is large, there is a demand for suppressing map updates caused by estimating different positions with a processing load smaller than that of loop closure.

[0005] The present invention has been made in consideration of these points, and has an object to reduce the processing load of updating a map based on an estimated position. [Means for solving the problem]

[0006] A self-location estimation device according to a first aspect of the present invention includes an acquisition unit that acquires, at each unit time elapsed, contour line information indicating the contour line of an object around a vehicle; a calculation unit that, at a predetermined time, calculates a coordinate group corresponding to the contour line of the object in a predetermined coordinate system based on the contour line information acquired by the acquisition unit at a time prior to the predetermined time, and calculates, for each of a plurality of divided areas obtained by dividing an area around the vehicle, an average and a variance of coordinates included in the coordinate group in the divided area; and an evaluation function including a movement amount of the vehicle, such that an evaluation result obtained by inputting the average and the variance calculated by the calculation unit and the coordinate group acquired by the acquisition unit at the predetermined time is minimized. The system includes an identification unit that identifies the amount of movement of the vehicle from the time unit time ago to the specified time, and a generation unit that generates map information indicating the vehicle's driving path and the position of the object by estimating coordinates corresponding to a first position of the vehicle at the specified time and a group of coordinates corresponding to the outline of the object based on the amount of movement of the vehicle identified by the identification unit and the average and the variance calculated by the calculation unit, wherein the calculation unit identifies a weighting coefficient that becomes smaller with each passing time as the unit time elapses, for each piece of outline information, based on the time acquired by the acquisition unit, and calculates a weighted average and weighted variance using a weighting coefficient corresponding to the outline information as the average and the variance.

[0007] When the total amount of movement of the vehicle in a predetermined time period is less than a threshold value, the calculation unit may specify a maximum value that the weighting coefficient can take as the weighting coefficient at a time when the predetermined time period has elapsed.

[0008] The identification unit may store in a memory unit the amount of movement of the vehicle identified for each time the unit time elapses and the time at which the amount of movement of the vehicle was identified, and the calculation unit may calculate a total amount of movement of the vehicle in the specified time by referring to the amount of movement of the vehicle and the time stored in the memory unit.

[0009] The acquisition unit may acquire a second position of the vehicle, which is different from the first position of the vehicle, from an external device, and the calculation unit may identify, when a distance between the first position of the vehicle and the second position of the vehicle is equal to or greater than a threshold, the maximum value that the weighting coefficient can take as the weighting coefficient at the time when the unit time has elapsed.

[0010] The calculation unit may specify, as the weighting coefficient, an inverse of a sum obtained by adding a predetermined value to the number of times the unit time has elapsed.

[0011] A self-location estimation method according to a second aspect of the present invention includes an acquisition step of acquiring contour line information indicating the contour line of an object around a vehicle at each unit time elapsed, a calculation step of calculating, at a predetermined time, a coordinate group corresponding to the contour line of the object in a predetermined coordinate system based on the contour line information acquired in the acquisition step at a time prior to the predetermined time, and calculating, for each of a plurality of divided areas obtained by dividing an area around the vehicle, an average and a variance of coordinates included in the coordinate group in the divided area, and calculating an average and a variance of coordinates included in the divided area in the coordinate group from the predetermined time so as to minimize an evaluation result obtained by inputting the average and the variance calculated in the calculation step and the coordinate group acquired in the acquisition step at the predetermined time into an evaluation function including an amount of movement of the vehicle. The method includes a determination process for determining the amount of movement of the vehicle from a time unit time ago to the specified time, and a generation process for generating map information indicating the vehicle's travel path and the object's position by estimating coordinates corresponding to a first position of the vehicle at the specified time and a group of coordinates corresponding to the object's outline based on the amount of movement of the vehicle determined in the determination process and the average and the variance calculated in the calculation process, wherein in the calculation process, a weighting coefficient that decreases with each passing time as the unit time elapses is determined for each piece of outline information based on the time acquired in the acquisition process, and a weighted average and weighted variance using a weighting coefficient corresponding to the outline information are calculated as the average and the variance. Effect of the Invention

[0012] According to the present invention, it is possible to reduce the processing load of updating a map based on an estimated position. [Brief description of the drawings]

[0013] [Figure 1] FIG. 2 is a diagram for explaining an overview of a vehicle S according to the present embodiment. [Diagram 2] FIG. 13 is a diagram showing an example of a group of coordinates corresponding to the outline of an object. [Diagram 3] FIG. 13 is a diagram illustrating an operation of calculating a weighted average and a weighted variance. [Figure 4] 11 is a sequence showing an operation of updating map information. [Diagram 5] 1 is a sequence showing an operation for identifying a weighting factor. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0014] <Overview of Vehicle S> FIG. 1 is a diagram for explaining an overview of a vehicle S according to this embodiment. The vehicle S shown in FIG. 1 includes a measurement device 10, an attitude angle detection device 11, a receiving device 12, a display device 13, and a self-position estimation device 20. The vehicle S has a function of estimating the position of the vehicle S based on the distance between the vehicle S and objects around the vehicle S detected by the measurement device 10, and generating map information indicating the travel path of the vehicle S and the positions of the objects. The surrounding objects are, for example, people, animals, plants, walls, and steps that make it difficult for the vehicle S to travel. The surrounding objects may include other vehicles different from the vehicle S.

[0015] The measuring device 10 is a three-dimensional laser measuring device, such as a LiDAR (Light Detection And Ranging) device, for measuring the distance between the vehicle S and an object around the vehicle S. The measuring device 10 generates contour line information indicating the position of the contour line of the object around the vehicle S based on point cloud data generated by measuring the distance between the vehicle S and the object around the vehicle S, and outputs the contour line information to the self-position estimation device 20.

[0016] The attitude angle detection device 11 is a device for detecting an attitude angle (e.g., a roll angle and a pitch angle with respect to a vertical axis) indicating the orientation of the vehicle S. The attitude angle detection device 11 detects an attitude angle of the vehicle S based on, for example, a translational velocity detected by a wheel encoder (not shown) equipped in the vehicle S and an angular velocity detected by a gyroscope (not shown) equipped in the vehicle S, and outputs the detected attitude angle to the self-position estimation device 20. Note that, in this embodiment, for the sake of simplicity, the attitude angle of the vehicle S at time t is referred to as an "attitude angle θ t " should be written.

[0017] The receiving device 12 is a device for receiving radio waves from an external positioning system such as a Global Navigation Satellite System (GNSS). The receiving device 12 outputs latitude and longitude calculated based on information contained in the received radio waves to the self-position estimation device 20 as position information of the vehicle S. The display device 13 is a display for displaying map information generated by the vehicle S.

[0018] The self-position estimation device 20 is a housing including electronic components or a printed circuit board on which electronic components are mounted. The self-position estimation device 20 executes a process of estimating the position of the vehicle S based on the outline information acquired from the measurement device 10 and the attitude angle acquired from the attitude angle detection device 11 at each unit time (e.g., 0.1 second) and updating the map information. The self-position estimation device 20 is, for example, a device that executes a SLAM (Simultaneous Localization And Mapping) process.

[0019] In SLAM, the position of the vehicle S is estimated at regular time intervals and the map information is updated, so even if the vehicle S has traveled to a position prior to the current time, the position of the vehicle S is re-estimated and the map information is updated. In addition, in SLAM, a different position may be estimated each time the vehicle S travels the same position due to the orientation of the vehicle S when measured by the measurement device 10 and measurement errors of the measurement device 10. Due to these operations, in the self-position estimation device 20 that executes SLAM processing, for example, the position of an object indicated by the map information and the traveling position of the vehicle S are updated to different positions as the vehicle travels around the circuit route.

[0020] In response to this, the self-position estimation device 20 executes a process (loop closure) for correcting the estimated position so that it is the same at a predetermined position included in the circular route, thereby preventing the object position and the traveling position of the vehicle S from being updated to different positions as the vehicle moves around. However, since the processing load for detecting the circular route is large in the loop closure, it is required that the vehicle S that has traveled the same position estimates the same position without using the loop closure.

[0021] Therefore, in the scan matching process included in the SLAM process, the self-position estimation device 20 calculates a weighted average and weighted variance using a weighting coefficient that decreases with each passing unit time when calculating the average and variance of the contour line information generated by the measurement device 10. The scan matching process is a process of superimposing position data at a predetermined time and position data at a time before the predetermined time to estimate the amount of movement of the vehicle S. In this embodiment, an NDT (Normal Distribution Transform) scan matching process is performed.

[0022] By operating the self-position estimation device 20 in this manner, the weighting coefficient of the position data when the vehicle travels through a given position for the first time can be increased, and the weighting coefficient of the position data when the vehicle travels through the given position for the second or subsequent time can be decreased. As a result, the position when the vehicle travels through the given position for the first time can have the greatest influence on the result of estimating the position, so that it is possible to suppress erroneous estimation of the position as a different position even when the vehicle travels through the same position for the second or subsequent time. Furthermore, since the position of the vehicle S can be estimated without using loop closing, the processing load can be reduced. The configuration and operation of the self-position estimation device 20 will be described in detail below.

[0023] <Configuration of self-location estimation device 20> The self-position estimation device 20 includes a storage unit 21 and a control unit 22. The control unit 22 includes an acquisition unit 221, a calculation unit 222, an identification unit 223, a generation unit 224, and a display control unit 225.

[0024] The storage unit 21 has a storage medium such as a read only memory (ROM), a random access memory (RAM), a hard disk drive (HDD), or a solid state drive (SSD). The storage unit 21 stores a program executed by the control unit 22 and various information for estimating the position where the vehicle S is traveling and generating and updating map information. The storage unit 21 stores the generated and updated map information.

[0025] The control unit 22 is a processor such as a CPU (Central Processing Unit) or an ECU (Electronic Control Unit). The control unit 22 executes a program stored in the storage unit 21 to function as an acquisition unit 221, a calculation unit 222, an identification unit 223, a generation unit 224, and a display control unit 225. The control unit 22 may be configured with one processor, or may be configured with multiple processors or a combination of one or more processors and an electronic circuit. The configuration of each unit realized by the control unit 22 will be described below.

[0026] The acquisition unit 221 acquires contour line information indicating the contour line of objects around the vehicle S at each unit time elapse. The unit time is, for example, 0.1 seconds. The acquisition unit 221 acquires contour line information including point cloud data indicating objects around the vehicle S detected by the measurement device 10, for example. The acquisition unit 221 acquires the attitude angle of the vehicle S detected by the attitude angle detection device 11 at each unit time elapse. The acquisition unit 221 acquires from the receiving device 12 a second position of the vehicle S that is different from a first position of the vehicle S estimated based on the contour line information acquired from the measurement device 10 and the attitude angle acquired from the attitude angle detection device 11.

[0027] The calculation unit 222 calculates the average and variance of point cloud data included in the contour information in the NDT scan matching process. At a predetermined time, the calculation unit 222 calculates a coordinate group corresponding to the contour line of an object in a predetermined coordinate system based on the contour line information acquired by the acquisition unit 221 at a time prior to the predetermined time. Then, for each of a plurality of divided areas obtained by dividing the area around the vehicle S, the calculation unit 222 calculates the average and variance of coordinates included in the divided area out of the coordinate group. The predetermined coordinate system is, for example, a coordinate system (hereinafter referred to as a "map coordinate system") whose center coordinate is the position where the vehicle S starts traveling.

[0028] FIG. 2 is a diagram showing an example of a coordinate group corresponding to the outline of an object. The area R is, for example, an area indicating the area in front of the vehicle S in the traveling direction of the vehicle S among the areas around the vehicle S in the map coordinate system. The area R is divided into a plurality of divided areas (areas B1 to B12 shown in FIG. 2). The coordinates P01 to P06 shown in FIG. 2 are a coordinate group corresponding to the outline of the object calculated by the calculation unit 222 at a predetermined time (for example, time t) based on the outline information acquired by the acquisition unit 221 at a time before the predetermined time (for example, time t-1). The coordinates P11 to P15 shown in FIG. 2 are a coordinate group corresponding to the outline of the object calculated by the calculation unit 222 at time t-1 based on the outline information acquired by the acquisition unit 221 at a time before the time t-1 (for example, time t-2). As an example, the time from time t-2 to time t-1 and the time from time t-1 to time t are unit times (0.1 seconds).

[0029] 2, at time t-1, the calculation unit 222 calculates a coordinate group (coordinates P11 to P15) by converting point cloud data included in the contour line information acquired by the acquisition unit 221 at time t-2 into coordinate data in the map coordinate system based on the position of the vehicle S estimated at time t-2. Similarly, at time t, the calculation unit 222 calculates a coordinate group (coordinates P01 to P06) by converting point cloud data included in the contour line information acquired by the acquisition unit 221 at time t-1 into coordinate data in the map coordinate system based on the position of the vehicle S estimated at time t-1. The calculation unit 222 calculates the average and variance of the coordinates included in the divided area, from the coordinates P11 to P15 and the coordinates P01 to P06, for each of the multiple divided areas (areas B1 to B12).

[0030] The calculation unit 222 specifies a weighting coefficient that decreases with each unit time that elapses, for each piece of contour line information, based on the time acquired by the acquisition unit 221, and calculates a weighted average and weighted variance using the weighting coefficient corresponding to the contour line information as the average and variance. By operating in this manner, the calculation unit 222 can increase the weighting coefficient of the position data when a specific position is traveled for the first time, and decrease the weighting coefficient of the position data when the specific position is traveled for the second or subsequent times. As a result, it becomes difficult to estimate a different position even when the same position is traveled for the second or subsequent times. Hereinafter, a method for calculating the weighted average and weighted variance using the specified weighting coefficient and "weighted NDT scan matching" which is NDT scan matching using the weighted average and weighted variance will be described in detail. In the following, two-dimensional coordinates may be used in some cases for simplicity of explanation.

[0031] First, NDT scan matching will be described. Point cloud data A at time t, point cloud data B at time t-1, position transformation matrix T where point cloud data A and point cloud data B overlap, and coordinates (x, y, 1) of each point. T , Attitude angle θ at time t-1 t-1 , the movement amount of vehicle S from time t-1 to time t (t x,t y ,φ) T When this is defined, the position transformation matrix T can be given as shown in equation (1).

number

[0032] In NDT scan matching, point a corresponding to point cloud data A is the nearest point. i and point b associated with point cloud data B i For point cloud data B, point b i The model is modeled with a normal distribution using other points within the division region to which b belongs (one of the regions B1 to B12 shown in Figure 2). i If there are N points in the divided region m to which m belongs, the average of the N points can be calculated using formula (2) and the variance of the N points can be calculated using formula (3). In addition, by using formulas (2) and (3), it can be expressed as formula (4).

number

[0033] By the way, b i a associated with i Ta, which is the result of applying the correct position transformation matrix T to i exists in the same divided region m, and b i Since there is a high probability that it follows the same distribution as

number

[0034] From equation (5), the probability distribution (mean and variance) Ta i The position transformation matrix T when the probability of observing is maximized is the optimal position transformation matrix, which can be calculated as shown in equation (6).

number

[0035] p(Ta i In order to maximize the output C(a) of the evaluation function in equation (7), it is necessary to minimize the exponent part excluding the constant (-1 / 2) shown in equation (5). Therefore, when the number of divided areas in which point cloud data exists is M, the evaluation function for identifying the amount of movement of the vehicle S can be expressed as in equation (7). The identification unit described later calculates the output C(a) of the evaluation function in equation (7). i ,b i ) is minimized.

number

[0036] Next, weighted NDT scan matching will be explained. In general weighted NDT scan matching, data x i The weighting coefficient corresponding to i If we define it as above, the weighted average can be expressed as equation (8) and the weighted variance can be expressed as equation (9).

number

[0037] In this embodiment, the weighting coefficient ω k is defined by using the cycle k as in formula (10). The cycle k has an initial value (i.e., k=0) at the time when the self-position estimation device 20 starts the operation of estimating the position of the vehicle S, and is incremented by 1 every time a unit time elapses.

number

[0038] FIG. 3 is a diagram showing the operation of calculating the weighted average and weighted variance. The horizontal axis in FIG. 3 indicates time. The vertical axis in FIG. 3 indicates cycle k, weighting coefficient, point cloud data included in the contour line information acquired by the acquisition unit 221, weighted average, and weighted variance. In FIG. 3, cycle k is incremented by 1 at the time when time T0, time T1, time T2, and time T3 have passed (i.e., the time immediately after time T0, time T1, time T2, and time T3). Note that the weighting coefficients shown in FIG. 3 indicate values ​​up to the first decimal place, and digits after the first decimal place are omitted.

[0039] For example, as shown in Fig. 3, at time T0, the calculation unit 222 determines that the weighting coefficient is 1 because the cycle k is 0. Then, the calculation unit 222 determines that the weighting coefficient corresponding to the point cloud data included in the contour line information L0 acquired by the acquisition unit 221 at time T0 is 1. Next, at time T1, the calculation unit 222 determines that the weighting coefficient corresponding to the contour line information L1 acquired by the acquisition unit 221 at time T1 is 0.5 because the cycle k is 1. Furthermore, the calculation unit 222 calculates a weighted average A1 and a weighted variance D1 using the contour line information L0 acquired by the acquisition unit 221 at time T0 and the weighting coefficient 1. As is clear from Fig. 3, the value of the weighting coefficient gradually decreases as time passes.

[0040] Here, the calculation formulas for the weighted average and weighted variance at time T0, time T1, and time T2 shown in Fig. 3 will be described. It is assumed that the acquisition unit 221 acquires contour information L0 in which n0 points are observed within a divided region m at time T0 (cycle k=0) shown in Fig. 3. At time T0, there is no point group immediately before, so the calculation unit 222 does not calculate the weighted average and weighted variance.

[0041] 3, it is assumed that the acquisition unit 221 has acquired contour information L1 in which n1 points are observed within a divided region m. At time T1, the calculation unit 222 calculates a weighted average A1 and a weighted variance D1 using the contour information L0 acquired by the acquisition unit 221 at time T0. The weighted average A1 can be expressed by equation (11), and the weighted variance D1 can be expressed by equation (12). Note that, x k,i are coordinates obtained by converting the point cloud data included in the contour line information acquired by the acquisition unit 221 at the time when the cycle is k into the map coordinate system, and x k,i =(x k,i ,y k,i ,1) T Let us assume that.

number

[0042] 3, it is assumed that the acquisition unit 221 acquires contour line information L2 in which n2 points are observed within a divided region m. At time T2, the calculation unit 222 calculates a weighted average A2 and a weighted variance D2 using contour line information L0 acquired by the acquisition unit 221 at time T0 and contour line information L1 acquired by the acquisition unit 221 at time T1. The weighted average A2 can be expressed by equation (13), and the weighted variance D2 can be expressed by equation (14).

number

[0043] In addition, by deriving the recurrence formulas of equations (15) and (16) using equations (11) and (12) and applying them to equations (13) and (14), the weighted average A2 at time T2 (cycle k=2) can be expressed as equation (17), and the weighted variance D2 can be expressed as equation (18).

number

[0044] Furthermore, similar to equations (15) and (16), by deriving the recurrence formula at time T3 (cycle k=3) as equations (19) and (20), the weighted average of the divided region m at any cycle k can be expressed as equation (21), and the weighted variance can be expressed as equation (22).

number

[0045] Then, by substituting the formulas (21) and (22) into the formula (7), weighted NDT scan matching can be performed using an evaluation function that uses the weighted average and the weighted variance. Note that the above formulas (7), (21), and (22) are expressed as follows: i This also holds true in the case of three-dimensional coordinates. The above describes the method of calculating the weighted average and weighted variance using weighting coefficients, and weighted NDT scan matching.

[0046] Incidentally, the vehicle S may stop due to congestion in the lane or when a traffic light indicates a stop (so-called red light). In this case, even if the position of the vehicle S does not change, the cycle k is added, so that an appropriate weighting factor may not be identified after the vehicle S resumes traveling. Therefore, when the total amount of movement of the vehicle S in a predetermined time is less than a threshold, the calculation unit 222 may identify the maximum value that the weighting factor can take (i.e., 1) as the weighting factor at the time when the predetermined time has elapsed. The predetermined time is a time equivalent to the time when the vehicle S stops due to a traffic light or congestion, and is determined by experiment or simulation. The threshold is a lower limit value of the amount of movement at which an appropriate weighting factor can be identified, and is determined by experiment or simulation.

[0047] The calculation unit 222 calculates the total amount of movement of the vehicle S in a predetermined time, for example, by referring to the amount of movement of the vehicle S and the time stored in the storage unit 21 at each time when a unit time has elapsed. If the calculated total amount of movement of the vehicle S is less than a threshold value, the calculation unit 222 assigns 0 to the cycle k (i.e., initializes the cycle k) and sets the weighting coefficient to 1. By operating the calculation unit 222 in this manner, the cycle k and the weighting coefficient can be initialized when the traveling state of the vehicle S changes, and therefore the position of the vehicle S can be estimated with high accuracy.

[0048] In SLAM, the greater the total amount of movement of the vehicle S after the start of position estimation, the more likely an estimation error is to occur. Therefore, the calculation unit 222 may initialize the cycle k and the weighting coefficient based on the distance between the position information (latitude and longitude) included in the radio wave received by the receiving device 12 and the position of the vehicle S estimated by the self-position estimation device 20. For example, when the distance between the first position of the vehicle S estimated by the generation unit 224 and the second position of the vehicle S received by the receiving device 12 is equal to or greater than a threshold, the calculation unit 222 specifies the maximum value that the weighting coefficient can take as the weighting coefficient at the time when a unit time has elapsed. The threshold is, for example, a value greater than a measurement error (e.g., several meters) by an external positioning system such as GNSS. By operating the calculation unit 222 in this manner, the weighting coefficient can be initialized even when the total amount of movement of the vehicle S becomes large, so that the position of the vehicle S can be estimated with high accuracy.

[0049] The identification unit 223 identifies the amount of movement of the vehicle S at each time when a unit time has elapsed. The identification unit 223 identifies the amount of movement of the vehicle S from a time unit time before the predetermined time to a predetermined time so that an evaluation result obtained by inputting the average and variance calculated by the calculation unit 222 and the coordinate group acquired by the acquisition unit 221 at a predetermined time into an evaluation function including the amount of movement of the vehicle S is minimized. The identification unit 223, for example, identifies the amount of movement of the vehicle S from a time unit time before the predetermined time to a predetermined time, so that an evaluation result obtained by inputting the average and variance calculated by the calculation unit 222 and the coordinate group acquired by the acquisition unit 221 at a predetermined time into an evaluation function including the amount of movement of the vehicle S is minimized. i and b i In order to determine the amount of movement of vehicle S such that i ,b i ) is minimized.

[0050] For example, the identification unit 223 inputs the weighted average calculated by the calculation unit 222 using the formula (21), the weighted variance calculated using the formula (22), and a plurality of coordinate data (coordinate group) obtained by converting the point cloud data included in the contour line information acquired by the acquisition unit 221 into the map coordinate system into formula (7). Then, the identification unit 223 inputs the weighted average calculated by the calculation unit 222 using the formula (21), the weighted variance calculated using the formula (22), and a plurality of coordinate data (coordinate group) obtained by converting the point cloud data included in the contour line information acquired by the acquisition unit 221 into the map coordinate system into formula (7). i and b iBy the specification unit 223 operating in this manner, the amount of movement of the vehicle S can be specified with high accuracy at each unit time elapsed.

[0051] Furthermore, the identification unit 223 may store the movement amount of the vehicle S identified for each unit time elapsed and the time at which the movement amount of the vehicle S was identified in the storage unit 21. By the identification unit 223 storing in the storage unit 21 in this manner, the calculation unit 222 can calculate the total value of the movement amount of the vehicle S in a predetermined time, and therefore the weight coefficient can be initialized at an appropriate timing.

[0052] The generation unit 224 generates map information indicating the travel route of the vehicle S and the positions of objects around the vehicle S. For example, the generation unit 224 estimates coordinates corresponding to the position of the vehicle S at a predetermined time and a coordinate group corresponding to the outline of the object based on the movement amount of the vehicle S identified by the identification unit 223 and the weighted average and weighted variance calculated by the calculation unit 222. Then, the generation unit 224 generates map information by updating the map information stored in the storage unit 21 based on the estimated coordinates of the vehicle S and the coordinate group corresponding to the outline of the object around the vehicle S. The display control unit 225 causes the display device 13 to display the map information generated by the generation unit 224.

[0053] <Processing sequence in the self-location estimation device 20> Figures 4 and 5 are diagrams showing an example of a processing sequence in the self-location estimation device 20. The processing sequence shown in Figure 4 is a sequence showing an operation of updating map information, and the processing sequence shown in Figure 5 is a sequence showing an operation of identifying a weighting coefficient. The operation shown in Figure 5 is the operation of S12 shown in Figure 4. The self-location estimation device 20 repeats the processing sequences shown in Figures 4 and 5 at regular time intervals (at each time a unit time has elapsed).

[0054] First, the operation of updating the map information will be described. In Fig. 4, the acquisition unit 221 acquires the attitude angle of the vehicle S detected by the attitude angle detection device 11 (S10). The acquisition unit 221 acquires contour line information indicating the contour line of an object around the vehicle S detected by the measurement device 10 (S11).

[0055] The calculation unit 222 specifies the weighting coefficient by substituting the number of cycles (one cycle is, for example, 0.1 seconds) that have elapsed from the time when the self-position estimation device 20 started estimating the position of the vehicle S to the current time into the formula (10) (S12). Details of the operation of S12 will be described later. The calculation unit 222 calculates the weighted average and the weighted variance using the formulas (21) and (22) (S13). The identification unit 223 identifies the movement amount of the vehicle S that minimizes the output result of the evaluation function of the formula (7) (S14). The generation unit 224 estimates the coordinates corresponding to the position of the vehicle S and a group of coordinates indicating the outline of the object around the vehicle S based on the movement amount of the vehicle S identified by the identification unit 223 and the weighted average and weighted variance calculated by the calculation unit 222, and updates the map information stored in the storage unit 21 (S15).

[0056] Next, the operation of the calculation unit 222 to specify the weighting coefficient will be described. In Fig. 5, if the unit time has not elapsed (NO in S20), the calculation unit 222 repeats the process of S20 until the time when the unit time has elapsed. If the unit time has elapsed (YES in S20), the calculation unit 222 specifies whether or not the number of times the unit time has elapsed from the time when the self-position estimation device 20 started estimating the position of the vehicle S to the current time is greater than 1 (S21).

[0057] If the number of times is 1 or less (NO in S21), the calculation unit 222 substitutes 0 into the cycle k in formula (10) (S22). If the number of times is 2 or more (YES in S21), the calculation unit 222 calculates the total movement amount of the vehicle S in a predetermined time by referring to the movement amount and time of the vehicle S stored in the storage unit 21 (S23). If the calculated total movement amount of the vehicle S is less than the first threshold (NO in S24), the calculation unit 222 substitutes 0 into the cycle k in formula (10) (S22). If the total movement amount of the vehicle S is equal to or greater than the first threshold (YES in S24), the calculation unit 222 acquires the position information of the vehicle S detected by the receiving device 12 at a time unit time before the current time and output to the acquisition unit 221 (S25). Then, the calculation unit 222 calculates the distance between the position indicated by the acquired position information and the position of the vehicle S estimated by the generation unit 224 at the time unit time before the current time (S26).

[0058] If the calculated distance is equal to or greater than the second threshold (NO in S27), the calculation unit 222 assigns 0 to the cycle k in formula (10) (S22). If the calculated distance is less than the second threshold (YES in S27), the calculation unit 222 adds 1 to the cycle k (S28). Then, the calculation unit 222 calculates the weighting coefficient shown in formula (10) (S29). If the self-location estimation device 20 has not received an operation to end the process (NO in S30), it repeats the processes from S20 to S29. If the self-location estimation device 20 has received an operation to end the process (YES in S30), it ends the process.

[0059] <Effects of the self-location estimation device 20> As described above, the self-position estimation device 20 has a calculation unit 222 that calculates, at a predetermined time, a coordinate group corresponding to the outline of an object in a map coordinate system based on outline information acquired by the acquisition unit 221 at a time prior to the predetermined time, and calculates the average and variance of coordinates included in the coordinate group for each of a plurality of divided areas obtained by dividing the area around the vehicle S, and an identification unit 223 that identifies the amount of movement of the vehicle S from a time unit time before the predetermined time to the predetermined time so that an evaluation function including the amount of movement of the vehicle S is input with the average and variance calculated by the calculation unit 222 and the coordinate group acquired by the acquisition unit 221 at the predetermined time as its input, so as to minimize the evaluation result.

[0060] Then, the calculation unit 222 specifies a weighting coefficient that decreases with each unit time that elapses, for each piece of contour line information, based on the time acquired by the acquisition unit 221, and calculates a weighted average and a weighted variance using a weighting coefficient corresponding to the contour line information as an average and a variance. By configuring the self-position estimation device 20 in this way, it is possible to increase the weighting coefficient of the position data when the vehicle travels for the first time at a specific position, and to decrease the weighting coefficient of the position data when the vehicle travels for the second or subsequent times. As a result, the position when the vehicle travels for the first time has the greatest influence on the result of estimating the position, and therefore it is possible to suppress erroneous estimation of the same position as a different position even when the vehicle travels for the second or subsequent times. Furthermore, since the position of the vehicle S can be estimated without using loop closing, it is possible to reduce the processing load.

[0061] Although the present invention has been described above using the embodiments, the technical scope of the present invention is not limited to the scope described in the above embodiments, and various modifications and changes are possible within the scope of the gist of the present invention. For example, all or part of the device can be configured by distributing or integrating functionally or physically in any unit. In addition, new embodiments resulting from any combination of multiple embodiments are also included in the embodiments of the present invention. The effect of the new embodiment resulting from the combination combines the effect of the original embodiment. [Explanation of symbols]

[0062] 10. Measuring Equipment 11 Attitude angle detection device 12 Receiving device 13 Display device 20 Self-position estimation device 21 Memory section 22 Control section 221 Acquisition Department 222 Calculation section 223 Specific part 224 Generation part 225 Display control unit

Claims

1. an acquisition unit that acquires contour information indicating contours of objects around the vehicle at each unit time; a calculation unit that calculates, at a predetermined time, a coordinate group corresponding to the outline of the object in a predetermined coordinate system based on the outline information acquired by the acquisition unit at a time prior to the predetermined time, and calculates, for each of a plurality of divided areas obtained by dividing an area around the vehicle, an average and a variance of coordinates included in the divided area out of the coordinate group; an identification unit that identifies a movement amount of the vehicle from a time that is the unit time before the predetermined time to the predetermined time so that an evaluation result obtained by inputting the average and the variance calculated by the calculation unit and the coordinate group acquired by the acquisition unit at the predetermined time into an evaluation function including the movement amount of the vehicle is minimized; a generation unit that generates map information indicating a travel path of the vehicle and a position of the object by estimating coordinates corresponding to a first position of the vehicle at the predetermined time and a coordinate group corresponding to an outline of the object based on the movement amount of the vehicle identified by the identification unit and the average and the variance calculated by the calculation unit, the calculation unit specifies, for each of the contour line information, a weighting coefficient that decreases with each passing time of the unit time based on the time acquired by the acquisition unit, and calculates, as the average and the variance, a weighted average and a weighted variance using a weighting coefficient corresponding to the contour line information. Self-location estimation device.

2. When a total movement amount of the vehicle in a predetermined time is less than a threshold, the calculation unit specifies a maximum value that the weighting coefficient can take as the weighting coefficient at a time when the predetermined time has elapsed. The self-location estimation device according to claim 1 .

3. the determination unit stores in a storage unit the amount of movement of the vehicle determined for each unit time elapsed and the time at which the amount of movement of the vehicle was determined; the calculation unit calculates a total movement amount of the vehicle during the predetermined time by referring to the movement amount of the vehicle and the time stored in the storage unit; The self-location estimation device according to claim 2 .

4. The acquisition unit acquires a second position of the vehicle, which is different from the first position of the vehicle, from an external device; the calculation unit, when a distance between the first position of the vehicle and the second position of the vehicle is equal to or greater than a threshold, specifies a maximum value that the weighting coefficient can take as the weighting coefficient at a time when the unit time has elapsed. The self-location estimation device according to claim 1 .

5. The calculation unit specifies, as the weighting coefficient, an inverse of a sum obtained by adding a predetermined value to the number of times the unit time has elapsed. The self-location estimation device according to claim 1 .

6. an acquisition step of acquiring contour line information indicating contour lines of objects around the vehicle at each unit time; a calculation step of calculating, at a predetermined time, a coordinate group corresponding to the outline of the object in a predetermined coordinate system based on the outline information acquired in the acquisition step at a time prior to the predetermined time, and calculating, for each of a plurality of divided areas obtained by dividing an area around the vehicle, an average and a variance of coordinates included in the divided area among the coordinate group; a specifying step of specifying the amount of movement of the vehicle from a time before the unit time from the predetermined time to the predetermined time so that an evaluation result obtained by inputting the average and the variance calculated in the calculating step and the coordinate group acquired in the acquiring step at the predetermined time into an evaluation function including the amount of movement of the vehicle is minimized; a generating step of estimating coordinates corresponding to a first position of the vehicle at the predetermined time and a group of coordinates corresponding to an outline of the object, based on the amount of movement of the vehicle identified in the identifying step and the average and the variance calculated in the calculating step, thereby generating map information indicating a travel path of the vehicle and a position of the object, In the calculation step, a weighting coefficient that decreases with each passing time of the unit time is specified for each of the contour line information based on the time acquired in the acquisition step, and a weighted average and a weighted variance using a weighting coefficient corresponding to the contour line information are calculated as the average and the variance. Self-localization method.

Images