JP2020095435A - Moving body - Google Patents

Abstract

To perform calculation for maintaining a range of presence probability distribution of a current position of a moving body as a narrow state using an information processing device of the moving body having limited calculation capability during short processing time, and estimate a position of the moving body with high accuracy.SOLUTION: In a moving body having: a sensor collecting surrounding information; a sensor processing unit processing information acquired by the sensor; a three-dimensional point acquisition unit; a filter unit; a matching unit; and a self-position correction unit, the three-dimensional point acquisition unit acquires a three-dimensional point obtained from the sensor, the filter unit obtains a referable three-dimensional point from a current temporary position of the moving body among three-dimensional points included in a map, the matching unit performs matching between the three-dimensional point recorded in the three-dimensional point acquisition unit and the referable three-dimensional point obtained by the filter unit, and the self-position correction unit obtains a current position of the moving body in the map based on the three-dimensional point that has been successfully matched.SELECTED DRAWING: Figure 1

Description

The present invention relates to a mobile body.

BACKGROUND ART Autonomous traveling technology and driving support technology have been developed in which a moving body such as a robot or an automobile collects information about its surroundings, estimates the current position and running state of the moving body, and controls the traveling of the moving body.

For example, in Patent Document 1, for the purpose of improving the accuracy of self-position estimation, a plurality of three-dimensional points whose distance from a distance sensor is less than a predetermined distance threshold, among a plurality of three-dimensional points, Even if the distance from the distance sensor is equal to or greater than the distance threshold, the plurality of three-dimensional points whose feature amount is equal to or greater than the feature amount threshold are included in a distance section in which a predetermined number or more is extracted and extracted. A method of estimating a self-position by matching a plurality of three-dimensional points with a plurality of three-dimensional points indicated by an environment map is disclosed.

Patent Document 2 discloses a two-dimensional image at the current position of a moving object for the purpose of suppressing the accumulation of errors and appropriately calculating the true current position even on a monotonous road that is unlikely to become a target. A technique for collating an observation image with a reference image, which is a two-dimensional image corresponding to a provisional current position prepared in advance, by image matching, and calculating the true current position of the moving body based on the matching result is available. It is disclosed.

JP, 2017-83230, A JP, 2013-61270, A

In Patent Document 1, when extracting a plurality of three-dimensional points, it is not always possible to extract appropriate three-dimensional points.

In Patent Document 2, since image matching that requires a large amount of data to be processed is performed, it is difficult to show the result within a sufficiently short time with the limited calculation capacity of the arithmetic unit incorporated in the mobile body. There are cases.

Since there is a limit to the processing capability of the information processing device built in the mobile body itself, it is necessary to reduce the number of data to be processed.

An object of the present invention is to perform a calculation for maintaining a state in which the range of the existence probability distribution of the current position of the mobile object is narrow in a short processing time by using the information processing device of the mobile object whose calculation ability is limited, and to move the mobile object. It is to estimate the position of the body with high accuracy.

In order to solve the above problems, the mobile object of the present invention is a sensor that collects information about the surroundings, a sensor processing unit that processes information acquired by the sensor, a three-dimensional point acquisition unit, a filter unit, and a matching unit. And a self-position correction unit, the three-dimensional point acquisition unit acquires the three-dimensional points obtained from the sensor, and the filter unit, among the three-dimensional points included in the map, the current temporary position of the moving body. The matching unit obtains a referable three-dimensional point from the position, the matching unit performs matching between the three-dimensional point recorded in the three-dimensional point acquisition unit and the referable three-dimensional point obtained by the filter unit, and the self-position correction unit Calculates the current position of the moving body on the map based on the three-dimensional points that have been successfully matched.

According to the present invention, by using a mobile information processing device having a limited calculation capability, a calculation is performed in a short processing time to maintain a state in which the existence probability distribution of the current position of the mobile is narrow, The position of the body can be estimated with high accuracy.

It is a block diagram which shows the mobile body which concerns on one Embodiment of this invention. It is a flowchart which shows the information processing process in the sensor process part 14 of FIG. It is a block diagram which shows the sensor process part 14 of FIG. It is a schematic diagram which shows the example of the detection range of the sensor in the three-dimensional point distinction step S204 of FIG. 2A. It is a schematic diagram which shows the detection error of 3D point distinction step S204 of FIG. 2A. It is a top view which shows the Example of this invention. It is a top view which shows an Example in case the number of moving bodies is two. It is a top view which shows an Example in case the number of moving bodies is two and a fixed sensor is used. It is a top view showing an example when there is a passerby. It is a top view which shows the conditions of a matching step and a position correction step. It is a flowchart which shows the procedure of a matching step and a position correction step.

The present invention relates to a technique for estimating the position of a moving body such as a robot or an automobile.

Hereinafter, a position estimation device for a mobile body according to an embodiment of the present invention will be described with reference to the drawings.

In the present specification, the “three-dimensional point” represents the coordinates in the space located on the surface and inside of an object having a shape, and the points obtained by the sensor, the points included in the map, etc. Shall be included.

FIG. 1 is a configuration diagram showing a moving body according to an embodiment of the present invention.

In the figure, the moving body 100 includes a position estimating device 1. The position estimation device 1 can communicate with an external storage device 19 existing outside the moving body 100. This communication is preferably wireless. The moving body 100 is an automobile, a robot, or the like.

The position estimation device 1 includes a signal reception unit 2, one or more sensors 12 (12a, 12b,..., 12n), and an information processing device 13. These components are connected to each other by a bus 18.

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, a control unit 15 (CPU) that performs processing based on the sensor processing result, It includes a memory 16 and a display unit 17 such as a display. In the information processing device 13, the sensor processing unit 14 and the control unit 15 execute a predetermined computer program. The contents to be executed will be described later.

The signal receiving unit 2 receives a signal from the outside. The signal reception unit 2 is, for example, a GPS (Global Positioning System) that estimates the current position in absolute coordinates of the world. Further, the signal receiving unit 2 may be an RTK-GPS (Real Time Kinematic GPS) that estimates the current position more accurately than GPS. The signal receiving unit 2 may be a quasi-zenith satellite system. Further, the signal reception unit 2 may receive a signal from a beacon fixed at a known position. The signal reception unit 2 may also receive from a sensor such as a wheel encoder, an IMU (Inertial Measurement Unit), a gyro, or the like that estimates the position by relative coordinates. Further, the signal reception unit 2 may receive information such as the lane of the traveling environment, the sign, the traffic state, the shape, size, and height of the three-dimensional object. Finally, any method may be used as long as it can be used for estimation, control and recognition of the current position of the mobile unit 100.

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.

The information processing device 13 processes the information acquired by the sensor 12 to calculate the position or the movement amount of the moving body 100. The information processing device 13 may perform display according to the calculated position or movement amount. Further, the information processing device 13 may output a signal related to control of the moving 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, as the information acquired from the distant view, a feature such as a three-dimensional object or a landmark for position estimation may be extracted.

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

In the case where the sensor 12 is a monocular camera, if the road surface is flat, the pixel position on the image and the actual ground positional relationship (x, y) are constant, so the distance from the sensor 12 to the feature point is geometrically determined. Can be calculated. When the sensor 12 is a stereo camera, the distance to the feature point on the image can be measured more accurately. If the sensor 12 is a laser sensor, more accurate and distant information can be acquired.

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

Further, it is desirable that the sensors 12a, 12b,..., 12n are arranged so as not to be affected by environmental disturbance 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 rain, it is difficult for the raindrops to adhere to the lens of the sensor 12b that is opposite to the traveling direction or faces downward. Therefore, even if the information acquired by the sensor 12a is unclear due to the effect of the raindrops, the information acquired by the sensor 12b is less likely to be 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, 12b,..., 12n may be acquired under different acquisition conditions (aperture value, white balance, cycle, etc.). For example, by installing a sensor whose parameters have been adjusted for a bright place and a sensor whose parameters have been adjusted for a dark place, it is possible to perform imaging regardless of whether the environment is bright or dark.

The sensors 12a, 12b,..., 12n acquire information when receiving an acquisition start command from the control unit 15 or at regular time intervals. The data of the acquired information and the acquisition time are stored in the memory 16.

The memory 16 includes a main storage device (main memory) of the information processing device 13 and an auxiliary storage device such as a storage.

The sensor processing unit 14 performs various information processes based on the information data and the acquisition time stored in the memory 16. 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 the processing by the sensor processing unit 14 but also for the judgment 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 has information on the environment in which the mobile body 100 travels, including a map. The information in the external storage device 19 is, for example, the shape or position of a stationary object (tree, building, road, lane, signal, sign, road surface paint, road edge, etc.) in the traveling environment. Each piece of information in the external storage device 19 may be represented by a mathematical expression. For example, the line information does not have to be composed of a plurality of points, and only the inclination and the intercept of the line are required. Further, the information in the external storage device 19 may be represented by a point cloud without being distinguished. The point cloud may be represented by 3D (x, y, z), 4D (x, y, z, color) or the like. Finally, the information in the external storage device 19 may be stored in any format as long as the traveling environment is detected from the current position of the mobile body 100 and map matching can be performed.

When receiving the acquisition start command from the control unit 15, the external storage device 19 sends information to the memory 16. The external storage device 19 transmits/receives information via the bus 18 when installed in the mobile body 100.

On the other hand, when the external storage device 19 is not installed in the mobile body 100, the signal receiving unit 2 connects the mobile body 100 and the external storage device 19. This connection can be made by using, for example, Local Area Network (LAN), Wide Area Network (WAN), or the like.

The sensor processing unit 14 acquires the information of the external storage device 19 from the information acquired by the sensor 12 based on the current position of the moving body 100 and the internal parameters of the sensor 12 stored in the external storage device 19 and the memory 16. To do.

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 mobile body 100 is traveling to estimate the position of the mobile body 100. For example, the sensor processing unit 14 calculates the movement amount of the moving body 100 based on 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 the characteristic from each of the information acquired in time series. The sensor processing unit 14 further extracts the same feature from the next and subsequent information. Then, the sensor processing unit 14 calculates the movement amount of the moving body 100 by tracking the characteristics.

The control unit 15 outputs a command regarding the moving speed to the moving body 100 based on the information processing result of the sensor processing unit 14. For example, the control unit 15 commands to increase or decrease the moving speed of the moving object 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, or the like. Alternatively, a command for maintaining the output may be output.

FIG. 2A is a flowchart showing an information processing process in the sensor processing unit 14 of FIG.

In FIG. 2A, the current position estimation step S201 is a step of estimating the provisional current position of the moving body 100. The current position estimation step S201 is composed of, for example, a GPS that estimates an absolute position. Further, the current position estimation step S201 may be configured by odometry for estimating the relative position from a certain reference point. Although a position error occurs in the above-described position estimation method based on GPS or odometry, it is corrected in a position correction step S206 described later.

The environmental information extraction step S202 is a step of extracting environmental information around the moving body 100 by the sensors 12a, 12b,..., 12n installed in the moving body 100. Here, the extraction of the environment information includes acquisition of a grayscale image, a color image, a three-dimensional point, etc. of the environment around the moving body 100 by the sensors 12a, 12b,..., 12n. The extraction of the environmental information may be parallax image creation from the images acquired by the sensors 12a, 12b,..., 12n. Further, the extraction of the environmental information may be the extraction of the features included in the information acquired by the sensors 12a, 12b,..., 12n. The extraction of the environmental information may be extraction of line information included in the information acquired by the sensors 12a, 12b,..., 12n.

The external storage device information extraction step S203 is a step of acquiring information from the external storage device 19 based on a command from the control unit 15. All the information in the external storage device 19 may be acquired by a command from the control unit 15. Moreover, you may acquire only the information around the moving body 100.

The three-dimensional point distinguishing step S204 distinguishes the three-dimensional point having the highest matching probability from the external storage device information extracted in the external storage device information extracting step S203. The three-dimensional point distinction step S204 will be described later.

The matching step S205 is a step of matching the information acquired by the sensors 12a, 12b,..., 12n in the environment information extraction step S202 with the three-dimensional points discriminated in the three-dimensional point discrimination step S204. The matching step S205 will be described later.

The position correction step S206 is a step of correcting the current position estimated in the current position estimation step S201 using the result of the matching step S205. The position correction step S206 will be described later.

FIG. 2B shows a configuration of the sensor processing unit 14 of FIG.

In the figure, the sensor processing unit 14 includes an input/output unit 210, a three-dimensional point acquisition unit 211, a filter unit 212, a matching unit 213, and a self-position correction unit 214.

The input/output unit 210 inputs/outputs information necessary for the sensor processing unit 14 to correct the position of the moving body 100. For example, the input/output unit 210 acquires the information acquired in the environment information extraction step S202 by the sensors 12a, 12b,..., 12n stored in the memory 16. Further, the input/output unit 210 acquires information from the external storage device 19 in the information extraction step S203 of the external storage device 19 registered in the memory 16. When the amount of information acquired from the external storage device 19 is large and cannot be stored in the memory 16, only information close to the current position of the mobile unit 100 estimated in the current position estimation step S201 may be acquired. Further, only the information included in a certain area when viewed from the current position of the mobile unit 100 estimated in the current position estimation step S201 may be acquired.

The three-dimensional point acquisition unit 211 acquires a three-dimensional point based on the information acquired by the input/output unit 210 and temporarily records it. If the sensors 12a, 12b,..., 12n are cameras and the information acquired by the input/output unit 210 is an image, the two-dimensional image is converted into a three-dimensional point. The conversion method may be, for example, one in which each pixel of the image is converted into a three-dimensional point based on the parallax image. When the sensors 12a, 12b,..., 12n are monocular cameras, techniques such as structure from motion, flat surface model, ORB-SLAM, and LSD-SLAM may be used. When the sensors 12a, 12b,..., 12n are laser sensors, the information of the traveling environment is extracted at three-dimensional points, and other processing is not necessary.

In the filter unit 212, the three-dimensional point acquisition unit 211 filters the acquired three-dimensional points in the three-dimensional point discrimination step S204. In order to reduce the processing time, the average value of three-dimensional points within a certain area is calculated and narrowed down to one three-dimensional point. Also, three-dimensional points in a region of the traveling environment with few features such as corners and edges may be deleted. In addition, the filter unit 212 filters visible three-dimensional points in the external storage device 19 at the temporary position of the moving body 100 estimated in the current position estimation step S201.

When performing the matching step S205, if the number and the 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 possibility that the matching will fail increases. Therefore, by extracting the visible three-dimensional point of the external storage device 19 at the temporary position of the moving body 100 having a high matching probability, it is possible to perform the matching in the matching step S205 with high accuracy. It should be noted that the case where the matching can be performed with a predetermined accuracy in this way is referred to as “matching has succeeded”.

The matching unit 213 performs matching between the 3D point obtained by the 3D point acquisition unit 211 and the visible 3D point obtained by the filter unit 212 in a matching step S205.

The self-position correction unit 214 performs a position correction step S206 to correct the current position of the moving body 100.

The current position information corrected by the self-position correction unit 214 is transmitted to the input/output unit 210, and the current position information corrected by the command of the control unit 15 is transmitted to the memory 16. Further, the current position information corrected by the self-position correction unit 214 may be directly transmitted from the input/output unit 210 to the control unit 15 according to a command from the control unit 15.

In the above description, the input/output unit 210, the three-dimensional point acquisition unit 211, the filter unit 212, the matching unit 213, and the self-position correction unit 214 are included in the sensor processing unit 14 according to the description of FIG. 2B. However, the present invention is not limited to this, and the input/output unit 210, the three-dimensional point acquisition unit 211, the filter unit 212, the matching unit 213, and the self-position correction unit 214 are not included in the sensor processing unit 14 and are independent. May be a constituent element of.

Next, details of the three-dimensional point discrimination step S204 will be described with reference to FIGS. 3A and 3B.

FIG. 3A is a schematic diagram showing an example of the detection range of the sensor in the three-dimensional point discrimination step S204 of FIG. 2A.

FIG. 3B is a schematic diagram showing a detection error in the three-dimensional point discrimination step S204 of FIG. 2A.

In FIG. 3A, reference numeral 1001 indicates the coordinates (x, y) of the traveling environment of the moving body 100 registered in the external storage device 19.

The temporary current position (X, Y) 1002 is the position of the moving body 100 at the coordinates 1001 estimated in the current position estimation step S201.

It is assumed that the moving body 100 is traveling indoors. When the moving body 100 is at the provisional current position 1002, the information about the surroundings of the moving body 100 registered in the external storage device 19 is the three-dimensional point group 1004, the three-dimensional point group 1005, the three-dimensional point group 1006L, and the three-dimensional point group. It will be 1006R. For simplification, the heights of the three-dimensional point group 1004, the three-dimensional point group 1005, the three-dimensional point group 1006L, and the three-dimensional point group 1006R with respect to the ground are constant. In the figure, a circle indicates that "reference is possible", and a circle indicates that "reference is not possible".

The acquisition angle of the sensors 12a, 12b,..., 12n is FOV. Since the three-dimensional point cloud 1004 is outside the acquisition angle FOV of the sensors 12a, 12b,..., 12n based on the provisional current position 1002 of the moving object 100 and the acquisition angle FOV, it cannot be referred to. Further, the three-dimensional point group 1005, the three-dimensional point group 1006L, and the three-dimensional point group 1006R are within the acquisition angle FOV of the sensors 12a, 12b,..., 12n, but the sensors 12a, 12b,. Since only the closest information in each direction can be obtained, the 3D point group 1005 can be referred to, and the 3D point group 1006L and the 3D point group 1006R cannot be referred to.

Therefore, even if the three-dimensional point 1006L and the three-dimensional point 1006R that cannot be referenced from the current position 1002 of the moving body 100 are used in the matching step S205 of the matching unit 213, the matching cannot be performed. Therefore, the three-dimensional point 1006L and the three-dimensional point 1006R cannot be used for matching.

Therefore, the filter unit 212 excludes the non-referenceable three-dimensional points 1006L and 1006R.

Note that, for simplicity, the three-dimensional point group 1004, the three-dimensional point group 1005, the three-dimensional point group 1006L, and the three-dimensional point group 1006R have been described on the assumption of an indoor wall, a chair, a table, and the like. It may be composed of various three-dimensional objects.

In FIG. 3B, the existence probability distribution 301 represents the existence probability distribution of the current position of the mobile body 100. The existence probability distribution 301 is estimated using a time series filter such as Kalman Filter, Particle Filter, and Bayesian Filter, for example.

The three-dimensional points 302 are a plurality of three-dimensional points extracted in the external storage device information extraction step S203. For simplicity, the heights of the three-dimensional points 302 from the ground are set to be constant.

The three-dimensional point 303 is information on the pillar in the traveling environment acquired from the current position of the moving body 100. The existence probability distribution 304 is the existence probability distribution of the current position of the pillar.

The angle θ _N is the direction from the centerline of the field of view from one of the sensors 12 of mobile 100.

The extracted three-dimensional point 306 is the referable three-dimensional point extracted in the three-dimensional point discrimination step S204, and is the three-dimensional point used in the matching step S205.

The non-referenceable three-dimensional point 307 is a three-dimensional point which is discriminated as a non-referenceable three-dimensional point in the three-dimensional point discrimination step S204 and is not used in the matching step S205.
The existence probability distribution 304 of the extracted three-dimensional points 303 is assumed to be the existence probability distribution 301 of which the range is similar to the moving body 100.

If the information acquisition error of the sensors 12a, 12b,..., 12n is not 0, the existence probability distribution 304 of the extracted three-dimensional points 303 becomes larger than the existence probability distribution 301 of the moving body 100. If the information acquisition error of the sensors 12a, 12b,..., 12n of the moving body 100 is known, the existence probability distribution 304 is calculated in accordance with the existence probability distribution 301. For example, when the sensors 12a, 12b,..., 12n are stereo cameras, the information acquisition error is proportional to the distance of the acquired information, and therefore the existence probability distribution 304 is calculated including the error.

Therefore, it is unclear whether the extracted three-dimensional point 303 should be matched with the three-dimensional point 302L or the three-dimensional point 302R among the three-dimensional points 302.

Therefore, the existence probability distribution 301 of the angle θ _N and the current position distinguishes the three-dimensional point 302L from the three-dimensional point 302R.

Conventional sensors detect a three-dimensional object closest to the sensor and cannot detect other three-dimensional objects behind the three-dimensional object.

Therefore, in the present invention, if a three-dimensional object closest to the sensor in the direction of the angle θ _N is searched for and extracted, the probability of matching can be increased.

Next, the setting of the angle θ _N will be described.

The angle θ _N is adjusted within a predetermined angle range θ _A to θ _B, and a three-dimensional point having the highest matching probability is searched for. For example, when the acquisition angle of the sensor is FOV, θ _A =−FOV and θ _B =+FOV. _A predetermined interval dθ (=θ _N −θ _N−1 ) is set in the range from θ _A to θ _B. Here, dθ is a fixed value. You may set d(theta) according to the resolution of a sensor.

Further, dθ may be matched with the existence probability distribution 301 of the moving body 100. For example, if the reliability of the self-position is low, the provisional current position error estimated in the current position estimation step S201 is large, so dθ is set small. On the other hand, if the reliability of the self-position is high, the provisional current position error estimated in the current position estimation step S201 is small, so dθ is set to be large.

Further, θ _A and θ _B may be changed based on the size of the existence probability distribution 301. For example, when the existence probability distribution 301 is large, there is a high probability that the estimated azimuth error of the current position is large. Therefore, θ _A is θ _A ′ (=θ _A −α) and θ _B is θ _B ′ (= θ _B +α).

FIG. 4 shows an embodiment of the present invention.

In the figure, a three-dimensional point 401 is a three-dimensional point of the traveling environment of the moving body 100 registered in the external storage device 19.

The existence probability distribution 402 is an existence probability distribution of the mobile unit 100. It is assumed that the existence probability distribution 402 is small at the time when the moving body 100 starts traveling.

The range 403 is a range searched by the mobile unit 100 in the three-dimensional point discrimination step S204.

In this case, in the external storage device information extraction step S203, θ _A =−FOV and θ _B =+FOV are set, and the information of the external storage device 19 around the moving body 100 within the range 403 is extracted. Then, in the three-dimensional point discrimination step S204, the referenceable and non-referenceable three-dimensional points are distinguished.

When the moving body 100 travels in an environment with few features such as a corridor, the position cannot be corrected even by performing the matching step S205. When the control unit 15 recognizes that the moving body 100 is traveling in an environment with few characteristics based on the information registered in the external storage device 19, the moving body 100 moves according to a command from the control unit 15, and Start the search. In the search for the feature, for example, based on the information registered in the external storage device 19, the position of the feature closest to the current position of the mobile body 100 is extracted. Next, the mobile unit 100 moves to a position where the characteristic can be detected by the command of the control unit 15, and the position is corrected in the matching step S205 and the position correction step S206.

When the mobile body 100 moves to the position of the extracted feature, the position error estimated by the mobile body 100 in the current position estimation step S201 becomes large, and the existence probability distribution of the mobile body 100 becomes the existence probability distribution 404. Therefore, in the external storage device information extraction step S203, θ _{A is set} to θ _A ′ (=θ _A −α), θ _{B is set} to θ _B ′ (=θ _B +α), and the external storage devices 19 around the moving body 100 are stored. Extract information. Then, in the three-dimensional point discrimination step S204, a search is performed in the range 405 from θ _A ′ (=−FOV−α) to θ _B ′ (=+FOV+α) to discriminate the three-dimensional points that can be referred to.

After correcting the current position of the moving body 100 in the position correction step S206, when performing the matching step S205 next time, in the external storage device information extraction step S203, θ _A =−FOV and θ _B =+FOV are set. Using the newly set θ _A and θ _B , the referenceable and non-referenceable three-dimensional points are discriminated in the three-dimensional point discrimination step S204.

Further, when the moving body 100 travels in an environment with few features and moves to the position of the closest feature, the control unit 15 selects a route in which the position error estimated in the current position estimation step S201 does not increase. For example, when the current position of the moving body 100 is estimated by the wheel encoder in the current position estimation step S201, the error is proportional to the distance traveled, and therefore the shortest distance is calculated as much as possible. Further, in order to avoid the azimuth error estimated by the moving body 100, a route that can be moved in a straight line with less rotation is calculated. Therefore, when traveling in an environment with few characteristics, even if there is a dynamic obstacle, the moving body 100 inevitably stops and continues to move straight after the dynamic obstacle disappears.

FIG. 5 shows a case where two moving bodies travel.

In the figure, a three-dimensional point 501 is a three-dimensional point of the traveling environment of the moving body 100 registered in the external storage device 19.

The existence probability distribution 502 is an existence probability distribution of the current position of the mobile body 100.

The three-dimensional point 503 is a three-dimensional point extracted by the sensor from the current position of the moving body 100.

The existence probability distribution 504 is the existence probability distribution of the three-dimensional points 503, and has the same size as the existence probability distribution 502. In this case, it is unclear whether the 3D point 503 extracted by the sensor should be matched with the 3D point 505R or the 3D point 505L of the 3D point 501, and there is a high possibility of erroneous matching.

The existence probability distribution 506 is the existence probability distribution of the second moving body 100b existing in the environment where the moving body 100 is traveling. The existence probability distribution 506 is smaller than the existence probability distribution 504. The moving body 100 and the moving body 100b share information on their respective positions and existence probability distributions. For information sharing, for example, a LAN (Local Area Network) is connected to transmit and receive information.

The timing at which the mobile unit 100 and the mobile unit 100b share information is immediately before the three-dimensional point discrimination step S204. In addition, the timing at which the mobile object 100 and the mobile object 100b share information may be performed in a cycle shorter than the cycle of the three-dimensional point discrimination step S204. Finally, any cycle may be set as long as the latest information can be shared in accordance with the cycle of the three-dimensional point discrimination step S204 that requires the information of the existence probability distribution.

A distance 507 from the current position of the moving body 100b to the moving body 100 is calculated by a sensor. Because of the existence probability distribution 506, there is an error in the distance 507 to the moving body 100 calculated by the sensor. Therefore, the moving body 100 exists between the distance 508 and the distance 509.

By merging the distance 508 and the distance 509 with the existence probability distribution 502, the existence probability distribution of the mobile unit 100 is updated to be the range represented by reference numeral 510. Then, the existence probability distribution 504 of the three-dimensional points 503 extracted by the sensor is also updated and becomes a narrow range indicated by reference numeral 511. In other words, the surrounding information is obtained from the sensor of the moving body 100b that is another moving body, and is combined with the surrounding information from the sensor of the moving body 100 to obtain the current position of the moving body 100 (presence probability distribution 510). ).

As a result, the existence probability distribution 511 becomes smaller than the existence probability distribution 504. Then, the three-dimensional point 503 extracted by the sensor is matched with the three-dimensional point 505L included in the range of the existence probability distribution 504 to correct the position of the moving body 100.

For the sake of simplicity, the description has been given with the moving body 100 and the moving body 100b stopped, but the above position correction may be performed while one or both moving vehicles are running. Further, for the sake of simplicity, the description has been given using two moving bodies, but two or more moving bodies may be used.

FIG. 6 shows an embodiment of two moving bodies and a fixed camera.

In this figure, the existence probability distribution 601 is the existence probability distribution of the current position of the mobile unit 100.

The existence probability distribution 602 is an existence probability distribution of the current position of the moving body 100b. The existence probability distribution 602 is assumed to be larger than the existence probability distribution 601.

In this case, even if the distance to the moving body 100 is calculated by the sensor installed on the moving body 100b, the distance between the distance 603 and the distance 604 becomes larger than the existence probability distribution 601, so that the two moving bodies 100 and 100b are used. Position correction does not work.

In this figure, a fixed camera 605 (fixed sensor) installed in the traveling environment of the moving body 100 can be used, and a distance 606 and a distance 607 from the fixed camera 605 to the moving body 100 and the moving body 100b can be obtained. In this case, even if an error occurs in the distance 606 and the distance 607, the moving body 100b exists between the distance 608 and the distance 609, and the moving body 100 exists between the distance 610 and the distance 611. Should be. Therefore, the existence probability distribution 601 becomes the existence probability distribution 612, and the existence probability distribution 602 becomes the existence probability distribution 613, both of which are narrow ranges.

Since the existence probability distribution 612 and the existence probability distribution 613 are smaller than the existence probability distribution 601 and the existence probability distribution 602, respectively, the referenceable three-dimensional points in the three-dimensional point classification step S204 can be distinguished, and the matching step S205 can be performed accurately. The position of the moving body 100 can be corrected.

In addition, the sensor installed in the moving body 100b calculates the distance 614 to the moving body 100, the existence probability distribution 617 is calculated between the distance 615 and the distance 616, and in the three-dimensional point discrimination step S204 to the position correction step S206. The position may be corrected. The existence probability distribution 617 has a narrower range than the existence probability distribution 612. In other words, the surrounding information is acquired from the fixed sensor installed in the traveling environment of the moving object 100, and combined with the surrounding information from the sensor of the moving object 100, the current position of the moving object 100 (presence probability distribution 617). ).

For simplicity, the description has been given using two moving bodies and one fixed camera, but the position correction may be performed using two or more moving bodies and a plurality of fixed cameras.

Further, for simplicity, the fixed camera 605 has been described, but any sensor other than the camera may be used as long as it is a sensor capable of estimating the moving body 100 and calculating the distance.

FIG. 7 shows a case where there is a passerby.

In this figure, a fixed camera 701 is a sensor fixed in the environment in which the moving body 100 travels.

The existence probability distribution 702 is an existence probability distribution of the current position of the mobile body 100.

The existence probability distribution 703 is an existence probability distribution of the current position of the moving body 100b.

When there is a passerby 704 or the like between the moving body 100b and the fixed camera 701, the fixed body 701 cannot detect the moving body 100b. In other words, passers-by 704, etc. may be obstacles to detection.

The fixed camera 701 calculates the distance 705 to the moving body 100, and as described in FIGS. 5 and 6, the existence probability distribution 702 of the moving body 100 is reduced, and the existence probability distribution 702 is set to the existence probability distribution 702b.

On the other hand, the moving body 100b can be detected from the current position of the moving body 100, and after the distance 706 is calculated, the existence probability distribution 703 of the moving body can be reduced to obtain the existence probability distribution 703b.

Next, in order to correct the current position of the moving body 100b, the process proceeds to the three-dimensional point distinguishing step S204, the matching step S205, and the position correction step S206.

In the three-dimensional point discrimination step S204, all the three-dimensional points 707 around the moving body 100b can be extracted based on the current position and the acquisition range of the sensor, but the fixed camera 701 detects the moving body 100b by the influence of the passerby 704. It can no longer be detected. Therefore, the fixed camera 701 transmits the presence of the passerby 704 and the direction α of the passerby to the moving body 100b to the moving body 100b.

As a result, three-dimensional points other than the direction α in which the probability of matching is low are extracted in the three-dimensional point discrimination step S204, and position estimation is performed in the matching step S205 and the position correction step S206.

In the figure, for simplicity, the case of one passerby has been described, but there may be a plurality of passersby. Further, when it is not possible to detect another moving body or a fixed sensor or correct the position due to the influence of a plurality of passers-by, it temporarily stops and waits until there are no passers-by. Further, when the passerby does not disappear for a while, the moving body may avoid the passerby and move to a position where another moving body or a fixed sensor can be detected.

Although a passerby has been described here, animals other than humans, objects and the like are generally referred to as “obstacles”.

In summary, when the 3D points acquired by the 3D point acquisition unit include the 3D points of the obstacle, the matching unit performs matching using the 3D points excluding the 3D points of the obstacle. I do.

Details of the matching step S205 and the position correction step S206 of FIG. 2A will be described below with reference to FIGS. 8A and 8B.

FIG. 8A is a top view showing conditions of the matching step and the position correction step.

FIG. 8B is a flowchart showing the procedure of the matching step and the position correction step.

In FIG. 8A, a three-dimensional point 801 (marked with ●) is a traveling environment of the moving body 100 registered in the external storage device 19.

The three-dimensional point 802 (circle) is a three-dimensional point extracted by the sensor from the current position of the moving body 100. As a method of matching the three-dimensional points 801 and 802, for example, ICP Matching (ICP is an abbreviation of Iterative Closest Point) is used. Also, the line matching may be performed by converting the three-dimensional points 801 and 802 into lines. Finally, if the relative positions (X, Y, Z, roll, pitch, yaw) of the three-dimensional point 801 and the three-dimensional point 802 are output, a matching method other than ICP may be applied. Here, roll, pitch, and yaw are the angle at which the moving body 100 rotates about the front-rear axis (the axis in the traveling direction), the angle at which the tip of the moving body 100 moves up and down, the moving body, respectively. It represents the angle at which the tip of 100 vibrates to the left and right.

Layers 803R and 803L indicated by broken lines in this figure are divisions for digitizing the surrounding state of the moving body 100. Here, for the sake of simplicity, the layers 803R and 803L are obtained by dividing the traveling environment into a distance and a position, and dividing the traveling environment into regions each including a point group of three-dimensional points. It is represented as a set. The layer 803R is located on the right side in the traveling direction of the moving body 100, and the layer 803L is located on the left side in the traveling direction of the moving body 100. The "position" is the coordinate of the traveling surface and is the two-dimensional coordinate.

Specifically, the layers 803R and 803L are configured by a distance, a position, a height, a color, etc. as shown in the following formula (1).

Layer (n)=f (distance, position, height, color,...) (1)
In the formula, the parentheses on the right side describe the attributes of the layers 803R and 803L. The attributes of the respective layers 803R and 803L are numerical values of respective characteristics (distances, positions, heights, colors, etc. at respective points constituting the point group of the three-dimensional points included in the respective layers 803R and 803L ( (Feature amount), the average value of the point group is calculated and shown. In other words, the layers 803R and 803L have attributes as variables.

A more specific example of the above formula (1) is represented by the following formula (2).

Layer (i)=f (distance d, position p)=f(d _i-1 <d<d _i , p=R) (2)
In the formula, i is an integer of 1 or more and is a code representing each area. d _i is a distance (a value specified according to a predetermined rule) from the moving body 100 to the boundary between the adjacent area (for example, i+1). R is a numerical value representing the right side of the moving body 100.

In the above formula (2), the attributes are limited to the distance and the position. The equation also relates to layer 803R.

Similarly, the layer 803L is represented by the following formula (3).

Layer (j)=f (distance d, position p)=f(d _j-1 <d<d _j , p=L) (3)
In the formula, j is an integer of 1 or more and is a code representing each area. d _j is a distance (a value specified according to a predetermined rule) from the moving body 100 to the boundary with the adjacent area (for example, j+1). L is a numerical value indicating the left side of the moving body 100.

In the external storage device information extraction step S203, the external storage device information is extracted, and the external storage device information is input to each of the layers described above based on the distance and the position. In the three-dimensional point distinction step S204, the referable three-dimensional points that can be matched in each layer are extracted, and the information obtained by the sensor in the environment information extraction step S202 is input to each layer.

Therefore, each of the layers 803R and 803L has the external storage device information and the information extracted by the sensor. Therefore, by applying ICP Matching to each layer in the matching step S205, a corrected attribute candidate represented by the following formula (4) is obtained.

Layer (k)=(X _k , Y _k , Z _k , roll _k , pitch _k , yaw _k ,...) (4)
In the formula, k is an integer of 1 or more and is a code representing each area.

In the position correction step S206, the position candidates corrected from the respective layers are fused, and the corrected position of the mobile unit 100 is output. The fused position is, for example, the average of the positions obtained in each layer. The fused position may be the median value of the positions obtained in each layer. Further, the fused position may be the mode value of the position obtained in each layer.

Further, the points T _k (k is an integer of 1 or more, which is a code representing each region) in each layer may be weighted and combined.

Each attribute is represented by the following equations (5) to (10).

X=Σ(X _k ·T _k )/ΣT _k (5)
Y=Σ(Y _k ·T _k )/ΣT _k (6)
Z=Σ(Z _k ·T _k )/ΣT _k (7)
roll=Σ(roll _k ·T _k )/ΣT _k (8)
pitch=Σ(pitch _k ·T _k )/ΣT _k (9)
yaw=Σ(yaw _k ·T _k )/ΣT _k (10)
When the sensor is a camera, the calculated distance error is proportional to the distance. Therefore, the layer distance d _k (k is an integer of 1 or more and is a code representing each area) is used as a weight. Good. Further, the scores output from ICP Matching (difference between the information extracted by the sensor and the external storage device information) may be weighted, and the position candidates corrected from the respective layers may be fused.

FIG. 8B shows the procedure of matching step S205 and position correction step S206.

In the process shown in FIG. 8B, matching is performed using attributes.

In the figure, in the layering step S810, the three-dimensional point 802 extracted by the sensor from the current position of the moving body 100 and the three-dimensional point 801 registered in the external storage device 19 are layered using the above equation (1). It is a step of dividing.

Each layer matching step S820 is a step of matching the three-dimensional point 802 detected by the sensor of each layer and the three-dimensional point 801 acquired from the external storage device 19 which are divided in the layer dividing step S810.

The matching result fusing step S830 fuses the results of the layer matching steps S820 obtained for the respective layers. At the time of fusion, the above formulas (4) to (10) are used.

1: Position estimation device, 2: Signal reception part, 12, 12a, 12b, 12n: Sensor, 13: Information processing device, 14: Sensor processing part, 15: Control part, 16: Memory, 17: Display part, 19: External storage device, 100: moving body, 210: input/output unit, 211: three-dimensional point acquisition unit, 212: filter unit, 213: matching unit, 214: self-position correction unit, 301, 304: existence probability distribution, 302, 303: three-dimensional point, 605: fixed camera.

Claims (7)

A sensor that collects surrounding information,
A sensor processing unit that processes information acquired by the sensor;
A three-dimensional point acquisition unit,
The filter part,
Matching section,
In a moving body having a self-position correction unit,
The three-dimensional point acquisition unit acquires the three-dimensional points obtained from the sensor,
The filter unit obtains a three-dimensional point that can be referred to from the current temporary position of the moving body among the three-dimensional points included in the map,
The matching unit performs matching between the three-dimensional points recorded in the three-dimensional point acquisition unit and the referenceable three-dimensional points obtained by the filter unit,
The moving body, wherein the self-position correcting unit obtains the current position of the moving body in the map based on the three-dimensional points that have been successfully matched. The three-dimensional point acquisition unit acquires surrounding information from a fixed sensor installed in the traveling environment of the moving body, or a sensor included in another moving body, and with the surrounding information from the sensor included in the moving body. The moving body according to claim 1, wherein the moving body is obtained by combining the present positions. The self-position correction unit uses the existence probability distribution of the moving body at the current temporary position and the position information of the moving body obtained by the fixed sensor or the other moving body to determine whether the moving body exists. The mobile object according to claim 2, wherein the probability distribution range is corrected to a narrow range. The moving body according to claim 3, wherein the existence probability distribution of the other moving body is smaller than the existence probability distribution of the moving body at the current temporary position. In the case where the three-dimensional point acquired by the three-dimensional point acquisition unit includes a three-dimensional point of a moving obstacle,
The moving body according to claim 1, wherein the matching unit performs the matching using three-dimensional points excluding the three-dimensional points of the obstacle. The moving body according to claim 1, wherein the matching unit performs the matching using a layer having an attribute as a variable. The moving body according to claim 6, wherein the matching unit sets a weight on the attribute of the layer to perform the matching.

Images