WO2022024563A1 - Own-position estimation device and program - Google Patents

Abstract

An own-position estimation device according to one embodiment of the present invention is provided with: a current position estimation unit for provisionally estimating the current own-position; a point group data selection unit for calculating a range in which information can be acquired by a sensor of a moving body on the basis of the provisional own-position among point group data included in map information, and selecting point group data that is in the acquirable range; a point quantity adjustment unit for comparing the number of points of the point group data selected from the map information and a maximum processable number of points, which is the maximum number of points that can be processed in the maximum allowable processing time when the position is estimated, and adjusting the number of points of the selected point group data so as to be within a range that is no greater than the maximum processable number of points, on the basis of the comparison result; and a matching unit for matching the adjusted point group data and the point group data in the information acquired by the sensor.

Description

Self-position estimator and program

The present invention relates to a self-position estimation device and a program, and particularly to a technique for estimating the position of a moving object such as a robot or an automobile.

Autonomous driving technology and driving support technology have been developed in which moving objects such as robots and automobiles collect information around them, estimate the current position and traveling state of the moving objects, and control the traveling of the moving objects. The reliability of the current position estimation depends on the accuracy and amount of the collected surrounding information. On the other hand, when the amount of collected information is large, the processing load becomes high and the reliability of position estimation becomes low.

For example, Patent Document 1 discloses a mobile body position estimation device and a mobile body position estimation method that can reduce the processing load required for the position estimation of the mobile body. In Patent Document 1, by securing a predetermined number of predetermined feature points satisfying a predetermined standard and tracking only the predetermined number of the predetermined feature points, the position of the moving body can be estimated with a relatively small processing load. I have to.

International Publication No. 15/049717

However, as in Patent Document 1, when the number of predetermined feature points satisfying a predetermined standard is large, the processing load cannot be reduced. Further, when there are few predetermined feature points satisfying a predetermined standard, highly accurate position estimation cannot be performed.

From the above situation, there has been a demand for a method that enables position estimation with optimized position estimation accuracy and processing load.

In order to solve the above problems, the self-position estimation device of one aspect of the present invention is a self-position estimation device that estimates its own position by comparing the information of the traveling environment collected by the sensor with the map information. And take the following configuration.
In the self-position estimation device, the sensor acquires information based on the current position estimation unit that temporarily estimates the current position of itself and the current temporary position estimated by the current position estimation unit among the point cloud data included in the map information. The point cloud data selection unit that calculates the possible range and selects the point cloud data in the acquireable range, the score of the point cloud data selected from the map information, and the maximum allowable processing time when estimating the position. Compare with the maximum processable score, which is the maximum score that can be processed in, and adjust the score of the selected point cloud data within the range below the maximum processable score based on the comparison result. A matching unit that matches the group data with the point cloud data of the information acquired by the sensor, and a current position correction unit that corrects its own current temporary position estimated by the current position estimation unit based on the matching result of the matching unit. And.

According to at least one aspect of the present invention, position estimation with optimized position estimation accuracy and processing load is possible.
Issues, configurations and effects other than those described above will be clarified by the following description of the embodiments.

It is an overall block diagram of the position estimation apparatus mounted on the moving body which concerns on one Embodiment of this invention. It is a block diagram which shows the internal structure example of the sensor processing part of FIG. It is a flowchart which shows the procedure example of the information processing which a sensor processing part executes. It is a graph which shows the detail of the maximum score calculation in the maximum score recording process of step S2. It is a figure which shows the example (no obstacle) of the unmatchable area calculation process of step S12, and the unmatchable area recording process of step S13. It is a figure which shows the example (with obstacles) of the unmatchable area calculation process of step S12, and the unmatchable area recording process of step S13. It is a figure which shows the detail of the score reduction process of a step S9. It is a figure which shows the detail of the score increase process of a step S8. It is a figure which shows the 1st example of the self-position estimation process which concerns on one Embodiment of this invention. It is a figure which shows the 2nd example of the self-position estimation process which concerns on one Embodiment of this invention. It is a figure which shows the 3rd example of the self-position estimation process which concerns on one Embodiment of this invention. It is a figure which shows the 4th example of the self-position estimation process which concerns on one Embodiment of this invention. It is a figure which shows the 5th example of the self-position estimation process which concerns on one Embodiment of this invention. It is a figure which shows the sixth example of the self-position estimation process which concerns on one Embodiment of this invention. It is a figure explaining the selection criteria of the point cloud data at the time of carrying out the point cloud reduction. It is a figure which shows the example of the self-position estimation process using the point cloud selected in FIG.

Hereinafter, an example of a mode for carrying out the present invention will be described with reference to the attached drawings. In the present specification and the accompanying drawings, components having substantially the same function or configuration are designated by the same reference numerals and duplicated description will be omitted.

In the present specification, the "three-dimensional point" represents the coordinates in the space located on the surface and inside of the object having a shape, and refers to the points obtained by the sensor, the points included in the map, and the like. It shall include. Further, a plurality of three-dimensional points are called a "point cloud". The three-dimensional point data (point cloud data) may include color information.

[Overall configuration of position estimation device]
FIG. 1 is a block diagram of a position estimation device according to an embodiment of the present invention.
The position estimation device 1 (an example of the self-position estimation device) is mounted on a moving body 100 such as an automobile or a robot. The position estimation device 1 can communicate with the external storage device 19 existing outside the position estimation device 1. Wireless is desirable as this communication means.

The position estimation device 1 has a signal reception unit 2, one or more sensors 12a, 12b, ..., 12n, and an information processing device 13. These components are interconnected by a bus 18. In the present specification, the sensors 12a, 12b, ..., 12n are referred to as sensors 12 when it is not necessary to distinguish them.

The information processing device 13 is, for example, a general computer (electronic computer), and includes a sensor processing unit 14 that processes information acquired by the sensor 12 and a control unit 15 (for example, a CPU) that performs processing based on the sensor processing results. , A memory 16 and a display unit 17 such as a display. In the information processing apparatus 13, each function according to the present embodiment is realized by the sensor processing unit 14 and the control unit 15 reading and executing a computer program recorded in the memory 16 or a storage medium (not shown).

The signal reception unit 2 receives a signal from the outside. For example, the signal receiving unit 2 is a receiver of a Global Positioning System (GPS) that estimates the current position in absolute coordinates of the world. Further, the signal receiving unit 2 may be a receiver of RTK-GPS (RealTimeKinematicGPS) that estimates the current position more accurately than GPS. Further, the signal receiving unit 2 may be a receiver of the quasi-zenith satellite system. Further, the signal receiving unit 2 may receive a signal from a beacon fixed at a known position. Further, the signal receiving unit 2 may receive a signal from a sensor that estimates the position in relative coordinates, such as a wheel encoder, an inertial measurement unit (IMU), and a gyro. Further, the signal receiving unit 2 may receive information such as a lane, a sign, a traffic condition, a shape, a size, and a height of a three-dimensional object in the traveling environment. Finally, any method may be used as long as it can be used for the current position estimation, control, and recognition of the mobile body 100 on which the position estimation device 1 is mounted.

The sensor 12 is, for example, a still camera or a video camera. Further, the sensor 12 may be a monocular camera or a compound eye camera. Further, the sensor 12 may be a laser sensor. Finally, any sensor may be used as long as the shape information of the traveling environment (around the moving body 100) can be extracted.

The information processing device 13 processes the information acquired by the sensor 12 to calculate the position or the amount of movement of the moving body 100. The information processing apparatus 13 may display according to the calculated position or movement amount. Further, the information processing apparatus 13 may output a signal related to the control of the mobile body 100.

Next, the details of the sensor 12 will be described.
The sensor 12a is installed in front of the moving body 100, for example. The sensor 12a has a lens and acquires distant view information in front of the moving body 100. In this case, the information acquired from the distant view may be such that features such as a three-dimensional object or a landmark (predetermined stationary object) for position estimation are extracted.

The other sensors 12b, ..., 12n are installed at positions different from the sensor 12a, and image a direction or region different from the sensor 12a. The sensor 12b may be installed downward, for example, behind the moving body 100. In this case, the sensor 12b acquires the near view information behind the moving body 100. The near view information may be the road surface around the moving body 100, or the white line around the moving body 100, the road surface paint, or the like may be detected.

When the sensor 12 is a monocular camera, if the road surface is flat, the pixel position on the image and the actual ground position relationship (x, y) are constant, so the distance from the sensor 12 to the feature point is geometrically Can be calculated. When the road surface is not flat, the distance from the moving body 100 to the object corresponding to the feature point is based on the amount of movement of the feature points on the image in time series and the amount of movement of the moving body 100 received from the signal receiving unit 2. The distance can be estimated. Further, when the sensor 12 is a stereo camera, the distance to the feature point on the image can be measured more accurately. Further, when the sensor 12 is a laser sensor, it is possible to acquire distant information more accurately.

In the following description, an example in which a monocular camera, a stereo camera, or a laser sensor is used will be described, but other sensors (camera with a wide-angle lens, TOF camera, etc.) may be used as long as the distance to the surrounding stereoscopic object can be calculated. ..

Further, it is desirable that the sensors 12a, 12b, ..., 12n are arranged so as not to be affected by environmental disturbances such as rain and sunlight at the same time. As a result, for example, even if raindrops adhere to the lens of the sensor 12a during rainfall, the raindrops are unlikely to adhere to the lens of the sensor 12b in the opposite direction or downward in the traveling direction. Therefore, even if the information acquired by the sensor 12a is unclear due to the influence of raindrops, the information acquired by the sensor 12b is not easily affected by the raindrops. Further, even if the information of the sensor 12a is unclear due to the influence of sunlight, the information acquired by the sensor 12b may be clear.

Further, the sensors 12a to 12n may be acquired under different acquisition conditions (aperture value, white balance, period, etc.). For example, by installing a sensor whose parameters are adjusted for a bright place and a sensor whose parameters are adjusted for a dark place, it may be possible to take an image regardless of the brightness of the environment.

Sensors 12a to 12n acquire information when receiving a command to start acquisition from the control unit 15 or at regular time intervals. The acquired information data is stored in the memory 16 together with the acquisition time.

The memory 16 includes a main storage device (main memory) of the information processing device 13 and an auxiliary storage device such as storage. Information such as a computer program, a table, and a file that realizes each function according to the present embodiment is recorded in the memory 16. As the memory 16, a semiconductor memory, a hard disk, a recording device such as an SSD (Solid State Drive), or a recording medium such as an IC card or an optical disk can be used. The sensor processing unit 14 performs various information processing based on the information data stored in the memory 16 and the acquisition time. In this information processing, for example, intermediate information is created and stored in the memory 16. The intermediate information may be used not only for processing by the sensor processing unit 14 but also for determination and processing by the control unit 15 and the like.

The bus 18 can be configured by IEBUS (Inter Equipment Bus), LIN (Local Interconnect Network), CAN (Controller Area Network), or the like.

The external storage device 19 stores information on the environment in which the mobile body 100 travels, including map information. The information stored in the external storage device 19 is, for example, the shape and position of a stationary object (tree, building, road, lane, signal, sign, road surface paint, roadside, etc.) in the traveling environment. Each information of the external storage device 19 may be expressed by a mathematical formula. For example, the line information does not consist of a plurality of points, but only the slope and intercept of the line. Further, the information of the external storage device 19 (for example, the type of a stationary object) may be represented by a point cloud without distinction. The point cloud may be represented by 3D (x, y, z), 4D (x, y, z, color) or the like. Finally, if the traveling environment (surrounding information) can be detected from the current position of the moving body 100 and map matching can be performed, the information of the external storage device 19 may be stored in any format.

When receiving a command to start acquisition from the control unit 15, the external storage device 19 sends information to the memory 16. When the external storage device 19 is installed in the mobile body 100, the external storage device 19 transmits / receives information to / from the memory 16 via the bus 18. On the other hand, when the external storage device 19 is not installed in the mobile body 100, the signal receiving unit 2 transmits / receives information between the position estimation device 1 and the external storage device 19. This communication can be performed by, for example, LAN (Local Area Network), WAN (Wide Area Network), or the like.

The sensor processing unit 14 processes the information acquired by the sensor 12. For example, the sensor processing unit 14 processes the information acquired by the sensor 12 while the moving body 100 is traveling to detect an obstacle. Further, the sensor processing unit 14 processes, for example, the information acquired by the sensor 12 while the moving body 100 is traveling, and recognizes a predetermined landmark.

The sensor processing unit 14 is based on the current position of the moving body 100 stored in the external storage device 19 and the memory 16 and the internal parameters of the sensor 12, and the information of the external storage device 19 is based on the information acquired by the sensor 12. (For example, point group data) is acquired.

The sensor processing unit 14 identifies a plurality of position candidates of the moving body based on the information acquired by the sensor 12, and estimates the position of the moving body 100 based on the plurality of position candidates and the moving speed of the moving body 100. ..

The sensor processing unit 14 may process the information acquired by the sensor 12 while the moving body 100 is traveling to estimate the position of the moving body 100. For example, the sensor processing unit 14 calculates the movement amount of the moving body 100 from the information acquired by the sensor 12 in time series, and adds the movement amount to the past position to estimate the current position. The sensor processing unit 14 may extract features from each of the information acquired in time series. The sensor processing unit 14 further extracts the same features from the next and subsequent information. Then, the sensor processing unit 14 calculates the movement amount of the moving body 100 by tracking the feature.

Further, the sensor processing unit 14 may perform different tasks by using the information acquired by the sensors 12a to 12n, respectively. For example, the position of the moving body 100 is estimated based on the information acquired by the sensor 12a and the sensor 12b, and obstacle detection is performed by two sensors (not shown). Finally, it suffices if the results based on the information obtained by the respective sensors 12a to 12n are fused and the moving body 100 can be controlled by the control unit 15. When the CPU of the control unit 15 can process only by a single thread, the information obtained by each of the sensors 12a to 12n is processed in the order of the sensors 12a to 12n. On the other hand, when the CPU of the control unit 15 can process in multiple threads, the information obtained by the sensors 12a to 12n is processed at the same time.

The control unit 15 outputs a command regarding the movement speed to the moving body 100 based on the result of information processing of the sensor processing unit 14. For example, the control unit 15 commands and decreases the moving speed of the moving body 100 according to the resolution of the three-dimensional object in the information, the number of outliers among the features in the information, the type of information processing, and the like. A command or a command to maintain may be output.

[Internal configuration of sensor processing unit]
Next, the configuration of the sensor processing unit 14 installed in the mobile body 100 will be described.
FIG. 2 is a block diagram showing an example of the internal configuration of the sensor processing unit 14.
The sensor processing unit 14 includes an input / output unit 141, a current position estimation unit 142, a point cloud data acquisition unit 143, a filter unit 144, a data score adjustment unit 145, a matching unit 146, an obstacle detection unit 147, and a current position correction unit 148. Has.

The input / output unit 141 inputs / outputs necessary information for correcting the position of the moving body 100 in the sensor processing unit 14. For example, the input / output unit 141 reads out the information on the traveling environment acquired by the sensors 12a, 12b, ..., 12n stored in the memory 16. Further, the input / output unit 141 acquires information from the external storage device 19 by the information extraction process (external storage device) (S5 in FIG. 3) registered in the memory 16. When the amount of information acquired from the external storage device 19 is too large to be stored in the memory 16, only the information close to the current position of the moving body 100 estimated by the current position estimation process (S3 in FIG. 3) may be acquired. Further, only the information contained in a certain area when viewed from the current position of the moving body 100 estimated by the current position estimation process may be acquired.

The current position estimation unit 142 tentatively estimates its own current position. The position estimated by the current position estimation unit 142 is not limited to the position in absolute coordinates (latitude, longitude), but may be the position in relative coordinates.

The point cloud data acquisition unit 143 (an example of the point cloud data selection unit) acquires a three-dimensional point based on the information acquired by the input / output unit 141 and temporarily records it. When the sensors 12a, 12b, ..., 12n are cameras and the information acquired by the input / output unit 141 is an image, the two-dimensional image is converted into a three-dimensional point. The conversion method may be, for example, to make each pixel of the image a three-dimensional point based on the parallax image. When the sensors 12a to 12n are monocular cameras, technologies such as SFM (Structure From Motion), Flat surface model, ORB-SLAM, and LSD-SLAM may be used. The point cloud data acquisition unit 143 calculates the shape of the traveling environment of the moving body 100 by using these technologies such as SFM and SLAM. When the sensors 12a to 12n are laser sensors, information on the traveling environment is extracted at three-dimensional points, so that no other processing is required.

The filter unit 144 filters the three-dimensional points acquired by the point cloud data acquisition unit 143. In addition, in order to reduce the processing time, the filter unit 144 may calculate the average value of the three-dimensional points within a certain area and perform the processing to narrow down to one three-dimensional point (voxel). Further, the three-dimensional points in the area of the driving environment having few features such as corners and edges may be deleted.

Further, in the filter unit 144, the visible three-dimensional point of the external storage device 19 at the temporary position of the moving body 100 estimated by the current position estimation process (step S3 in FIG. 3) is filtered. For example, in the filter unit 144, the sensor 12 provides information on the traveling environment based on the current temporary position estimated by the current position estimation unit 142 among the information (point cloud data) included in the map information stored in the external storage device 19. Is calculated, and the point cloud data within the acquireable range is selected from the external storage device 19. The filter unit 144 is an example of a point cloud data selection unit.

In the matching process (step S10 in FIG. 3) described later, if the number and shape of the information acquired by the sensors 12a, 12b, ..., 12n and the information acquired from the external storage device 19 are significantly different, the matching (matching) fails. The possibility is high. Therefore, by extracting the visible three-dimensional points of the external storage device 19 at the temporary position of the moving body 100 having a high matching probability, accurate matching can be performed in the matching process. In addition, when the matching can be performed with a predetermined accuracy in this way, it is called "matching is successful".

The data score adjustment unit 145 compares the score of the point cloud data selected from the map information with the maximum processable score Nmax (see FIG. 4), and based on the comparison result, performs the maximum processing of the score of the selected point cloud data. Adjust within the range of possible points or less. Here, the maximum processable score is the maximum score that can be processed in the maximum allowable processing time when estimating the position.

The matching unit 146 matches the three-dimensional points acquired by the point cloud data acquisition unit 143 with the visible three-dimensional points acquired by the filter unit 144. Here, the matching unit 146 matches the point cloud data after the score is adjusted by the data score adjusting unit 145 with the point cloud data of the information acquired by the sensor 12. For example, the matching unit 146 uses ICP (Iterative Closest Point) technology to perform map matching in which the information acquired by the sensor 12 is compared with the shape information of the driving environment created in advance.

The obstacle detection unit 147 processes the information acquired by the sensor 12 while the moving body 100 is traveling, and detects obstacles around the sensor 12. The filter unit 144 described above selects point cloud data from the external storage device 19 in consideration of the obstacle detection result in the obstacle detection unit 147.

The current position correction unit 148 corrects the current temporary position of the moving body 100. The current position correction unit 148 self estimated by the current position estimation unit 142 based on the matching result of the matching unit 146 (the amount of deviation between the point cloud data obtained by the sensor 12 and the point cloud data of the external storage device 19). Correct the current temporary position of. Then, the current position correction unit 148 outputs the corrected current position information to the control unit 15, and the like.

The current position correction unit 148 transmits the corrected current position information to the input / output unit 141, and the input / output unit 141 transmits the corrected current position information according to the command of the control unit 15 to the memory 16. Further, the input / output unit 141 may directly transmit the current position information corrected by the current position correction unit 148 to the control unit 15 by a command of the control unit 15.

The matching unit 146 described above may also serve as the current position estimation unit 142 and / or the obstacle detection unit 147.

Further, in the above description, according to the description of FIG. 2, the input / output unit 141, the current position estimation unit 142, the point cloud data acquisition unit 143, the filter unit 144, the data score adjustment unit 145, the matching unit 146, and the obstacle detection unit Although 147 and the current position correction unit 148 are included in the sensor processing unit 14, the present invention is not limited thereto. The input / output unit 141, the current position estimation unit 142, the point cloud data acquisition unit 143, the filter unit 144, the data point adjustment unit 145, the matching unit 146, the obstacle detection unit 147, and the current position correction unit 148 are used as the sensor processing unit 14. It may be an independent component that is not included.

[Information processing by sensor processing unit]
Next, the procedure of information processing executed by the sensor processing unit 14 will be described with reference to FIG.
FIG. 3 is a flowchart showing an example of an information processing procedure executed by the sensor processing unit 14.

First, the sensor processing unit 14 executes a sensor acquisition range recording process for recording the acquisition range of the sensor 12 in the memory 16 (S1). For example, the maximum angle (angle of view) that can be acquired in the horizontal and vertical directions from the center of the sensor 12 is recorded in the memory 16. Further, since the accuracy of the information acquired by the sensor 12 depends on the distance to the object, the maximum distance (range) in which the accuracy does not deteriorate from the predetermined value is recorded in the memory 16. This process may be performed in advance before the sensor processing unit 14 starts this flowchart.

Next, the sensor processing unit 14 executes a maximum score recording process for recording the maximum score calculated based on the calculation speed of the information processing apparatus 13 in the memory 16 (S2). The details of this maximum score recording process will be described later with reference to FIG.

Next, the current position estimation unit 142 executes the current position estimation process for estimating the current temporary position based on the information received from the signal reception unit 2 (S3). Further, the current position estimation process may estimate the current temporary position based on the information acquired by the sensor 12 such as odometry and landmark matching. In the landmark matching, the information acquired by the sensor 12 and the landmark information acquired from the external storage device 19 are collated. Further, the current position estimation process may be a fusion result of the various position estimation methods described above (odometry, landmark matching, information received from the signal receiving unit 2, etc.). For example, the current position may be predicted based on the result of fusing various position estimation methods with a Kalman filter. Finally, any sensor or method may be used as long as the current temporary position of the moving body 100 can be estimated.

Next, the point cloud data acquisition unit 143 executes an information extraction process (sensor) for extracting information (surrounding information) of the traveling environment of the moving body 100 by the sensor 12 (S4). For the sake of simplicity, in this embodiment, the information extracted by this information extraction process (sensor) is used as a point cloud. Not limited to this example, a method of using a mathematical formula (function, curve) or a color (image matching) as information can be considered.

Next, the point cloud data acquisition unit 143 extracts information around the moving body 100 from the external storage device 19 based on the current temporary position obtained in the current position estimation process (S3) (external storage). Device) is executed (S5). For the sake of simplicity, in this embodiment, the information extracted by this information extraction process (external storage device) is set as a point cloud, and the extracted points are taken as N.

Next, the sensor processing unit 14 performs a memory reference process for extracting the maximum score (maximum processable score) and the sensor acquisition range recorded in the maximum score recording process (S2) and the sensor acquisition range recording process (S1) from the memory 16. Execute (S6). Here, the maximum number of points that can be processed is Nmax.

Next, the data score adjustment unit 145 executes a score confirmation process for comparing the score N of the point cloud with the maximum processable score Nmax (S7). Here, the data score adjusting unit 145 proceeds to the score increasing process in step S8 when N <Nmax, and proceeds to the score reducing process in step S9 when N ≧ Nmax.

Next, the data score adjusting unit 145 increases the score N to the maximum processable score Nmax when N <Nmax (S8). The details of this score increase process will be described later with reference to FIG.

Further, the data score adjusting unit 145 reduces the score N to the maximum processable score Nmax when N ≧ Nmax (S9). The details of this score reduction process will be described later with reference to FIG.

Next, after the processing of step S8 or S9, the matching unit 146 performs the information (point group data) extracted by the sensor 12 in the information extraction processing (sensor) of step S4 and the information extraction processing (external storage device) of step S5. The information (point group data) extracted from the external storage device 19 is matched (S10).

Next, the obstacle detection unit 147 executes an obstacle detection process for detecting the three-dimensional shape of an obstacle (another vehicle, a pedestrian, an obstacle to movement, etc.) around the moving body 100 and its position (the obstacle detection unit 147). S11).

In the obstacle detection process, an obstacle is detected based on the information of the driving environment acquired by the sensor 12. For example, when the sensor 12 is an image acquisition device (camera), the obstacle detection unit 147 detects an obstacle by using an image processing technique or a deep learning technique. Further, in the obstacle detection process, detection may be performed based on the information received by the signal receiving unit 2. For example, an obstacle is detected by a surveillance camera installed in the traveling environment of the mobile body 100, and the detection result and the position of the surveillance camera with respect to the mobile body 100 are transmitted to the signal receiving unit 2. Finally, it suffices to detect an obstacle that may hinder the matching between the information extracted by the information extraction process (sensor) in step S4 and the information extracted by the information extraction process (external storage device) in step S5. ..

Next, the matching unit 146 executes the matching process in step S10, and then executes the unmatchable area calculation process for calculating the information and the area that could not be matched due to an obstacle or the like (S12). The details of this unmatchable region calculation process will be described later with reference to FIGS. 5 and 6.

Next, the matching unit 146 executes a matching load area recording process of recording the unmatched area calculated in the matching non-matchable area calculation process of step S12 in the memory 16 (S13).

Next, the current position correction unit 148 executes the current position correction process for correcting the current position tentatively estimated by the current position estimation process in step S3 using the result of the matching process in step S10 (S14).

Next, the sensor processing unit 14 determines whether or not to end a series of processes in this flowchart (S15). The determination criteria are, for example, the number of predetermined position estimations, the mileage of the moving body 100, the current position of the moving body 100, and the end command received by the signal receiving unit 2. When the end determination is made (YES in S15), the sensor processing unit 14 ends a series of processes in this flowchart. If the end determination is not made (NO in S15), the sensor processing unit 14 returns to the current position estimation process in step S3.

[Maximum score recording process (maximum score calculation)]
Here, the details of the calculation of the maximum score in the maximum score recording process in step S2 will be described with reference to FIG.

FIG. 4 is a graph showing a method for calculating the maximum score in the maximum score recording process in step S2. In FIG. 4, the function F400 is a function representing the relationship between the score of the point cloud and the processing time (ms) at the score.

The calculation speed of the information processing apparatus 13 depends on the score extracted by the information extraction process (sensor) in step S4 and the score extracted by the information extraction process (external storage device) in step S5. To obtain the function F400, first execute the matching process of step S10 with a plurality of different points, calculate the time required for each point (hereinafter referred to as "processing time"), and combine each point and the processing time 401 (plot). Data) is calculated. Next, a polynomial or a spline is applied to the plurality of obtained combinations 401 to perform interpolation processing. In order to accurately obtain the function F400, it is desirable to calculate the combination 401 with the information processing apparatus 13 used when the position estimation method of the present embodiment is actually executed.

If it is difficult for the information processing apparatus 13 to calculate the combination 401 when the position estimation method of the present embodiment is actually executed, the function (F') is calculated by another information processing apparatus. Then, the function F = f (F', S) is based on the difference S in the calculation speed between the information processing apparatus 13 when the position estimation method of the present embodiment is actually executed and another information processing apparatus. May be asked.

Maximum processing time Tmax402 is the maximum processing time that can be tolerated when performing position estimation. In order to realize highly accurate position estimation, it is necessary to carry out position estimation within a predetermined maximum cycle, and the maximum processing time Tmax402 is calculated based on the predetermined maximum cycle. For example, when the maximum period required to execute the position estimation method of the present embodiment is 10 Hz, Tmax = 100 ms. The maximum processable score Nmax403 is a score obtained when the maximum processing time Tmax402 is substituted into the function F400. In FIG. 4, the function F is obtained and stored in advance, but the present invention is not limited to this example. For example, a reference table in which the relationship between the score of the point cloud and the processing time at that score is registered may be created, and the processing time may be obtained by interpolation for the score not in the reference table.

[Unmatchable area calculation process and non-matching area recording process]
Next, the non-matching area calculation process in step S12 and the non-matching area recording process in step S13 will be described with reference to FIGS. 5 and 6.

(When there are no obstacles)
First, a case where there is no obstacle around the moving body 100 will be described with reference to FIG.
FIG. 5 is a diagram showing an example (without obstacles) of the non-matching area calculation process and the non-matchable area recording process. The alternate long and short dash line indicates the range in which the sensor 12 installed in the moving body 100 can acquire information (for example, the shooting range (angle of view) of the camera). In addition, in FIG. 5, the information acquisition range of the sensor 12 is shown in a state where the moving body 100, the landmark 500a, and the landmark 500b are overlooked.

In FIG. 5, the landmark 500a and the landmark 500b are in the traveling environment (surroundings) of the moving body 100. The fact that the object is in the traveling environment can be rephrased as the object is included in the information acquisition range of the sensor 12.

The information 501 is information (for example, a point cloud) extracted from the landmarks 500a and 500b by the information extraction process (sensor) (S4 in FIG. 3) at the current position (provisional) of the moving body 100.
The information 502 is information on the traveling environment (for example, a point cloud) extracted from the external storage device 19 by the information extraction process (external storage device) (S5) based on the current position (provisional) of the moving body 100.

The matching result 503 is a result obtained by executing a matching process (S10) using the information 501 acquired by the sensor 12 and the information 502 extracted from the external storage device 19. In the example of FIG. 5, since there are no obstacles such as other vehicles and pedestrians around the moving body 100, the matching of the information 501 and the information 502 can be performed well. Therefore, there is no unmatchable area.

(If there are obstacles)
Next, the case where there is an obstacle around the moving body 100 will be described with reference to FIG.
FIG. 6 is a diagram showing an example (with obstacles) of the non-matching area calculation process and the non-matchable area recording process.

In the example of FIG. 6, the information of the landmark 500b cannot be extracted due to the influence of the obstacle 610 (for example, a passerby) existing in the information acquisition range of the sensor 12 indicated by the alternate long and short dash line, and the information extraction process (sensor) (S4). The information extracted in is information 501b. The information 501b includes information obtained from the landmark 500a and information on the obstacle 610.

Therefore, when the matching process (S10) is performed using the information 501b acquired by the sensor 12 and the information 502 extracted from the external storage device 19, the matching result 503b is output. Among the information 502 extracted by the information extraction process (external storage device) (S5) from the external storage device 19, there is information that could not be matched. The matching unit 146 represents a plurality of three-dimensional positions of the information that could not be matched in the non-matching region calculation process (S12) by the three-dimensional region 504b (non-matching region) shown by the alternate long and short dash line. Then, the matching unit 146 cannot match the information of the position (for example, the distance from the moving body 100 or the absolute position), the length (depth), the height (H), and the width (W) of the three-dimensional region 504b. Recording is performed in the memory 16 in the area recording process (S13).

For the sake of simplicity, the three-dimensional region 504b has been described as a cube, but the three-dimensional region 504b may be represented by a sphere or a cylinder. Finally, the three-dimensional region 504b may have any three-dimensional shape as long as it contains information that could not be matched. Further, the three-dimensional region 504b may be obtained by using the obstacle detection process (S11) by the obstacle detection unit 147. The three-dimensional shape and position of the three-dimensional region 504b are set based on the shape and position of the obstacle 610 output from the obstacle detection process. Specific examples of the unmatchable region calculation process (S12) by the obstacle detection process will be described later with reference to FIGS. 10 and 11.

For the sake of simplicity, the matching non-matching area calculation process (S12) was described using a moving object (obstacle 610), but the matching process (S10) may fail due to a cause other than the obstacle. For example, the environment may change depending on the construction work or the season, and the environment may differ between when the information is registered in the external storage device 19 and when matching. Therefore, when the information extracted by the information extraction process (sensor) (S4) and the information extracted by the information extraction process (external storage device) (S5) are matched by the matching process (S10), there is a region that cannot be matched. As described above, it is possible to detect a region that could not be matched by the present invention.

[Score reduction processing]
Next, the details of the score reduction process in step S9 will be described with reference to FIG. 7.
FIG. 7 is a diagram showing details of the score reduction process by the data score adjusting unit 145.
In FIG. 7, the landmarks 701a, 701b, and 701c are landmarks in the traveling environment (surroundings) of the moving body 100, and the information (point cloud data) of each landmark is registered in the external storage device 19.

Information 702 is information extracted from the external storage device 19 based on the current position of the mobile body 100. Here, for the sake of simplicity, the information 702 is used as a point cloud, and the score N of the point cloud included in the information 702 is assumed to be larger than the maximum processable score Nmax403 (see FIG. 4) (N> Nmax).

Since the score N of the information 702 is larger than the maximum processable score Nmax403, the processing cannot be completed within the maximum processing time Tmax402. Therefore, it is necessary to reduce the score N of the information 702 to the maximum processable score Nmax403. Here, the score is randomly reduced, or the score is reduced by using voxels. Further, only the information that can be matched at the current position of the moving body 100 may be left, and other information may be reduced. Finally, any method may be used as long as the score N becomes smaller than the maximum processable score Nmax403.

Information 703 shows information (N = Nmax) after reducing the score N of the information 702 extracted from the external storage device 19 to the maximum processable score Nmax403. In the information 703, the score of the point cloud of the landmarks 701a, 701b, 701c included in the information 702 is reduced.

[Point increase processing]
Next, the details of the score increasing process in step S8 will be described with reference to FIG.
FIG. 8 is a diagram showing details of the score increasing process by the data score adjusting unit 145.
In FIG. 8, the information 702b is information extracted from the external storage device 19 based on the current position of the mobile body 100. Here, for the sake of simplicity, the information 702b is used as a point cloud, and the score Nb of the point cloud included in the information 702b is assumed to be larger than the maximum processable score Nmax403 (see FIG. 4) (Nb> Nmax).

The area 704 is a non-matching area calculated by the non-matching area calculation process in step S11.
The information 703b is information after the score Nb of the information 702b extracted from the external storage device 19 is reduced to the maximum processable score Nmax403, and the score is Nb'(Nb'= Nmax).

On the other hand, the information 702b includes the area 704 calculated by the unmatchable area calculation process in step S12. The information contained in this area 704 is not used when the matching process (S10) is performed. Therefore, the information (point cloud) in the area 704 included in the information 703b is deleted, and the score Nb'becomes the score Nc (Nc <Nb'). Since Nc <Nmax, the processing time is shorter than the maximum processing time Tmax402 when the matching processing (S10) is performed.

Therefore, on the contrary, it is possible to increase the score Nc to the maximum processable score Nmax. Therefore, in the score increasing process (S8), a point not selected outside the area 704 is selected from the information 702b, and the score is increased from the information 703b. Information 705 is information after increasing the score Nc to the maximum processable score Nmax403. Further, when the point cloud is increased by the score increase processing (S8), the data score adjustment unit 145 increases the point cloud by searching for a random, voxel, or matching point cloud based on the same criteria as the score reduction processing (S9). .. The shaded point cloud in the information 705 is the point cloud increased from the information 703b.

[Example of self-position estimation processing]
Next, a specific example of the self-position estimation process by the sensor processing unit 14 will be described with reference to FIGS. 9 to 14.

FIG. 9 is a diagram showing a first example of self-position estimation processing by the sensor processing unit 14. For the sake of simplicity, it is assumed that the moving body 100 is not moving.

In FIG. 9, the landmarks 901a and the landmarks 901b are landmarks in the traveling environment (surroundings) of the moving body 100. Information (point cloud data) of each landmark is registered in the external storage device 19.
The information 902a is information on the landmarks 901a and the landmarks 901b extracted by the information extraction process (sensor) (S4) at the current temporary position of the moving body 100. For the sake of simplicity, information 902a is used as a point cloud.

Information 903a is information on the traveling environment of the mobile body 100 extracted from the external storage device 19 by the information extraction process (external storage device) (S5) based on the current temporary position of the mobile body 100. The score of the point cloud included in the information 903a is defined as the score Na. The matching unit 146 refers to the maximum processable score Nmax403 registered in the memory 16 in advance in the memory reference process (S6), and compares the score Na with the maximum processable score Nmax403 in the score confirmation process (S7). For the sake of explanation, Na <Nmax is set in the example of FIG.

The matching result 904a is a result of matching the information 902a acquired by the sensor 12 in the matching process (S10) with the information 903a extracted from the external storage device 19. The current position correction unit 148 corrects the current position tentatively estimated in the current position correction process (S14) based on the matching result 904a. Further, in the example of FIG. 9, it is assumed that there is no region that could not be matched (matching OK).

FIG. 10 is a diagram showing a second example of self-position estimation processing by the sensor processing unit 14, and shows a state at the next timing of FIG. The timing interval is, for example, a cycle in which the sensor processing unit 14 executes information processing.

In FIG. 10, the obstacle 1005 is an obstacle within the acquisition range of the sensor 12 installed in the moving body 100.
The information 902b is information on the landmark 901a and the obstacle 1005 extracted by the information extraction process (sensor) (S4) at the current temporary position of the moving body 100.

Information 903b is information on the traveling environment of the mobile body 100 extracted from the external storage device 19 by the information extraction process (external storage device) (S5) based on the current temporary position of the mobile body 100. It is assumed that the point cloud score Nb included in the information 903b is smaller than the maximum processable score Nmax403 (Nb <Nmax).

The matching result 904b is a result of matching the information 902b acquired by the sensor in the matching process (S10) with the information 903b extracted from the external storage device 19. In this case, there is a region that could not be matched due to the influence of the obstacle 1005, and the corresponding region 906b is calculated by the matching non-matchable region calculation process (S12). Therefore, information such as the shape and position of the area 906b is registered in the memory 16 in the non-matching area recording process (S13) (matching NG).

FIG. 11 is a diagram showing a third example of self-position estimation processing by the sensor processing unit 14, and shows a state at the next timing of FIG.

In FIG. 11, the information 902c is information on the landmark 901a and the obstacle 1005 extracted by the information extraction process (sensor) (S4) at the current temporary position of the moving body 100. That is, the information 902c is the same as the information 902b.

Information 903c is information on the traveling environment of the mobile body 100 extracted from the external storage device 19 by the information extraction process (external storage device) (S5) based on the current temporary position of the mobile body 100. It is assumed that the score Nc of the point cloud included in the information 903c is smaller than the maximum processable score Nmax403 (Nc <Nmax).

The data score adjustment unit 145 refers to the area 906b (non-matching area) registered in the memory 16 in the memory reference process (S6). The matching unit 146 performs the matching process (S10) at the previous timing, and then outputs the area 906b in the matching non-matching area calculation process (S12). Therefore, the data score adjustment unit 145 deletes the information in the area 906b in the score reduction process (S9) before performing the matching process this time. As a result, the score Nc of the information 903c is reduced to the score Nc'. Since Nc'<Nmax, the point cloud outside the region 906b is increased to the maximum processable point Nmax403, and the matching process is performed. Therefore, as shown by the matching result 904c, the influence of the obstacle 1005 is reduced, and the matching process can be completed accurately and within the maximum processing time Tmax402.

FIG. 12 is a diagram showing a fourth example of self-position estimation processing by the sensor processing unit 14, and shows a state at the next timing of FIG. At this timing, the passerby as the obstacle 1005 is moving from the inside to the outside of the information acquisition range of the sensor 12.

In FIG. 12, the information 902d is the information of the landmarks 901a and the landmarks 901b extracted by the information extraction process (sensor) (S4) at the current temporary position of the moving body 100. That is, the information 902d is the same as the information 902a in FIG.

Information 903d is information on the traveling environment of the mobile body 100 extracted from the external storage device 19 by the information extraction process (external storage device) (S5) based on the current temporary position of the mobile body 100. It is assumed that the score Nd of the point cloud included in the information 903d is smaller than the maximum processable score Nmax403 (Nd <Nmax).

Similar to the timing of FIG. 11, the data score adjusting unit 145 deletes the information in the area 906b (non-matching area) in the score reduction process (S9) before performing the matching process (S10), and the information 903d. The score Nd of is reduced to the score Nd'. Further, since Nd'<Nmax, the point cloud outside the region 906b is increased to the maximum processable point Nmax403, and the matching process is performed. In this case, as shown in the matching result 904d, the information of the landmark 901b in the information 902d does not match the information obtained by deleting the area 906 from the information 903d.

On the other hand, at this timing, since the obstacle 1005 is out of the information acquisition range of the sensor 12, the area 906b becomes unnecessary. Hereinafter, a method of deleting the area 906b from the information of the external storage device 19 will be described with reference to FIG.

FIG. 13 is a diagram showing a fifth example of self-position estimation processing by the sensor processing unit 14, and shows a state at the next timing of FIG. The passerby as the obstacle 1005 remains out of the information acquisition range of the sensor 12.

In FIG. 13, the information 902e is the information of the landmarks 901a and the landmarks 901b extracted by the information extraction process (sensor) (S4) at the current temporary position of the moving body 100. That is, the information 902e is the same as the information 902d in FIG.

The information 902e is the same as the information 902d in FIG.
The information 903e is information on the traveling environment of the mobile body 100 extracted from the external storage device 19 by the information extraction process (external storage device) (S5) based on the current temporary position of the mobile body 100. It is assumed that the point cloud score Ne included in the information 903e is smaller than the maximum processable score Nmax403 (Ne <Nmax).

There is a probability that the obstacle 1005 that was within the information acquisition range of the sensor 12 will be out of the information acquisition range of the sensor 12 in chronological order. On the other hand, since there is a probability that the obstacle 1005 remains within the information acquisition range of the sensor 12, the area 906b (unmatchable area) is not deleted at this timing, and the unmatchable area calculation process is performed in chronological order (stepwise). In S12), the size of the region 906b is reduced. In the case of FIG. 13, the obstacle 1005 is continuously out of the information acquisition range of the sensor 12 from FIG.

The area 906e represents an area after the area 906b is reduced. In the score reduction process (S9), the information in the area 906e (matching impossible area) is deleted, and the score Ne of the information 903e is reduced to the score Ne'. Then, the data score adjusting unit 145 considers the area 906e (the score of the included point cloud), increases the score Ne of the information 903e to the maximum processable score Nmax403 by the score increasing process (S8), and then the matching section 146. The matching process (S10) is performed in. After that, the matching result 904e is obtained. As a result, on the outside of the region 906e, matching is possible even at the location where the region 906b was.

When the area is made smaller in the unmatchable area calculation process (S12), it is made smaller by using a constant parameter K (0 <K <1) in time series. For example, assuming that the length, height, and width of the region 906b are Lb, Hb, and Wb, the size of the region 906e is set to Lb * K, Hb * K, and Wb * K in the unmatchable region calculation process (S12). do. Further, when the area 906e becomes smaller in time series and the area 906e does not contain any one point of the information 903e, the area 906e is deleted. The shape of the region 906b is not limited to a rectangle.

FIG. 13 shows an example in which the obstacle 1005 is out of the information acquisition range of the sensor 12 as expected, but the case where the obstacle 905 remains within the information acquisition range of the sensor 12 will be described. In this case, after the matching process (S10) is performed, the region 906e that could not be matched is detected by the matching non-matching region calculation process (S12), so that the region 906e is newly increased by the matching non-region calculation process. As an example, the area 906e may be increased to about the area 606.

FIG. 14 is a diagram showing a sixth example of self-position estimation processing by the sensor processing unit 14, and shows a state at the next timing of FIG.

In FIG. 14, the information 902f is the information of the landmarks 901a and the landmarks 901b extracted by the information extraction process (sensor) (S4) at the current temporary position of the moving body 100. That is, the information 902f is the same as the information 902e in FIG.

Information 903f is information on the traveling environment of the mobile body 100 extracted from the external storage device 19 by the information extraction process (external storage device) (S5) based on the current temporary position of the mobile body 100. It is assumed that the score Nf of the point cloud included in the information 903f is smaller than the maximum processable score Nmax403 (Nf <Nmax).

At this timing, it is assumed that the area 906e (FIG. 13) excluded from the matching process becomes smaller in time series and completely disappears. Therefore, the information 903f including the landmark 901a and the point cloud corresponding to the landmark 901b and the information 902f acquired by the sensor 12 are matched in the matching process of step S10, and the matching result 904f is output.

Although it is not essential to display the result obtained by the matching process, it may be displayed by the display unit 17 for debugging or user reference. Further, not only the result of the matching process but also the score at the time of the matching process, the maximum processable score Nmax403 registered in the memory 16, the result of the unmatchable area calculation process, and the like may be displayed.

[Point group selection criteria when reducing points]
Next, the selection criteria of the point cloud when the score reduction process of step S9 is performed will be described with reference to FIGS. 15 and 16.

FIG. 15 is a diagram illustrating a selection criterion of point cloud data when the data score adjusting unit 145 performs the score reduction process (S9). The upper side of FIG. 15 shows an example of the positional relationship between the sensor 12 and the landmark, and the lower side of FIG. 15 shows a point cloud extracted from the external storage device 19. For the sake of simplicity, it is assumed that the moving body 100 is not moving.

Coordinates 1500 are coordinates (x, y, z) based on the sensor 12 installed on the moving body 100. At coordinates 1500, the direction in which the sensor 12 is facing (the same as the traveling direction of the moving body 100 in this example) is the z-axis, and the direction orthogonal to the z-axis is the y-axis, y-axis, and the height direction of the landmark. The direction orthogonal to the z-axis is defined as the x-axis.

The landmark 1501 is a landmark in the traveling environment of the moving body 100.
The landmark 1502 is a landmark in the traveling environment of the moving body 100, and is farther from the moving body 100 on the z-axis than the landmark 1501. For the sake of simplicity, the positions of the moving body 100, the landmark 1501 and the landmark 1502 in FIG. 15 are fixed with respect to the x-axis. Landmarks 1501 and 1502 are included in the information acquisition range of the sensor 12.

The point cloud 1503 is information (point cloud) of the landmarks 1501 and the landmarks 1502 registered in the external storage device 19.
The point cloud 1504 is information on the traveling environment of the moving body 100 (point group) extracted from the external storage device 19 by an information extraction process (external storage device) (S5 in FIG. 3) based on the current temporary position of the moving body 100. ). It is assumed that the score of the point cloud included in the point cloud 1504 is Nt, and the score Nt is larger than the maximum processable score Nmax403 (Nt> Nmax).

Here, the priority according to the distance will be described as a selection criterion of the point cloud when the score increase process (S8) and the score decrease process (S9) are performed.

First, the point cloud that is likely to match the point cloud registered in the external storage device 19 when extracted from the current position of the moving body 100 by the sensor 12 is preferentially selected. For example, in the case of a camera or a laser sensor, the sensor 12 extracts (acquires) information from the surface (boundary line) of the three-dimensional object, so that even if there is information on the back side of the three-dimensional object, it cannot be extracted. Therefore, in the score reduction process, the point cloud 1505 (indicated by a black circle) on the front side (current position side) of the landmark 1501 and the point cloud on the front side of the landmark 1502, which can be easily extracted (referenced) from the current position of the moving body 100, are used. 1506 (indicated by black circles) is extracted, and the point cloud 1505b and the point cloud 1506b (indicated in gray) that are difficult to extract are deleted. As a result, the score Nt is reduced to the score Nt'(Nt'<Nmax). Therefore, by using the point cloud 1505 and the point cloud 1506 that represent the boundary line between the landmark 1501 and the landmark 1502, the matching process can be performed accurately and with a low load.

On the other hand, even if the point group 1504 of the driving environment extracted from the external storage device 19 in the information extraction process (external storage device) (S5) is reduced by the score reduction process, the points representing the boundary line between the landmark 1501 and the landmark 1502. There is a case where the total score Nt'of the group 1505 and the point group 1506 is larger than the maximum processable score Nmax403 (Nt'> Nmax). The correspondence in such a case will be described.

The accuracy of the sensor 12 installed on the mobile body 100 depends on the resolution and calibration. For example, in the case of a laser sensor, since the distance from the laser sensor to the object is measured by the TOF principle, the accuracy of the measured distance deteriorates due to the influence of the reflectance of the object and the reflection (noise) of the environment. Further, since the laser sensor has a limited resolution and information acquisition range, the measurement accuracy of a distant object or an object having a certain range (distance) or more deteriorates. In the case of a camera, the measurement accuracy varies greatly depending on the resolution of the image and the algorithm used when creating the parallax image. Basically, the farther the object is from the camera, the worse the measurement accuracy.

There are external parameters and internal parameters in the calibration. The external parameter is the fixed position of the sensor. Internal parameters include the lens arrangement (distortion) in the sensor, the adjustment parameters of the parallax image, and the like.

Therefore, when it is necessary to further reduce the score Nt', the information far from the current position of the moving body 100 on the z-axis is deleted in the score reduction process. For example, the point cloud 1506 on the surface of the landmark 1502 is deleted, and the matching process is performed leaving the point cloud 1505 on the surface of the landmark 1501 closer to the moving body 100. For the sake of simplicity, the score of the point cloud 1505 is set to be smaller than the maximum processable score Nmax403, but if it is necessary to further reduce the score of the point cloud 1505, it is moved more from the point cloud 1505 as described above. The point cloud far from the body 100 is preferentially deleted.

Further, although the case of the score reduction process has been described, even if the score needs to be adjusted in the score increase process, the point cloud closer to the moving body 100 is preferentially increased and the matching process is performed.

FIG. 16 is a diagram showing an example of self-position estimation processing using the point cloud selected in FIG.
The point cloud 1508 is a point cloud of the landmark 1502 extracted by the information extraction process (sensor) (S4) from the current position of the moving body 100 at the timing t0.
The point cloud 1509 is a point cloud of the landmark 1501 extracted by the information extraction process (sensor) (S4) from the current position of the moving body 100 at the timing t0.

It is assumed that the score Nt of the point cloud 1504 in the driving environment extracted from the external storage device 19 in the information extraction process (external storage device) (S5) is larger than the maximum processable score Nmax403. Further, it is assumed that the point cloud 1505 and the point cloud 1506 representing the boundary line between the landmark 1501 and the landmark 1502 are extracted by the score reduction process (S9). Further, it is assumed that the total score Nt'of the point cloud 1505 and the point cloud 1506 is still larger than the maximum processable score Nmax403. In this case, the point cloud 1505 that is preferentially left closer to the moving body 100 is preferentially left in the point reduction process, and the point cloud 1506 that is farther than the point cloud 1505 from the moving body 100 is deleted, and the matching process is performed.

Next, at the timing t1, the obstacle 1601 (for example, a passerby) enters the information acquisition range of the sensor 12, and the point cloud 1508b of the landmark 1502 is subjected to the information extraction process (sensor) (S4) from the current position of the moving body 100. And, the point cloud 1601b of the obstacle 1601 is extracted. Then, before the information extraction process (external storage device) (S5) extracts the driving environment information from the external storage device 19, the obstacle detection process (S11) detects the obstacle 1601 and the matching impossible area calculation process (S5). It is assumed that the region 1510 (non-matching region) representing the obstacle 1601 is calculated in S12). Next, the information extraction process (external storage device) (S5) extracts the point cloud 1504 of the traveling environment from the external storage device 19.

Here, since the score Nt is larger than the maximum processable score Nmax403, the score Nt is reduced to the maximum processable score Nmax403 in the score reduction process (S9). However, since the point cloud 1505 (lower side in FIG. 16) close to the moving body 100 is included in the region 1510 with reference to the region 1510 representing the obstacle 1601 registered in the memory 16, the point cloud 1505 is selected. Is impossible. Therefore, the point cloud 1506 that represents the boundary line of the distant landmark 1502 is selected and the matching process is performed.

The lower part of FIG. 16 shows the case where the obstacle 1601 is not within the information acquisition range of the sensor 12 at the next timing t2. Before the information extraction process (external storage device) (S5) extracts the driving environment information from the external storage device 19, the obstacle detection process (S11) detects the obstacle, but the obstacle 1601 disappears and the obstacle is detected. There is no output of detection processing. Therefore, the non-matching area is not output in the non-matching area calculation process (S12).

The information extraction process (external storage device) (S5) extracts the point cloud 1504 of the driving environment from the external storage device 19, and since the score Nt is larger than the maximum processable score Nmax403, the score is reduced by the score reduction process (S9). .. However, unlike the timing t1, there is no unmatchable area registered in the memory 16, so the point cloud 1505 of the landmark 1501 close to the moving body 100 is left, and the point cloud 1506 of the landmark 1502 farther away is deleted. do.

Finally, the point cloud 1505 of the landmark 1501 extracted from the external storage device 19, and the point cloud 1508c and the landmark 1501 of the landmark 1502 extracted by the information extraction process (sensor) (S4) from the current position of the moving body 100. Matching processing is performed using the point cloud 1509c.

As described above, the self-position estimation device (position estimation device 1) according to the embodiment is self-estimating the self-position by comparing the driving environment information collected by the sensor (sensor 12) with the map information. It is a position estimation device. This self-position estimation device has a current position estimation unit (current position estimation unit 142) that temporarily estimates its current position, and a current temporary position estimation unit that estimates the point cloud data included in the map information. The point cloud data selection unit (filter unit 144) that calculates the range in which the sensor can acquire information based on the position and selects the point cloud data in the acquireable range, the score of the point cloud data selected from the map information, and Compare with the maximum processable score, which is the maximum score that can be processed in the maximum allowable processing time when estimating the position, and based on the comparison result, the score of the selected point cloud data is less than or equal to the maximum processable score. Matching between the point cloud adjustment unit (data point cloud adjustment unit 145) that adjusts within the range, the matching unit (matching unit 146) that matches the adjusted point cloud data with the point cloud data of the information acquired by the sensor, and the matching unit. Based on the result, it includes a current position correction unit (current position correction unit 148) that corrects its own current temporary position estimated by the current position estimation unit.

According to the self-position estimation device configured as described above, the points of the point cloud data selected from the map information can be dynamically adjusted within the range of the maximum processable points or less corresponding to the maximum processing time, so that the position is estimated. It is possible to estimate the position by optimizing the accuracy and processing load of the.

Further, in the self-position estimation device of the present embodiment, when the score of the selected point cloud data is larger than the maximum processable score Nmax, the score adjustment unit (data score adjustment unit 145) of the selected point cloud data. It is configured to reduce the score to the maximum processable score Nmax.

According to the above configuration, the processing load can be reduced by reducing the number of points of the selected point cloud data to the maximum number of points that can be processed Nmax.

Further, in the self-position estimation device of the present embodiment, the point cloud adjustment unit (data point cloud adjustment unit 145) collects the selected point cloud data when the score of the selected point cloud data is less than the maximum processable point cloud Nmax. It is configured to increase the score up to the maximum processable score Nmax.

According to the above configuration, the accuracy of position estimation can be improved by increasing the number of points of the selected point cloud data to the maximum number of points that can be processed Nmax.

Further, in the self-position estimation device of the present embodiment, the matching unit (matching unit 146) stores the point cloud data (for example, the information of the area 906b) that could not be matched, and the stored point cloud data is used for the next position estimation. At the time of, it is configured not to select from the point cloud data included in the map information.

According to the above configuration, since the point cloud data that could not be matched is not selected from the point cloud data included in the map information at the next position estimation, the influence of the point cloud data that could not be matched is eliminated and the robustness is achieved. Can be improved.

Further, in the self-position estimation device of the present embodiment, the point cloud adjustment unit (data point cloud adjustment unit 145) is currently used among the selected point cloud data when reducing the points of the selected point cloud data to the maximum processable point cloud Nmax. It is configured to preferentially reduce point cloud data (eg, point cloud 1506) farther from the temporary position of.

According to the above configuration, by preferentially reducing the point cloud data farther from the current temporary position, it is possible to reduce the processing load and minimize the decrease in the accuracy of the position estimation.

Further, in the self-position estimation device of the present embodiment, the point cloud adjustment unit (data point cloud adjustment unit 145) is currently used among the selected point cloud data when increasing the points of the selected point cloud data to the maximum processable point cloud Nmax. It is configured to preferentially increase the point cloud data (for example, the point cloud data 1505) closer to the temporary position of.

According to the above configuration, by preferentially increasing the point cloud data closer to the current temporary position, it is possible to improve the accuracy of the position estimation while minimizing the increase in the processing load.

Further, in the self-position estimation device of the present embodiment, the maximum processable score Nmax is a score calculated based on the correspondence relationship (for example, function F) between the different score obtained in advance and the processing time at the score. be. In this way, by obtaining the relationship between the score and the processing time in advance, the maximum processable score Nmax that changes in time series can be calculated at any time.

Further, in the self-position estimation device of the present embodiment, the matching unit (matching unit 146) is configured to calculate and store the position and shape of the region (for example, region 906b) including the point cloud data that could not be matched. ing. By storing the position and shape of the area including the point cloud data that could not be matched in this way, the corresponding point cloud data can be excluded in the next matching.

Further, in the self-position estimation device of the present embodiment, the matching unit (matching unit 146) determines the size of the region that could not be matched in time series with respect to the map information according to the matching result obtained in time series. It is configured to adjust and store the points of the point cloud data (for example, reduced from the area 906b to the area 906e).

According to the above configuration, the continuity of the current position of the time series is maintained by adjusting the area or point cloud data that could not be matched in the time series and reflecting it in the matching process of the time series.

It should be noted that the present invention is not limited to the above-described embodiment, and it goes without saying that various other application examples and modifications can be taken as long as the gist of the present invention described in the claims is not deviated. For example, the above-described embodiment is a detailed and specific description of the configuration of the position estimation device and the moving body in order to explain the present invention in an easy-to-understand manner, and is not necessarily limited to the one including all the components described above. .. It is also possible to add, replace, or delete other components with respect to a part of the configuration of one embodiment.

Further, each of the above configurations, functions, processing units, etc. may be realized by hardware, for example, by designing a part or all of them by an integrated circuit. As hardware, FPGA (Field Programmable Gate Array), ASIC (Application Specific Integrated Circuit), or the like may be used.

Further, in the flowchart shown in FIG. 3, a plurality of processes may be executed in parallel or the processing order may be changed as long as the processing results are not affected.

Further, in the above-described embodiment, the control lines and information lines are shown as necessary for explanation, and not all the control lines and information lines are necessarily shown in the product. In practice, it can be considered that almost all components are interconnected.

In addition, when terms such as "parallel" and "orthogonal" are used in the present specification, each term does not mean only strict "parallel" and "orthogonal", but "parallel" in a strict sense. It includes "parallel" and "orthogonal", and also includes the meanings of "substantially parallel" and "substantially orthogonal" within the range in which the function can be exerted.

1 ... Position estimation device, 12, 12a to 12n ... Sensor, 13 ... Information processing device, 14 ... Sensor processing unit, 15 ... Control unit, 16 ... Memory, 17 ... Display unit, 19 ... External storage device, 100 ... Moving object , 141 ... Input / output unit, 142 ... Current position estimation unit, 143 ... Point cloud data acquisition unit, 144 ... Filter unit, 145 ... Data score adjustment unit, 146 ... Matching unit, 147 ... Obstacle detection unit, 148 ... Current position Correction part, 400 ... Function, 402 ... Maximum processing time, 401 ... Combination, 403 ... Maximum processing points

Claims (10)

1. It is a self-position estimation device that estimates its own position by comparing the driving environment information collected by the sensor with the map information.
   The current position estimation unit that tentatively estimates the current position of itself,
   Of the point cloud data included in the map information, the range in which the sensor can acquire the information is calculated based on the current temporary position estimated by the current position estimation unit, and the point cloud data in the acquireable range is obtained. Point cloud data selection unit to be selected and
   The score of the point cloud data selected from the map information is compared with the maximum number of points that can be processed, which is the maximum number of points that can be processed in the maximum allowable processing time when estimating the position, and the selection is made based on the comparison result. A point cloud adjustment unit that adjusts the points of the point cloud data in the range equal to or less than the maximum processable points.
   A matching unit that matches the adjusted point cloud data with the point cloud data of the information acquired by the sensor.
   A self-position estimation device including a current position correction unit that corrects the current temporary position of the self estimated by the current position estimation unit based on the matching result of the matching unit.
2. The self-position estimation according to claim 1, wherein in the score adjusting unit, when the score of the selected point cloud data is larger than the maximum processable score, the score of the selected point cloud data is reduced to the maximum processable score. Device.
3. The self according to claim 1 or 2, wherein in the score adjusting unit, when the score of the selected point cloud data is smaller than the maximum processable score, the score of the selected point cloud data is increased to the maximum processable score. Position estimator.
4. The third aspect of claim 3 is to store the point cloud data that could not be matched in the matching unit, and prevent the stored point cloud data from being selected from the point cloud data included in the map information at the next position estimation. Self-position estimation device.
5. The claim that the point cloud adjusting unit preferentially reduces the point cloud data farther from the current temporary position among the selected point cloud data when the points of the selected point cloud data are reduced to the maximum processable points. 2. The self-position estimation device according to 2.
6. Claim 3 in which, when the score adjustment unit increases the score of the selected point cloud data to the maximum processable score, the point cloud data closer to the current temporary position among the selected point cloud data is preferentially increased. The self-position estimation device described in.
7. The self-position estimation device according to claim 1, wherein the maximum processable score is a score calculated based on a correspondence relationship between a different score obtained in advance and a processing time at the score.
8. The self-position estimation device according to claim 4, wherein the matching unit calculates and stores the position and shape of a region including point cloud data that could not be matched.
9. Claim 4 that the matching unit adjusts and stores the size of a region that could not be matched with the map information in time series or the score of point cloud data according to the matching result obtained in time series. Or the self-position estimation device according to 8.
10. A computer equipped with a self-position estimation device that estimates its own position by comparing the driving environment information collected by the sensor with the map information.
    A procedure for tentatively estimating the current position of the self in absolute coordinates or the current position of the self in relative coordinates, and
    A procedure for calculating a range in which the sensor can acquire the information based on the estimated current temporary position among the point cloud data included in the map information, and selecting the point cloud data in the acquireable range.
    The score of the point cloud data selected from the map information is compared with the maximum number of points that can be processed, which is the maximum number of points that can be processed in the maximum allowable processing time when estimating the position, and the selection is made based on the comparison result. The procedure for adjusting the score of the point cloud data in the range of the maximum processable score or less, and
    A procedure for matching the adjusted point cloud data with the point cloud data of the information acquired by the sensor, and
    The procedure to correct the estimated self's current temporary position based on the matching result, and
    A program to execute.

Images