JP2021047099A - Vehicle own-location estimation device and own-location estimation method - Google Patents

Abstract

To provide an own-location estimation device that can estimate a highly accurate own-location to be required upon executing self-driving even when it snows.SOLUTION: The present disclosure relates to an own-location estimation device that is connected to a map database storing map information and an ambient environment observation sensor acquiring three-dimensional point group data on an ambient environment, and estimates an own-location of a vehicle. The own-location estimation device has: a rough location estimation unit that estimates an own-location in a general way; and a three-dimensional collation unit that estimates a detailed own-location, in which the three-dimensional collation unit includes: a collation-purpose map extraction unit that generates three-dimensional map data by extracting outline shape information observable when it snows from the map information; a sensing result extraction unit that obtains the three-dimensional point group data from the ambient environment observation sensor; and a location correction unit that three-dimensionally collates the three-dimensional map data with the three-dimensional point group data, thereby correcting the own-location estimated by the rough location estimation unit.SELECTED DRAWING: Figure 4

Description

The present invention is a vehicle self-position estimation device that collates the surrounding environment structure in which the vehicle travels with the structural information recorded in the map information and estimates a detailed position indicating where the vehicle is on the map. And self-position estimation method.

Conventionally, as a technology for correcting the approximate current position of a vehicle detected by a positioning device, the distance from the vehicle to the intersection is obtained by using a camera mounted on the vehicle, and the position of the vehicle in the map information is specified. Techniques have been proposed to correct the approximate current position of the vehicle.

For example, in the vehicle position determination method of Patent Document 1, the processing load of the feature recognition unit is changed by changing the area to be recognized based on the driving conditions such as fine weather, snowfall, high-speed driving, and left and right driving lanes. There is a description about the self-position estimation technology that reduces.

JP-A-2017-9554

In Patent Document 1, when the shape of the traveling environment changes due to snowfall, or when a road where road markings cannot be seen continues on a snow-packed road, an image taken by a camera is taken as shown in paragraph 0028 and FIG. 2 of the same document. By recognizing the road markings and guide signs included in the upper half area, the estimation accuracy of the current position of the vehicle can be corrected to an error of about several meters.

However, the technique of Patent Document 1 has a problem that the self-position estimation error cannot be suppressed to several cm or less required for automatic driving and cannot be used for automatic driving.

Therefore, in the present invention, the situation where the environmental structure existing in the traveling environment is shielded by the snowfall, the situation where the shape of the environmental structure changes continues, and the situation where the road markings cannot be seen on the snow-packed road continue. Even in this case, it is an object of the present invention to provide a self-position estimation device capable of highly accurate self-position estimation required at the time of executing automatic operation.

In order to solve the above problems, the self-position estimation device of the present invention is connected to a map database that stores map information and an ambient environment observation sensor that acquires three-dimensional point group data of the surrounding environment, and is connected to the vehicle's self. It is a self-position estimation device that estimates a position, and has a rough position estimation unit that estimates a rough self-position and a three-dimensional collation unit that estimates a detailed self-position. A matching map extraction unit that generates three-dimensional map data that extracts observable external shape information during snowfall from map information, a sensing result extraction unit that obtains the three-dimensional point group data from the surrounding environment observation sensor, and the three-dimensional It is assumed that the map data and the three-dimensional point group data are three-dimensionally collated to have a position correction unit that corrects the self-position estimated by the rough position estimation unit.

According to the self-position estimation device of the present invention, the environmental structure existing in the traveling environment is shielded by the snow cover, the shape of the environmental structure continues to change, and the road marking is not visible on the snow-packed road. It is possible to estimate the self-position with high accuracy required at the time of executing the automatic operation even when the above is continued.

Block diagram of the self-position estimation device according to the first embodiment Flowchart of processing performed by the rough position estimation unit of the first embodiment Explanatory diagram explaining an example of calculating the evaluation value for a road link Functional block diagram of the 3D collation unit during snowfall Functional block diagram of the collation map extractor Functional block diagram of the sensing result extraction unit Flow chart implemented by the coarse position estimation unit of the second embodiment The block diagram of the self-position estimation apparatus of Example 3. Examples of snow-free and snow-covered environments

Hereinafter, examples of the self-position estimation device according to the present invention will be described with reference to the drawings.

FIG. 1 is a block diagram illustrating a configuration of a self-position estimation device 10 according to a first embodiment of the present invention, a sensor connected to the configuration, and the like. As shown here, the self-position estimation device 10 is connected to a vehicle behavior observation sensor 1, an absolute position acquisition sensor 2, a map database 3, an ambient environment observation sensor 4, a snow cover determination unit 5, and a vehicle control unit 6. Further, the self-position estimation device 10 includes a rough position estimation unit 11, a map matching unit 12, and a three-dimensional collation unit 13, and the three-dimensional collation unit 13 includes a general-purpose collation unit 14, a snow cover collation unit 15, and a snow cover collation unit 15. have. Specifically, the self-position estimation device 10 is a computer provided with hardware such as an arithmetic unit such as a CPU, a main storage device such as a semiconductor memory, an auxiliary storage device, and a communication device, and is a main storage device. Each function of the coarse position estimation unit 11 and the like is realized by the arithmetic unit executing the program loaded in the above. In the following, the details of each part will be described while appropriately omitting such well-known techniques in the computer field. explain.

The vehicle behavior observation sensor 1 is a sensor including one or more for observing the relative motion of the own vehicle, such as an acceleration sensor, an angular velocity sensor, an angular acceleration sensor, a wheel pulse sensor, and a vehicle speed sensor. By accumulating the sensing results obtained from these vehicle behavior observation sensors 1 in time series, it is possible to continuously estimate the relative movement and attitude change of the own vehicle. Since errors and noise mixed in the sensing result are also accumulated, the accuracy of the estimation result by the own vehicle behavior observation sensor 1 generally deteriorates gradually.

The absolute position acquisition sensor 2 includes, for example, a GNSS (Global Navigation Satellite System) receiver, a radio wave receiver such as a sudride (pseudo satellite) or a beacon (radio beacon), a bar code installed on the environment side, or the like. It is a sensor consisting of one or more for recognizing the absolute position of the own vehicle, such as a recognition device for a visible marker or a magnetic marker. From the sensing results obtained from these absolute position acquisition sensors 2, it is possible to estimate the position and posture of the own vehicle at that time. The estimation of the vehicle position and posture by the absolute position acquisition sensor 2 is discrete from the viewpoint of time or space. For example, the positioning interval of GNSS is often about 1 to 5 seconds, and marker recognition. The device is limited to a location near the point where the marker is installed.

The rough position estimation unit 11 estimates the approximate self-position (rough position) that changes from moment to moment by using the sensing results of the vehicle behavior observation sensor 1 and the absolute position acquisition sensor 2. Further, as the initial value of the self-position estimation here, the self-position estimation result calculated by the three-dimensional collation unit 13 at the previous time can be used. Since the self-position (coarse position) estimated by the rough position estimation unit 11 may have an error of about several meters, it can be used for car navigation or the like to guide the route to the destination, but it is automatic. The accuracy is not available for driving.

The map matching unit 12 improves the accuracy of the self-position estimation result by collating the map information from the map database 3 with the self-position estimation result from the rough position estimation unit 11. For example, assuming a situation in which the own vehicle passes through an intersection, the own vehicle should be traveling along the shape of the intersection stored in the map database 3, so the self-position estimation result by the rough position estimation unit 11 If does not match the shape of the intersection, it is treated as if there is an error in the self-position estimation result, and the self-position is corrected according to the map.

The ambient environment observation sensor 4 is, for example, a stereo camera, a monocular camera, a LiDAR (Laser Imaging Detection and Ranging), a millimeter wave, an ultrasonic sonar, a TOF camera (Time of Flight), or the like around the vehicle. It is a sensor consisting of one or more for observing. Using these, information such as the shape, pattern, color, size, or set of three-dimensional point clouds of obstacles and environmental structures around the vehicle is acquired, and the position and attitude relative to the vehicle are acquired. To do.

The snow cover determination unit 5 receives information from the surrounding environment observation sensor 4 as input, and determines whether or not there is snow cover in the area being observed. Since a plurality of known techniques have been proposed for this determination, they may be used. For example, in the case of a monocular camera, sunlight is reflected well on the snow surface during the daytime, so the brightness at the time of imaging becomes high and it can be observed as a white area, and if there is an automatic exposure function, the shutter time becomes short. There is. Therefore, the threshold values obtained experimentally are obtained, and when the number of pixels having a brightness value larger than the predetermined threshold value is equal to or more than a certain value in the imaging region and the shutter time is shorter than the predetermined threshold value, snow cover occurs. It can be determined that there is. The snow cover determination unit 5 may be provided with dedicated hardware, or may be realized by the same computer as the self-position estimation device 10.

The three-dimensional collation unit 13 estimates the self-position that changes from moment to moment with an accuracy of about several cm, and has a general-purpose collation unit 14 and a snow cover collation unit 15, and the determination result of the snow cover determination unit 5 is obtained. The collation unit to be used is switched accordingly. For example, if the determination result is no snow, the use of the general-purpose collating unit 14 is selected, and if the determination result is snow, the use of the snow-corresponding collating unit 15 is selected. When it is determined that the amount of snow is small, both collation units may be used together.

The general-purpose collation unit 14 includes map information from the map database 3, self-position estimation results from the map matching unit 12, and shapes, patterns, colors, and sizes of obstacles and environmental structures from the surrounding environment observation sensor 4. Alternatively, information such as a three-dimensional point cloud is acquired. Then, from the map information, a peripheral map including the three-dimensional structure near the self-position estimation result is extracted. Next, the position and orientation of the extracted three-dimensional structure are compared with the position and orientation of obstacles and environmental structures obtained by the surrounding environment observation sensor 4, and the surrounding environment observation with respect to the map database 3 is performed. The viewpoint of the sensor 4 is calculated. Since a plurality of known techniques have been proposed for this collation technique, they may be used.

For example, there is a technology called ICP (Iterative Closest Point). This is because when collating two three-dimensional point clouds A and B, the point clouds are first superimposed on the assumption of a tentative viewpoint, and the closest point b in the point cloud B is set with respect to a certain point a in the point cloud A. Calculate and calculate the squared distance between points a and b. This operation is performed on all the points in the point cloud A, and the distances between the paired points are totaled. Next, the temporary viewpoint is moved in an arbitrary direction, and the distances between the paired points are summed in the same manner. By moving the tentative viewpoint in the direction in which the total distance becomes smaller, it is possible to finally find the viewpoint where the point group A and the point group B are closest to each other. By using such a technique, the general-purpose collation unit 14 can estimate the self-position with an accuracy of about several cm.

The snow cover matching unit 15 basically performs the same processing as the general-purpose matching unit 14, and estimates the detailed self-position. However, since the snow-covered collation unit 15 is used when the snow-covered determination unit 5 determines that there is snow, processing specialized for the snow-covered situation is also performed. The details of this process will be described later.

The self-position calculated and updated by the three-dimensional collation unit 13 is fed back to the input of the rough position estimation unit 11 and can be used as the initial self-position at the next time.

The vehicle control unit 6 enables driving support including automatic driving that does not require the driver's operation for the driver's driving operation, and overtakes the preceding vehicle, maintains the lane, automatically joins the highway, and the like. Implement adaptive driving control including, automatic avoidance control for obstacles, and driving support control such as temporary stop.

Hereinafter, the details of the rough position estimation unit 11, the map matching unit 12, and the three-dimensional collation unit 13 of this embodiment will be sequentially described.

<Rough position estimation unit 11>
FIG. 2 shows a flowchart of the process executed by the rough position estimation unit 11.

As described above, the rough position estimation unit 11 estimates the approximate self-position (rough position) that changes from moment to moment from the sensing results of the vehicle behavior observation sensor 1 and the absolute position acquisition sensor 2. The vehicle behavior observation sensor 1 continuously obtains estimation results, but errors accumulate over time. On the other hand, the absolute position acquisition sensor 2 does not accumulate errors, but can obtain its own position only discretely. Therefore, the self-position is updated based on the output of the absolute position acquisition sensor 2 only when the estimation result of the absolute position acquisition sensor 2 is obtained and the state is reliable, and in other cases, the vehicle behavior is observed. It is updated so that the self-position and the orientation are relatively changed based on the output of the sensor 1. Specifically, the process in the rough position estimation unit 11 includes the following steps.

Step S1: The sensor output value is received from the vehicle behavior observation sensor 1 and the absolute position acquisition sensor 2 after adding the sensor time. The sensor time uses that time for sensors that have a real-time clock inside, and for sensors that do not have a real-time clock inside, such as acceleration sensors and angular velocity sensors, the device that receives the sensing time and sensor output. In consideration of the offset time including the transmission time between, the time is given by the device that receives the sensor output, and is stored or stored.

Step S2: Since the output of the absolute position acquisition sensor 2 is discrete in terms of time or space, the output is not always obtained. Therefore, it is necessary to determine whether or not the value has been updated from the previous value. If the output has not been updated, the process proceeds to step S5. If it is updated, proceed to step S3.

Step S3: The accuracy of the output of the absolute position acquisition sensor 2 may deteriorate depending on the situation. For example, in a GNSS receiving device, accurate position information may not be obtained in a tunnel or an underground parking lot where radio waves from a positioning satellite are difficult to reach. Further, in an urban area where many high-rise buildings are lined up, incorrect position information may be obtained due to a multipath phenomenon in which satellite radio waves are reflected and diffracted by the buildings and received. In order to judge such a situation, there are indexes such as positioning status, DOP value (Dilution Of Precision), error elliptical radius, number of satellites used, speed accuracy, and azimuth accuracy. When these indexes are better than a predetermined threshold value, it is determined that the accuracy is above a certain level. It is obvious to the worker concerned that it is better than a predetermined threshold value, but for example, in the case of positioning status, it is better in the order of 2D positioning, 3D positioning, RTK-FLOAT positioning, and RTK-FIX positioning. Increases. The smaller the DOP value and the radius of the error ellipse, the better, and the larger the number of satellites used, the better. If the accuracy is not above a certain level, the process proceeds to step S5. If the accuracy is in a good state above a certain level, the process proceeds to step S4. When determining whether or not the accuracy is above a certain level, a plurality of index values may be combined by appropriately using the AND condition and the OR condition.

Step S4: When the latest output by the absolute position acquisition sensor 2 is obtained with good accuracy, the cumulative error by the vehicle behavior observation sensor 1 can be reset by reflecting this output result in the self-position. it can. If the reflection to the self-position is sufficiently reliable, the value of the absolute position acquisition sensor 2 may be set to the self-position as it is, but the output value stabilized by using the Kalman filter or the particle filter is used. It may be set to its own position.

Step S5: When the output of the absolute position acquisition sensor 2 is old or the output accuracy is deteriorated, the self-position changed in a short time is updated by adding the output of the vehicle behavior observation sensor 1. At that time, the output of the vehicle behavior observation sensor 1 may be added to the self-position as it is, but it may be stabilized by using a Kalman filter or a particle filter.

Step S6: When the output of the absolute position acquisition sensor 2 is obtained with good accuracy, the reliability of the self-position estimation is set to be high, and the reliability is increased as the output of the vehicle behavior observation sensor 1 is accumulated. Set so that the degree gradually decreases. For example, it can be set as in the following equation 1.

(Reliability) = α ÷ (Accuracy index of absolute position acquisition sensor)
−β × (Number of self-position updates by vehicle behavior observation sensor)… (Equation 1)
Alternatively, when the Kalman filter is used, the term of the covariance matrix may be added to the formula for calculating the reliability.

A concrete numerical example is given. Assuming that (the accuracy index of the absolute position acquisition sensor 2) is the error elliptical radius of the GNSS receiver having an output period of 1 [Hz] and the range is 1 to 50 [m], the smaller the value, the better the accuracy. (Number of self-position updates by the vehicle behavior observation sensor 1) is defined as an output cycle 20 [Hz] acceleration sensor and each acceleration sensor. Further, α = 1000 and β = 2. In this case, the reliability is set from 20 to 1000 according to the output cycle of the GNSS receiver, and then the reliability gradually decreases over 1 second.

<Map matching unit 12>
Next, the processing flow of the map matching unit 12 will be described.

The map matching unit 12 calculates which road on the map the self-position estimated by the rough position estimation unit 11 is traveling on, and if it does not deviate from the road, the self-position so as to get on the road. To correct. A plurality of known methods have been proposed for this map matching method. A specific example is shown below.

First, road information is acquired from the map database 3. This road information includes one or more of a road node number, a node position, a road link number, a road link width, and the like.

Next, for each road link existing around the self-position estimated by the rough position estimation unit 11, a perpendicular line from the self-position is drawn, and an intersection with each road link is obtained as a road link candidate point. Further calculates the length of a perpendicular, the evaluation amount with the difference of the road link heading and the host vehicle orientation theta _V.

FIG. 3 is an explanatory diagram for explaining an example in the case of calculating the evaluation value for the road link. For example, the position of the vehicle and (vehicle position P _V) and attitude (vehicle direction theta _V), a plurality of road links at the vehicle position within a predetermined distance from the road node N _A1 to road node N _A2 road link _{L a} of which the road node _{N B1} and road link _{L B} to road node _{N B2.} Further, the length of the perpendicular drawn from the vehicle position P _V relative to the road link L _A to D _A, the intersection of the road link and P _A. Similarly, the length of the perpendicular drawn from the vehicle position P _V relative to the road link L _B D _B, an intersection between the road link and P _B. Then, to calculate the evaluation value for each road link candidate points P _A and P _B. The difference between the road link directions θ _A and θ _B and the vehicle direction θ _V is called the azimuth error. The distances D _A and D _B to the road link are called position errors. The covariance error is calculated for this azimuth error and the position error, and the evaluation amount of each road link candidate point is calculated based on this. Based on this calculation result, the road link candidate point having the highest evaluation amount with the highest probability of traveling on the road is set as the first road link candidate point.

Next, it is determined whether or not the own vehicle deviates from the road based on the position and direction of the road link and their variances, the width information, and the estimated position and the estimated direction and their covariance. If it is determined not to deviate it is corrected so as to approach the vehicle position P _V in the first road link candidate points. At that time, the position of the first road link candidate points may be directly set on the vehicle position P _V, but the output value was stabilized using a Kalman filter or a particle filter is set to the vehicle position P _V Good.

<Three-dimensional collation unit 13>
As described above, the three-dimensional collation unit 13 has a general-purpose collation unit 14 and a snow cover collation unit 15 inside, and whether to use either one or both depending on the result from the snow cover determination unit 5. To switch. For example, if the determination result of the snow cover determination unit 5 is "no snow cover" for a predetermined time or longer, only the result of the general-purpose collation unit 14 is used, and if the check result is "with snow cover" for a predetermined time or longer, snow cover is supported. The result of the collating unit 15 is used. If the result of the snow cover determination unit 5 fluctuates, it is expected that the road condition is such that the snow cover is partially left and the snow removal is incomplete. Use both of 15. When both are used, a plurality of viewpoints of the surrounding environment observation sensor 4 in the map, which are output as collation results, appear, so they are integrated using a weighted average. An output value stabilized by using a Kalman filter or a particle filter may be used.

<Snow cover collation unit 15>
FIG. 4 is a functional block diagram of the snow cover matching unit 15. As shown here, the snow-covered collation unit 15 includes an external shape information extraction unit 16, a collation map extraction unit 17, a sensing result extraction unit 18, a sensing result collation unit 19, and a position correction unit 20.

<External information extraction unit 16>
Based on the self-position (see FIG. 3) output from the map matching unit 12, the external shape information extraction unit 16 reads the map information stored in the map database 3 corresponding to a predetermined range near the self-position, and has a three-dimensional shape. Convert to the external shape information related to. For example, the map information stored in the map database 3 includes the area shape of the paved road surface, which is the shape of the road itself, the installation position of the guard rail and the soundproof wall on the road, the height of the guard rail and the soundproof wall, and the speed limit / running regulation.・ Shape type and installation position of pillars that take the shape of rods, L-shapes, pedestrian crossings, etc. on which signs indicating road guidance are installed, height of sign pillars, mounting height and shape of sign signs, electric poles Installation position and height of the road, installation position and shape of the curved mirror on the road, installation height of the mirror, position and shape of road markings such as pedestrian crossings and road marking lines, shape and installation height of traffic lights, Including one or more such as. Further, not only the road but also the shape of a building or a structure such as a building, a fence, or a tree outside the road may be included. Further, the 3D shape, the point cloud information, the texture information, etc. obtained by the external sensor mounted on the traveling vehicle may be collected in the server and include the information distributed by the communication means or the recording medium. The external shape information extraction unit 16 reads the stored map information and converts it into the external shape information of the contained object.

For example, when the location information of a traffic sign with a height of 2 m and a speed limit of 50 km / h is read from map information, a cylinder with a thickness of 0.1 m and a length of 2 m exists at a point 1 m outside the road. External information that there is a disk-shaped plate with a radius of 0.6 m at the same point 1 m outside the road and 2 m high, and external information that the character 50 is posted on the disk. Converts to external shape information and outputs.

In addition to the method of outputting the coordinate values of the 3D point cloud, this external information is also output in the vector format such as the central axis of the cylinder or disk and its radius, and the space to be expressed is divided into rectangular parallelepipeds of a certain size and the object is displayed. There are many output formats, such as a voxel format output method that adds presence / absence information. In this embodiment, it is assumed that the coordinate value is output as a 3D point cloud, but the present invention is not limited to this format.

<Map extraction unit for collation 17>
FIG. 5 shows a block diagram of the collation map extraction unit 17. As shown here, the collation map extraction unit 17 is composed of a support column extraction unit 17a, a low-level external information deletion unit 17b, and a vertical surface extraction unit 17c.

From the external shape information output from the external shape information extraction unit 16, the support column extraction unit 17a provides external shape information corresponding to the support column on which signs such as speed limit signs, regulation signs, and caution signs are installed, and for automobiles and pedestrians. , Extract the external shape information corresponding to the support column where the traffic light for road trains is installed. Signs and traffic lights are of high importance in terms of road traffic, and it can be expected that snow will be removed so that they can be seen at least even when there is snow. Therefore, it can be expected that the success rate in the sensing result collation unit 19 will be improved by extracting the outer shape information of the sign and the support of the traffic light as a collation map.

Further, the low-level external information deletion unit 17b determines the snow cover height observed by the surrounding environment observation sensor 4, the height of the viewpoint in the map output in the past from the three-dimensional collation unit 13, and the mounting height when mounted on the vehicle. Based on either the snow height expressed as a difference or a general snow height, the remaining external shape information excluding a certain height from the road surface (that is, the external shape that seems to appear on the snow cover) Information) may be extracted. This makes it possible to extract a collation map excluding external information that is not observed during snowfall.

Alternatively, the low-level external shape information deletion unit 17b excludes the remaining external shape information from the road surface with respect to the external shape information on the other side of the shield that continuously exists at a constant height such as a guardrail or a soundproof wall. May be extracted. This makes it possible to extract a collation map excluding external information that is not observed due to shielding.

The vertical face extraction unit 17c extracts information on the vertical face that can be collated even during snowfall from the external shape information output by the external shape information extraction unit 16. Specifically, it is assumed that snow will accumulate on the surface of an object that is nearly horizontal, and the shape information stored in the map database 3 may differ from the shape of the observation results in the actual environment during snow accumulation. There is sex. Therefore, from the external shape information output by the external shape information extraction unit 16, by extracting a surface close to vertical within a predetermined angle threshold from the vertical direction, there is little possibility that snow will accumulate, and the apparent shape will not change easily during snow accumulation. Objects can be extracted.

By using the support column extraction unit 17a or the like described above, the collation map extraction unit 17 extracts a collation map composed of external information groups such as columns and wall surfaces that can be collated even when it is snowing.

<Sensing result extraction unit 18>
FIG. 6 shows a block diagram of the sensing result extraction unit 18. The sensing result extraction unit 18 corresponds to the external shape information extracted by the collation map extraction unit 17 from the information such as the shape, pattern, color, and size of obstacles and environmental structures obtained from the surrounding environment observation sensor 4. Extract the shape information that is likely to be used. Note that the shape information acquired by the ambient environment observation sensor 4 may not be able to observe all parts of the target because it is already shielded by snow and other structures, so it is necessary to exclude the lower part of the observation result. Absent.

As shown in FIG. 6, the sensing result extraction unit 18 is composed of a support column extraction unit 18a, a low-level external information deletion unit 18b, a vertical surface extraction unit 18c, a snow cover area removal unit 18d, and a moving body removal unit 18e.

From the sensing result output from the ambient environment observation sensor 4, the support column extraction unit 18a is used for columns on which signs such as speed limit signs, regulation signs, and caution signs are installed, for automobiles, pedestrians, trams, and the like. Extract the pillars on which the traffic lights are installed. Specifically, the ambient environment observation sensor 4 can recognize which type of object is being observed by using a pattern identification technique using machine learning or the like. Further, in a sensor such as a stereo camera or LiDAR that can recognize distance information, the observation target can be extracted from the background object by using the distance information. Even with a monocular camera that cannot acquire distance information, it is possible to extract traffic lights and sign posts by using known Semantic Segmentation technology. As mentioned above, signs and traffic lights are of high importance in road traffic, and it can be expected that snow will be removed so that they can be seen at least even during snowfall. Therefore, it can be expected that the success rate in the sensing result collating unit 19 will be improved by extracting the signs, the traffic lights, and their columns.

The low-level external shape information deletion unit 18b extracts the remaining external shape information excluding a certain height from the upper end of the column for the external shape information on the other side of the shield that continuously exists at a constant height such as a guardrail or a soundproof wall. It may be extracted. This makes it possible to extract a collation map excluding external information that is not observed due to shielding.

The vertical surface extraction unit 18c extracts the vertical surface that is not easily affected by the shape change due to snow cover from the sensing result obtained from the ambient environment observation sensor 4. Specifically, since the nearly horizontal surface in the observation result may be a surface generated by snow cover, the shape information stored in the map database 3 may differ from the shape of the observation result. .. Therefore, from the sensing results obtained from the ambient environment observation sensor 4, by extracting a surface close to the vertical within a predetermined angle threshold value from the vertical direction, an object that is unlikely to be a surface generated by snow cover can be obtained. Can be extracted. As a result, it is expected that the sensing result not included in the map to be collated generated by the snow cover will be prevented from being erroneously collated, and the success rate in the sensing result collation unit 19 will be improved.

The snow cover area removing unit 18d removes the surface generated by the snow cover from the sensing result obtained from the ambient environment observation sensor 4. Specifically, the observation result consisting of the snow-covered region may be removed by using a known Semantic Segmentation technique or the like, and in LiDAR, the region obtained from the snow-covered region when the reflectance of the laser is higher than a predetermined threshold value. It may be determined that this is the case and excluded. As a result, it is expected that the effect of suppressing erroneous collation on the surface not included in the map of the collation target generated by the snow cover and improving the success rate in the sensing result collation unit 19 can be expected.

The moving body removing unit 18e removes the observation information obtained by the moving body such as a traveling vehicle or a pedestrian from the sensing result obtained from the surrounding environment observation sensor 4. Specifically, by performing sensing multiple times and extracting the difference in the sensing results in consideration of the amount of movement of the own vehicle, moving objects in the environment may be extracted and removed, including Deep Learning. Areas of vehicles and pedestrians may be extracted by using machine learning or the like, and the corresponding areas may be removed from the sensing result. As a result, it is possible to prevent erroneous collation of observation results from a moving object that is not included in the map to be collated, and to expect an effect of improving the success rate in the sensing result collation unit 19.

Although described as the vehicle is traveling on a horizontal road surface, the vehicle direction theta _V can also be be obtained by using an acceleration sensor mounted as a vehicle behavior observation sensors 1. In this case, when the output value of the vehicle behavior observation sensor 1 is input to the sensing result extraction unit 18, the vehicle orientation θ _V is estimated, and then a surface close to the vertical within a predetermined angle threshold is extracted from the vertical direction. The angle calculation can be corrected, increasing the accuracy of the extraction. The connection with the vehicle behavior observation sensor 1 is not shown in FIG.

<Sensing result collation unit 19>
Sensing result matching section 19, an output of the matching map extracting unit 17 collates the output of the sensing result extraction unit 18 calculates the vehicle position P _V and the subject vehicle azimuth theta _V in the map information. Since a plurality of known techniques have been proposed for this collation technique, they may be used. For example, the above-mentioned technique called ICP may be used.

In the sensing result collating unit 19, the lower part of the shape information may be observed by only one of the output of the collating map extraction unit 17 and the output from the sensing result extraction unit 18 due to the influence of shielding or the like. Therefore, an ICP improved so that the weighting becomes stronger at a high ground clearance may be used. For example, specifically, the following may be performed.

When collating two three-dimensional point clouds A and B, first, assuming a tentative viewpoint, the point clouds are overlapped, and the closest point b in the point cloud B is calculated with respect to a certain point a in the point cloud A. , The squared distance D (a, b) of the point a and the point b is calculated. This operation is performed for all the points in the point cloud A, and the distance between the paired points is calculated. Normally, this distance is summed as it is and used as the matching error evaluation value, but in the improved ICP, the heights of points a and b from the road surface are set to Ha and Hb, respectively, and the conversion function T (x) increases monotonically. Therefore, the error evaluation value is calculated as Σ {D (a, b) × T (Ha × Hb)}. As a result, when the deviation of the point having a high height from the road surface is large, the error evaluation value becomes larger than when the deviation of the point having a low height from the road surface is large.

After that, as in the case of normal ICP, the temporary viewpoint is moved in an arbitrary direction, the distance between the paired points is calculated, and the error evaluation value is calculated. By moving the tentative viewpoint in the direction in which the error evaluation value becomes smaller, it is possible to finally obtain the viewpoint where the point group A and the point group B are closest to each other. The viewpoint position is because it corresponds to the sensor origin of the surrounding environment observation sensors 4, converts the vehicle position P _V using the relative relationship between the mounting position and the vehicle origin of the surrounding environment observation sensors 4. This relative relationship is known information when the vehicle is constructed.

In sensing result verification unit 19, and the vehicle position P _V obtained by the method described above, the error evaluation value, the number of points correspondence is obtained each point of the point group A and point cloud B is within a predetermined distance Output as the number of matching areas.

<Position correction unit 20>
The position correction unit 20 calculates the collation reliability from the output of the sensing result collation unit 19 and corrects the self-position. The collation reliability is set so that the smaller the error evaluation value, the better, and the larger the number of matching areas, the better. For example, the matching reliability can be defined by the following equation 2.

(Collation reliability) = γ ÷ (error evaluation value) + δ × (number of matching areas)… Equation 2
With a weight corresponding to the matching reliability, the self-position output by the map matching unit 12, corrected and to approach the vehicle position P _V output by sensing result verification unit 19, the vehicle control unit 6 Generate a self-position to output.

Incidentally, verification if reliability is a sufficiently large predetermined threshold or more, may be outputted replaced by the vehicle position P _V obtained its own position to be output to the vehicle control unit 6 by sensing result verification unit 19. Further, although it was described above that the correction is performed by weighting, it may be stabilized by using a Kalman filter or a particle filter.

As described above, according to the self-position estimation device 10 of the present embodiment, a predetermined time and area are extracted from the information obtained from the map and the sensor, respectively, and the map in which the environmental information is recorded and the map observed. By extracting a common area that can be collated with the surrounding environment structure, the environmental structure existing in the driving environment is shielded by snowfall, or the shape of the environmental structure continues to change, or on a snow-packed road. Even if the road markings remain invisible, detailed estimation of the self-position is possible.

Next, the self-position estimation device 10 according to the second embodiment of the present invention will be described with reference to the flowchart of FIG. 7. It should be noted that the common points with the first embodiment will be omitted.

The rough position estimation unit 11 of the first embodiment is based on the output of the absolute position acquisition sensor 2 when the output of the absolute position acquisition sensor 2 is updated, as is clear from the processes after step S2 in the flowchart of FIG. However, in the rough position estimation unit 11 of this embodiment, the self-position is updated without depending on the output update timing of the absolute position acquisition sensor 2, so that the processing cycle fluctuates due to the processing load. However, it is possible to estimate the self-position with high accuracy. Hereinafter, only the steps changed from the first embodiment will be described with reference to FIG. 7.

For example, when the cycle in which the GNSS receiver performs positioning using the positioning satellite radio waves fluctuates depending on the processing load, or when the sensing cycle and the self-position estimation cycle are different, the final output value output by each sensor is used. If it is used as it is, the time consistency between the sensors cannot be obtained and the position accuracy of the self-position estimation deteriorates. Therefore, in this embodiment, the sensor output is accumulated, and the accumulated sensing result is used at the timing of self-position estimation.

In that case, self-position estimation can be performed by adding steps S11 and S12 described below after step S1.

Step S11: It is determined whether or not it is the start timing of self-position estimation, and if it is not the start timing, the process proceeds to step S1. If it is the start timing, the process proceeds to step S12.

Step S12: Since the sensor output to which the time is given in step S1 is the output value at the past timing, the output value at the time of calculating the self-position estimation is interpolated or predicted from the past value and the time is synchronized between the plurality of sensors. Need to take. For example, a Kalman filter is used for each sensor, or a linear model or a polynomial model is used to predict the sensor value at that time.

In this way, when self-position estimation is performed asynchronously with the sensor output time, it is possible to perform self-position estimation with high accuracy by adding the above step. Further, even if the processing cycle fluctuates due to the processing load, it becomes possible to accurately estimate the self-position.

Next, the self-position estimation device 10 according to the third embodiment of the present invention will be described with reference to the block diagram of FIG. It should be noted that the common points with the above-described embodiment will be omitted.

In the snow-covered collation unit 15 of the first embodiment, the collation map extraction unit 17 and the sensing result extraction unit 18 extracted predetermined information based on the output of the snow-covered determination unit 5. In 15, the matching accuracy is improved and the self-position estimation is performed by feeding back and utilizing the matching residual between the map database 3 and the surrounding environment observation sensor 4, that is, the difference between the past map information and the currently observed environmental information. It improves accuracy.

Therefore, in this embodiment, in addition to the output of the snow cover determination unit 5, the output of the surrounding environment recognition sensor 4 is used to control the output of the matching map extraction unit 17 and the sensing result extraction unit 18. This makes it possible to extract a matching map and a sensing result that are more suitable for the snow conditions.

8, the sensing result matching section 19, an output of the matching map extracting unit 17 described above, sensing result matches the output from the extraction unit 18, the vehicle position in the map information P _V and the subject vehicle azimuth theta _V In addition to the calculation function, the snow cover state is output to the snow cover state recording unit 7 from the information of the point cloud remaining without being collated.

For example, in the sensing result collation unit 19, when collating two three-dimensional point clouds A and B, first, a tentative viewpoint is assumed and the point clouds are overlapped, and the point cloud B with respect to a certain point a of the point cloud A The closest point b is calculated, and the squared distance D (a, b) between the points a and b is calculated. This operation is performed for all the points in the point cloud A, and the distance between the paired points is calculated. Normally, this distance is summed as it is and used as the matching error evaluation value, but in the improved ICP, the heights of points a and b from the road surface are set to Ha and Hb, respectively, and the conversion function T (x) increases monotonically. Therefore, the error evaluation value is calculated as Σ {D (a, b) × T (Ha × Hb)}. As a result, when the deviation of the point having a high height from the road surface is large, the error evaluation value becomes larger than when the deviation of the point having a low height from the road surface is large.

Similar to normal ICP, the temporary viewpoint is moved in an arbitrary direction, the distance between the paired points is calculated, and the error evaluation value is calculated. By moving the tentative viewpoint in the direction in which the error evaluation value becomes smaller, it is possible to finally obtain the viewpoint where the point group A and the point group B are closest to each other.

At this time, if the squared distance D (a, b) between the points a and b is within a predetermined distance, a flag is added assuming that the points a and b correspond to each other, and the squared distance D ( When a and b) are larger than a predetermined distance, the point a and the point b are not flagged as not corresponding to each other. By doing so, the points that cannot be finally associated with any points can be extracted as unflagged points. It can be expected that many points that are not associated with any of these points are extracted by the above-mentioned conversion function T (x) at points having a low height from the road surface. Of the point clouds output from the collation map extraction unit 17, points that are not associated with any points and have a low height can be regarded as points that cannot be sensed due to snow cover or the like. The snow height can be estimated based on the point height information.

For example, suppose that there is a pole with a height of 2.0 m, and 1.8 m from the top is extracted as a point cloud by the collation map extraction unit 17. Further, assuming that the pole-shaped point cloud having a height of 1.0 m corresponding to the region is output from the sensing result extraction unit 17, the point cloud of the sensing result is obtained by the above-mentioned conversion function T (x). It will match the top of the point cloud extracted from the map. Assuming that the above-mentioned threshold value of the square distance is 0.1 m, the lower 0.7 m point cloud is extracted as a point that cannot be associated with any point. From this, it can be seen that the snow height is 0.7 m higher than the current system assumes. This is recorded in the snow condition recording unit 7.

Further, the sensing result collation unit 19 reads out the difference in the snow cover height recorded in the snow cover state recording unit 7, and if it is larger than a predetermined value, the point cloud height of the collation map is low. By deleting it, it is possible to suppress erroneous matching between the extracted matching map and the sensing result, and to enable stable self-position estimation.

Although the output to the snow cover state recording unit 7 has been described from the viewpoint of the snow cover height, it is not necessary to limit the output to this.

FIG. 9 shows an example of the environment E1 without snow and the environment E2 after snow. Another example of the content to be output to the snow cover state recording unit 7 will be described with reference to this figure.

For example, in an environment E1 without snow, it is assumed that there is a roadside zone white line L1 indicating a division between a roadside zone and a roadway, and a lane dividing line L2. Generally, snow is removed when snow is accumulated, but when the amount of snow is large, snow may remain on the shoulder of the road, the white line L1 on the roadside may be hidden by snow, and only the lane dividing line 2 may be visible.

In the sensing result collating unit 19, when a specific type of target is collated only less than a predetermined number of collations while traveling for a certain period of time, the snow cover state recording unit 7 indicates that the target of that type is a road in a state where it cannot be sensed. Output to. Further, in the sensing result collation unit 19, the snow cover state recording unit 7 deletes the target of the type that cannot be collated from the output from the sensing result extraction unit 18, so that the extracted collation map and the sensing result are erroneously used. It suppresses matching and enables stable self-position estimation.

The information recorded in the snow cover state recording unit 7 may be deleted after a certain period of time, and verification may be attempted again for the target of the type.

1 Vehicle behavior observation sensor 2 Absolute position acquisition sensor 3 Map database 4 Surrounding environment observation sensor 5 Snow cover judgment unit 6 Vehicle control unit 7 Snow cover status recording unit 10 Self-position estimation device 11 Rough position estimation unit 12 Map matching unit 13 Three-dimensional collation unit 14 General-purpose collation unit 15 Snow-covered collation unit 16 External shape information extraction unit 17 Verification map extraction unit 17a Strut extraction unit 17b Low-level external information deletion unit 17c Lead face extraction unit 18 Sensing result extraction unit 18a Support column extraction unit 18b Low-level external shape information deletion unit 18c Vertical surface extraction unit 18d Snow cover area removal unit 18e Moving object removal unit 19 Sensing result collation unit 20 Position correction unit

Claims (10)

A map database that stores map information and
Connected to the ambient environment observation sensor, which acquires 3D point cloud data of the ambient environment,
It is a self-position estimation device that estimates the self-position of the vehicle.
A rough position estimater that estimates the approximate self-position,
It has a three-dimensional collation unit that estimates the detailed self-position, and
The three-dimensional collation unit
A collation map extraction unit that generates three-dimensional map data that extracts observable external shape information during snowfall from the map information, and
A sensing result extraction unit that obtains the three-dimensional point cloud data from the ambient environment observation sensor,
A position correction unit that corrects the self-position estimated by the rough position estimation unit by three-dimensionally collating the three-dimensional map data with the three-dimensional point cloud data.
A self-position estimation device characterized by having. In the self-position estimation device according to claim 1,
The collation map extraction unit is a self-position estimation device characterized by generating the three-dimensional map data by extracting the outer shape information of the columns included in the map information. In the self-position estimation device according to claim 1,
The collation map extraction unit is a self-position estimation device characterized by generating the three-dimensional map data by extracting external shape information having a height equal to or higher than a predetermined height included in the map information. In the self-position estimation device according to claim 1,
The collation map extraction unit is a self-position estimation device characterized by generating the three-dimensional map data by extracting the outline information of the vertical plane included in the map information. In the self-position estimation device according to any one of claims 1 to 4.
Further, a self-position estimation device including a snow cover state recording unit that records the result of the three-dimensional collation. It is a self-position estimation method that estimates the self-position of the vehicle.
The first step in estimating the approximate self-position,
The second step of generating 3D map data by extracting the external shape information that can be observed during snowfall from the map information,
The third step of obtaining 3D point cloud data from the ambient environment observation sensor,
The fourth step of correcting the estimated self-position by three-dimensionally collating the three-dimensional map data with the three-dimensional point cloud data,
A self-position estimation method characterized by having. In the self-position estimation method according to claim 6,
The second step is a self-position estimation method characterized in that the three-dimensional map data is generated by extracting the outer shape information of the columns included in the map information. In the self-position estimation method according to claim 6,
The second step is a self-position estimation method characterized in that the three-dimensional map data is generated by extracting external shape information having a height equal to or higher than a predetermined height included in the map information. In the self-position estimation method according to claim 6,
The second step is a self-position estimation method characterized in that the three-dimensional map data is generated by extracting the external shape information of the vertical plane included in the map information. In the self-position estimation method according to any one of claims 6 to 9.
Further, a self-position estimation method comprising a fifth step of recording the result of the three-dimensional collation.

Images