Position estimation system

Abstract

To provide a position estimation system capable of generating a large number of current position candidates suitable for deriving an actual current position (true value).SOLUTION: A position estimation system mounted on a moving object, generating current position candidates based on information from sensors and extracting the candidate with the highest probability as the current position of the moving object from the current position candidates, includes: straight line recognition means for recognizing straight lines included in a space in which the moving object exists, based on information from the sensor mounted on the moving object; landmark recognition means for recognizing landmarks included in a space in which the moving object exists, based on information from the sensor mounted on the moving object; generating means for generating a plurality of the current position candidates of the moving object at the position at a relative distance between the recognized landmark and the moving object in the same direction as, the opposite direction to, and the perpendicular direction to the direction of many parallel lines among the recognized straight lines to the recognized landmark; and an extraction unit that extracts the candidate with the highest probability as the current position of the moving object from the current position candidates.SELECTED DRAWING: Figure 1

Description

Technical Field

[0001] The present invention relates to a position estimation system for a moving object.

Background Art

[0002] As a moving object, for example, a transport robot has been developed to collect information around it and estimate the current position of the moving object. Current position estimation corrects a provisional current position by matching the information around the moving object collected from the provisional current position of the moving object with a map of a traveling environment created in advance. On the other hand, when the provisional current position is unknown or the error is large, even if matching is performed, the provisional current position cannot be corrected (hereinafter, this event is referred to as "the moving object is lost"), and thus transportation becomes impossible.

[0003] For example, Non-Patent Document 1 generates a plurality of candidates for the current position having random positions and orientations based on a predetermined number (P) using a Particle Filter when the moving object is lost. Next, based on the information around the moving object actually collected while the moving object is traveling and the predicted information around the moving object based on the map information, the existence probability of each candidate for the current position is calculated. Near the candidate for the current position with a high probability, new candidates are generated, and candidates for the current position with a low probability are deleted, thereby adjusting the number of candidates for the current position. Finally, the candidate for the current position with the highest probability is set as the current position.

Prior Art Documents

Non-Patent Documents

[0004]

Non-Patent Document 1

Summary of the Invention

Problems to be Solved by the Invention

[0005] However, as in Non-Patent Document 1, when generating candidates for the current position based on a randomly predetermined number of candidates without adapting to the size of the driving environment or the landmark arrangement, even if the moving object collects surrounding information, there is no guarantee that the candidate with the highest probability of the current position converges to the actual current position (true value).

[0006] Also, when there are many candidates for the position, the processing time increases. On the other hand, when there are few candidates, accurate position estimation cannot be performed.

[0007] From the above, an object of the present invention is to provide a position estimation system capable of generating a large number of candidates for the current position suitable for leading to the actual current position (true value).

Means for Solving the Problems

[0008] In order to solve the above problems, the position estimation system for a moving object of the present invention is "a position estimation system mounted on a moving object, which generates candidates for the current position based on information from sensors and extracts the candidate with the highest probability as the current position of the moving object from the candidates for the current position. It includes a straight line recognition means for recognizing a straight line included in the space where the moving object exists based on information from sensors mounted on the moving object, a landmark recognition means for recognizing a landmark included in the space where the moving object exists based on information from sensors mounted on the moving object, and for the recognized landmark, in the same direction, the opposite direction, and the orthogonal direction of the direction of the parallel lines with the largest number among the recognized straight lines, and at a position corresponding to the relative distance between the recognized landmark and the moving object, a generation means for generating a plurality of candidates for the current position of the moving object, and an extraction unit for extracting the candidate with the highest probability as the current position of the moving object from the candidates for the current position. A position estimation system characterized by that."

Effects of the Invention

[0009] According to the present invention, since a large number of candidates for the current position suitable for deriving the actual current position (true value) can be generated, a moving body equipped with a position estimation system can improve the accuracy of position estimation when it is lost. Further, according to an embodiment of the present invention, accurate and low-processing position estimation becomes possible, and optimization of candidate generation for the position is achieved.

Brief Description of the Drawings

[0010]

Figure 1

Figure 2

Figure 3

Figure 4a

Figure 4b

Figure 5a

Figure 5b

Figure 6a

Figure 6b

Figure 6c

Figure 6d

Mode for Carrying Out the Invention

[0011] Hereinafter, a position estimation system for a moving body according to an embodiment of the present invention will be described with reference to the drawings.

Example

[0012] FIG. 1 is a diagram showing a configuration example of a mobile object position estimation system according to Embodiment 1 of the present invention. In FIG. 1, the mobile object 1 is equipped with a position estimation system 10 according to an embodiment of the present invention. The position estimation system 10 composed of a computer has a plurality of sensors 12 (12a, 12b, ··· 12n), an input unit (signal reception unit) 2, a processing unit 14, a control unit 15, a memory 16, an output unit 17, etc. commonly connected by a bus 18, and various kinds of information are shared. Note that the position estimation system 10 is connected to an external storage device 19 via a communication unit (not shown).

[0013] Among these, the signal reception unit 2 receives a position signal from the outside (in this case, the position signal includes not only a signal directly indicating the position but also a signal that can become a position by its processing). The signal reception unit 2 is, for example, a GPS that estimates the current position in the absolute coordinates of the world. Also, the signal reception unit 2 may be an RTK-GPS that estimates the current position with higher accuracy than GPS. Also, the signal reception unit 2 may be a quasi-zenith satellite. Also, the signal reception unit 2 may be a beacon fixed at a known position. Also, the signal reception unit 2 may receive from sensors that estimate the position in relative coordinates, such as a wheel encoder, an IMU, or a gyro. Also, the signal reception unit 2 may receive information such as the shape, size, and height of three-dimensional objects in the driving environment. Finally, anything that can obtain information that can be used for estimating the current position, controlling, and recognizing the mobile object 1 is acceptable.

[0014] The sensor 12 is, for example, a still camera or a video camera. Also, the sensor 12 may be a monocular camera or a compound eye camera. Also, the sensor 12 may be a laser sensor. The sensor 12 may be anything that can finally extract landmark information of the driving environment from the output of the sensor 12.

[0015] Sensor 12a is installed, for example, in front of the moving body 1. The acquisition direction of sensor 12a is directed forward of the moving body 1. Sensor 12a acquires, for example, the distant view information in front of the moving body 1. The other sensors 12b, ··· sensors 12n are installed at positions different from that of sensor 12a and image different imaging directions or regions from sensor 12a. Sensor 12b may be installed, for example, behind and downward of the moving body 1. Sensor 12b may acquire the close view information behind the moving body 1.

[0016] When sensor 12 is a monocular camera, if the floor or ceiling is flat, the relationship (x, y) between the pixel position on the image and the actual ground position becomes constant, so the distance from sensor 12 to the landmark can be geometrically calculated. Also, when the floor or ceiling is not flat, the distance to the landmark can be estimated based on the amount of movement of the landmark in the time series on the image and the amount of movement of the moving body 1 received from the signal reception unit 2. When sensor 12 is a stereo camera, the distance to the landmark on the image can be accurately geometrically measured. Also, when sensor 12 is a laser, more accurate and distant information can be acquired. In the following description, cases using a camera or a laser will be described, but other sensors (such as a camera with a wide-angle lens or a TOF camera) may be used as long as the distance to surrounding three-dimensional objects can be calculated.

[0017] Also, sensors 12a, 12b, ··· sensors 12n are preferably installed on the moving body 1 under conditions where they are not simultaneously affected by environmental disturbances such as obstacles and sunlight. For example, while sensor 12a is installed forward in front of the moving body 1, sensor 12b may be installed rearward or downward behind the moving body 1. Thereby, for example, even when sensor 12a cannot acquire accurate information due to the influence of sunlight, it is less likely to affect sensor 12b in the reverse direction or downward direction of the traveling direction, so distance measurement can be performed without being affected by environmental disturbances.

[0018] Further, the sensors 12a, 12b, ···, 12n may acquire information under different acquisition conditions (aperture value, white balance, period, etc.). For example, by mounting a sensor with parameters adjusted for a bright place and a sensor with parameters adjusted for a dark place, it may be possible to perform imaging regardless of the brightness of the environment.

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

[0020] Also, different tasks may be performed using the information acquired by the sensors 12a, 12b, ···, 12n respectively. For example, the position of the moving body 1 is estimated based on the information acquired by the sensors 12a and 12b, and an obstacle is detected by the sensors 12c and 12d. Finally, the results based on the information obtained by each of the sensors 12a, 12b, ···, 12n are fused, and it is sufficient if the moving body 1 can be controlled by the control unit 15. Also, when the CPU of the control unit 15 can only process in a single thread, the information obtained by each of the sensors 12a, 12b, ···, 12n is processed in the order of the sensors 12a, 12b, ···, 12n. On the other hand, when the CPU of the control unit 15 can process in a multi-thread, the information obtained by the sensors 12a, 12b, ···, 12n can be processed simultaneously.

[0021] The processing unit 14 processes the information acquired by the signal reception unit 2 or the sensors 12 to calculate the position of the moving body 1, the recognition of the landmark, or the distance to the landmark. For example, the processing unit 14 calculates the movement amount of the moving body 1 from the information acquired by the sensors 12 in time series, and adds the movement amount to the past position to estimate the current position. The processing unit 14 may extract features from each piece of information acquired in time series.

[0022] The processing unit 14 further extracts the same features from the subsequent information. Then, the processing unit 14 calculates the moving amount of the moving body 1 by tracking the features. In addition, the processing unit 14 calculates the shape of the driving environment of the moving body 1 using Simultaneous Localization and Mapping (SLAM) or Structure from Motion technology. Further, the processing unit 14 performs map matching that compares the information acquired by the sensor 12 with the shape information of the pre-created driving environment using the Iterative Closest Point (ICP) technology, and estimates the position of the moving body 1.

[0023] In addition, the processing unit 14 processes the information obtained from the external storage device 19. The processing unit 14 may perform a display according to the calculated position or moving amount, or may output a signal related to the control of the moving body 1. For example, the processing unit 14 processes the information acquired by the sensor 12 during the travel of the moving body 1 to detect an obstacle. Also, for example, the processing unit 14 processes the information acquired by the sensor 12 during the travel of the moving body 1 to recognize a predefined landmark. Further, the processing unit 14 generates candidates for the current position. Also, the processing unit 14 updates each candidate for the current position based on the calculated moving amount of the moving body 1. Further, the processing unit 14 predicts the position of the landmark based on each updated candidate for the current position and the information received from the external storage device 19. Details of the processing unit 14 will be described later.

[0024] The control unit 15 outputs a command regarding the moving speed to the moving body 1 based on the result of the information processing by the processing unit 14. For example, the control unit 15 may output a command to increase, decrease, or maintain the moving speed of the moving body 1 according to the resolution of the three-dimensional object in the information, the number of outliers among the features in the information, or the type of information processing.

[0025] Memory 16 includes the main storage device (main memory) of the mobile body 1 and auxiliary storage devices such as storage. The processing unit 14 performs various information processes based on the information data stored in the memory 16 and the acquisition time. In this information process, for example, intermediate information is created and stored in the memory 16. The intermediate information may be used for judgments and processes by the control unit 15 and others in addition to the processing by the processing unit 14.

[0026] The output unit 17 outputs the state of the mobile body 1 and performs outputs such as displaying the next operation. The output unit 17 outputs, for example, the information obtained by the sensor 12. Also, the output unit 17 outputs the processing result of the processing unit 14 and the commands of the control unit 15. For example, it outputs the destination to which the mobile body 1 will move next and the landmarks recognizable from the current position. Also, the output unit 17 outputs the surrounding obstacles recognized by the sensor 12. Also, the output unit 17 outputs map information. Note that the function of the output unit is mounted on the mobile body 1, but the output destination, for example, the monitor, may be mounted on or not mounted on the mobile body 1.

[0027] The bus 18 can be composed of an IEBUS (Inter Equipment Bus), a LIN (Local Interconnect Network), a CAN (Controller Area Network), etc.

[0028] The external storage device 19 has map information of the environment in which the mobile body 1 travels. The map information of the external storage device 19 is, for example, the shape and position of stationary objects (walls, pillars, signboards, lighting, tables, chairs, etc.) in the traveling environment. Each map information of the external storage device 19 may be represented by a mathematical formula. For example, it is not necessary to configure line information with a plurality of points, and only the slope and intercept of the line may be sufficient. Also, without distinguishing the map information of the external storage device 19, it may be represented by a point cloud. The point cloud may be represented by 3D (x, y, z), 4D (x, y, z, color), etc. Finally, as long as the traveling environment can be detected from the current position of the mobile body 1 and map matching can be performed, the map information of the external storage device 19 can be in any form.

[0029] When the control unit 15 receives a command to start acquisition, the external storage unit 19 sends map information to the memory 16. When the external storage device 19 is mounted on the moving body 1, information is transmitted and received via the bus 18. On the other hand, when the external storage device 19 is not mounted on the moving body 1, the signal reception unit 2 connects the moving body 1 and the external storage device 19. The connection between the moving body 1 and the external storage device 19 is made, for example, via a Local Area Network (LAN). Also, the connection between the moving body 1 and the external storage device 19 may be made via a Wide Area Network (WAN). Also, when receiving a command from the control unit 15, the external storage device 19 sends map information of the driving environment of the moving body 1 to the processing unit 14.

[0030] Figure 2 shows an example of a processing flow in a position estimation system for a moving body according to an embodiment of the present invention. Here, an example of the processing content in the processing unit 14 is shown. Note that the flow in Figure 2 is activated on the premise that the moving body is lost, and it is assumed that before being lost, travel control was performed based on separately initialized position information.

[0031] First, at the beginning of this series of processes, in the information acquisition process step S201, the sensor 12 acquires information on the driving environment, for example, at a predetermined time interval.

[0032] Next, in the straight line recognition process step S202, a straight line in the driving environment is recognized from the information obtained in the information acquisition process step S201. When the sensor 12 is a camera, the straight line recognition process step S202 extracts a straight line from the image obtained by the camera sensor 12. As a method of extraction, for example, the Hough Transform can be used. Also, when the sensor 12 is a laser, a straight line is extracted based on the positional relationship of the acquired corners and planes.

[0033] Here, the driving environment in which the moving body 1 is arranged can be indoors or outdoors as long as it is an area where a straight line exists. However, in the following description of the present invention, an indoor environment constructed by humans is assumed and exemplified as the driving environment. In particular, in the case of indoors, since a large number of straight lines constituting a rectangular pattern are arranged, it can be said that it is easy to extract a large number of straight lines constituting the rectangular pattern.

[0034] When a plurality of lines can be extracted from the driving environment, calculate the direction of each extracted line and extract the line in the most frequent direction. Also, because there is an error in the sensor 12, before extracting the line in the most frequent direction, calculate the relative angle to the lines having other directions. If the calculated relative angle is 90 degrees, the probability that it is a line constituting a rectangular pattern is high, so the line is extracted, and if the relative angle is different from 90 degrees, it is not extracted. Finally, since it is necessary to estimate the orientation of the moving body 1 with respect to the rectangular pattern, when a plurality of lines are extracted, calculate the average or median of the plurality of extracted lines. The detailed processing content of the straight line recognition processing step S202 will be described later with reference to FIG. 3.

[0035] In the confirmation processing step S203 of FIG. 2, it is confirmed whether or not a straight line can be recognized from the information of the driving environment by the processing of the straight line recognition processing step S202. If a line cannot be recognized from the current position of the moving body 1 due to the influence of an obstacle or disturbance, etc., return to the information acquisition processing step S201 and extract information on a new environment. The information on the new environment in this case is the information in the new environment after the moving body has moved. However, in a more proactive sense, it is desirable to switch a plurality of sensors installed above, in front, behind, left, and right, and for example, after checking the front sensor, perform processing on the left, right, or rear sensor and also execute processing to increase the possibility of detecting a line. After that, if a straight line can be recognized, proceed to the landmark recognition processing step S204.

[0036] In the landmark recognition processing step S204, a specific landmark pre-registered in the external storage device 19 is recognized from the information acquired from the driving environment by the sensor 12. The landmark is, for example, a stationary object such as a signboard, lighting, table, chair, device, etc., or the landmark is an edge or corner of the driving environment, and since its position and shape are stored in the external storage device 19, it is possible to recognize the landmark by referring to the external storage device 19 using the information acquired from the driving environment by the sensor 12.

[0037] Also, in the landmark recognition processing step S204, after recognizing a specific landmark, the relative distance between the moving body 1 and the recognized landmark is measured. When the sensor 12 is a sensor capable of calculating distance information, such as a stereo camera or a lidar, the distance information measured by the sensor 12 can be used as the relative distance.

[0038] On the other hand, when the sensor 12 is a monocular camera that cannot directly calculate the distance, it is possible to measure the relative distance to the landmark recognized by deep learning or geometrically. When the landmark is other than a point cloud, it is necessary to determine the coordinates of the relative distance in advance. For example, when a landmark is recognized from an image in the landmark recognition processing step S204, the center of the landmark on the image is calculated, and the relative distance from the sensor 12 is measured with the center as the coordinate reference. Also, instead of the center, a corner on the image may be used as the coordinate reference.

[0039] Finally, using the output of the sensor 12, the position of the landmark recognized in the landmark recognition processing step S204 should match the position of the landmark registered in the external storage device 19.

[0040] In the confirmation processing step S205, it is confirmed whether or not a landmark can be recognized from the information of the driving environment acquired by the sensor 12. If the landmark cannot be recognized from the current position of the moving body 1 due to the influence of obstacles or disturbances, etc., the process returns to the information acquisition processing step S201, and new environmental information is extracted by changing the sensor or the like. If a landmark can be recognized, the process proceeds to the candidate generation processing step S206.

[0041] In the candidate generation processing step S206, candidates for the current position are generated based on the processing in the straight line recognition processing step S202, the processing in the landmark recognition processing step S204, and the landmarks pre-registered in the external storage device 19. The detailed processing content of the candidate generation processing step S206 will be described later with reference to FIG. 4.

[0042] In the prediction processing step S207, the positions of the candidates for the current position respectively generated in the candidate generation processing step S206 are updated, and the positions of the landmarks are predicted at the updated positions. The detailed processing content of the prediction processing step S207 will be described later with reference to FIG. 5.

[0043] In the current position extraction processing step S208, the actual current position is extracted from the candidates for the current position respectively obtained in the prediction processing step S207.

[0044] FIG. 3 is a diagram for explaining the detailed processing content of the straight line recognition processing step S202 by way of an example. In this example, for the reason of simplicity of explanation, the sensor 12 is a camera, and the example is that the ceiling of the driving environment is imaged. In this example, the frame 300 is the frame (image) acquired by the sensor 12, and the coordinates 301 are the coordinates of the frame 300 shown in the U-axis - V-axis.

[0045] A large number of lines are reflected in the frame 300. Among these, the lines 302a to 302g are parallel lines reflected in the frame 300. The line 303 has a direction different from that of the parallel lines 302a to 302g. The lines 304a to 304b are lines arranged at 90 degrees with respect to the parallel lines 302a to 302g. Also, the walls 305a to 305c are the walls of the driving environment reflected in the frame 300.

[0046] To recognize the lines constituting the rectangular pattern of the driving environment from among these various lines, first, all the lines (lines 302a to 302g, line 303, lines 304a to 304b) reflected in the frame 300 are extracted.

[0047] Next, calculate the directions of the extracted lines respectively, and obtain the direction of the line with the largest number. In this case, the directions of the parallel lines 302a to 302g are extracted as the direction of the line with the largest number. Also, even if there are lines (outliers) with different directions like the line 303, since the number is small compared to the number of lines 302a to 302g, it is not recognized in the straight line recognition processing step S202.

[0048] Also, in this process, it is difficult to extract the parallel lines 302a to 302g due to the influence of obstacles, and there may be a case where the line 303 is misrecognized. Therefore, the reliability of straight line recognition is increased using the directions of the lines 304a to 304b. For example, since the line 303 is not arranged at 90 degrees with respect to the lines 304a to 304b, the possibility that it is a line forming a rectangle is low. On the other hand, since the parallel lines 302a to 302g are arranged at 90 degrees with respect to the lines 304a to 304b, the probability that they are lines forming a rectangle pattern is high. Therefore, the directions of the parallel lines 302a to 302g are recognized. Also, if the lines 304a to 304b arranged at 90 degrees with respect to the lines 302a to 302g are not shown in the image acquired by the sensor 12, straight line recognition can be performed based on the directions of the lines recognized in the past in time series and the past movement amount of the moving body 1. In this way, in the straight line recognition processing step S202, the direction of the line with the largest number is extracted.

[0049] FIG. 4a and FIG. 4b are diagrams for explaining the detailed processing content of the candidate generation processing step S206 with an example. In this example, for the reason of simplicity of explanation, it is assumed that the sensor 12 is a camera and images the ceiling of the driving environment. Also, the frame 300 is the frame acquired by the sensor 12. The coordinate 301 is the coordinate of the frame 300 shown by the U-axis - V-axis.

[0050] FIG. 4a is a frame with the same content as FIG. 3 acquired by the sensor 12. However, here, attention is paid to the line direction 402 recognized in the straight line recognition processing step S202 and the landmark 403 recognized in the landmark recognition processing step S204 and shown. In this figure, the landmark 403 is indicated by a circle mark.

[0051] Figure 4b shows an example of a driving environment registered in the external storage device 19, and is an example of the arrangement information of landmarks managed under the coordinates 404 shown on the X-axis - Y-axis. Here, landmarks 405a to 405n of the driving environment registered in the external storage device 19 and their positions are shown. In addition, in Figure 4b, candidates 407a to 407d for the current position when temporarily focusing on the landmark 405n are illustrated. Note that the distance d406 is the relative distance from the sensor 12 recognized in the landmark recognition processing step S204 to the recognized landmark 405n.

[0052] Regarding the explanation in Figure 4b, the matters that can be confirmed by the processing up to the confirmation processing step S205 are only three points: the existence of the landmark, the direction of the straight line, and the fact that the relative distance to the landmark has been determined. It is not known which of the multiple landmarks illustrated in Figure 4b is the confirmed landmark, nor the current position (direction, orientation) with respect to the confirmed landmark.

[0053] From these facts, in the candidate generation processing step S206, candidates 407a to 407d for the current position are generated as follows. Here, the idea of generating the current position candidates will be explained using the landmark 405n in Figure 4b.

[0054] First, based on the direction 402 of the line recognized in the straight line recognition processing step S202 and the distance d406 to the landmark recognized in the landmark recognition processing step S204, a candidate 407a for the current position is calculated. In this process, since the driving environment is an indoor environment and many rectangular patterns are arranged, the fact that the direction 402 of the line recognized in the straight line recognition processing step S202 is one direction of the rectangular pattern is utilized to generate candidates 407b to 407d for the current position rotated 90 degrees, 180 degrees, and 270 degrees with respect to the direction 402 of the line, that is, in the remaining three directions. This means that candidates 407b to 407d for the current position are additionally arranged in the direction opposite to or orthogonal to the recognized direction 402 of the line.

[0055] In this description, the landmark 405n in FIG. 4b is used for explanation. However, on the premise, it is not known whether the landmark recognized in the landmark recognition process step S204 is actually the landmark 405n registered in the external storage device 19. For this reason, candidates for the current position are generated for all of the landmarks 405a to 405n registered in the external storage device 19. Also, since the four directions of the rectangle and the number n of the landmarks registered in the external storage device 19 are known, the number (n * 4) of candidates for the current position can be accurately calculated. That is, different from a known example of randomly generating candidates for the current position, the present invention can calculate the number of candidates including the accurate position and orientation of the candidates for the current position.

[0056] Note that the number (n * 4) of the candidates for the current position described above is the minimum number required for position estimation. However, when the disturbance of the traveling environment or the error of the sensor 12 is large, the candidates for the current position may be increased to improve the reliability of the position estimation. For example, in addition to the candidates for the current position generated by the present invention, candidates for the current position may be randomly generated as in the prior art. Also, a parameter k for increasing the minimum required number (n * 4) of the candidates for the current position can be used. In that case, the number of candidates for the current position to be generated becomes n * 4 * k, and the parameter k is adjusted according to the disturbance of the traveling environment and the error of the sensor 12. Also, instead of randomly, the number of candidates for the current position may be adjusted based on the error of the sensor 12 and the generated candidates for the current position (n * 4). For example, when the error of the sensor 12 is 1 meter, other candidates for the current position within 1 meter from the center of each generated candidate for the current position (n * 4) may be generated. Finally, if there is information that can identify the actual current position of the moving body 1, the parameter k may be adjusted based on that information. For example, when the actual current position of the moving body 1 can be narrowed down to a specific area on the map based on the information received by the signal receiving unit 2, it is not necessary to generate n * 4, and fewer candidates for the current position than n * 4 are generated around the specified area. Furthermore, additional candidates can be arranged at 45-degree intervals between the candidates arranged at 90-degree intervals.

[0057] Using FIGS. 5a and 5b, the processing content of prediction processing step S207 will be described with an example. FIG. 5a is a diagram showing candidates 502a to 502d of the current position after adding a movement amount D501 to candidates 407b to 407d of the current position in FIG. 4b, respectively. By adding the movement amount D501, 407a has shifted to the position of 502a, 407b has shifted to the position of 502b, 407c has shifted to the position of 502c, and 407d has shifted to the position of 502d, respectively.

[0058] In FIG. 5b, candidates 503a and 503b of the current position (after adding the movement amount D501) are the latest candidates of the current position when the landmark 505 is actually recognized by the sensor 12. FIG. 5b shows the positions after converting the landmark 505 actually recognized by the sensor 12 into the coordinates of the respective candidates 503a to 503b of the current position.

[0059] On the other hand, landmarks 504a to 504b indicate the positions of the nearest landmarks to candidates 503a to 503b of the current position based on the information in the external storage device 19. Also, the landmark 505 is the landmark actually recognized at the actual current position of the moving body 1.

[0060] In the positional relationship of FIG. 5b, in the prediction processing step S207, the actually recognized landmark 505 is compared with the landmarks 504a to 504b obtained from the coordinates of the candidates 503a to 503b of the current position. For the candidate 503a of the current position, the distance between 505 in its vicinity and 504a, and for the candidate 503b of the current position, the distance between 505 in its vicinity and 504b are respectively evaluated.

[0061] According to the evaluation result, in the current position extraction processing step S208, candidates of the current position are extracted. In this case, since the distance from the landmark 505 to the landmark 504a is closer than the distance from the landmark 505 to the landmark 504b, the actual current position of the moving body 1 is set as the candidate 503a of the current position, and 503a is extracted as the candidate of the current position in the current position extraction processing step S208.

[0062] On the other hand, since there is a possibility that an error may occur in the sensor 12, observations may be increased to improve the robustness of the current position extracted in the current position extraction process step S208. For example, the distances from the landmarks 504a and 504b recognized in time series to the landmark 505 are calculated, the average of the calculated distances is calculated, and the candidate for the current position with the smallest average distance is set as the actual current position. Also, using Bayesian probability, the existence probabilities of the respective current position candidates may be calculated for the prediction of the recognizable landmark from each of the calculated latest current position candidates and the actually recognized landmark by the sensor 12. Then, the existence probabilities of the respective current position candidates are calculated in time series, and if there is a candidate for the current position whose existence probability is greater than a predetermined existence probability threshold, that candidate for the current position is set as the current position.

[0063] The position estimation system described above can accurately generate position candidates based on the relative position from the extracted landmark and the straight line pattern on the assumption that there is a straight line pattern with the landmarks arranged in the driving environment, so that the position candidates can be accurately calculated, and thus position estimation can be performed with optimal processing time and accuracy. In particular, accurate and low-processing position estimation and optimization of position candidate generation can be achieved.

Example

[0064] In the second embodiment, the operation of the moving body 1 equipped with the position estimation system of the first embodiment in the driving environment will be described.

[0065] Fig. 6a shows a state where the moving body 1 is performing a conveyance operation within a factory (traveling environment). Here, the coordinates 601 indicated by the X-axis - Y-axis are the coordinates of the landmark information of the traveling environment registered in the external storage device 19. Also, the sensor acquisition range 602 is the information acquisition range of the sensor 12. For simplicity, the sensor 12 is assumed to be a single camera. Also, the landmarks 603a to 603e are lights arranged on the ceiling inside the factory and are the information registered in the external storage device 19. In this state, the moving body 1 performs map matching based on the landmark information obtained by the sensor 12 and estimates its position. In this case, it is assumed that the moving body 1 can perform the conveyance operation without problems.

[0066] Fig. 6b shows a state where an obstacle exists within the sensor acquisition range 602. In this state, an obstacle 604 (for example, an operator) enters the sensor acquisition range 602, and the moving body 1 can no longer estimate its position. Therefore, the current position of the moving body 1 with respect to the coordinates 601 becomes unknown.

[0067] Fig. 6c shows the operation after the moving body 1 becomes unable to estimate its position. At this time, according to the processing of Fig. 2, first, in the information acquisition processing step S201, an image of the traveling environment is acquired by the sensor 12. Next, in the straight line recognition processing step S202, straight line recognition is performed from the image acquired in the information acquisition processing step S201. It is assumed that the straight line 605 is recognized from a certain box 604a to 604n inside the factory by this recognition processing.

[0068] Next, in the landmark recognition processing step S204, the landmark 603a is recognized, and it is assumed that the distance d606 from the sensor 12 to the landmark 603a is calculated. Then, based on the distance d606, the direction of the recognized straight line 605, and the number of landmarks registered in the external storage device 19, in the candidate generation processing step S206, candidates for the current position are generated. In this case, since there are 5 landmarks in the external storage device 19, in the candidate generation processing step S206, 4 * 5 = 20 candidates for the current position are generated. Note that the candidates 607a to 607n for the current position are the candidates for the current position generated in the step candidate generation.

[0069] Figure 6d shows the operation when the moving body 1 recovers the position estimation. In Figure 6d, the candidates 608a to 608n of the latest current position are the latest positions of the respective generated current position candidates calculated by the prediction means 207.

[0070] Suppose that the landmark 609 is recognized by the sensor 12 at a certain time. After converting the landmark 609 actually recognized by the sensor 12 into the coordinates of the respective current position candidates 607a to 607n, it is compared with the respective landmark information registered in the external storage device 19. Therefore, since the prediction based on the information registered in the external storage device 19 that is closest to the landmark 609 converted into the coordinates of the current position candidate 608b is for the landmark 603b, the current position extraction means 208 sets the current position candidate 608b as the current position. Accordingly, the moving body 1 can continue the conveyance work in the factory.

Explanation of Signs

[0071] 1: Moving body 2: Signal reception unit 12: Sensor 14: Processing unit 15: Control unit 16: Memory 17: Output unit 19: External storage device

Claims

1. A position estimation system mounted on a moving body, which generates candidates for the current position based on information from sensors and extracts the candidate with the highest probability as the current position of the moving body from the candidates for the current position, comprising: The position estimation system includes: A straight line recognition means for recognizing a straight line included in the space where the moving body exists based on information from sensors mounted on the moving body; A landmark recognition means for recognizing a landmark included in the space where the moving body exists based on information from sensors mounted on the moving body; A landmark position acquisition means for acquiring the positions of landmarks registered in a storage device; For the landmark registered in the storage device, the direction of the parallel lines with the largest number among the recognized straight lines, and for the landmark registered in the storage device, the directions obtained by rotating the parallel lines by 90 degrees, 180 degrees, and 270 degrees, and at the position of the relative distance between the recognized landmark and the moving body, a generation means for generating a plurality of candidates for the current position of the moving body; A movement amount addition means for adding the movement amounts calculated based on the information sequentially acquired by the sensors for each of the plurality of generated candidates for the current position of the moving body; An extraction means for extracting, as the candidate with the highest probability as the current position of the moving body, the candidate for the current position of the moving body where the distance between the position obtained by converting the position of the recognized landmark into the coordinates of the candidate for the current position after adding the movement amount and the position of the landmark registered in the storage device is the closest. A position estimation system characterized by comprising.

2. The position estimation system according to claim 1, wherein: The extraction means includes a prediction means for predicting the current position of the moving body based on the information of the landmark acquired by the moving body and predetermined map information, comparing a plurality of the candidates for the current position with the prediction result of the prediction means, and extracting, as the candidate with the highest probability as the current position of the moving body, the candidate among the plurality of candidates for the current position. A position estimation system characterized by comprising an extraction means.

3. The position estimation system according to claim 1 or claim 2, wherein the generation means calculates the number and positions of candidates for the current position based on the relative distance between the landmark and the moving object, the direction of the line recognized by the line recognition means, and the number of landmarks based on predetermined map information.

4. The position estimation system according to claim 2, wherein the extraction means compares the positions of recognizable landmarks from each position candidate predicted by the prediction means with the positions of landmarks actually recognized by the sensor from the current position, and calculates the probability that each position candidate is the current position.

5. The position estimation system according to claim 2, wherein the prediction means calculates the latest position of each position candidate based on the past positions of each position candidate and the amount of movement of the moving object calculated by the sensor, and predicts the positions of the recognizable landmarks by the sensor based on the calculated position candidates and the map information.

6. The position estimation system according to any one of claims 1 to 5, characterized in that the process of extracting the candidate with the highest probability as the current position of the moving object from the candidates for the current position is executed when the moving object is lost.

Images