Location estimation system and location estimation method

Abstract

To accurately estimate a location of a mobile body even in topography with few features.SOLUTION: A location estimation system that estimates the location of a mobile body includes: a topographic information acquisition unit that acquires first topographic information of the outside environment; a movement amount detection sensor that detects the amount of movement of the mobile body; a storage unit that stores topographic map information including second topographic information of the outside environment; and a calculation unit that estimates an absolute location of the mobile body based on the location coordinates of the topographic map information. The calculation unit generates multiple particles at an initial location of the mobile body, transitions multiple particles at a location relative to the initial location calculated based on the amount of movement, acquires the first topographic information, obtains the second topographic information at each location of the multiple transitioned particles from the topographic map information, calculates likelihood of the multiple particles based on an error obtained by matching the first topographic information and the second topographic information, and estimates an absolute location of the mobile body based on the calculated likelihood.SELECTED DRAWING: Figure 3

Description

[Technical Field]

[0001] The present disclosure relates to a location estimation system and a location estimation method. [Background technology]

[0002] Conventionally, a mobile object position estimation device is known that estimates the position of a mobile object based on the positions of feature points extracted from camera images captured by multiple cameras and the positions of feature points stored in map data (see, for example, Patent Document 1). [Prior art documents] [Patent documents]

[0003] [Patent Document 1] International Publication No. 2021 / 100650 Summary of the Invention [Problem to be solved by the invention]

[0004] Another method for estimating a position is known as skyline matching, which matches the ridgelines of the terrain in the environment surrounding a moving object with the ridgelines of the terrain in previously acquired terrain data, and estimates the absolute position of the moving object from the degree of match.

[0005] Incidentally, the external environments in which a moving object moves include environments with few feature topography, such as underwater or the surface of the moon. In such external environments, it becomes difficult for the position estimation device and skyline matching of Patent Document 1 to accurately estimate the position based on feature points.

[0006] Therefore, an object of the present disclosure is to provide a position estimation system and a position estimation method that can accurately estimate the position of a moving object even on terrain with few features. [Means for solving the problem]

[0007] The position estimation system of the present disclosure is a position estimation system that estimates the position of a moving body moving in an external environment, and includes: a topographical information acquisition unit provided on the moving body that acquires first topographical information of the external environment; a movement amount detection sensor that detects physical quantities related to the movement of the moving body; a memory unit that stores topographical map information including position coordinates of the external environment and second topographical information of the external environment that is associated with the position coordinates and acquired in advance; and a calculation unit that estimates the position of the moving body at the position coordinates of the topographical map information as an absolute position, wherein the calculation unit performs the steps of generating a plurality of particles at the position coordinates of the topographical map information, with the initial position of the moving body as the center; The method includes the steps of: calculating a relative position of the moving body with respect to the initial position based on the physical quantity; shifting the generated particles around the calculated relative position; acquiring the first terrain information at the relative position using the terrain information acquisition unit; acquiring the second terrain information at each position of the shifted particles from the terrain map information; matching the acquired first terrain information with the second terrain information to obtain an error; and calculating a likelihood for each of the plurality of particles based on the obtained error; and estimating the absolute position of the moving body based on the calculated likelihood of the plurality of particles.

[0008] The position estimation method disclosed herein is a position estimation method executed by a position estimation system that estimates the position of a moving body moving in an external environment, in which terrain map information including position coordinates of the external environment and second terrain information of the external environment corresponding to the position coordinates is acquired in advance, and the position estimation system executes the following steps: generating a plurality of particles centered on the initial position of the moving body at the position coordinates of the terrain map information; calculating a relative position of the moving body with respect to the initial position based on physical quantities related to the movement of the moving body, and transitioning the generated plurality of particles centered on the calculated relative position; acquiring first terrain information of the external environment at the relative position; acquiring the second terrain information at the positions of each of the transitioned plurality of particles from the terrain map information, matching the acquired first terrain information with the second terrain information to acquire an error, calculating a likelihood for each of the plurality of particles based on the acquired error; and estimating the absolute position of the moving body based on the calculated likelihood of the plurality of particles. [Effects of the Invention]

[0009] According to the present disclosure, the position of a moving body can be estimated with high accuracy even on terrain with few features. [Brief explanation of the drawings]

[0010] [Figure 1] FIG. 1 is a diagram schematically illustrating a position estimation system according to this embodiment. [Figure 2] FIG. 2 is a block diagram of the position estimation system according to this embodiment. [Figure 3] FIG. 3 is a flowchart illustrating the location estimation method according to this embodiment. [Figure 4] FIG. 4 is an explanatory diagram illustrating the position estimation method according to this embodiment. [Figure 5] FIG. 5 is an explanatory diagram relating to the absolute position estimated by the position estimation method according to this embodiment. [Figure 6] FIG. 6 is an explanatory diagram of a plurality of particles before and after resampling in the position estimation method according to this embodiment. [Figure 7] FIG. 7 is a diagram comparing the estimated absolute positions of this embodiment and the conventional method. [Figure 8] FIG. 8 is a diagram comparing estimated errors between this embodiment and the prior art. DETAILED DESCRIPTION OF THE INVENTION

[0011] Hereinafter, embodiments of the present disclosure will be described in detail with reference to the drawings. However, this disclosure is not limited to these embodiments. Furthermore, the components in the following embodiments include those that are easily replaceable by those skilled in the art, or those that are substantially identical. Furthermore, the components described below can be combined as appropriate, and when there are multiple embodiments, the respective embodiments can also be combined.

[0012] [Present embodiment] The position estimation system 1 according to this embodiment is a system that estimates the position of a moving object 5 moving in an external environment. The external environment to which the position estimation system 1 of this embodiment is applied is, for example, an environment with few distinctive topography, such as the surface of the moon or underwater. In the following, a case where the system is applied to the surface of the moon will be described as the external environment. The moving object 5 is, for example, a rover moving on the surface of the moon.

[0013] FIG. 1 is a diagram schematically illustrating a position estimation system according to this embodiment. FIG. 2 is a block diagram related to the position estimation system according to this embodiment. FIG. 3 is a flowchart related to the position estimation method according to this embodiment. FIG. 4 is an explanatory diagram illustrating the position estimation method according to this embodiment. FIG. 5 is an explanatory diagram related to the absolute position estimated by the position estimation method according to this embodiment. FIG. 6 is an explanatory diagram related to a plurality of particles before and after resampling in the position estimation method according to this embodiment. FIG. 7 is a diagram comparing the estimated absolute positions of this embodiment and the conventional method. FIG. 8 is a diagram comparing the estimated errors of this embodiment and the conventional method.

[0014] (Location estimation system) 1 and 2, a position estimation system 1 will be described. The position estimation system 1 includes various sensors provided on a mobile object 5 and a position estimation device 6 connected via a communication network 7. The mobile object 5 moves on the surface of the moon, while the position estimation device 6 is provided in a space other than the surface of the moon, such as on the Earth or in outer space, and the mobile object 5 and the position estimation device 6 communicate with each other via wireless communication.

[0015] (Mobile) The moving body 5 has a camera 11, a movement amount detection sensor 12, and an attitude detection sensor 13. The camera 11 captures images of the terrain along which the moving body 5 moves, acquiring an image of the terrain. The movement amount detection sensor 12 is a sensor that detects physical quantities related to the movement of the moving body 5, and is, for example, an acceleration sensor or a gyro sensor, and acquires information on acceleration and angular velocity. The moving body 5 calculates and acquires the amount of movement from a predetermined reference position based on the acceleration and angular velocity information. The attitude detection sensor 13 is, for example, a star tracker, a sun sensor, a gyro sensor, etc., and acquires information on the attitude of the moving body 5. The moving body 5 calculates and acquires, for example, the roll angle and pitch angle of the moving body 5 based on the information on the attitude of the moving body 5. The moving body 5 transmits the acquired image, information on the amount of movement detected by the movement amount detection sensor 12, and information on the attitude detected by the attitude detection sensor 13 to the position estimation device 6.

[0016] (Position estimation device) The position estimation device 6 includes a calculation unit 21 and a storage unit 22. The calculation unit 21 includes an integrated circuit such as a CPU (Central Processing Unit). The calculation unit 21 estimates the position of the mobile object 5 based on information transmitted from the mobile object 5.

[0017] The storage unit 22 is any storage device such as a semiconductor storage device or a magnetic storage device. The storage unit 22 stores various programs and various data. The storage unit 22 stores terrain map information M as various data. The terrain map information M includes position coordinates of the external environment and terrain information of the external environment (second terrain information) that is associated with the position coordinates and that has been acquired in advance. The terrain information is ridge line information related to the ridge lines of the terrain, and is terrain information that has been measured in advance.

[0018] (Position estimation method) Next, a position estimation method executed by the position estimation system 1 will be described with reference to Figs. 3 to 6. As shown in Figs. 3 and 4, in the position estimation method, first, the calculation unit 21 of the position estimation device 6 generates a plurality of (M) particles P in the position coordinates of the topographical map information, with the initial position T1 of the moving object 5 as the center (step S1). In step S1, the initial position T1 is set as an initial absolute position in the position coordinates of the topographical map information, and is set as the position at the start point of position estimation. Note that, although the initial position T1 is set as the position at the start point of position estimation in step S1, after position estimation, the estimated absolute position may be updated as the initial position T1. Also, in step S1, the plurality of particles P are distributed with a variance σ 2 The particles P are dispersed based on normal random numbers. However, the dispersion of the particles P is not particularly limited to the above-mentioned dispersion.

[0019] Next, in the position estimation method, the calculation unit 21 calculates a relative position T2 of the moving body 5 with respect to the initial position T1 based on the amount of movement detected by the movement amount detection sensor 12, and moves the generated multiple particles P around the calculated relative position T2 (step S2). In step S2, the position estimation device 6 acquires information on the amount of movement transmitted from the moving body 5, and the calculation unit 21 of the position estimation device 6 calculates the relative position T2 based on the amount of movement displaced from the initial position T1.

[0020] Thereafter, in the position estimation method, the calculation unit 21 determines whether or not an image of the terrain at the relative position T2 captured by the camera 11 provided on the moving body 5 has been acquired (step S3). If the calculation unit 21 determines that an image of the terrain has not been acquired (step S3: No), it estimates the absolute position based on the plurality of particles P after the transition (step S4). In step S4, the average value of the positions of the plurality of particles P after the transition is set as the estimated position. After executing step S4, the calculation unit 21 proceeds to step S2, and repeatedly executes steps S2 to S4 until an image of the terrain is acquired.

[0021] When the calculation unit 21 determines that an image of the terrain has been acquired (step S3: Yes), it matches the terrain information acquired from the terrain map information M with the terrain information acquired from the image of the camera 11 at the positions of the multiple particles P after the transition, and calculates the likelihood of each particle P based on the matching result (step S5). Specifically, in step S5, the calculation unit 21 uses the terrain map information M to acquire ridge line information D2 of the terrain predicted when the position of each particle P is viewed from the moving body 5 (hereinafter referred to as second ridge line information D2). Also, in step S5, the calculation unit 21 uses the image of the camera 11 to acquire ridge line information D1 of the actual terrain when the position of each particle P is viewed from the moving body 5 (hereinafter referred to as first ridge line information D1). Then, in step S5, the calculation unit 21 calculates the square sum error between the first ridge line information D1 and the second ridge line information D2 for each particle P, and calculates the likelihood of each particle P based on the calculated square sum error.

[0022] Here, the likelihood of a particle P is defined as the probability of the existence of a moving object 5. The smaller the square sum error between the first ridge line information D1 and the second ridge line information D2, the higher the probability of the existence of a moving object 5 in the particle P is interpreted as. In other words, the calculation unit 21 calculates the probability of the existence of a moving object 5 from the square sum error between the first ridge line information D1 and the second ridge line information D2, and acquires the probability of the existence of a moving object 5 as the likelihood of the particle P. Therefore, the smaller the square sum error, the higher the likelihood of the particle P, and the larger the square sum error, the lower the likelihood of the particle P.

[0023] Furthermore, when acquiring second ridge line information D2 based on topographic map information M, calculation unit 21 performs attitude correction based on information about the attitude of moving body 5. That is, calculation unit 21 acquires information about the attitude of moving body 5 at the relative position, i.e., the roll angle and pitch angle, and performs correction based on the acquired roll angle and pitch angle with respect to a camera coordinate system based on the horizontal, before acquiring second ridge line information D2. Then, calculation unit 21 matches second ridge line information D after attitude correction with first ridge line information D1.

[0024] Next, in the position estimation method, the calculation unit 21 uses the likelihood of each particle P as a weight and estimates the weighted average value of the multiple particles as the absolute position T3 (step S6). Fig. 5 shows the calculated likelihoods of the multiple particles P, the estimated absolute positions T3, and the true values ​​T4. In Fig. 5, the horizontal axis represents the position in the X direction, and the vertical axis represents the position in the Y direction.

[0025] In the position estimation method, the calculation unit 21 determines whether to end the estimation of the absolute position T3 of the moving object 5 (step S7), and if it determines that the estimation should be ended (step S7: Yes), it ends the position estimation method. On the other hand, if the calculation unit 21 does not determine that the estimation should be ended (step S7: No), it selects (resampling) multiple (M) particles P according to the likelihood of each particle P (step S8). Also, in step S8, the absolute position T3 estimated by the calculation unit 21 is treated as the initial position T1. Note that in step S8, the likelihood values ​​of the multiple particles P after resampling are reset.

[0026] Fig. 6 shows a plurality of particles P before and after resampling. In Fig. 6, the horizontal axis represents the position in the X direction, and the vertical axis represents the position in the Y direction. Regarding the likelihood of the plurality of particles P before resampling, the closer they are to the absolute position T3, the higher the probability of the existence of a moving object 5. Therefore, among the plurality of particles P after resampling, more particles P that are closer to the estimated absolute position T3 are selected compared to the plurality of particles P before resampling.

[0027] In the position estimation method, after executing step S8, the calculation unit 21 proceeds to step S2 again, and repeatedly executes steps S2 to S8 until the estimation of the absolute position T3 is completed.

[0028] Next, the results of comparing the position estimation method of this embodiment with a conventional position estimation method will be described with reference to FIGS.

[0029] 7 shows the calculated likelihoods of multiple particles P, the absolute positions T3 estimated in this embodiment, the absolute positions T5 estimated in the conventional method, and the true values ​​T4. In FIG. 7, the horizontal axis represents the position in the X direction, and the vertical axis represents the position in the Y direction. The conventional absolute positions T5 are absolute positions estimated by skyline matching. Looking at FIG. 7, it was confirmed that the error between the absolute positions T3 in this embodiment and the true values ​​T4 is smaller than the error between the conventional absolute positions T5 and the true values ​​T4.

[0030] 8 shows first ridge line information D1 acquired from camera 11, second ridge line information D2 after attitude correction obtained from terrain map information M in this embodiment, and second ridge line information D3 without attitude correction conventionally obtained from terrain map information M. In FIG. 8, the horizontal axis represents the azimuth angle centered on the moving body 5 (camera 11), and the vertical axis represents the elevation angle centered on the moving body 5 (camera 11). Looking at FIG. 8, it was confirmed that the error between the first ridge line information D1 and the second ridge line information D2 of this embodiment is smaller than the error between the first ridge line information D1 and the conventional second ridge line information D3.

[0031] In this embodiment, the external environment is described as being the surface of the moon, but the system may also be applied underwater. When applied underwater, the system may acquire the terrain information using a camera 11, sonar, or the like as a terrain information acquisition unit that acquires the terrain information. In this case, the terrain information acquired by the camera 11, sonar, or the like (first terrain information) is matched with the terrain information acquired from the terrain map information M (second terrain information), and the likelihood of each particle P is calculated from the error.

[0032] In this embodiment, the likelihoods of the particles P are weighted, and the weighted average value is estimated as the absolute position T3, but this configuration is not particularly limited. For example, the position of the particle P with the highest likelihood among the likelihoods of the particles P may be estimated as the absolute position T3.

[0033] As described above, the position estimation system 1 and the position estimation method described in this embodiment can be understood, for example, as follows.

[0034] A position estimation system 1 according to a first aspect is a position estimation system 1 for estimating the position of a moving body 5 moving in an external environment, and includes a topographical information acquisition unit (camera 11) provided on the moving body 5 for acquiring first topographical information (first ridge line information D1) of the external environment, a movement amount detection sensor 12 for detecting physical quantities related to the movement of the moving body, a memory unit 22 for storing topographical map information M including position coordinates of the external environment and second topographical information (second ridge line information D2) of the external environment that is associated with the position coordinates and acquired in advance, and a calculation unit 21 for estimating the position of the moving body 5 at the position coordinates of the topographical map information M as an absolute position T3, and the calculation unit 21 performs the following steps: a step S1 of generating a plurality of particles P at the position coordinates of the topographical map information M, with an initial position T1 of the moving body 5 as the center; The method executes the following steps: a step S2 of calculating a relative position T2 of the moving body 5 with respect to the initial position T1 based on the physical quantity detected by the motion detection sensor 12, and shifting the generated plurality of particles P around the calculated relative position T2; a step S3 of acquiring the first terrain information at the relative position by the terrain information acquisition unit; a step S5 of acquiring the second terrain information at each position of the shifted plurality of particles P from the terrain map information M, matching the acquired first terrain information with the second terrain information to acquire an error, and calculating a likelihood for each of the plurality of particles P based on the acquired error; and a step S6 of estimating the absolute position T3 of the moving body 5 based on the calculated likelihood of the plurality of particles P.

[0035] According to this configuration, by sensor fusion using the terrain information acquisition unit and the movement amount detection sensor 12, it is possible to estimate the absolute position T3 of the moving body 5 based on the likelihood of multiple particles P from the initial position T1 (absolute position T3) and relative position T2 of the moving body 5. Therefore, even in terrain with few features, it is possible to accurately estimate the absolute position T3 of the moving body 5, which has a high probability of existence.

[0036] As a second aspect, in the position estimation system 1 relating to the first aspect, the external environment is the ground, the terrain information acquisition unit is an imaging unit (camera 11) that images the terrain, the first terrain information and the second terrain information are first ridge line information D1 and second ridge line information D2, which are information about the outer contours of the terrain, and in steps S3 and S5 of acquiring the first terrain information, the calculation unit 21 executes step S3 of acquiring an image at the relative position imaged by the imaging unit and step S5 of acquiring the first ridge line information D1 included in the acquired image, and in step S5 of calculating the likelihood, matches the first ridge line information with the second ridge line information D2 at each position of the transitioned multiple particles P.

[0037] According to this configuration, the camera 11 provided on the moving body 5 can be used to acquire the first ridge line information D1 of the terrain, and the absolute position T3 of the moving body 5 can be estimated.

[0038] As a third aspect, in the position estimation system 1 relating to the first or second aspect, in step S6 of estimating the absolute position T3 of the moving body 5, the calculation unit 21 estimates the absolute position T3 by a weighted average value in which the likelihood of the calculated multiple particles P is used as a weighting factor.

[0039] According to this configuration, the absolute position T3 can be estimated statistically, and therefore the absolute position T3 can be estimated with high reliability.

[0040] As a fourth aspect, the position estimation system 1 relating to any one of the first to third aspects further includes an attitude detection sensor 13 that detects a physical quantity related to the attitude of the moving body 5, and the calculation unit 21 corrects the second terrain information based on the attitude of the moving body 5 detected by the attitude detection sensor 13.

[0041] According to this configuration, it is possible to reduce the error between the first topographical information and the second topographical information after the attitude correction by correcting the second topographical information based on the attitude of the moving body 5. This makes it possible to improve the accuracy of estimating the absolute position of the moving body 5.

[0042] A position estimation method according to a fifth aspect is a position estimation method executed by a position estimation system 1 that estimates the position of a moving body 5 moving in an external environment, wherein terrain map information M including position coordinates of the external environment and second terrain information (second ridge line information D2) of the external environment associated with the position coordinates is acquired in advance, and the method includes a step S1 of generating a plurality of particles P at the position coordinates of the terrain map information M, with an initial position T1 of the moving body 5 as the center; a step S2 of calculating a relative position T2 of the moving body 5 with respect to the initial position T1 based on the physical quantity detected by the movement amount detection sensor 12, and generating the generated particles P with the calculated relative position T2 as the center. The position estimation system 1 is caused to perform the steps of: step S2 of transitioning a plurality of particles P; step S3 of acquiring the first terrain information at the relative position using the terrain information acquisition unit; step S5 of acquiring the second terrain information at each position of the transitioned plurality of particles P from the terrain map information M, matching the acquired first terrain information with the second terrain information to acquire an error, and calculating a likelihood for each of the plurality of particles P based on the acquired error; and step S6 of estimating the absolute position T3 of the moving body 5 based on the calculated likelihood of the plurality of particles P.

[0043] According to this configuration, by sensor fusion using the terrain information acquisition unit and the movement amount detection sensor 12, it is possible to estimate the absolute position T3 of the moving body 5 based on the likelihood of multiple particles P from the initial position T1 (absolute position T3) and relative position T2 of the moving body 5. Therefore, even in terrain with few features, it is possible to accurately estimate the absolute position T3 of the moving body 5, which has a high probability of existence. [Explanation of symbols]

[0044] 1. Location estimation system 5. Mobile 6 Position estimation device 11 Camera 12 Movement detection sensor 13 Attitude detection sensor 21 Arithmetic section 22 Memory section M Topographical Map Information T1 initial position T2 relative position T3 Absolute position P Particle D1 First Ridge Information D2 Second Ridge Information

Claims

1. A position estimation system for estimating the position of a moving object moving in an external environment, a topographical information acquisition unit provided in the moving body and acquiring first topographical information of the external environment; a movement amount detection sensor that detects a physical amount related to the movement of the moving body; a storage unit that stores topographical map information including position coordinates of the external environment and second topographical information of the external environment that is associated with the position coordinates and that has been acquired in advance; a calculation unit that estimates the position of the moving object in the position coordinates of the terrain map information as an absolute position, The calculation unit generating a plurality of particles at the position coordinates of the terrain map information, with the initial position of the moving object as the center; calculating a relative position of the moving object with respect to the initial position based on the physical amount detected by the movement amount detection sensor, and causing the generated particles to move around the calculated relative position; acquiring the first topographical information at the relative position by the topographical information acquisition unit; acquiring the second terrain information at the respective positions of the transitioned particles from the terrain map information, matching the acquired first terrain information with the second terrain information to acquire an error, and calculating a likelihood for each of the plurality of particles based on the acquired error; and estimating the absolute position of the moving object based on the calculated likelihoods of the plurality of particles.

2. the external environment is the ground, the topographical information acquisition unit is an imaging unit that images the topography, the first topographical information and the second topographical information are first ridge line information and second ridge line information, which are information on the contour of the topographical information, The calculation unit In the step of acquiring first topographical information, acquiring an image at the relative position captured by the imaging unit; acquiring the first edge line information included in the acquired image; In the step of calculating the likelihood, The position estimation system according to claim 1 , wherein the first edge line information is matched with the second edge line information at each of the positions of the transitioned particles.

3. The calculation unit 2. The position estimation system according to claim 1, wherein in the step of estimating the absolute position of the moving object, the absolute position is estimated by a weighted average value in which the likelihoods of the calculated plurality of particles are used as weights.

4. further comprising an attitude detection sensor that detects a physical quantity related to the attitude of the moving body; The calculation unit The position estimation system according to claim 1 , wherein the second topographical information is corrected based on the attitude of the moving object detected by the attitude detection sensor.

5. A position estimation method executed by a position estimation system that estimates the position of a moving object moving in an external environment, comprising: terrain map information including position coordinates of the external environment and second terrain information of the external environment associated with the position coordinates is acquired in advance; generating a plurality of particles at the position coordinates of the terrain map information, with the initial position of the moving object as the center; calculating a relative position of the moving object with respect to the initial position based on a physical quantity related to the movement of the moving object, and causing the generated particles to transition around the calculated relative position; acquiring first topographical information of the external environment at the relative position; acquiring the second terrain information at the respective positions of the transitioned particles from the terrain map information, matching the acquired first terrain information with the second terrain information to acquire an error, and calculating a likelihood for each of the plurality of particles based on the acquired error; and estimating the absolute position of the moving object based on the calculated likelihoods of the plurality of particles.

Images