JPWO2012176249A1 - Self-position estimation device, self-position estimation method, self-position estimation program, and moving object - Google Patents

Abstract

  The degree of coincidence calculation unit 12 matches the position information obtained when the position information acquired by the position information acquisition unit 11 at the time t is translated and rotated with respect to the position information acquired at the time t-1. Calculate the degree. The estimation unit 13 changes the parallel movement amount T and the rotational movement amount R of the position information at time t, searches for the parallel movement amount T and the rotational movement amount R that maximize the degree of coincidence, and estimates its own position. Then, the coincidence calculation unit 12 gives a predetermined point to the coincidence when a measurement point with position information at time t exists within a distance ε centering on a measurement point with position information at time t−1.

Description

  The present invention relates to a technique for estimating the self-position of a mobile object, and more particularly to a technique for estimating the self-position of a mobile object that operates in an unknown environment.

  In order for a robot to move in an unknown environment, it is necessary to model the environment on a computer. As a technique for modeling an unknown environment, simultaneous localization and mapping (SLAM) is known (for example, Non-Patent Document 1). It models the environment by simultaneously performing relative self-location estimation and map generation in an unknown environment. Many SLAMs are based on the assumption that the surrounding environment is static, and there is a problem that a matching error occurs in a dynamic environment.

  Therefore, when operating a robot in an actual environment where the work area is shared with people, it is necessary to cope with dynamic environmental changes such as traffic of people. As a SLAM that can be used in a dynamic environment, a landmark-based approach that considers outliers has been proposed (for example, Non-Patent Document 2).

  However, the method of Non-Patent Document 2 has a need to observe a sufficient number of landmarks and a problem of hiding due to a dynamic obstacle.

  In addition, a method is also proposed in which a stationary object and a moving object are distinguished and matched based on the shape of a sensing target (Non-Patent Document 3) and the nearest point distance between frames (Non-Patent Document 4).

  However, the methods of Non-Patent Documents 3 and 4 have a problem that the measurement accuracy strongly depends on the performance of the discriminator that determines whether the measurement accuracy is a moving object.

  Also, as a technique for matching position information composed of a plurality of measurement points, such as two-dimensional position information measured by a laser range finder, iterative closest point (ICP) is a technique for minimizing the L2 norm. ) Is also known (Non-patent Document 5).

  FIG. 23 is a diagram showing matching errors when using ICP. In FIG. 23, a measurement point measured by a position sensor mounted on the robot and an actual position of an object existing around the moving body are superimposed on a two-dimensional coordinate space indicating the moving surface of the robot. It is plotted. Then, an error between the measurement point and the actual position occurs in the area surrounded by the dotted line of the circle in FIG.

  Thus, in the method shown in Non-Patent Document 5, many matching errors occur in a dynamic environment as shown in FIG.

S. Thrunet al .: “Simultaneous Mapping and Localization with Sparse Extended Information Filters: Theory and Initial Results”, Algorithmic Foundations of Robotics V, Vol.7, pp.363-380, 2003. Denis F Wolf, Gaurav s Sukhatme: “Mobile Robot Simultaneous Localization and Mapping in Dynamic Environments” A. Ess, B. Leibe, K. Schindler, L. vanGool: “Moving Obstacle Detection in Highly Dynamic Scenes”, ICRA09, 2009. Taku Shikina, Shinichi Yuta: “Making a map with a mobile robot in a corridor with many passersby”, ROBOMEC2010, 1A2-D29, June 14-16, 2010. Sebastian Thrun, Wolfram Burgard, Dieter Fox: “Probabilistic robotics”, The MIT Press, 2005.

  The objective of this invention is providing the technique which can estimate the self-position of a moving body accurately in the unknown environment where a moving object exists.

  A self-position estimation apparatus according to an aspect of the present invention is a self-position estimation apparatus that estimates a self-position of a moving body, and is position information that acquires position information of each object existing in a surrounding space of the moving body in time series The second position information acquired at the second time which is the time before or after the first time, the first position information acquired at the first time by the acquisition unit and the position information acquisition unit. A degree of coincidence calculation unit for calculating a degree of coincidence between the first position information and the second position information when the first position information and the second position information are translated, and a parallel movement amount and a rotation movement amount of the first position information. And an estimation unit that searches the parallel movement amount and the rotational movement amount to maximize the degree of coincidence and estimates the self position, and the coincidence degree calculation unit configures the first position information The second position within a certain distance with respect to a certain measurement point If there is measurement points constituting the broadcast, the coincidence degree in imparting a predetermined point, and calculates the total value of the grant and the point as the matching degree.

  A mobile object according to another aspect of the present invention includes the above-described self-position estimation device and a position sensor that acquires the position information. A self-position estimation method and a self-position estimation program according to still another aspect of the present invention have the same features as the above self-position estimation apparatus.

It is a block diagram of the mobile body to which the self-position estimation apparatus by embodiment of this invention was applied. It is a flowchart which shows operation | movement of the self-position estimation apparatus by embodiment of this invention. It is a flowchart which shows the detail of the search process shown to S2 of FIG. It is a flowchart which shows the detail of the calculation process of an evaluation value. (A) is a conceptual diagram explaining the calculation method of an evaluation value. (B) shows how the measurement points a _{1 to} a ₃ constituting the first position information are rotationally moved. When L2 norm is employ | adopted as an evaluation value, it is the figure which showed the reason a matching error arises in the positional information of the time t-1 and the positional information of the time t in a dynamic environment. (A) is an external view of SICK LSM100. (B) is an external view of the mobile body by embodiment of this invention. It is the table | surface which showed the specification of the position sensor. It is a figure explaining the process in which a search candidate is produced | generated. It is the conceptual diagram which showed the hash table. (A) is a diagram showing an experiment place, and (B) is a diagram showing a dynamic environment in which a person moves to and from this experiment place. A two-dimensional map of the surrounding space generated in a static environment in which no person goes to and from the experimental place is shown. It is the table | surface which showed the experimental result at the time of using L2 norm, M-estimator, and L0 norm as an evaluation value. It is a two-dimensional map created when the L2 norm is adopted. It is a two-dimensional map created when M-estimator is adopted. It is a two-dimensional map created when the L0 norm is adopted. It is the table | surface which put together the experimental result at the time of conducting the experiment which compares calculation time using Brute-force, kd-tree, and LSH. It is the two-dimensional map produced | generated using RBPF-SLAM in a static environment. It is a two-dimensional map generated by matching position information with an evaluation value E using an L0 norm using position information measured while moving a moving body in a dynamic environment. It is the two-dimensional map produced | generated by matching position information with the evaluation value using L2 norm. It is the two-dimensional map produced | generated using RBPF-SLAM. It is the two-dimensional map produced | generated using the self-position estimation apparatus by this Embodiment. It is the figure which showed the error of the matching at the time of using ICP.

  Hereinafter, a self-position estimation apparatus according to an embodiment of the present invention will be described. FIG. 1 is a block diagram of a moving body to which a self-position estimation apparatus according to an embodiment of the present invention is applied. The self-position estimation apparatus includes a position information acquisition unit 11, a coincidence calculation unit 12, an estimation unit 13, a map generation unit 14, and an odometry calculation unit 15. Then, the self-position estimation device estimates the self-position of the moving body based on the position information acquired by the position sensor 16. The moving body includes a self-position estimation device, a position sensor 16, and a rotation amount sensor 17.

  In the present embodiment, the self-position estimation device is configured by a computer including a CPU, a ROM, a RAM, a hard disk, and the like, for example. The position information acquisition unit 11, the matching degree calculation unit 12, the estimation unit 13, the map generation unit 14, and the odometry calculation unit 15 are realized by, for example, the CPU executing a self-position estimation program stored in the hard disk. . The self-position estimation program is recorded on a computer-readable recording medium such as a DVD-ROM and provided to the user. The user installs this recording medium in the computer, thereby causing the computer to function as a self-position estimation device.

  The position information acquisition unit 11 acquires the position information of each object existing in the space around the moving body measured by the position sensor 16 in a time series at a constant cycle. Here, the position information is composed of measurement points indicating locations where objects exist. In the present embodiment, a laser range finder is employed as the position sensor 16. Therefore, the measurement points include an angle from the reference direction and a distance from the origin in a two-dimensional local coordinate space in which the position of the position sensor 16 is the origin and the front direction of the position sensor 16 is the reference direction. Represented by dimensional polar coordinate data. In the following description, in the local coordinate space, it is assumed that the y axis is set in the reference direction, the x axis is set in a direction that passes through the origin and is orthogonal to the reference direction.

  The degree-of-match calculation unit 12 acquires the first position information acquired at the first time by the position information acquisition unit 11 at the second time acquired at the second time which is the time before or after the first time. The degree of coincidence between the first position information and the second position information when the position information is translated and rotated is calculated. In the present embodiment, the first time is assumed to be time t and the second time is assumed to be time t-1, but the present invention is not limited to this, and the first time is assumed to be time t-1 and the second time is assumed to be time t. Also good. The time t indicates the acquisition timing of the position information acquired by the position sensor in time series. Moreover, the acquisition timing of position information is described as a frame as needed.

  In this embodiment, the degree of coincidence is expressed using an evaluation value that evaluates the degree of coincidence between the position information at time t and the position information at time t-1. Specifically, the evaluation value E is represented by Expression (1).

E (R, T) = Σ _{i = 1} ⁿ f (Ra _i + T, {b _j }) (1)
f (a _i , {b _j }) = 0 (∋j, | a _i −b _j | ≦ ε) or 1 (otherwise)

However, a _i represents each measurement point of position information at time t, and b _j represents each measurement point of position information at time t−1. R represents the rotational movement amount of each measurement point at time t with respect to time t−1, and T represents the parallel movement amount of each measurement point with respect to time t−1 with respect to time t−1. Show. i is an index for specifying a measurement point of position information at time t, and j is an index for specifying a measurement point of position information at time t-1. ε is a distance considering the accuracy of the laser range finder. n indicates the number of measurement points.

Equation (1) is a measurement point a _i is R rotational movement, and, at the time obtained by T translated, when the measurement points a _i does not exist within distance ε around the measurement point b _j, evaluation value It can be interpreted that a penalty of 1 is imposed on E. Therefore, in formula (1), the smaller the evaluation value E, the higher the degree of coincidence between the position information at time t-1 and the position information at time t.

In the expression (1), f (a _i , {b _j }) = 1 (∋j, | a _i −b _j | ≦ ε) or 0 (otherwise) may be used. In this case, when the measurement point a _i exists within the distance ε with the measurement point b _j as the center, 1 point is given to the evaluation value E, and the evaluation value E increases as the matching degree increases.

In the calculation of Expression (1), it is not necessary to explicitly determine the corresponding point, and it is only necessary to determine whether or not the measurement point a _i exists within the distance ε with the measurement point b _j as the center. That is, in the expression (1), the evaluation value E is defined using the L0 norm. In this manner, a method for searching whether or not the measurement point a _i exists within the distance ε with respect to the measurement point b _j is called r-neighbor search.

  The norm means a distance, and there are an L2 norm and the like in addition to the L0 norm. The L2 norm is the sum of squared distances, and has been widely used.

FIG. 5A is a conceptual diagram illustrating a method for calculating the evaluation value E. As shown in FIG. 5A, a certain measurement point a ₂ constituting the position information at time t is located within a distance ε with respect to a certain measurement point b constituting the position information at time t−1. Therefore, Formula (1) becomes f (x) = 0, and no penalty is imposed on the evaluation value E. On the other hand, another measurement point a ₁ constituting the position information at time t is not located within the distance ε with respect to the measurement point b. Therefore, Formula (1) becomes f (x) = 1, and a penalty of 1 is imposed on the evaluation value E.

FIG. 5B shows a state where the measurement points a _{1 to} a ₃ constituting the position information at time t are rotationally moved. FIG. 5C is a diagram showing how the evaluation value E is calculated at the measurement points a _{1 to} a ₃ and the measurement points b _{1 to} b ₃ that have been rotated and moved.

In FIG. 5C, the measurement points a ₁ and a ₂ are located within the distance ε from the measurement points b ₁ and b ₂ , but the measurement point a ₃ does not exist within the distance ε from the measurement point b _3. . Therefore, the evaluation value E is 1.

  Returning to FIG. 1, the estimating unit 13 changes the parallel movement amount T and the rotational movement amount R of the position information at time t, searches for the parallel movement amount T and the rotational movement amount R that maximize the degree of coincidence, and Estimate the position. That is, the estimation unit 13 searches for the rotational movement amount R and the parallel movement amount T that minimize the evaluation value E shown in Expression (1). A technique for performing such a search, creating a map based on the search result, and estimating the self-position is called SLAM.

  Specifically, in the position information at time t−1, the estimation unit 13 translates the local coordinate space of the position information at time t by the parallel movement amount T and rotates it by the rotational movement amount R, and at time t The process of calculating the evaluation value E between the position information of −1 and the position information at time t is repeated, and the parallel movement amount T and the rotational movement amount R that minimize the evaluation value E are searched.

  Then, the estimation unit 13 obtains the parallel movement amount T that minimizes the evaluation value E as the movement amount of the moving body from time t-1 to time t, and adds the movement amount to the position of the moving body at time t-1. Is obtained as the position of the moving body at time t.

  Further, the estimation unit 13 obtains the rotational movement amount R that minimizes the evaluation value E as the amount of change in the direction of the moving body from time t-1 to time t, and the amount of change in the direction of the moving body at time t-1. Is added as the direction of the moving object at time t.

  Here, as the direction of the moving object at the time t, for example, an angle of the reference direction of the moving object at the time t with respect to a predetermined reference direction in a two-dimensional global coordinate space indicating the surrounding space of the moving object is adopted. Can do. As the position of the moving body at time t, for example, the position of the moving body in the global coordinate space can be employed.

  As the rotational movement amount R, for example, a 2 × 2 matrix indicating rotation of the moving body at time t with respect to the moving body at time t−1 can be employed. Further, as the parallel movement amount T, for example, it is possible to employ values indicating x and y components indicating the movement amount of the moving body from time t-1 to time t.

  FIG. 6 is a diagram illustrating the reason why a matching error occurs between the position information at time t-1 and the position information at time t in a dynamic environment when the L2 norm is adopted as the evaluation value E. At time t, the moving object 41 is a moving object such as a human. The stationary object 42 is a stationary object such as a wall. If only the stationary object 42 exists in the position information at the time t and the time t-1, when the minimum value of the evaluation value E is obtained, the stationary object 42 at the time t-1 matches the stationary object 42 at the time t. And no error occurs.

  However, since the moving object 41 moves every moment when the moving object 41 exists, the positional relationship between the moving object 41 and the stationary object 42 at time t and the positional relationship between the moving object 41 and the stationary object 42 at time t−1. Will be different.

  Therefore, even if the minimum value of the evaluation value E is obtained, when the L2 norm is used as the evaluation value E, the stationary object at time t−1 is affected by the moving object 41 with respect to the stationary object 42 at time t−1. 42 cannot be matched. As a result, a matching error occurs in the stationary object 42.

  Thus, although a matching error occurs in a dynamic environment, the present inventors have found that the matching error can be reduced by calculating the evaluation value E using the L0 norm. Therefore, in this embodiment, the evaluation value E is calculated using the L0 norm.

  The norm indicates a distance, and the L2 norm, which is the sum of squares of the distance, is commonly used. When searching for the minimum evaluation value E using the L2 norm, a non-linear least square method can be used when obtaining an optimal solution. On the other hand, the process for obtaining the minimum evaluation value E using the L0 norm is classified as a discrete optimization problem, and thus is NP-hard. Therefore, when the L0 norm is adopted, it is necessary to search for the minimum evaluation value E by changing the parallel movement amount T and the rotational movement amount R of the position information measured at time t in a brute force manner.

  Returning to FIG. 1, the map generation unit 14 plots the position and orientation of the moving object at time t estimated by the estimation unit 13 in the global coordinate space, and uses the plotted position and orientation as a reference at time t. A two-dimensional map of the surrounding space is generated by plotting the position information.

  The position sensor 16 is configured by a laser range finder, acquires position information in time series at a constant period, and supplies the position information to the position information acquisition unit 11. As the laser range finder, SICK's LMS100 can be adopted. FIG. 7A is an external view of the LCK 100 of SICK. FIG. 7B is an external view of a moving body according to the embodiment of the present invention. As shown in FIG. 7B, the position sensor 16 is attached to the front side of the moving body.

  FIG. 8 is a table showing the specifications of the position sensor 16. As shown in FIG. 8, the position sensor 16 has an angle of view of 270 degrees, a resolution of 0.5 degrees, a measurable range of 20 m, a frequency of 30 Hz, and the number of measurement points that can be acquired. 540.

  Returning to FIG. 1, the odometry calculation unit 15 calculates the odometry of the moving body according to the measurement data supplied from the rotation amount sensor 17 attached to the wheel of the moving body, and supplies the odometry to the estimation unit 13. Here, odometry is the self-position of the moving body at time t with respect to time t-1 estimated according to the measurement data from the rotation amount sensor 17. In the present embodiment, the odometry calculation unit 15 calculates odometry in time series with the same cycle as the position sensor 16.

  The rotation amount sensor 17 is attached to each of the pair of left and right wheels of the moving body, and supplies measurement data indicating the rotation amount of the wheels of the moving body to the odometry calculation unit 15. Here, the rotation amount sensor 17 acquires measurement data at the same cycle as the position sensor 16.

  As shown in FIG. 7B, the moving body includes a pair of left and right wheels 91 and 91 provided at the front and a pair of left and right wheels 92 and 92 provided at the rear. The pair of rotation amount sensors 17 are attached to the wheels 92 and 92, respectively, and measure the rotation amounts of the wheels 92 and 92, respectively.

  In addition, as shown in FIG. 7B, the moving body is attached to the frame 93 provided on the upper part of the wheels 91, 92, the computer 94 placed on the front side of the bottom surface 931 of the frame 93, and the back side of the bottom surface 931. Motor 95 and the like. The position sensor 16 is attached to a bar 932 provided on the upper part of the frame 93.

  The computer 94 implements the functions of the position information acquisition unit 11 to the odometry calculation unit 15 described above. The computer 94 is mounted with a drive control program for the moving body, and controls the driving of the moving body. For example, the computer 94 refers to the two-dimensional map generated by the map generation unit 14 and drives the motor 95 so as to move toward a predetermined goal while avoiding a collision with an object.

  The motor 95 is constituted by a pair of motors attached to the wheels 92 and 92, for example, and is driven based on a signal from the computer 94. The pair of left and right motors 95 turns the moving body by changing the amount of rotation of the wheels 92 and 92 in accordance with a signal from the computer 94.

  Next, the operation of the self-position estimation apparatus shown in FIG. 1 will be described. FIG. 2 is a flowchart showing the operation of the self-position estimation apparatus according to the embodiment of the present invention. First, the position information acquisition unit 11 acquires position information at time t (S1).

  Next, the estimation unit 13 generates a plurality of search candidates by changing the parallel movement amount T and the rotational movement amount R of the position information at time t, and sends the evaluation value E of each search candidate to the matching degree calculation unit 12. A search candidate that minimizes the evaluation value E is calculated (S2).

  Next, the estimation unit 13 estimates the self position of the moving body from the rotational movement amount R and the parallel movement amount T of the search candidate searched in S2 (S3). Next, the map generation unit 14 plots the self-position estimated by the estimation unit 13 in a two-dimensional coordinate space, and generates a two-dimensional map of the surrounding space of the moving object (S4). When the process of S4 ends, the process returns to S1, and the processes of S1 to S4 are repeated.

  FIG. 3 is a flowchart showing details of the search process shown in S2 of FIG. First, the estimation unit 13 assigns 0 to the variable k and initializes the variable k (S21).

  Next, the estimation unit 13 generates n1 (n1 is an integer of 2 or more) search candidates. Specifically, the estimation unit 13 obtains a prior probability of the position of the moving object from the self position of the moving object estimated by the odometry calculation unit 15, and randomly determines the rotational movement amount R of the n1 pattern according to the prior probability. The parallel movement amount T is determined, the position information at time t is shifted with respect to the position information at time t-1 using the determined rotational movement amount R and parallel movement amount T of the n1 pattern, and n1 search candidates are obtained. Generate (S22).

  Here, as the prior probability, a probability that the number of search candidates increases as the position is closer to the self-position estimated by the odometry calculation unit 15 can be employed. And n1 search candidates are produced | generated by the lottery process according to this prior probability.

  FIG. 9 is a diagram for explaining processing for generating search candidates. In FIG. 9, the process proceeds toward (A) to (D). Each point in FIG. 9 represents the position of a point obtained by collecting the elements of the rotational movement amount R and the parallel movement amount T of the position information at time t. In FIG. 9A, n1 search candidates are generated by the process of S22.

  Next, the degree-of-matching calculation unit 12 calculates an evaluation value E with respect to the position information at time t using Equation (1) for each of the n1 search candidates (S23).

  Next, when the variable k is less than the constant K (NO in S24), the estimation unit 13 calculates the weight value wj of each of the n1 search candidates from the evaluation value E of the n1 search candidates (S25). Here, the weight value wj is defined by Equation (2).

wj = exp ((m−E (R, T)) / k) (2)
Here, m and k are constants. As m, for example, the assumed maximum value of the evaluation value E can be adopted. j is an index for specifying a search candidate. As shown in Expression (2), the weight value wj increases as the evaluation value E decreases.

  Next, the estimation unit 13 extracts n2 (n2 <n1) search candidates from the n1 search candidates based on the weight value wj (S26). In this case, the estimation unit 13 may perform a lottery process in which the lottery probability becomes higher as the value of the weight value wj is larger, and may extract n2 search candidates. Search candidates may be extracted.

  By the process of S26, n2 search candidates are extracted as shown in FIG. 9B from the n1 search candidates shown in FIG. 9A. In the example of FIG. 9B, since n2 = 3 is set, it can be seen that three search candidates are extracted.

  Returning to FIG. 3, in S27, the estimation unit 13 generates n3 search candidates for each of the n2 search candidates. In this case, the estimation unit 13 determines the rotational movement amount R and the parallel movement amount T of the n3 pattern according to the normal distribution of (σx, σy, σθ) with reference to the origin of n2 search candidates, and n3 pieces The search candidates may be generated.

  For example, when n2 = 3, n3 search candidates are generated with reference to the origin of three search candidates. In this case, the number of n3 pieces may be changed according to the value of the weight value wj of n2 search candidates. For example, it is assumed that search candidates in the case of n2 = 3 are Z1 to Z3, and the weight values of the search candidates Z1 to Z3 are w1 to w3. However, w1> w2> w3. In this case, if the number of search candidates generated from the search candidates Z1 to Z3 is n3 (1) to n3 (3), the values of n3 (1) to n3 (3) are determined according to the weight value w1. Then, search candidates may be generated. Note that n3 (1)> n3 (2)> n3 (3).

  As shown in FIG. 9C, it can be seen that the processing of S27 generates n3 search candidates based on n2 search candidates, and a total of n2 × n3 search candidates are generated.

  Next, the estimation unit 13 increments the variable k by 1 (S28), and returns the process to S23. In S24, when the variable k becomes equal to or greater than the constant K (NO in S23), the estimation unit 13 specifies a search candidate having the smallest evaluation value E (S29).

  In S23 after the second loop, the evaluation value E is calculated for all search candidates calculated in S27. In this way, the estimation unit 13 repeats the processes of S23 to 28 K times, and specifies a search candidate having the smallest evaluation value E. Then, in generating search candidates, priority sampling is performed in which search candidates having a rotational movement amount R and a parallel movement amount T that are likely to be solutions are generated preferentially. Therefore, it becomes possible to efficiently obtain search candidates as solutions.

  As a method for obtaining a solution for minimizing the evaluation value E, a method using Bayesian estimation is known. This technique is used in the technique of calculating the evaluation value E using the L2 norm to prevent the reliability of position estimation from becoming temporarily poor due to the influence of a moving object and to increase the robustness of position estimation. .

  In the present embodiment, as shown in S26, the search candidates are narrowed down from n1 to n2, and the subsequent processing is performed. As described above, this embodiment employs a technique of narrowing down the solution and proceeding to the next step, such as maximuma-posteriori (MAP) estimation instead of Bayesian estimation. For this reason, there remains concern about the accuracy of the solution, but the L0 norm is robust against moving objects, so there is no problem even if MAP estimation is applied. As a result, an increase in the number of search candidates can be suppressed, and the process for searching for the minimum evaluation value E can be speeded up.

  In FIG. 3, S22 corresponds to the first process, S25 and S26 correspond to the second process, S27 corresponds to the third process, and S24 and S29 correspond to the fourth process.

  Next, details of the evaluation value calculation process shown in S23 of FIG. 3 will be described. LSH (Locality Sensitive Hashing) is known as an algorithm for converting vector data into hash values (Alexandr Andoni, Mayur Datar, Nicole Immorlica, Piotr Indyk, and Vahab Mirrokni: “Locality-Sensitive Hashing Scheme Based on p-Stable Distributions ”, Nearest Neighbor Methods in Learning and Vision, MIT Press, 2006.).

  As described above, in the evaluation value E using the L0 norm, the process of searching for the minimum evaluation value E is difficult to optimize and is often calculated by the brute force method. Accordingly, the amount of calculation increases explosively depending on the search range and resolution. Therefore, as a method for calculating the evaluation value E, it is preferable to employ a method as fast as possible.

  When searching for the minimum evaluation value E in the evaluation value E using the L0 norm, the calculation load when obtaining the vicinity of r is large. The amount of calculation of a simple algorithm for obtaining the vicinity of r (for example, Bruteforce) is O (n), but if kd-tree or a Voronoi diagram is used, the amount of calculation can be reduced to O (logn).

  However, LSH is known to be much faster than these approaches, though approximate. Therefore, in this embodiment, LSH is adopted. LSH has the property of taking the same hash value with higher probability as the data distance is closer. Considering that only data classified into the same hash value is a candidate for a neighboring point, the amount of calculation can be greatly reduced.

  FIG. 4 is a flowchart showing details of the calculation process of the evaluation value E. First, the coincidence calculation unit 12 generates a plurality of hash tables from the position information at time t−1 (S41). Here, the hash table is a table indicating whether or not there is a measurement point at each position in the lattice space by projecting each measurement point of the position information at time t-1 onto the lattice space using a predetermined hash function. The lattice space is a space generated by dividing the local coordinate space of the position information at time t-1 into a lattice shape based on the distance ε. Here, the hash function h is expressed by Expression (3).

h (b) = <(p · b + q) / ε> (3)
However, <> represents a maximum integer of (p · b + q) / ε or less. p is determined by a normal distribution with 1 as an average. q is randomly determined in the range of 0 to ε by a uniform distribution. b indicates a measurement point.

  In this embodiment, the following two hash functions hx and hy are employed.

hx (b) = <(p · bx + qx) / ε> (3-1)
hy (b) = <(p · by + qy) / ε> (3-2)
Note that qx and qy are x and y components of q, and are randomly determined in the range of 0 to ε by uniform distribution. bx and by are x and y components of the measurement point b.

  The measurement point b is located in the local coordinate space defined by the x axis and the y axis. Therefore, the coincidence calculation unit 12 obtains the value of the x coordinate in the lattice space of the measurement point b using hx (b) as shown in the equation (3-1), and as shown in the equation (3-2). Then, the value of the y coordinate in the lattice space of the measurement point b is obtained using hy (b).

  FIG. 10 is a conceptual diagram showing a hash table. FIG. 10A shows a lattice space set in the local coordinate space of the position information at time t-1.

  FIG. 10B shows hash tables T1 to T3 generated based on the lattice space shown in FIG. In FIG. 10A, the measurement point b is plotted at a position before the calculation of <> in the expressions (3-1) and (3-2). In FIG. 10A, for example, a measurement point b exists on the grid in the first row and the first column. Accordingly, when the hash value hx (b), hy (b) is obtained by performing <> operation on the measurement point b, the measurement point b is one line 1 in the hash table T1, as shown in FIG. 10B. It will be located in the bin of the row. Therefore, for example, a label of 1 indicating the presence of the measurement point b is set in the bin of the first row and the first column of the hash table T1. On the other hand, in FIG. 10A, for example, the measurement point b does not exist in the grid in the first row and the third column. Therefore, as shown in FIG. 10B, for example, a label of 0 indicating that the measurement point b does not exist is set in the bin of the first row and the third column of the hash table T1.

  In this way, the measurement point b is projected onto the lattice space using the equations (3-1) and (3-2), and the hash table T1 is generated.

  Here, the coincidence degree calculation unit 12 prepares a plurality of types of hash functions represented by the expressions (3-1) and (3-2), and generates a plurality of hash tables. In the example of FIG. 10B, three hash functions h1 (= (h1x (b), h1y (b))) to h3 (= (h3x (b), h3y (b))) are used. Hash tables T1 to T3 are generated.

  In this case, the coincidence degree calculation unit 12 may generate a plurality of types of hash functions by setting the values of p, qx, and qy shown in equations (3-1) and (3-2) to different values. .

  In this way, by adopting a plurality of hash tables T1 to T3, it is said that there is no hash value in the hash table even though the measurement point a is within the distance ε of the measurement point b. Such a search error can be prevented.

  In the above description, the hash functions indicated by (3-1) and (3-2) are used, but the hash functions indicated by (3-1 ') and (3-2') may be adopted.

hx (b) = <(bx + qx) / pε> (3-1 ′)
hy (b) = <(by + qy) / pε> (3-2 ′)

  The difference from (3-1) and (3-2) is that p is moved to the denominator and multiplied by ε. As a result, the grid length set in the local coordinate space changes, and the hash table can have more diversity.

  Returning to FIG. 4, in S <b> 42, the coincidence calculation unit 12 projects each measurement point a of the position information at time t onto the projection space using the hash functions h <b> 1 to h <b> 3, and the hash value h <b> 1 (a) of the measurement point a. -H3 (a) is obtained.

  Next, if any of the obtained hash values h1 (a) to h3 (a) exists in the corresponding hash tables T1 to T3, the coincidence calculation unit 12 evaluates the evaluation value E without penalizing the evaluation value E. E is calculated (S43).

  For example, if a label of 1 is set in the bin of the hash table T1 indicated by the hash value h1 (a), 0 is set in the bins of the hash tables T2 and T3 indicated by the hash values h2 (a) and h3 (a). Is set, the measurement point b is regarded as being located within the distance ε from the measurement point a, and no penalty is imposed on the evaluation value E.

  The coincidence degree calculation unit 12 performs such processing for each measurement point a, and calculates the total value of the assigned penalty as the evaluation value E. Thus, by generating a hash table in advance based on the measurement point b at time t−1, it is possible to reduce the amount of calculation required for referring to the hash table to O (1).

  In the above embodiment, the position information is two-dimensional. However, the position information is not limited to this and may be three-dimensional. In this case, for example, a depth sensor may be employed as the position sensor 16. Then, the surrounding space may be represented by a three-dimensional global coordinate space, and the estimated position of the moving body may be plotted to adopt a three-dimensional map.

<Experimental example>
Next, an experiment of a moving body to which the self-position estimation apparatus according to the embodiment of the present invention is applied will be described.

<Experimental environment>
In this experiment, a wheelchair-type mobile robot EMC-230 was adopted as a moving body, and a laser range finder (LRF) having the specifications shown in FIG. 8 was attached thereto for sensing. Information sensed by the LRF is expressed in polar coordinates and is equally spaced in the angular direction. Since the density of points becomes sparse as the distance from the sensor increases, segmentation is performed and interpolation is performed so that the distance between points in the segment is equal to or less than ε.

  The first floor of the information building of the Nara Institute of Science and Technology Graduate School was used as the experiment site. The dynamic environment was measured by LRF, and the position information at time t and time t−1 was matched. In this experiment, the dynamic environment was measured while the moving body was stationary. The dynamic environment was a hallway with a width of about 10m, and a range of 20m in depth, and about 25 pedestrians were moving.

  FIG. 11 (A) is a diagram showing an experimental place, and FIG. 11 (B) is a diagram showing a dynamic environment in which a person moves to and from this experimental place.

  FIG. 12 shows a two-dimensional map of the surrounding space generated in a static environment in which no person goes to the experimental place. This two-dimensional map is generated using RBPF-SLAM and can be regarded as a true value.

<Experimental result>
In this experiment, the search range when searching for the minimum evaluation value E was set to plus or minus 20 [cm] in the x-axis direction and y-axis direction, respectively, and the angle was set to plus or minus 0.3 [rad]. In this search range, the rotational movement amount R and the parallel movement amount T are changed by the round robin method in steps of 1.0 [cm] and 0.01 [rad] (= about 0.57 [deg]). The minimum value of the evaluation value E was searched.

  And when L0 norm was employ | adopted as the evaluation value E, when the L2 norm was employ | adopted, the position estimation precision of the matching at the time of employ | adopting M-estimator was compared. At this time, the following evaluation function ρ (d) was used for the distance d based on the Biweight method for the M-estimator.

^{ρ (d) = B 2/} 2 (d ≧ B)
or
^{(B 2/2) (1-} (1- (d / B) 2) 3) (d <B)

  Here, B is an arbitrary residual value and is set to 0.01 [m]. FIG. 13 is a table showing experimental results when the L2 norm, M-estimator, and L0 norm are used as the evaluation value E.

  In the L2 norm, a matching error of a maximum of 14 [cm] occurs, and the mean square error (RMSE) is also about 10 [cm]. On the other hand, in the L0 norm, the matching error is 1 [cm] at the maximum, and is found to be within the measurement error of the LRF.

  Next, the position information for 200 frames was measured by LRF, and the position information of adjacent frames was matched using the L2 norm, M-estimator, and L0 norm to generate a two-dimensional map.

  FIG. 14 is a two-dimensional map created when the L2 norm is adopted. FIG. 15 is a two-dimensional map created when the M-estimator is employed. FIG. 16 is a two-dimensional map created when the L0 norm is adopted.

  As shown in FIG. 14, when the L2 norm is adopted, it can be seen that due to the matching error, the moving body is misrecognized as moving, and the generation of the two-dimensional map has failed.

  As shown in FIG. 15, when the M-estimator was adopted, there was no large matching error, but due to the accumulated matching error, the mobile object was misrecognized and the generation of the two-dimensional map failed. Yes. On the other hand, as shown in FIG. 16, when the L0 norm is adopted, matching errors between frames hardly occur, and the self-position estimation apparatus can measure the state where the moving body is stationary.

  As can be seen from the experimental results, it can be seen that when the L0 norm is employed, the robustness is higher in a dynamic environment than when other methods are employed.

  Next, in order to verify the superiority of the self-position estimation apparatus according to the present embodiment, the calculation time was compared when LSH was adopted, when Brute-force was adopted, and when kd-tree was adopted. . The evaluation value E was L0 norm. The search range is the same as the above value. Total 24,000 evaluation values E are calculated, the total calculation time (Average calculation time), the number of L0 norms that can be calculated per second (Frequency of calculation aec.), And the average of the measurement points where r neighbors were found The number (Average match points) was determined.

  FIG. 17 is a table summarizing experimental results in an experiment in which calculation times are compared using True-force, kd-tree, and LSH.

  As shown in FIG. 17, it can be seen that the total calculation time is shorter when LSH is used compared to the true-force and kd-tree. However, LSH has as few as 15 average match points at which r neighboring points can be found, as compared to true-force and kd-tree. This is because the neighborhood point search by LSH is approximate.

  The influence of the difference in the number of coincidence points is examined by generating a two-dimensional map by matching a plurality of frames. FIG. 18 is a two-dimensional map generated using RBPF-SLAM in a static environment. This two-dimensional map is regarded as a true value.

  FIG. 19 is a two-dimensional map generated by using position information measured while moving a moving body in a dynamic environment and matching the position information with the evaluation value E using the L0 norm. The points present in this two-dimensional map indicate the trajectory of the moving person.

  FIG. 20 is a two-dimensional map generated by matching the positional information with the evaluation value E using the L2 norm. Comparing the shapes of the edges of both two-dimensional maps, it can be seen that the L0 norm generates a two-dimensional map relatively accurately even in a dynamic environment.

  Furthermore, a large-scale two-dimensional map as shown in FIG. 21 was generated. FIG. 21 is a two-dimensional map generated using RBPF-SLAM. This two-dimensional map is regarded as a true value.

  Since only matching between frames is performed in the L0 norm, minute matching errors accumulate, and the map is distorted as shown in FIG. FIG. 22 is a two-dimensional map generated using the L0 norm. This two-dimensional map is generated under the same environment as in FIG. The distortion appearing in FIG. 22 can be improved by applying a technique such as loop closing.

  The technical features of the above-described self-position estimation apparatus and moving body are summarized as follows.

  (1) The above self-position estimation apparatus is a self-position estimation apparatus that estimates the self-position of a moving body, and obtains position information that obtains position information of each object existing in the surrounding space of the moving body in time series. And the first position information acquired at the first time by the position information acquisition unit into the second position information acquired at the second time which is the time before or after the first time in time series. A degree of coincidence calculation unit for calculating a degree of coincidence between the first position information and the second position information when the first position information and the second position information are translated, and a change in a parallel movement amount and a rotation movement amount of the first position information; And searching for the parallel movement amount and the rotational movement amount that maximize the degree of coincidence, and estimating the self-position, wherein the coincidence degree calculating unit constitutes the first position information The second position information is constructed within a certain distance for a certain measurement point. If there measurement point exists, the matching degree in imparting a predetermined point, and calculates the total value of the grant and the point as the matching degree.

  According to this configuration, the first position information is shifted with respect to the second position information, and the parallel movement amount and the rotational movement amount that maximize the matching degree are searched. The degree of coincidence is given a predetermined point if a measurement point having the second position information is located within a certain distance with respect to a measurement point constituting the first position information. That is, in this configuration, the degree of coincidence is calculated using a robust L0 norm even in a dynamic environment including a moving object as well as a stationary object. Therefore, the self position of the moving object can be accurately estimated in an unknown environment including the moving object.

  (2) The degree-of-match calculation unit generates a lattice space by dividing the peripheral space into a lattice shape based on the fixed distance, and uses the predetermined hash function to calculate each measurement point of the second position information. Projecting onto the lattice space, generating a hash table indicating the presence or absence of the measurement points at each position in the lattice space, and projecting each measurement point of the first position information onto the lattice space using the hash function When the position of a certain measurement point in the lattice space is present in the hash table, the point is preferably given to the degree of coincidence.

  According to this configuration, each measurement point of the second position information is projected onto the lattice space using a predetermined hash function, and a hash table indicating the presence / absence of each measurement point is generated. Then, each measurement point of the first position information is also projected onto the lattice space using a hash function, and if the position of the projected measurement point exists in the hash table, it is in the vicinity of a certain measurement point constituting the first position information. A certain measurement point constituting the second position information is considered to exist, and a predetermined point is given to the degree of coincidence. As described above, in this configuration, since the degree of coincidence is calculated by comparing the hash values of the measurement points of the first and second position information, the parallel movement amount and the rotational movement amount that maximize the degree of coincidence are calculated. It can be obtained at high speed.

  (3) The coincidence calculation unit generates a plurality of the hash tables using a plurality of different hash functions, and projects a measurement point having the first position information onto the lattice space using each hash function. When any position exists in the corresponding hash table, it is preferable to give the point to the degree of coincidence.

  According to this configuration, since a plurality of hash tables are employed, it happens to happen even though the measurement point with the first position information is within the range of the distance ε with respect to the measurement point with the second position information. Search errors such as no hash value in the hash table can be prevented.

  (4) The estimation unit randomly changes the parallel movement amount and the rotational movement amount to generate n1 (n1 is a positive integer) search candidates from the first position information, and Based on the matching degree of the search candidates, a weight value of each search candidate is calculated, and based on the calculated weight value, n2 (n2 <n1) search candidates are extracted, and the extracted n2 pieces For each search candidate, a third process of generating n3 (n3 is a positive integer) search candidates by changing the parallel movement amount and the rotational movement amount based on a predetermined probability distribution, and the n3 It is preferable to include a fourth process for calculating the weight value for each search candidate and obtaining a search candidate having the maximum calculated weight value as a search solution.

  According to this configuration, the first position information is shifted to a position that is likely to be a solution, a plurality of search candidates are generated, and the degree of coincidence with the second position information is obtained. Therefore, search candidates that maximize the degree of matching can be calculated efficiently.

  (5) Preferably, in the fourth process, the second and third processes are repeated a predetermined number of times, and a search candidate that maximizes the weight value is obtained as the search solution.

  According to this configuration, since the second and third processes are repeated, a search candidate that maximizes the degree of coincidence can be calculated more accurately.

  (6) An odometry calculating unit that calculates odometry of the moving body is further provided, and the first process calculates the parallel movement amount and the rotational movement amount based on a prior probability of the position of the moving body obtained from the odometry. It is preferable to generate n1 search candidates by randomly changing the search candidates.

  According to this configuration, in the first process, search candidates can be generated by shifting the first position information with a focus on positions that are likely to be solutions, and search accuracy can be further improved.

  (7) The mobile body includes any one of (1) to (6) and a position sensor that acquires the position information.

According to this configuration, it is possible to provide a moving body that can move while accurately estimating its own position in an unknown environment.

Claims (9)

A self-position estimation device for estimating a self-position of a moving object,
A position information acquisition unit that acquires, in a time series, position information of each object existing in the surrounding space of the moving body;
The first position information acquired at the first time by the position information acquisition unit is parallel to the second position information acquired at the second time which is the time before or after the first time in time series. A degree of coincidence calculation unit that calculates the degree of coincidence between the first position information and the second position information when moved and rotated;
An estimation unit that changes the parallel movement amount and the rotational movement amount of the first position information, searches for the parallel movement amount and the rotational movement amount that maximizes the degree of coincidence, and estimates the self-position;
The coincidence degree calculation unit assigns a predetermined point to the coincidence degree when a certain measurement point constituting the second position information exists within a certain distance with respect to a certain measurement point constituting the first position information. And a self-position estimation device that calculates the total value of the given points as the degree of coincidence.   The coincidence calculation unit generates a lattice space by dividing the surrounding space into a lattice shape based on the fixed distance, and uses the predetermined hash function to set each measurement point of the second position information in the lattice space. Projecting, generating a hash table indicating the presence / absence of the measurement point at each position in the lattice space, projecting each measurement point of the first position information onto the lattice space using the hash function, and performing a measurement The self-position estimating apparatus according to claim 1, wherein when the position of the point in the lattice space exists in the hash table, the point is given to the degree of coincidence.   The coincidence calculation unit generates a plurality of the hash tables using a plurality of different hash functions, and projects a measurement point having the first position information onto the lattice space using each hash function, The self-position estimation apparatus according to claim 2, wherein the point is assigned to the degree of coincidence when the position of is present in a corresponding hash table. The estimation unit includes
A first process for generating n1 (n1 is a positive integer) search candidates from the first position information by randomly changing the parallel movement amount and the rotational movement amount;
A second process of calculating a weight value of each search candidate based on the matching degree of each search candidate and extracting n2 (n2 <n1) search candidates based on the calculated weight value;
A third process for generating n3 (n3 is a positive integer) search candidates by changing the parallel movement amount and the rotational movement amount based on a predetermined probability distribution for each of the extracted n2 search candidates; ,
4. A fourth process according to claim 1, further comprising: a fourth process for calculating the weight value for each of the n3 search candidates and obtaining a search candidate having the maximum calculated weight value as a search solution. Self-position estimation device.   5. The self-position estimation apparatus according to claim 4, wherein in the fourth process, the second and third processes are repeated a predetermined number of times, and a search candidate having the maximum weight value is obtained as the search solution. An odometry calculating unit for calculating odometry of the mobile body;
6. The first process generates n1 search candidates by randomly changing the parallel movement amount and the rotational movement amount based on a prior probability of the position of the moving body obtained from the odometry. The self-position estimation apparatus described. A self-position estimation apparatus according to any one of claims 1 to 6;
A moving body comprising: a position sensor that acquires the position information. A self-position estimation method for estimating a self-position of a moving object,
A position information acquisition step in which a computer acquires position information of each object existing in the surrounding space of the moving body in time series;
The computer replaces the first position information acquired at the first time by the position information acquisition step with the second position information acquired at the second time which is time before or after the first time. A degree of coincidence calculation step of calculating a degree of coincidence between the first position information and the second position information when translated and rotationally moved.
An estimation in which the computer searches the parallel movement amount and the rotational movement amount to maximize the matching degree by estimating the self-position by changing the parallel movement amount and the rotational movement amount of the first position information. With steps,
In the coincidence calculation step, if there is a measurement point with the second position information within a certain distance from the measurement point with the first position information, a predetermined point is given to the coincidence, and the addition of the points A self-position estimation method for calculating a value as the degree of coincidence. A self-position estimation program for estimating the self-position of a moving object,
A position information acquisition unit that acquires, in a time series, position information of each object existing in the surrounding space of the moving body;
The first position information acquired at the first time by the position information acquisition unit is parallel to the second position information acquired at the second time which is the time before or after the first time in time series. A degree of coincidence calculation unit that calculates the degree of coincidence between the first position information and the second position information when moved and rotated;
A computer serving as an estimation unit that changes the parallel movement amount and the rotational movement amount of the first position information, searches for the parallel movement amount and the rotational movement amount that maximizes the degree of coincidence, and estimates the self-position. Function
The coincidence calculation unit gives a predetermined point to the coincidence when the measurement point with the second position information exists within a certain distance from the measurement point with the first position information, and adds the points A self-position estimation program for calculating a value as the degree of coincidence.

Images