JP7302966B2 - moving body - Google Patents

Description

The present invention relates to mobile objects.

2. Description of the Related Art Autonomous driving technology and driving support technology have been developed in which mobile bodies such as robots and automobiles collect information about their surroundings, estimate the current position and running state of the mobile body, and control the running of the mobile body.

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

Japanese Patent Laid-Open No. 2002-200000 discloses a two-dimensional image of the current position of a moving object, which is aimed at suppressing the accumulation of errors and appropriately calculating the true current position even on a monotonous road where there are no likely targets. A technology that compares an observed image with a reference image, which is a two-dimensional image corresponding to a provisional current position prepared in advance, by image matching, and calculates the true current position of a moving object based on the matching result. disclosed.

JP 2017-83230 A JP 2013-61270 A

In Patent Document 1, when extracting a plurality of 3D points, it is not always possible to extract an appropriate 3D point.

In Patent Document 2, since image matching is performed with a large amount of data that needs to be processed, it is difficult to show the result within a sufficiently short time with the limited computing power of the arithmetic unit built into the mobile object. Sometimes.

Since there is a limit to the processing capability of an information processing device built into a mobile object itself, there is a need to reduce the amount of data to be processed.

An object of the present invention is to use an information processing device of a mobile object with limited computing power to perform calculations to maintain a narrow range of the existence probability distribution of the current position of the mobile object in a short processing time, and to To estimate the position of a body with high accuracy.

In order to solve the above problems, a moving body of the present invention includes a sensor that collects surrounding information, a sensor processing unit that processes the information acquired by the sensor, a three-dimensional point acquisition unit, a filter unit, and a matching unit. and a self-position correction unit, wherein the 3D point acquisition unit acquires the 3D points obtained from the sensor, and the filter unit acquires the current temporary position of the moving object among the 3D points included in the map. A 3D point that can be referenced from the position is obtained, and the matching unit matches the 3D point recorded in the 3D point acquisition unit with the 3D point that can be referenced by the filter unit, and the self-position correction unit finds the current position of the moving object on the map based on the successfully matched 3D points.

According to the present invention, an information processing apparatus for a mobile body with limited computing power is used to perform calculations in a short processing time to maintain a narrow range of the existence probability distribution of the current position of the mobile body. The position of the body can be estimated with high accuracy.

1 is a configuration diagram showing a mobile object according to an embodiment of the present invention; FIG. FIG. 2 is a flow diagram showing an information processing process in the sensor processing unit 14 of FIG. 1; 2 is a configuration diagram showing a sensor processing unit 14 of FIG. 1; FIG. 2B is a schematic diagram showing an example of a sensor detection range in the three-dimensional point discrimination step S204 of FIG. 2A; FIG. FIG. 2B is a schematic diagram showing detection errors in the three-dimensional point discrimination step S204 of FIG. 2A; It is a top view which shows the Example of this invention. FIG. 10 is a top view showing an embodiment in which two moving bodies are used; FIG. 10 is a top view showing an example in which there are two moving bodies and fixed sensors are used; It is a top view which shows an Example when a passerby is present. FIG. 4 is a top view showing conditions of a matching step and a position correction step; FIG. 4 is a flow chart showing procedures of a matching step and a position correction step;

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

A position estimation device for a moving object according to an embodiment of the present invention will be described below with reference to the drawings.

In this specification, the term "three-dimensional point" refers to coordinates in space located on the surface and inside of an object having a shape. shall include

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

In this figure, a mobile object 100 has a position estimation device 1 . The position estimation device 1 can communicate with an external storage device 19 located outside the mobile object 100 . This communication is preferably wireless. The mobile object 100 is an automobile, a robot, or the like.

The position estimation device 1 has a signal reception unit 2, one or more sensors 12 (12a, 12b, . . . , 12n), and an information processing device 13. FIG. These components are interconnected by 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 17 such as a display. In the information processing device 13, the sensor processing unit 14 and the control unit 15 execute predetermined computer programs. The contents to be executed will be described later.

The signal reception unit 2 receives signals from the outside. The signal reception unit 2 is, for example, a GPS (Global Positioning System) for estimating the current position with absolute coordinates in the world. Further, the signal receiving unit 2 may be RTK-GPS (Real Time Kinematic GPS) that estimates the current position more accurately than GPS. Also, the signal reception unit 2 may be a quasi-zenith satellite system. Alternatively, 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 signals from sensors that estimate the position using relative coordinates, such as wheel encoders, IMUs (Inertial Measurement Units), and gyros. The signal reception unit 2 may also receive information such as lanes, signs, traffic conditions, and the shape, size, and height of three-dimensional objects in the driving environment. Ultimately, any method may be used as long as it can be used for current position estimation, control, and recognition of the moving object 100 .

Sensor 12 is, for example, a still camera or a video camera. Sensor 12 may also be a monocular camera or a compound camera. Alternatively, the sensor 12 may be a laser sensor.

The information processing device 13 processes the information acquired by the sensor 12 and calculates the position or movement amount of the moving body 100 . The information processing device 13 may display according to the calculated position or movement amount. The information processing device 13 may also output a signal related to control of the moving body 100 .

Next, details of the sensor 12 will be described.

The sensor 12a is installed in front of the moving body 100, for example. The sensor 12 a 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 extract features such as three-dimensional objects or landmarks for position estimation.

The other sensors 12b, . . . , 12n are installed at different positions than the sensor 12a and image different directions or areas than the sensor 12a. For example, the sensor 12b may be installed behind the moving body 100 facing downward. In this case, the sensor 12b acquires the near-field information behind the mobile object 100 . The foreground information may be the road surface around the moving object 100, or the white lines around the moving object 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 positional relationship (x, y) on the ground are constant. can be calculated. If the sensor 12 is a stereo camera, the distance to the feature point on the image can be measured more accurately. Moreover, when the sensor 12 is a laser sensor, more accurate and distant information can be obtained.

In the following explanation, examples using a monocular camera, a stereo camera, or a laser sensor will be explained, but other sensors (a camera with a wide-angle lens, a TOF camera, etc.) may be used as long as the distance to the surrounding three-dimensional object can be calculated. .

Also, it is desirable that the sensors 12a, 12b, . As a result, for example, even if raindrops adhere to the lens of the sensor 12a when it is raining, the raindrops are less likely to adhere to the lens of the sensor 12b facing the opposite direction or downward. Therefore, even if the information obtained by the sensor 12a is unclear due to raindrops, the information obtained by the sensor 12b is less affected by the raindrops. Even if the information from the sensor 12a is unclear due to the influence of sunlight, the information acquired by the sensor 12b may be clear.

Also, the sensors 12a, 12b, . For example, by installing a sensor whose parameters are adjusted for bright places and a sensor whose parameters are adjusted for dark places, imaging may be possible regardless of the brightness of the environment.

The sensors 12a, 12b, . The acquired information data and 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 processing based on the information data and 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 processing by the sensor processing unit 14 but also for judgment and processing by the control unit 15 and the like.

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

The external storage device 19 has information of the environment in which the moving body 100 runs, including a map. The information in the external storage device 19 is, for example, the shapes and positions of stationary objects (trees, buildings, roads, lanes, traffic lights, signs, road surface paint, roadsides, etc.) in the driving environment. Each piece of information in the external storage device 19 may be represented by a formula. For example, the line information does not consist of a plurality of points, but only the slope and the intercept of the line. Also, the information in the external storage device 19 may be represented by a point group without distinction. The point cloud may be represented in 3D (x, y, z), 4D (x, y, z, color), and so on. Ultimately, the information in the external storage device 19 may be stored in any format as long as the running environment can be detected from the current position of the mobile object 100 and map matching can be performed.

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

On the other hand, when the external storage device 19 is not installed in the mobile body 100 , the connection between the mobile body 100 and the external storage device 19 is performed by the signal reception unit 2 . This connection can be made by, for example, a Local Area Network (LAN), a 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. do.

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

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

Based on the information processing result of the sensor processing unit 14 , the control unit 15 outputs a command regarding the moving speed to the moving body 100 . For example, the control unit 15 may issue a command to increase or decrease the moving speed of the moving object 100 according to the resolution of a three-dimensional object in the information, the number of outliers among the features in the information, the type of information processing, or the like. Or you may output a command to keep it.

FIG. 2A is a flow diagram showing the information processing process in the sensor processor 14 of FIG.

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

The environment information extraction step S202 is a step of extracting environment information around the moving body 100 using the sensors 12a, 12b, . . . Here, the extraction of environmental information includes acquisition of grayscale images, color images, three-dimensional points, etc. of the environment around the moving object 100 by the sensors 12a, 12b, . . . , 12n. Also, the extraction of the environmental information may be parallax image creation from the images acquired by the sensors 12a, 12b, . . . , 12n. Also, the extraction of environmental information may be extraction of features included in the information acquired by the sensors 12a, 12b, . . . , 12n. Also, the extraction of the environment 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 . Alternatively, only information about the surroundings of the moving object 100 may be acquired.

A 3D point distinguishing step S204 distinguishes a 3D point with the highest probability of matching from the external storage device information extracted in the external storage device information extraction step S203. The three-dimensional point discrimination step S204 will be described later.

The matching step S205 is a step of matching the information acquired by the sensors 12a, 12b, . 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 the configuration of the sensor processing section 14 of FIG.

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

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

The 3D point acquisition unit 211 acquires a 3D point based on the information acquired by the input/output unit 210 and temporarily records it. When the sensors 12a, 12b, . The transform method may be, for example, based on the parallax image to make each pixel of the image into a three-dimensional point. Also, if the sensors 12a, 12b, . If the sensors 12a, 12b, .

In the filter unit 212, the 3D points acquired by the 3D point acquisition unit 211 are filtered in a 3D point discrimination step S204. In order to reduce the processing time, the average value of the 3D points within a certain area is calculated and narrowed down to one 3D point. Also, 3D points in areas of the driving environment with few features such as corners and edges may be deleted. Further, in the filter unit 212, the 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 are filtered.

When performing the matching step S205, if the number and shape of the information acquired by the sensors 12a, 12b, . Therefore, by extracting the visible three-dimensional points of the external storage device 19 at the temporary positions of the moving object 100 with a high matching probability, matching can be performed with high accuracy in the matching step S205. It should be noted that the case where matching is achieved with a predetermined accuracy in this way is referred to as "successful matching".

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

The self-position correction unit 214 performs 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 from the control unit 15 is transmitted to the memory 16 . Further, the current position information corrected by the self-position correcting section 214 may be transmitted directly from the input/output section 210 to the control section 15 in response to a command from the control section 15 .

In the above description, according to the description of FIG. 2B, 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. However, the present invention is not limited to this. may be a component of

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

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

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

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

A 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.

Assume that the moving object 100 is traveling indoors. When the mobile object 100 is at the temporary current position 1002, the information around the mobile object 100 registered in the external storage device 19 is a three-dimensional point group 1004, a three-dimensional point group 1005, a three-dimensional point group 1006L, and a three-dimensional point group. 1006R. For the sake of simplicity, the height 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 is assumed to be constant. In the figure, a circle mark indicates that it is "referenceable", and a ● mark indicates that it is "not referenceable".

Let FOV be the acquisition angle of the sensors 12a, 12b, . . . , 12n. Based on the temporary current position 1002 of the moving body 100 and the acquisition angle FOV, the three-dimensional point cloud 1004 is outside the acquisition angle FOV of the sensors 12a, 12b, . Also, the three-dimensional point cloud 1005, the three-dimensional point cloud 1006L, and the three-dimensional point cloud 1006R are within the acquisition angle FOV of the sensors 12a, 12b, ..., 12n, but the sensors 12a, 12b, ..., 12n Since only the closest information in each direction can be obtained, the 3D point group 1005 can be referred to, and the 3D point groups 1006L and 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, matching cannot be performed. Therefore, the 3D points 1006L and 1006R cannot be used for matching.

Therefore, the filter unit 212 excludes the 3D points 1006L and 1006R that cannot be referenced.

For the sake of 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 were explained assuming indoor walls, chairs, tables, etc. It may be composed of three-dimensional objects.

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

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

A three-dimensional point 303 is information about a pillar in the traveling environment acquired from the current position of the mobile object 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 the vehicle 100 .

The extracted 3D point 306 is a referable 3D point extracted in the 3D point discrimination step S204, and is a 3D point used in the matching step S205.

A non-referenceable three-dimensional point 307 is a three-dimensional point that is distinguished as a non-referenceable three-dimensional point in the three-dimensional point distinguishing step S204 and is not used in the matching step S205.
Existence probability distribution 304 of extracted three-dimensional point 303 is assumed to be existence probability distribution 301 whose range is similar to mobile object 100 .

, 12n, the existence probability distribution 304 of the extracted three-dimensional point 303 is larger than the existence probability distribution 301 of the moving object 100. When the information acquisition error of the sensors 12a, 12b, . If the information acquisition errors of the sensors 12a, 12b, . For example, if the sensors 12a, 12b, .

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

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

A conventional sensor detects a three-dimensional object that is closest to the sensor, and cannot detect another three-dimensional object even if there is another three-dimensional object behind the sensor.

Therefore, in the present invention, if the three-dimensional object closest to the sensor in the direction of the angle _θN is searched and extracted, the probability of matching increases.

Next, the setting of the angle _θN will be explained.

The angle θ _N is adjusted within a predetermined angle range θ _A to θ _B to search for the three-dimensional point with the highest matching probability. 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 assumed to be a fixed value. dθ may be set according to the resolution of the sensor.

Also, dθ may be matched with the existence probability distribution 301 of the moving object 100 . For example, if the reliability of the self-position is low, the temporary 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 temporary current position error estimated in the current position estimation step S201 is small, so dθ is set large.

Also, θ _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 azimuth error of the estimated current position is large. θ _B +α).

FIG. 4 shows an embodiment of the invention.

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

An existence probability distribution 402 is the existence probability distribution of the moving object 100 . It is assumed that the existence probability distribution 402 is small at the time when the moving object 100 starts running.

A range 403 is a range searched by the moving body 100 in the three-dimensional point discrimination step S204.

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

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

When the moving object 100 moves to the position of the above extracted feature, the position error of the moving object 100 estimated in the current position estimation step S201 increases, and the existence probability distribution of the moving object 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 device 19 around the moving body 100 Extract information. Then, in a three-dimensional point distinguishing step S204, a search is performed in a range 405 from θ _A ' (=-FOV-α) to θ _B ' (=+FOV+α) to distinguish three-dimensional points that can be referenced.

After correcting the current position of the moving body 100 in the position correction step S206, when performing the matching step S205 next, θ _A =−FOV and θ _B =+FOV are set in the external storage device information extraction step S203. The newly set θ _A and θ _B are used to distinguish between referable and non-referable 3D points in the 3D point distinguishing step S204.

Also, when the moving object 100 travels in an environment with few features and moves to the position of the closest feature, the control unit 15 selects a route that does not increase the position error estimated in the current position estimation step S201. 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 traveled distance, so the shortest distance is calculated as much as possible. Also, in order to avoid the estimated azimuth error of the moving object 100, a route that rotates as little as possible and allows straight movement is calculated. Therefore, when traveling in an environment with few features, even if there is a dynamic obstacle, the moving body 100 does not avoid it, but stops temporarily, and continues moving straight after the dynamic obstacle disappears.

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

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

An existence probability distribution 502 is the existence probability distribution of the current position of the moving object 100 .

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

The existence probability distribution 504 is the existence probability distribution of the three-dimensional point 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 object 100b existing in the environment while the moving object 100 is running. It is assumed that the existence probability distribution 506 is smaller than the existence probability distribution 504 . The moving object 100 and the moving object 100b share information on each other's position and existence probability distribution. Information sharing involves, for example, connecting to a LAN (Local Area Network) to transmit and receive information.

The timing at which the moving body 100 and the moving body 100b share information is immediately before the three-dimensional point discrimination step S204. Further, the timing at which the moving body 100 and the moving body 100b share information may be performed in a cycle shorter than the cycle of the three-dimensional point distinguishing step S204. Ultimately, any period may be set as long as the latest information can be shared in accordance with the period 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 the sensor. Due to existence probability distribution 506, there is an error in distance 507 to moving object 100 calculated by the sensor. Therefore, the moving body 100 exists between the distances 508 and 509 .

By fusing the distances 508 and 509 with the presence probability distribution 502 , the presence probability distribution of the moving object 100 is updated to 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 to become a narrow range indicated by reference numeral 511 . In other words, the surrounding information is acquired from the sensors of the moving object 100b, which is another moving object, and combined with the surrounding information from the sensors of the moving object 100, the current position of the moving object 100 (existence 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 within the range of the existence probability distribution 504, and the position of the moving object 100 is corrected.

For the sake of simplification, the description has been made with the moving body 100 and the moving body 100b stopped, but the above position correction may be performed while one or both of them are running. Also, for the sake of simplicity, two moving bodies have been described, but two or more moving bodies may be used.

FIG. 6 is an example of two mobile and fixed cameras.

In this figure, an existence probability distribution 601 is the existence probability distribution of the current position of the moving object 100 .

The existence probability distribution 602 is the existence probability distribution of the current position of the moving object 100b. Assume that existence probability distribution 602 is greater than existence probability distribution 601 .

In this case, even if the distance to the moving object 100 is calculated by the sensor installed on the moving object 100b, the distance between the distance 603 and the distance 604 is larger than the existence probability distribution 601. Position correction does not work.

In this figure, a fixed camera 605 (fixed sensor) installed in the traveling environment of the mobile object 100 can be used, and distances 606 and 607 from the fixed camera 605 to the mobile objects 100 and 100b are obtained. In this case, even if an error occurs in the distances 606 and 607, the moving object 100b exists between the distances 608 and 609, and the moving object 100 exists between the distances 610 and 611. should be. Therefore, the existence probability distribution 601 becomes the existence probability distribution 612, the existence probability distribution 602 becomes the existence probability distribution 613, and both have 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, it is possible to distinguish three-dimensional points that can be referred to in the three-dimensional point distinguishing step S204, and accurately in the matching step S205. The position of the moving body 100 can be corrected.

Further, a sensor installed on the moving object 100b calculates a distance 614 to the moving object 100, calculates an existence probability distribution 617 between the distance 615 and the distance 616, and calculates the existence probability distribution 617 between the distance 615 and the distance 616. Position correction may be performed. The existence probability distribution 617 has a narrower range than the existence probability distribution 612 . In other words, surrounding information is acquired from fixed sensors installed in the traveling environment of the moving object 100, and combined with surrounding information from the sensors possessed by the moving object 100, the current position of the moving object 100 (existence probability distribution 617 ).

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

Also, for the sake of simplicity, the fixed camera 605 has been described, but any sensor other than the camera may be used as long as it can estimate the moving object 100 and calculate the distance.

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

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

An existence probability distribution 702 is the existence probability distribution of the current position of the moving object 100 .

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

If there is a passerby 704 or the like between the moving object 100b and the fixed camera 701, the fixed camera 701 cannot detect the moving object 100b. In other words, passers-by 704 and the like can interfere with detection.

A distance 705 to the moving object 100 is calculated by the fixed camera 701, and as described with reference to FIGS. 5 and 6, the existence probability distribution 702 of the moving object 100 is reduced to the existence probability distribution 702b.

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

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

In the 3D point discrimination step S204, all the 3D points 707 around the moving object 100b can be extracted based on the current position and the sensor acquisition range. It is no longer detectable. Therefore, the presence of the passerby 704 and the direction α of the passerby with respect to the moving object 100b are transmitted from the fixed camera 701 to the moving object 100b.

As a result, a 3D point other than the direction α with a low matching probability is extracted in the 3D point discrimination step S204, and the position is estimated in the matching step S205 and the position correction step S206.

In this figure, for the sake of simplicity, the case of one passerby has been described, but there may be a plurality of passersby. In addition, when it is impossible to detect other moving bodies or fixed sensors or correct the position due to the influence of a plurality of passersby, it temporarily stops and waits until there are no passersby. Also, if the passerby does not disappear for a while, the moving object may avoid the passerby and move to a position where other moving objects or fixed sensors can be detected.

Although the passerby has been explained here, generally, animals other than humans, objects, etc. are collectively referred to as "obstacles".

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

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

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

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

In FIG. 8A , a three-dimensional point 801 (● mark) is the running environment of the moving object 100 registered in the external storage device 19 .

A three-dimensional point 802 (marked with a circle) is a three-dimensional point extracted from the current position of the moving object 100 by a sensor. As a method of matching the three-dimensional points 801 and 802, for example, ICP Matching (ICP is an abbreviation for Iterative Closest Point) is used. Alternatively, 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 points 801 and 802 are output, a matching method other than ICP may be applied. Here, roll, pitch, and yaw are respectively 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, and the angle at which the tip of the moving body 100 moves up and down. The tip of 100 represents the angle at which it oscillates to the left and right.

Layers 803R and 803L indicated by dashed lines in the figure are divisions for digitizing the surrounding conditions of the moving object 100. FIG. Here, for the sake of simplicity, the layers 803R and 803L divide the driving environment into distance and position, respectively, and divide them into regions containing point groups of three-dimensional points. It is represented as a set. The layer 803R is located on the right side of the traveling direction of the moving body 100, and the layer 803L is located on the left side of the moving body 100 in the traveling direction. Also, the "position" is the coordinates of the running surface and is two-dimensional coordinates.

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

Layer (n) = f (distance, position, height, color, ...) (1)
In the formula, the attributes of the respective layers 803R and 803L are described in parentheses on the right side. The attributes of the layers 803R and 803L are numerical values ( The average value of the point cloud is calculated and shown for the feature amount). In other words, 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 region. d _i is the distance (value specified according to a predetermined rule) from the moving body 100 to the boundary with the adjacent area (for example, i+1). R is a numerical value representing the right side of the moving body 100 .

In the above equation (2), attributes are limited to distance and position. This 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 region. d _j is the distance (a value specified according to a predetermined rule) from the moving body 100 to the border of the adjacent area (eg j+1). L is a numerical value representing 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 input to each layer based on the distance and position. In the three-dimensional point distinguishing step S204, three-dimensional points that can be referenced for each layer are extracted, and in the environment information extraction step S202, the information obtained by the sensor is input to each layer.

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

Layer (k)=( _Xk , _Yk , _Zk , _rollk , _pitchk , _yawk , ...) (4)
In the formula, k is an integer of 1 or more and a code representing each region.

In the position correction step S206, the corrected position candidates from each layer are merged, and the corrected position of the moving object 100 is output. The fused position is, for example, the average of the positions obtained in each layer. Alternatively, the fused position may be the median value of the positions obtained in each layer. Alternatively, the fused position may be the mode of the positions obtained in each layer.

Alternatively, the points T _k (k is an integer equal to or greater than 1 and is a code representing each region) in each layer may be used as weights for fusion.

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

X=Σ( _Xk · _Tk )/ _ΣTk (5)
Y=Σ( _Yk · _Tk )/ _ΣTk (6)
Z=Σ( _Zk · _Tk )/ _ΣTk (7)
roll=Σ(roll _k ·T _k )/ΣT _k (8)
pitch=Σ(pitch _k ·T _k )/ΣT _k (9)
yaw=Σ( _yawk · _Tk )/ _ΣTk (10)
Further, when the sensor is a camera, the calculated distance _error is proportional to the distance. good. Further, the score (the difference between the information extracted by the sensor and the information of the external storage device) output by ICP Matching may be used as a weight, and the modified position candidates from each layer may be merged.

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

In the process illustrated in FIG. 8B, attributes are used for matching.

In this figure, the layering step S810 divides 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 into layers using the above equation (1). This is the dividing step.

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

A matching result fusion step S830 fuses the results of each layer matching step S820 obtained for each layer. For fusion, the above formulas (4) to (10) are used.

1: position estimation device, 2: signal reception unit, 12, 12a, 12b, 12n: sensors, 13: information processing device, 14: sensor processing unit, 15: control unit, 16: memory, 17: display unit, 19: external storage device, 100: moving object, 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: 3D point, 605: fixed camera.

Claims (6)

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 part;
a matching unit;
In a moving body having a self-position correction unit,
The three-dimensional point acquisition unit acquires a three-dimensional point obtained from the sensor,
The filtering unit obtains a 3D point that can be referenced from the current temporary position of the moving body among the 3D points included in the 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,
The self-position correcting unit obtains the current position of the moving object on the map based on the successfully matched three-dimensional point,
The three-dimensional point acquisition unit acquires surrounding information from a fixed sensor installed in the running environment of the moving object or a sensor possessed by another moving object, and obtains surrounding information from the sensor possessed by the moving object. Find the current position by combining
The self-position correcting unit corrects the presence of the moving object by using the existence probability distribution at the current temporary position of the moving object and the position information of the moving object obtained by the fixed sensor or the other moving object. The range of the probability distribution is corrected to a narrow range,
the position information of the moving object is an error range of the distance to the moving object obtained by the fixed sensor or the other moving object;
The moving body, wherein the correction is performed by fusing the error range and the existence probability distribution of the moving body. The referable three-dimensional point is a three-dimensional point on the map within the range of the sensing angle that the sensor can acquire, and a plurality of three-dimensional points exist on the map in the same direction of the sensing angle. , is obtained by extracting a three-dimensional point closest to the moving object. 2. The moving body according to claim 1 , wherein the existence probability distribution of said other moving body is smaller than the existence probability distribution of said moving body at said current temporary position. When the three-dimensional point acquired by the three-dimensional point acquiring unit includes the three-dimensional point of a moving obstacle,
3. The moving body according to claim 1, wherein said matching unit performs said matching using three-dimensional points other than the three-dimensional points of said obstacles. 3. The moving body according to claim 1, wherein said matching unit performs said matching using a layer having attributes as variables. 6. The moving body according to claim 5, wherein said matching unit sets weights for said attributes of said layers and performs said matching.

Images