JP7795982B2 - Location estimation system and location estimation method - Google Patents

Description

This disclosure relates to a location estimation system and a location estimation method.

Conventionally, there is known a mobile object position estimation device 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).

International Publication No. 2021/100650

Another method for estimating position is a technique called skyline matching. Skyline matching matches the ridgelines of the terrain in the environment surrounding the 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.

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

Therefore, an objective 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 in terrain with few distinctive features.

The position estimation system disclosed herein is a position estimation system that estimates the position of a mobile body moving in an external environment, and includes: a terrain information acquisition unit provided on the mobile body that acquires first terrain information of the external environment; a movement amount detection sensor that detects physical quantities related to the movement of the mobile body; a memory unit that stores terrain map information including position coordinates of the external environment and second terrain 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 mobile body at the position coordinates of the terrain map information as an absolute position, and the calculation unit performs the steps of generating a plurality of particles at the position coordinates of the terrain map information, with the initial position of the mobile body as the center; and calculating the previous terrain information detected by the movement amount detection sensor. The method includes the steps of: calculating the 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 acquire an error; calculating the likelihood of each of the particles based on the acquired error; and estimating the absolute position of the moving body based on the calculated likelihood of the particles.

The position estimation method disclosed herein is a position estimation method executed by a position estimation system that estimates the position of a moving object moving in an external environment. 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. The position estimation method causes the position estimation system to perform the following steps: generate a plurality of particles centered on the initial position of the moving object at the position coordinates of the terrain map information; calculate a relative position of the moving object with respect to the initial position based on physical quantities related to the movement of the moving object and transition the generated plurality of particles centered on the calculated relative position; acquire first terrain information of the external environment at the relative position; acquire second terrain information for each of the transitioned positions from the terrain map information, match the acquired first terrain information with the second terrain information to acquire an error, calculate a likelihood for each of the plurality of particles based on the acquired error; and estimate the absolute position of the moving object based on the calculated likelihoods of the plurality of particles.

This disclosure makes it possible to accurately estimate the position of a moving object even in terrain with few distinctive features.

FIG. 1 is a diagram schematically illustrating a position estimation system according to this embodiment. FIG. 2 is a block diagram of a position estimation system according to this embodiment. FIG. 3 is a flowchart illustrating the location 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 relating to the absolute position estimated by the position estimation method according to this embodiment. FIG. 6 is an explanatory diagram of 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 estimated errors between this embodiment and the prior art.

Embodiments of the present disclosure are described in detail below 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 if there are multiple embodiments, the respective embodiments can also be combined.

[Present embodiment]
The position estimation system 1 according to this embodiment is a system for estimating the position of a mobile object 5 moving in an external environment. The external environment to which the position estimation system 1 according to 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 mobile object 5 is, for example, a rover moving on the surface of the moon.

Figure 1 is a schematic diagram of a position estimation system according to this embodiment. Figure 2 is a block diagram of the position estimation system according to this embodiment. Figure 3 is a flowchart relating to the position estimation method according to this embodiment. Figure 4 is an explanatory diagram illustrating the position estimation method according to this embodiment. Figure 5 is an explanatory diagram illustrating the absolute position estimated by the position estimation method according to this embodiment. Figure 6 is an explanatory diagram illustrating multiple particles before and after resampling in the position estimation method according to this embodiment. Figure 7 is a diagram comparing the estimated absolute positions of this embodiment and the conventional method. Figure 8 is a diagram comparing the estimated errors of this embodiment and the conventional method.

(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 lunar surface, 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.

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

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

The memory unit 22 is any memory device, such as a semiconductor memory device or a magnetic memory device. The memory unit 22 stores various programs and various data. The memory 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 ridgelines of the terrain, and is terrain information that has been measured in advance.

(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 terrain map information, centered on the initial position T1 of the moving object 5 (step S1). In step S1, the initial position T1 is set as an initial absolute position in the position coordinates of the terrain map information, and is set as a position at the starting point of position estimation. Note that, although the initial position T1 is set as the position at the starting 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 dispersed based on normal random numbers with a variance of σ ^2. Note that the dispersion of the plurality of particles P is not particularly limited to the above-mentioned dispersion.

Next, in the position estimation method, the calculation unit 21 calculates the 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 from the initial position T1.

Then, in the position estimation method, the calculation unit 21 determines whether an image of the terrain at 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 multiple particles P after the transition (step S4). In step S4, the average value of the positions of the multiple particles P after the transition is used 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.

When the calculation unit 21 determines that an image of the terrain has been acquired (Step S3: Yes), it matches the terrain information obtained from the terrain map information M with the terrain information obtained 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 results (Step S5). Specifically, in Step S5, the calculation unit 21 uses the terrain map information M to acquire ridge line information D2 of the predicted terrain when the position of each particle P is viewed from the mobile 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 mobile body 5 (hereinafter referred to as first ridge line information D1). Then, in Step S5, the calculation unit 21 calculates the squared 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 squared error.

Here, the likelihood of particle P is defined as the probability of the existence of a moving object 5. The smaller the square sum error between the first edge line information D1 and the second edge line information D2, the higher the probability of the existence of a moving object 5 in particle P. 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 edge line information D1 and the second edge line information D2, and obtains the probability of the existence of a moving object 5 as the likelihood of particle P. Therefore, the smaller the square sum error, the higher the likelihood of particle P, and the larger the square sum error, the lower the likelihood of particle P.

Furthermore, when the calculation unit 21 acquires the second ridge line information D2 based on the terrain map information M, it performs attitude correction based on the attitude information of the moving body 5. In other words, the calculation unit 21 acquires information on the attitude of the 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 the camera coordinate system based on the horizontal, before acquiring the second ridge line information D2. The calculation unit 21 then matches the second ridge line information D after the attitude correction with the first ridge line information D1.

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 multiple particles as the absolute position T3 (step S6). Figure 5 shows the calculated likelihoods of multiple particles P, the estimated absolute positions T3, and the true values T4. In Figure 5, the horizontal axis represents the position in the X direction, and the vertical axis represents the position in the Y direction.

In the position estimation method, the calculation unit 21 determines whether to end the estimation of the absolute position T3 of the moving body 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.

Figure 6 shows multiple particles P before and after resampling. In Figure 6, the horizontal axis represents position in the X direction, and the vertical axis represents position in the Y direction. The likelihood of multiple particles P before resampling is such that the closer they are to absolute position T3, the higher the probability of a moving object 5 being present. Therefore, after resampling, more particles P that are closer to the estimated absolute position T3 are selected than before resampling.

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 estimation of the absolute position T3 is complete.

Next, with reference to Figures 7 and 8, we will explain the results of comparing the position estimation method of this embodiment with conventional position estimation methods.

Figure 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 value T4. In Figure 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 position T5 is an absolute position estimated by skyline matching. Looking at Figure 7, it can be confirmed that the error between the absolute position T3 in this embodiment and the true value T4 is smaller than the error between the conventional absolute position T5 and the true value T4.

Figure 8 shows first ridge line information D1 obtained 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 Figure 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 Figure 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.

In this embodiment, the external environment is described as the surface of the moon, but it may also be applied underwater. When applied underwater, the terrain information acquisition unit that acquires the terrain information may use a camera 11 or sonar, etc. to acquire the terrain information. In this case, too, the terrain information acquired by the camera 11 or sonar, etc. (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.

Furthermore, in this embodiment, the likelihoods of multiple 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 multiple particles P may be estimated as the absolute position T3.

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

The position estimation system 1 according to the first aspect is a position estimation system 1 for estimating the position of a mobile body 5 moving in an external environment, and includes: a terrain information acquisition unit (camera 11) provided on the mobile body 5 for acquiring first terrain information (first ridge information D1) of the external environment; a movement amount detection sensor 12 for detecting physical quantities related to the movement of the mobile body; a memory unit 22 for storing terrain map information M including position coordinates of the external environment and second terrain information (second ridge 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 mobile body 5 at the position coordinates of the terrain 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 terrain map information M, with the initial position T1 of the mobile body 5 as the center; The method executes the following steps: (1) 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; (2) shifting the generated particles P around the calculated relative position T2; (3) acquiring the first terrain information at the relative position using the terrain information acquisition unit; (4) acquiring the second terrain information at each position of the shifted particles P from the terrain map information M, matching the acquired first terrain information with the second terrain information to obtain an error; (5) calculating a likelihood for each of the particles P based on the obtained error; and (6) estimating the absolute position T3 of the moving body 5 based on the calculated likelihoods of the particles P.

With this configuration, sensor fusion using the terrain information acquisition unit and the movement amount detection sensor 12 can 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, the absolute position T3 of a moving body 5 with a high probability of existence can be accurately estimated.

In a second aspect, in the position estimation system 1 according to the first aspect, the external environment is the ground, the terrain information acquisition unit is an imaging unit (camera 11) that captures images of 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 contours of the terrain, and the calculation unit 21, in steps S3 and S5 of acquiring the first terrain information, executes step S3 of acquiring an image at the relative position captured 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 of the transitioned positions of the multiple particles P.

With this configuration, the camera 11 mounted on the moving body 5 can be used to acquire first ridge information D1 of the terrain and estimate the absolute position T3 of the moving body 5.

As a third aspect, in the position estimation system 1 according 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 using a weighted average value in which the likelihoods of the calculated plurality of particles P are used as weights.

With this configuration, the absolute position T3 can be estimated statistically, allowing for highly reliable estimation of the absolute position T3.

In a fourth aspect, the position estimation system 1 according 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 topographical information based on the attitude of the moving body 5 detected by the attitude detection sensor 13.

With this configuration, by correcting the second terrain information based on the attitude of the moving body 5, it is possible to reduce the error between the second terrain information after the attitude correction and the first terrain information. This improves the accuracy of estimating the absolute position of the moving body 5.

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 information D2) of the external environment associated with the position coordinates is acquired in advance, and the method includes steps S1 to generate a plurality of particles P at the position coordinates of the terrain map information M, with the initial position T1 of the moving body 5 as the center; and, based on the physical quantity detected by the movement amount detection sensor 12, calculating a relative position T2 of the moving body 5 with respect to the initial position T1, 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 following steps: step S2 of transitioning multiple 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 multiple 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 the likelihood of each of the multiple 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 multiple particles P.

With this configuration, sensor fusion using the terrain information acquisition unit and the movement amount detection sensor 12 can 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, the absolute position T3 of a moving body 5 with a high probability of existence can be accurately estimated.

REFERENCE SIGNS LIST 1 Position estimation system 5 Mobile body 6 Position estimation device 11 Camera 12 Movement amount detection sensor 13 Attitude detection sensor 21 Calculation unit 22 Storage unit M Topographic map information T1 Initial position T2 Relative position T3 Absolute position P Particle D1 First ridge line information D2 Second ridge line information

Claims (5)

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. 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 information is matched with the second edge information at each of the positions of the transitioned particles. 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. 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. 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 moving the generated particles 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.